Try to fix georeferencing accuracy

opendm/georef.py

@@ -0,0 +1,56 @@
+import numpy as np
+from numpy import ndarray
+from typing import Tuple
+from pyproj import Proj
+from opensfm.geo import TopocentricConverter
+
+def topocentric_to_georef(
+        reflat: float, 
+        reflon: float, 
+        refalt: float, 
+        a_srs: str, 
+        x: ndarray, 
+        y: ndarray, 
+        z: ndarray,
+        x_offset: float = 0,
+        y_offset: float = 0,
+    ) -> Tuple[ndarray, ndarray, ndarray]:
+    reference = TopocentricConverter(reflat, reflon, refalt)
+    projection = Proj(a_srs)
+    lat, lon, alt = reference.to_lla(x, y, z)
+    easting, northing = projection(lon, lat)
+    return easting - x_offset, northing - y_offset, alt
+
+
+class TopocentricToProj:
+    def __init__(self, reflat:float, reflon:float, refalt:float, a_srs:str):
+        self.reference = TopocentricConverter(reflat, reflon, refalt)
+        self.projection = Proj(a_srs)
+
+    def convert_array(self, arr:ndarray, offset_x:float=0, offset_y:float=0):
+        x, y, z = arr['X'], arr['Y'], arr['Z']
+        easting, northing, alt = self.convert_points(x, y, z, offset_x, offset_y)
+        arr['X'] = easting
+        arr['Y'] = northing
+        arr['Z'] = alt
+        return arr
+
+    def convert_points(self, x:ndarray, y:ndarray, z:ndarray, offset_x:float=0, offset_y:float=0):
+        lat, lon, alt = self.reference.to_lla(x, y, z)
+        easting, northing = self.projection(lon, lat)
+        return easting - offset_x, northing - offset_y, alt
+
+    def convert_obj(self, input_obj:str, output_obj:str, offset_x:float=0, offset_y:float=0):
+        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 = self.convert_points(v[0], v[1], v[2], offset_x, offset_y)
+                        fout.write("v " + " ".join(map(str, list(vt))) + '\n')
+                    else:
+                        fout.write(line)
+
+
+

opendm/osfm.py

@@ -629,9 +629,12 @@ def ground_control_points(self, proj4):
 
         return result
 
+    def reference(self):
+        ds = DataSet(self.opensfm_project_path)
+        return ds.load_reference()
 
     def name(self):
