diff --git a/modules/odm_dem2mesh/src/main.cpp b/modules/odm_dem2mesh/src/main.cpp
index 4dbd6ac8..9036a896 100644
--- a/modules/odm_dem2mesh/src/main.cpp
+++ b/modules/odm_dem2mesh/src/main.cpp
@@ -1,6 +1,7 @@
 #include <iostream>
 #include <string>
 #include <fstream>
+#include <cmath>
 #include "CmdLineParser.h"
 #include "Logger.h"
 #include "Simplify.h"
@@ -54,7 +55,7 @@ cmdLineReadable
 	Verbose( "verbose" );
 
 cmdLineReadable* params[] = {
-    &InputFile , &OutputFile , &Verbose ,
+    &InputFile , &OutputFile , &MaxVertexCount , &Verbose ,
     NULL
 };
 
diff --git a/opendm/mesh.py b/opendm/mesh.py
index c02086c2..b48bfcf1 100644
--- a/opendm/mesh.py
+++ b/opendm/mesh.py
@@ -14,8 +14,8 @@ def create_25dmesh(inPointCloud, outMesh, dsm_resolution=0.05, depth=8, samples=
     # Create temporary directory
     mesh_directory = os.path.dirname(outMesh)
     tmp_directory = os.path.join(mesh_directory, 'tmp')
-    #if os.path.exists(tmp_directory):
-    #    shutil.rmtree(tmp_directory)
+    if os.path.exists(tmp_directory):
+        shutil.rmtree(tmp_directory)
     os.mkdir(tmp_directory)
     log.ODM_INFO('Created temporary directory: %s' % tmp_directory)
 
@@ -38,91 +38,29 @@ def create_25dmesh(inPointCloud, outMesh, dsm_resolution=0.05, depth=8, samples=
     mesh = dem_to_mesh(os.path.join(tmp_directory, 'mesh_dsm.tif'), outMesh, maxVertexCount, verbose)
 
     # Cleanup tmp
-    #if os.path.exists(tmp_directory):
-    #    shutil.rmtree(tmp_directory)
+    if os.path.exists(tmp_directory):
+        shutil.rmtree(tmp_directory)
 
     return mesh
 
 def dem_to_mesh(inGeotiff, outPointCloud, maxVertexCount, verbose=False):
-    log.ODM_INFO('Sampling points from DSM: %s' % inGeotiff)
+    log.ODM_INFO('Creating mesh from DSM: %s' % inGeotiff)
 
     kwargs = {
         'bin': context.odm_modules_path,
         'outfile': outPointCloud,
         'infile': inGeotiff,
+        'maxVertexCount': maxVertexCount,
         'verbose': '-verbose' if verbose else ''
     }
 
-    # TODO: finish here.
     system.run('{bin}/odm_dem2mesh -inputFile {infile} '
          '-outputFile {outfile} '
-         '- '
+         '-maxVertexCount {maxVertexCount} '
          ' {verbose} '.format(**kwargs))
 
     return outPointCloud
 
-# Old Python implementation of dem_to_points
-# def dem_to_points(inGeotiff, outPointCloud):
-#     log.ODM_INFO('Sampling points from DSM: %s' % inGeotiff)
-
-#     image = GeoImage.open([inGeotiff], bandnames=['z'], nodata=-9999)
-#     arr = image['z'].read_raw()
-
-#     mem_file = BytesIO()
-
-#     xmin, xmax, ymin, ymax = image.extent().x0(), image.extent().x1(), image.extent().y0(), image.extent().y1()
-#     ext_width, ext_height = xmax - xmin, ymax - ymin
-#     arr_height, arr_width = arr.shape
-#     vertex_count = (arr_height - 4) * (arr_width - 4)
-#     skirt_points = 0
-
-#     skirt_height_threshold = 1 # meter
-#     skirt_increments = 0.1
-
-#     for x in range(2, arr_width - 2):
-#         for y in range(2, arr_height - 2):
-#             z = arr[y][x]
-#             tx = xmin + (float(x) / float(arr_width)) * ext_width
-#             ty = ymax - (float(y) / float(arr_height)) * ext_height
-#             mem_file.write(struct.pack('ffffff', tx, ty, z, 0, 0, 1))
-
-#             # Skirting
-#             for (nx, ny) in ((x, y + 1), (x, y - 1), (x + 1, y), (x - 1, y)):
-#                 current_z = z
-#                 neighbor_z = arr[ny][nx]
-
-#                 if current_z - neighbor_z > skirt_height_threshold:
-#                     while current_z > neighbor_z:
-#                         current_z -= skirt_increments
-#                         mem_file.write(struct.pack('ffffff', tx, ty, current_z, 0, 0, 1))
-#                         skirt_points += 1
-
-#                     mem_file.write(struct.pack('ffffff', tx, ty, neighbor_z, 0, 0, 1))
-#                     skirt_points += 1
-
-#     log.ODM_INFO("Points count: %s (%s samples, %s skirts)", vertex_count + skirt_points, vertex_count, skirt_points)
-#     log.ODM_INFO('Writing points...')
-
-#     mem_file.seek(0)
-#     with open(outPointCloud, "wb") as f:
-#         f.write("ply\n")
-#         f.write("format binary_%s_endian 1.0\n" % sys.byteorder)
-#         f.write("element vertex %s\n" % (vertex_count + skirt_points))
-#         f.write("property float x\n")
-#         f.write("property float y\n")
-#         f.write("property float z\n")
-#         f.write("property float nx\n")
-#         f.write("property float ny\n")
-#         f.write("property float nz\n")
-#         f.write("end_header\n")
-#         shutil.copyfileobj(mem_file, f)
-
-#     mem_file.close()
-
-#     log.ODM_INFO('Wrote points to: %s' % outPointCloud)
-
-#     return outPointCloud
-
 
 def screened_poisson_reconstruction(inPointCloud, outMesh, depth = 8, samples = 1, maxVertexCount=100000, pointWeight=4, threads=context.num_cores, verbose=False):