From f03bbe7e9a62d54d46c07d4e5889a479069ed0be Mon Sep 17 00:00:00 2001 From: Piero Toffanin Date: Wed, 25 Apr 2018 10:00:56 -0400 Subject: [PATCH] Fixed GCPs, mesh transform, point cloud transform, rerun-from orthophoto issue --- modules/odm_georef/src/Georef.cpp | 73 ++++++++++++++++++++++++------- modules/odm_georef/src/Georef.hpp | 2 +- opendm/types.py | 7 ++- scripts/odm_georeferencing.py | 38 ++++++++-------- scripts/odm_orthophoto.py | 21 ++++++++- 5 files changed, 100 insertions(+), 41 deletions(-) diff --git a/modules/odm_georef/src/Georef.cpp b/modules/odm_georef/src/Georef.cpp index 58bbd1c1..d09adb68 100644 --- a/modules/odm_georef/src/Georef.cpp +++ b/modules/odm_georef/src/Georef.cpp @@ -15,7 +15,7 @@ using namespace std; std::ostream& operator<<(std::ostream &os, const GeorefSystem &geo) { - return os << geo.system_ << "\n" << static_cast(geo.eastingOffset_) << " " << static_cast(geo.northingOffset_); + return os << setiosflags(ios::fixed) << setprecision(6) << geo.system_ << "\n" << geo.eastingOffset_ << " " << geo.northingOffset_; } GeorefGCP::GeorefGCP() @@ -813,7 +813,7 @@ void Georef::performGeoreferencingWithGCP() log_ << "Reading mesh file " << inputObjFilename_ <<"\n"; log_ << '\n'; pcl::TextureMesh mesh; - if (loadObjFile(inputObjFilename_, mesh) == -1) + if (!loadObjFile(inputObjFilename_, mesh)) { throw GeorefException("Error when reading model from:\n" + inputObjFilename_ + "\n"); } @@ -943,7 +943,7 @@ void Georef::performGeoreferencingWithGCP() printFinalTransform(transFinal.transform_); // The transform used to transform model into the georeferenced system. - performFinalTransform(transFinal.transform_, mesh, meshCloud); + performFinalTransform(transFinal.transform_, mesh, meshCloud, true); } void Georef::createGeoreferencedModelFromGCPData() @@ -1025,6 +1025,7 @@ void Georef::createGeoreferencedModelFromExifData() FindTransform transFinal; transFinal.findTransform(cameras_[cam0].getPos(), cameras_[cam1].getPos(), cameras_[cam2].getPos(), cameras_[cam0].getReferencedPos(), cameras_[cam1].getReferencedPos(), cameras_[cam2].getReferencedPos()); + log_ << "Final transform:\n"; log_ << transFinal.transform_ << '\n'; @@ -1040,11 +1041,17 @@ void Georef::createGeoreferencedModelFromExifData() pcl::PointCloud::Ptr meshCloud (new pcl::PointCloud); pcl::fromPCLPointCloud2 (mesh.cloud, *meshCloud); - performFinalTransform(transFinal.transform_, mesh, meshCloud); + performFinalTransform(transFinal.transform_, mesh, meshCloud, true); } void Georef::createGeoreferencedModelFromSFM() { + // Read coordinate system from coord file generated by extract_utm tool + // UTM coordinates from OpenSfM transform + std::ifstream coordStream(inputCoordFilename_.c_str()); + if (!coordStream.good()) throw GeorefException("Failed opening coordinate file " + inputCoordFilename_ + " for reading." + '\n'); + std::getline(coordStream, georefSystem_.system_); + coordStream.close(); Mat4 transform; // get transform in correct format @@ -1074,12 +1081,12 @@ void Georef::createGeoreferencedModelFromSFM() l4 >> transform.r4c1_ >> transform.r4c2_ >> transform.r4c3_ >> transform.r4c4_; } - transform.r1c2_ = 0.0f; - transform.r2c1_ = 0.0f; + georefSystem_.eastingOffset_ = transform.r1c4_; + georefSystem_.northingOffset_ = transform.r2c4_; - // load mesh printFinalTransform(transform); + // load mesh log_ << '\n'; log_ << "Reading mesh file...\n"; pcl::TextureMesh mesh; @@ -1090,9 +1097,8 @@ void Georef::createGeoreferencedModelFromSFM() pcl::PointCloud::Ptr meshCloud (new pcl::PointCloud); pcl::fromPCLPointCloud2 (mesh.cloud, *meshCloud); - performFinalTransform(transform, mesh, meshCloud); - - // performFinalTransform(transFinal, mesh, meshCloud); + // OpenSfM transform has UTM offsets embedded in the translation + performFinalTransform(transform, mesh, meshCloud, false); } void Georef::chooseBestGCPTriplet(size_t &gcp0, size_t &gcp1, size_t &gcp2) @@ -1291,7 +1297,7 @@ void Georef::printFinalTransform(Mat4 transform) } -void Georef::performFinalTransform(Mat4 &transMat, pcl::TextureMesh &mesh, pcl::PointCloud::Ptr &meshCloud) +void Georef::performFinalTransform(Mat4 &transMat, pcl::TextureMesh &mesh, pcl::PointCloud::Ptr &meshCloud, bool addUTM) { Eigen::Transform transform; @@ -1310,11 +1316,23 @@ void Georef::performFinalTransform(Mat4 &transMat, pcl::TextureMesh &mesh, pcl:: transform(2, 2) = static_cast(transMat.r3c3_); transform(3, 2) = static_cast(transMat.r4c3_); - transform(0, 3) = static_cast(transMat.r1c4_); - transform(1, 3) = static_cast(transMat.r2c4_); + double transX = static_cast(transMat.r1c4_); + double transY = static_cast(transMat.r2c4_); + + transform(0, 3) = static_cast(0.0f); + transform(1, 3) = static_cast(0.0f); transform(2, 3) = static_cast(transMat.r3c4_); transform(3, 3) = static_cast(transMat.r4c4_); + log_ << '\n'; + log_ << "Applying transform to mesh...\n"; + // Move the mesh into position. + pcl::transformPointCloud(*meshCloud, *meshCloud, transform); + log_ << ".. mesh transformed.\n"; + + // Update the mesh. + pcl::toPCLPointCloud2 (*meshCloud, mesh.cloud); + // Iterate over each part of the mesh (one per material), to make texture file paths relative the .mtl file. for(size_t t = 0; t < mesh.tex_materials.size(); ++t) { @@ -1338,6 +1356,23 @@ void Georef::performFinalTransform(Mat4 &transMat, pcl::TextureMesh &mesh, pcl:: log_ << "Successfully saved model.\n"; } + transform(0, 3) = transX; + transform(1, 3) = transY; + + // GCPs and EXIF modes includes a translation + // but not UTM offsets. We want our point cloud + // and odm_georeferencing_model_geo.txt file + // to include the UTM offset. + // OpenSfM already has UTM offsets encoded + // in the translation matrix. + if (addUTM){ + georefSystem_.eastingOffset_ += transX; + georefSystem_.northingOffset_ += transY; + + transform(0, 3) = georefSystem_.eastingOffset_; + transform(1, 3) = georefSystem_.northingOffset_; + } + if(georeferencePointCloud_) { transformPointCloud(inputPointCloudFilename_.c_str(), transform, outputPointCloudFilename_.c_str()); @@ -1429,9 +1464,13 @@ void Georef::transformPointCloud(const char *inputFile, const Eigen::Transform (transform (0, 0) * verts[i].x + transform (0, 1) * verts[i].y + transform (0, 2) * verts[i].z + transform (0, 3)); - verts[i].y = static_cast (transform (1, 0) * verts[i].x + transform (1, 1) * verts[i].y + transform (1, 2) * verts[i].z + transform (1, 3)); - verts[i].z = static_cast (transform (2, 0) * verts[i].x + transform (2, 1) * verts[i].y + transform (2, 2) * verts[i].z + transform (2, 3)); + Scalar x = verts[i].x; + Scalar y = verts[i].y; + Scalar z = verts[i].z; + + verts[i].x = static_cast (transform (0, 0) * x + transform (0, 1) * y + transform (0, 2) * z + transform (0, 3)); + verts[i].y = static_cast (transform (1, 0) * x + transform (1, 1) * y + transform (1, 2) * z + transform (1, 3)); + verts[i].z = static_cast (transform (2, 0) * x + transform (2, 1) * y + transform (2, 2) * z + transform (2, 3)); } log_ << '\n'; @@ -1667,7 +1706,7 @@ bool Georef::loadObjFile(std::string inputFile, pcl::TextureMesh &mesh) } fs.close(); - return (0); + return true; } bool Georef::readHeader (const std::string &file_name, pcl::PCLPointCloud2 &cloud, diff --git a/modules/odm_georef/src/Georef.hpp b/modules/odm_georef/src/Georef.hpp index e5f2c89e..c8b50754 100644 --- a/modules/odm_georef/src/Georef.hpp +++ b/modules/odm_georef/src/Georef.hpp @@ -293,7 +293,7 @@ private: bool multiMaterial_; /**< True if the mesh has multiple materials. **/ std::vector companions_; /**< Materials (used by loadOBJFile). **/ - void performFinalTransform(Mat4 &transMat, pcl::TextureMesh &mesh, pcl::PointCloud::Ptr &meshCloud); + void performFinalTransform(Mat4 &transMat, pcl::TextureMesh &mesh, pcl::PointCloud::Ptr &meshCloud, bool addUTM); template void transformPointCloud(const char *inputFile, const Eigen::Transform &transform, const char *outputFile); diff --git a/opendm/types.py b/opendm/types.py index 79b62208..acb4bf61 100644 --- a/opendm/types.py +++ b/opendm/types.py @@ -348,8 +348,8 @@ class ODM_GeoRef(object): with open(_file) as f: offsets = f.readlines()[1].split(' ') - self.utm_east_offset = int(offsets[0]) - self.utm_north_offset = int(offsets[1]) + self.utm_east_offset = float(offsets[0]) + self.utm_north_offset = float(offsets[1]) def create_gcps(self, _file): if not io.file_exists(_file): @@ -377,6 +377,9 @@ class ODM_GeoRef(object): # Create a nested list for the transformation matrix with open(_file) as f: for line in f: + # Handle matrix formats that either + # have leading or trailing brakets or just plain numbers. + line = re.sub(r"[\[\],]", "", line).strip() self.transform += [[float(i) for i in line.split()]] self.utm_east_offset = self.transform[0][3] diff --git a/scripts/odm_georeferencing.py b/scripts/odm_georeferencing.py index 9b7a7547..0c7263bc 100644 --- a/scripts/odm_georeferencing.py +++ b/scripts/odm_georeferencing.py @@ -68,6 +68,7 @@ class ODMGeoreferencingCell(ecto.Cell): odm_georeferencing_model_ply_geo = os.path.join(r['georeferencing_dir'], tree.odm_georeferencing_model_ply_geo) odm_georeferencing_log = os.path.join(r['georeferencing_dir'], tree.odm_georeferencing_log) odm_georeferencing_transform_file = os.path.join(r['georeferencing_dir'], tree.odm_georeferencing_transform_file) + odm_georeferencing_model_txt_geo_file = os.path.join(r['georeferencing_dir'], tree.odm_georeferencing_model_txt_geo) if not io.file_exists(odm_georeferencing_model_obj_geo) or \ not io.file_exists(odm_georeferencing_model_ply_geo) or rerun_cell: @@ -84,7 +85,7 @@ class ODMGeoreferencingCell(ecto.Cell): 'transform_file': odm_georeferencing_transform_file, 'coords': tree.odm_georeferencing_coords, 'pc_geo': odm_georeferencing_model_ply_geo, - 'geo_sys': os.path.join(r['georeferencing_dir'], tree.odm_georeferencing_model_txt_geo), + 'geo_sys': odm_georeferencing_model_txt_geo_file, 'model_geo': odm_georeferencing_model_obj_geo, 'gcp': gcpfile, 'verbose': verbose @@ -100,20 +101,20 @@ class ODMGeoreferencingCell(ecto.Cell): # Check to see if the GCP file exists - #if not self.params.use_exif and (self.params.gcp_file or tree.odm_georeferencing_gcp): - # log.ODM_INFO('Found %s' % gcpfile) - # try: - # system.run('{bin}/odm_georef -bundleFile {bundle} -imagesPath {imgs} -imagesListPath {imgs_list} ' - # '-inputFile {model} -outputFile {model_geo} ' - # '-inputPointCloudFile {pc} -outputPointCloudFile {pc_geo} {verbose} ' - # '-logFile {log} -outputTransformFile {transform_file} -georefFileOutputPath {geo_sys} -gcpFile {gcp} ' - # '-outputCoordFile {coords}'.format(**kwargs)) - # except Exception: - # log.ODM_EXCEPTION('Georeferencing failed. ') - # return ecto.QUIT - if io.file_exists(tree.opensfm_transformation): + if not self.params.use_exif and (self.params.gcp_file or tree.odm_georeferencing_gcp): + log.ODM_INFO('Found %s' % gcpfile) + try: + system.run('{bin}/odm_georef -bundleFile {bundle} -imagesPath {imgs} -imagesListPath {imgs_list} ' + '-inputFile {model} -outputFile {model_geo} ' + '-inputPointCloudFile {pc} -outputPointCloudFile {pc_geo} {verbose} ' + '-logFile {log} -outputTransformFile {transform_file} -georefFileOutputPath {geo_sys} -gcpFile {gcp} ' + '-outputCoordFile {coords}'.format(**kwargs)) + except Exception: + log.ODM_EXCEPTION('Georeferencing failed. ') + return ecto.QUIT + elif io.file_exists(tree.opensfm_transformation) and io.file_exists(tree.odm_georeferencing_coords): log.ODM_INFO('Running georeferencing with OpenSfM transformation matrix') - system.run('{bin}/odm_georef -bundleFile {bundle} -inputTransformFile {input_trans_file} ' + system.run('{bin}/odm_georef -bundleFile {bundle} -inputTransformFile {input_trans_file} -inputCoordFile {coords} ' '-inputFile {model} -outputFile {model_geo} ' '-inputPointCloudFile {pc} -outputPointCloudFile {pc_geo} {verbose} ' '-logFile {log} -outputTransformFile {transform_file} -georefFileOutputPath {geo_sys}'.format(**kwargs)) @@ -132,7 +133,7 @@ class ODMGeoreferencingCell(ecto.Cell): if doPointCloudGeo: # update images metadata geo_ref = reconstruction.georef - geo_ref.parse_transformation_matrix(tree.opensfm_transformation) + geo_ref.extract_offsets(odm_georeferencing_model_txt_geo_file) # convert ply model to LAS reference system geo_ref.convert_to_las(odm_georeferencing_model_ply_geo, @@ -167,10 +168,9 @@ class ODMGeoreferencingCell(ecto.Cell): # We might be doing georeferencing for # multiple models (3D, 2.5D, ...) doPointCloudGeo = False - - else: - log.ODM_WARNING('Found a valid georeferenced model in: %s' - % odm_georeferencing_model_ply_geo) + else: + log.ODM_WARNING('Found a valid georeferenced model in: %s' + % odm_georeferencing_model_ply_geo) outputs.reconstruction = reconstruction diff --git a/scripts/odm_orthophoto.py b/scripts/odm_orthophoto.py index 64b21d7c..d2a04710 100644 --- a/scripts/odm_orthophoto.py +++ b/scripts/odm_orthophoto.py @@ -60,7 +60,25 @@ class ODMOrthoPhotoCell(ecto.Cell): } # Have geo coordinates? - if reconstruction.georef: # io.file_exists(tree.odm_georeferencing_coords): + georef = reconstruction.georef + + # Check if the georef object is initialized + # (during a --rerun this might not be) + # TODO: we should move this to a more central + # location (perhaps during the dataset initialization) + if georef and not georef.utm_east_offset: + if args.use_25dmesh: + odm_georeferencing_model_txt_geo_file = os.path.join(tree.odm_25dtexturing, tree.odm_georeferencing_model_txt_geo) + else: + odm_georeferencing_model_txt_geo_file = os.path.join(tree.odm_texturing, tree.odm_georeferencing_model_txt_geo) + + if io.file_exists(odm_georeferencing_model_txt_geo_file): + georef.extract_offsets(odm_georeferencing_model_txt_geo_file) + else: + log.ODM_WARNING('Cannot read UTM offset from {}. An orthophoto will not be generated.'.format(odm_georeferencing_model_txt_geo_file)) + + + if georef: if args.use_25dmesh: kwargs['model_geo'] = os.path.join(tree.odm_25dtexturing, tree.odm_georeferencing_model_obj_geo) else: @@ -78,7 +96,6 @@ class ODMOrthoPhotoCell(ecto.Cell): # Create georeferenced GeoTiff geotiffcreated = False - georef = reconstruction.georef if georef and georef.projection and georef.utm_east_offset and georef.utm_north_offset: ulx = uly = lrx = lry = 0.0