-        return os.path.basename(os.path.abspath(self.path("..")))
+        return os.path.basename(os.path.abspath(self.path("..")))    
 
 def get_submodel_argv(args, submodels_path = None, submodel_name = None):
     """

opendm/types.py

@@ -361,16 +361,20 @@ def __init__(self, root_path, gcp_file = None, geo_file = None, align_file = Non
         self.openmvs_model = os.path.join(self.openmvs, 'scene_dense_dense_filtered.ply')
 
         # filter points
+        self.filtered_point_cloud_topo = os.path.join(self.odm_filterpoints, "point_cloud_topocentric.ply")
         self.filtered_point_cloud = os.path.join(self.odm_filterpoints, "point_cloud.ply")
         self.filtered_point_cloud_stats = os.path.join(self.odm_filterpoints, "point_cloud_stats.json")
 
         # odm_meshing
+        self.odm_mesh_topo = os.path.join(self.odm_meshing, 'odm_mesh_topocentric.ply')
         self.odm_mesh = os.path.join(self.odm_meshing, 'odm_mesh.ply')
         self.odm_meshing_log = os.path.join(self.odm_meshing, 'odm_meshing_log.txt')
+        self.odm_25dmesh_topo = os.path.join(self.odm_meshing, 'odm_25dmesh_topocentric.ply')
         self.odm_25dmesh = os.path.join(self.odm_meshing, 'odm_25dmesh.ply')
         self.odm_25dmeshing_log = os.path.join(self.odm_meshing, 'odm_25dmeshing_log.txt')
 
         # texturing
+        self.odm_textured_model_obj_topo = 'odm_textured_model_topocentric.obj'
         self.odm_textured_model_obj = 'odm_textured_model_geo.obj'
         self.odm_textured_model_glb = 'odm_textured_model_geo.glb'
 

stages/mvstex.py

@@ -33,7 +33,7 @@ def add_run(nvm_file, primary=True, band=None):
             if not args.skip_3dmodel and (primary or args.use_3dmesh):
                 nonloc.runs += [{
                     'out_dir': os.path.join(tree.odm_texturing, subdir),
-                    'model': tree.odm_mesh,
+                    'model': tree.odm_mesh_topo,
                     'nadir': False,
                     'primary': primary,
                     'nvm_file': nvm_file,
@@ -43,7 +43,7 @@ def add_run(nvm_file, primary=True, band=None):
             if not args.use_3dmesh:
                 nonloc.runs += [{
                     'out_dir': os.path.join(tree.odm_25dtexturing, subdir),
-                    'model': tree.odm_25dmesh,
+                    'model': tree.odm_25dmesh_topo,
                     'nadir': True,
                     'primary': primary,
                     'nvm_file': nvm_file,
@@ -69,9 +69,8 @@ def add_run(nvm_file, primary=True, band=None):
             if not io.dir_exists(r['out_dir']):
                 system.mkdir_p(r['out_dir'])
 
-            odm_textured_model_obj = os.path.join(r['out_dir'], tree.odm_textured_model_obj)
+            odm_textured_model_obj = os.path.join(r['out_dir'], tree.odm_textured_model_obj_topo)
             unaligned_obj = io.related_file_path(odm_textured_model_obj, postfix="_unaligned")
-
             if not io.file_exists(odm_textured_model_obj) or self.rerun():
                 log.ODM_INFO('Writing MVS Textured file in: %s'
                               % odm_textured_model_obj)
@@ -91,10 +90,12 @@ def add_run(nvm_file, primary=True, band=None):
                 if (r['nadir']):
                     nadir = '--nadir_mode'
 
+
                 # mvstex definitions
+                # mtl and texture files would be the same between topo and proj geomodel, so create with the final name
                 kwargs = {
                     'bin': context.mvstex_path,
-                    'out_dir': os.path.join(r['out_dir'], "odm_textured_model_geo"),
+                    'out_dir': os.path.join(r['out_dir'], 'odm_textured_model_geo'),
                     'model': r['model'],
                     'dataTerm': 'gmi',
                     'outlierRemovalType': 'gauss_clamping',
@@ -125,6 +126,9 @@ def add_run(nvm_file, primary=True, band=None):
                         '{nadirMode} '
                         '{labelingFile} '
                         '{maxTextureSize} '.format(**kwargs))
+
+                # update the obj file name to topo for further conversion
+                shutil.move(os.path.join(r['out_dir'], tree.odm_textured_model_obj), odm_textured_model_obj)
 
                 if r['primary'] and (not r['nadir'] or args.skip_3dmodel):
                     # GlTF?
@@ -147,7 +151,7 @@ def add_run(nvm_file, primary=True, band=None):
                             shutil.rmtree(packed_dir)
 
                         try:
-                            obj_pack(os.path.join(r['out_dir'], tree.odm_textured_model_obj), packed_dir, _info=log.ODM_INFO)
+                            obj_pack(os.path.join(r['out_dir'], tree.odm_textured_model_obj_topo), packed_dir, _info=log.ODM_INFO)
 
                             # Move packed/* into texturing folder
                             system.delete_files(r['out_dir'], (".vec", ))

stages/odm_filterpoints.py

@@ -19,7 +19,7 @@ def process(self, args, outputs):
         inputPointCloud = ""
 
         # check if reconstruction was done before
-        if not io.file_exists(tree.filtered_point_cloud) or self.rerun():
+        if not io.file_exists(tree.filtered_point_cloud_topo) or self.rerun():
             if args.fast_orthophoto:
                 inputPointCloud = os.path.join(tree.opensfm, 'reconstruction.ply')
             else:
@@ -49,22 +49,22 @@ def process(self, args, outputs):
                 else:
                     log.ODM_WARNING("Not a georeferenced reconstruction, will ignore --auto-boundary")
 
-            point_cloud.filter(inputPointCloud, tree.filtered_point_cloud, tree.filtered_point_cloud_stats,
+            point_cloud.filter(inputPointCloud, tree.filtered_point_cloud_topo, tree.filtered_point_cloud_stats,
                                 standard_deviation=args.pc_filter, 
                                 sample_radius=args.pc_sample,
                                 boundary=boundary_offset(outputs.get('boundary'), reconstruction.get_proj_offset()),
                                 max_concurrency=args.max_concurrency)
 
             # Quick check
-            info = point_cloud.ply_info(tree.filtered_point_cloud)
+            info = point_cloud.ply_info(tree.filtered_point_cloud_topo)
             if info["vertex_count"] == 0:
                 extra_msg = ''
                 if 'boundary' in outputs:
                     extra_msg = '. Also, since you used a boundary setting, make sure that the boundary polygon you specified covers the reconstruction area correctly.'
                 raise system.ExitException("Uh oh! We ended up with an empty point cloud. This means that the reconstruction did not succeed. Have you followed best practices for data acquisition? See https://docs.opendronemap.org/flying/%s" % extra_msg)
         else:
             log.ODM_WARNING('Found a valid point cloud file in: %s' %
-                            tree.filtered_point_cloud)
+                            tree.filtered_point_cloud_topo)
 
         if args.optimize_disk_space and inputPointCloud:
             if os.path.isfile(inputPointCloud):

stages/odm_georeferencing.py

@@ -9,6 +9,7 @@
 import math
 from collections import OrderedDict
 from pyproj import CRS
+import pdal
 
 from opendm import io
 from opendm import log
@@ -23,6 +24,7 @@
 from opendm.boundary import as_polygon, export_to_bounds_files
 from opendm.align import compute_alignment_matrix, transform_point_cloud, transform_obj
 from opendm.utils import np_to_json
+from opendm.georef import TopocentricToProj
 
 class ODMGeoreferencingStage(types.ODM_Stage):
     def process(self, args, outputs):
@@ -113,19 +115,72 @@ def process(self, args, outputs):
 
             else:
                 log.ODM_WARNING("GCPs could not be loaded for writing to %s" % gcp_export_file)
-
+
+        if reconstruction.is_georeferenced():
+            # prepare pipeline stage for topocentric to georeferenced conversion
+            octx = OSFMContext(tree.opensfm)
+            reference = octx.reference()
+            converter = TopocentricToProj(reference.lat, reference.lon, reference.alt, reconstruction.georef.proj4())
+
+        if not io.file_exists(tree.filtered_point_cloud) or self.rerun():
+            log.ODM_INFO("Georeferecing filtered point cloud")
+            if reconstruction.is_georeferenced():
+                pipeline = pdal.Reader.ply(tree.filtered_point_cloud_topo).pipeline()
+                pipeline.execute()
+                arr = pipeline.arrays[0]
+                arr = converter.convert_array(
+                    arr,
+                    reconstruction.georef.utm_east_offset,
+                    reconstruction.georef.utm_north_offset
+                )
+                pipeline = pdal.Writer.ply(
+                    filename = tree.filtered_point_cloud,
+                    storage_mode = "little endian",
+                ).pipeline(arr)
+                pipeline.execute()
+            else:
+                shutil.copy(tree.filtered_point_cloud_topo, tree.filtered_point_cloud)
+
+        def georefernce_textured_model(obj_in, obj_out):
+            log.ODM_INFO("Georeferecing textured model %s" % obj_in)
+            if not io.file_exists(obj_out) or self.rerun():
+                if reconstruction.is_georeferenced():
+                    converter.convert_obj(
+                        obj_in, 
+                        obj_out, 
+                        reconstruction.georef.utm_east_offset, 
+                        reconstruction.georef.utm_north_offset
+                    )
+                else:
+                    shutil.copy(obj_in, obj_out)
+
+        #TODO: maybe parallelize this
+        #TODO: gltf export? Should problably move the exporting process after this
+        for texturing in [tree.odm_texturing, tree.odm_25dtexturing]:
+            if reconstruction.multi_camera:
+                primary = get_primary_band_name(reconstruction.multi_camera, args.primary_band)
+                for band in reconstruction.multi_camera:
+                    subdir = "" if band['name'] == primary else band['name'].lower()
+                    obj_in = os.path.join(texturing, subdir, tree.odm_textured_model_obj_topo)
+                    obj_out = os.path.join(texturing, subdir, tree.odm_textured_model_obj)
+                    georefernce_textured_model(obj_in, obj_out)
+            else:
+                obj_in = os.path.join(texturing, tree.odm_textured_model_obj_topo)
+                obj_out = os.path.join(texturing, tree.odm_textured_model_obj)
+                transform_textured_model(obj_in, obj_out)
+
         if not io.file_exists(tree.odm_georeferencing_model_laz) or self.rerun():
-            cmd = f'pdal translate -i "{tree.filtered_point_cloud}" -o \"{tree.odm_georeferencing_model_laz}\"'
-            stages = ["ferry"]
-            params = [
-                '--filters.ferry.dimensions="views => UserData"'
-            ]
-
+            pipeline = pdal.Pipeline()
+            pipeline |= pdal.Reader.ply(tree.filtered_point_cloud)
+            pipeline |= pdal.Filter.ferry(dimensions="views => UserData")
+
             if reconstruction.is_georeferenced():
                 log.ODM_INFO("Georeferencing point cloud")
 
-                stages.append("transformation")
                 utmoffset = reconstruction.georef.utm_offset()
+                pipeline |= pdal.Filter.transformation(
+                    matrix=f"1 0 0 {utmoffset[0]} 0 1 0 {utmoffset[1]} 0 0 1 0 0 0 0 1"
+                )
 
                 # Establish appropriate las scale for export
                 las_scale = 0.001
@@ -148,27 +203,37 @@ def powerr(r):
                 else:
                     log.ODM_INFO("No point_cloud_stats.json found. Using default las scale: %s" % las_scale)
 
-                params += [
-                    f'--filters.transformation.matrix="1 0 0 {utmoffset[0]} 0 1 0 {utmoffset[1]} 0 0 1 0 0 0 0 1"',
-                    f'--writers.las.offset_x={reconstruction.georef.utm_east_offset}' ,
-                    f'--writers.las.offset_y={reconstruction.georef.utm_north_offset}',
-                    f'--writers.las.scale_x={las_scale}',
-                    f'--writers.las.scale_y={las_scale}',
-                    f'--writers.las.scale_z={las_scale}',
-                    '--writers.las.offset_z=0',
-                    f'--writers.las.a_srs="{reconstruction.georef.proj4()}"' # HOBU this should maybe be WKT
-                ]
-
+                las_writer_def = {
+                    "filename": tree.odm_georeferencing_model_laz,
+                    "a_srs": reconstruction.georef.proj4(),
+                    "offset_x": utmoffset[0],
+                    "offset_y": utmoffset[1],
+                    "offset_z": 0,
+                    "scale_x": las_scale,
+                    "scale_y": las_scale,
+                    "scale_z": las_scale,
+                }
+                
                 if reconstruction.has_gcp() and io.file_exists(gcp_geojson_zip_export_file):
                     if os.path.getsize(gcp_geojson_zip_export_file) <= 65535:
                         log.ODM_INFO("Embedding GCP info in point cloud")
-                        params += [
-                            '--writers.las.vlrs="{\\\"filename\\\": \\\"%s\\\", \\\"user_id\\\": \\\"ODM\\\", \\\"record_id\\\": 2, \\\"description\\\": \\\"Ground Control Points (zip)\\\"}"' % gcp_geojson_zip_export_file.replace(os.sep, "/")
-                        ]
+                        las_writer_def["vlrs"] = json.dumps(
+                            {
+                                "filename": gcp_geojson_zip_export_file.replace(os.sep, "/"),
+                                "user_id": "ODM",
+                                "record_id": 2,
+                                "description": "Ground Control Points (zip)"
+                            }
+                        )
+
                     else:
                         log.ODM_WARNING("Cannot embed GCP info in point cloud, %s is too large" % gcp_geojson_zip_export_file)
 
-                system.run(cmd + ' ' + ' '.join(stages) + ' ' + ' '.join(params))
+                pipeline |= pdal.Writer.las(
+                    **las_writer_def
+                )
+
+                pipeline.execute()
 
                 self.update_progress(50)
 
@@ -202,7 +267,10 @@ def powerr(r):
                     export_to_bounds_files(outputs['boundary'], reconstruction.get_proj_srs(), bounds_json, bounds_gpkg)
             else:
                 log.ODM_INFO("Converting point cloud (non-georeferenced)")
-                system.run(cmd + ' ' + ' '.join(stages) + ' ' + ' '.join(params))
+                pipeline |= pdal.Writer.las(
+                    tree.odm_georeferencing_model_laz
+                )
+                pipeline.execute()
 
 
             stats_dir = tree.path("opensfm", "stats", "codem")
@@ -250,7 +318,7 @@ def transform_textured_model(obj):
                                 except Exception as e:
                                     log.ODM_WARNING("Cannot transform textured model: %s" % str(e))
                                     os.rename(unaligned_obj, obj)
-
+                        #TODO: seems gltf file is not converted in alignment?
                         for texturing in [tree.odm_texturing, tree.odm_25dtexturing]:
                             if reconstruction.multi_camera:
                                 primary = get_primary_band_name(reconstruction.multi_camera, args.primary_band)