Point cloud, textured model transform

opendm/align.py

@@ -1,15 +1,18 @@
 import os
 import shutil
+import json
 import codem
 import dataclasses
+import pdal
+import numpy as np
 from opendm import log
 
-def compute_alignment_matrix(input_laz, align_file, tmp_dir):
-    if os.path.exists(tmp_dir):
-        shutil.rmtree(tmp_dir)
-    os.mkdir(tmp_dir)
+def compute_alignment_matrix(input_laz, align_file, stats_dir):
+    if os.path.exists(stats_dir):
+        shutil.rmtree(stats_dir)
+    os.mkdir(stats_dir)
 
-    conf = dataclasses.asdict(codem.CodemRunConfig(align_file, input_laz, MIN_RESOLUTION=0.2, OUTPUT_DIR=tmp_dir)) # TODO: how to compute this
+    conf = dataclasses.asdict(codem.CodemRunConfig(align_file, input_laz, OUTPUT_DIR=stats_dir))
     fnd_obj, aoi_obj = codem.preprocess(conf)
     fnd_obj.prep()
     aoi_obj.prep()
@@ -29,6 +32,30 @@ def compute_alignment_matrix(input_laz, align_file, tmp_dir):
         None,
     )
 
-    return app_reg.get_registration_transformation()
-    # if os.path.exists(tmp_dir):
-    #     shutil.rmtree(tmp_dir)
+    reg = app_reg.get_registration_transformation()
+    matrix = np.fromstring(reg['matrix'], dtype=float, sep=' ').reshape((4, 4))
+    return matrix
+
+def transform_point_cloud(input_laz, a_matrix, output_laz):
+    pipe = [
+        input_laz,
+        {
+            'type': 'filters.transformation',
+            'matrix': " ".join(list(map(str, a_matrix.flatten()))),
+        },
+        output_laz,
+    ]
+    p = pdal.Pipeline(json.dumps(pipe))
+    p.execute()
+
+def transform_obj(input_obj, a_matrix, output_obj):
+    with open(input_obj, 'r') as fin:
+        with open(output_obj, 'w') as fout:
+            lines = fin.readlines()
+            for line in lines:
+                if line.startswith("v "):
+                    v = np.fromstring(line.strip()[2:] + " 1",  sep=' ', dtype=float)
+                    vt = v.dot(a_matrix)[:3]
+                    fout.write("v " + " ".join(map(str, list(vt))) + '\n')
+                else:
+                    fout.write(line)

opendm/io.py

@@ -85,3 +85,7 @@ def path_or_json_string_to_dict(string):
             raise ValueError("{0} is not a valid JSON file.".format(string))
     else:
         raise ValueError("{0} is not a valid JSON file or string.".format(string))
+    
+def touch(file):
+    with open(file, 'w') as fout:
+        fout.write("Done!\n")

stages/odm_georeferencing.py

@@ -18,7 +18,7 @@ from opendm import point_cloud
 from opendm.multispectral import get_primary_band_name
 from opendm.osfm import OSFMContext
 from opendm.boundary import as_polygon, export_to_bounds_files
-from opendm.align import compute_alignment_matrix
+from opendm.align import compute_alignment_matrix, transform_point_cloud, transform_obj
 
 class ODMGeoreferencingStage(types.ODM_Stage):
     def process(self, args, outputs):
@@ -170,18 +170,44 @@ class ODMGeoreferencingStage(types.ODM_Stage):
 
 
             if tree.odm_align_file is not None:
-                tmp_dir = tree.path("odm_georeferencing", "codem")
-                a_matrix = compute_alignment_matrix(tree.odm_georeferencing_model_laz, tree.odm_align_file, tmp_dir)
-                if a_matrix is not None:
-                    print(a_matrix)
-                    exit(1)
+                alignment_file = os.path.join(tree.odm_georeferencing, 'alignment_done.txt')
+
+                if not io.file_exists(alignment_file) or self.rerun():
+                    stats_dir = tree.path("opensfm", "stats", "codem")
+                    a_matrix = compute_alignment_matrix(tree.odm_georeferencing_model_laz, tree.odm_align_file, stats_dir)
+                    if a_matrix is not None:
+                        log.ODM_INFO("Alignment matrix: %s" % a_matrix)
+
+                        # Align point cloud
+                        unaligned_model = io.related_file_path(tree.odm_georeferencing_model_laz, postfix="_unaligned")
+                        if not os.path.isfile(unaligned_model):
+                            os.rename(tree.odm_georeferencing_model_laz, unaligned_model)
+                        try:
+                            transform_point_cloud(unaligned_model, a_matrix, tree.odm_georeferencing_model_laz)
+                            log.ODM_INFO("Transformed %s" % tree.odm_georeferencing_model_laz)
+                        except Exception as e:
+                            log.ODM_WARNING("Cannot transform point cloud: %s" % str(e))
+                            os.rename(unaligned_model, tree.odm_georeferencing_model_laz)
+                    
+                        # Align textured models
+                        for texturing in [tree.odm_texturing, tree.odm_25dtexturing]:
+                            obj = os.path.join(texturing, "odm_textured_model_geo.obj")
+                            if os.path.isfile(obj):
+                                unaligned_obj = io.related_file_path(obj, postfix="_unaligned")
+                                if not os.path.isfile(unaligned_obj):
+                                    os.rename(obj, unaligned_obj)
+                                try:
+                                    transform_obj(unaligned_obj, a_matrix, obj)
+                                    log.ODM_INFO("Transformed %s" % obj)
+                                except Exception as e:
+                                    log.ODM_WARNING("Cannot transform textured model: %s" % str(e))
+                                    os.rename(unaligned_obj, obj)
+                    else:
+                        log.ODM_WARNING("Alignment to %s will be skipped." % tree.odm_align_file)
+                    
+                    io.touch(alignment_file)
                 else:
-                    log.ODM_WARNING("Alignment to %s will be skipped." % tree.odm_align_file)
-            # Align
-            # - Compute alignment
-            # - Transform point cloud
-            # - Transform textured model(s)
-            # 
+                    log.ODM_WARNING("Already computed alignment")
 
             point_cloud.post_point_cloud_steps(args, tree, self.rerun())
         else: