diff --git a/src/base/similarity_transform.cc b/src/base/similarity_transform.cc index 45e9eda86..311ce7bee 100755 --- a/src/base/similarity_transform.cc +++ b/src/base/similarity_transform.cc @@ -37,6 +37,8 @@ #include "estimators/similarity_transform.h" #include "optim/loransac.h" +#include + namespace colmap { namespace { @@ -194,6 +196,14 @@ SimilarityTransform3::SimilarityTransform3(const double scale, transform_.matrix() = matrix; } +void SimilarityTransform3::Write(const std::string& path) { + std::ofstream file(path, std::ios::trunc); + CHECK(file.is_open()) << path; + // Ensure that we don't loose any precision by storing in text. + file.precision(17); + file << transform_.matrix() << std::endl; +} + SimilarityTransform3 SimilarityTransform3::Inverse() const { return SimilarityTransform3(transform_.inverse()); } diff --git a/src/base/similarity_transform.h b/src/base/similarity_transform.h index 3966b02ba..09ea37a47 100755 --- a/src/base/similarity_transform.h +++ b/src/base/similarity_transform.h @@ -61,6 +61,8 @@ class SimilarityTransform3 { SimilarityTransform3(const double scale, const Eigen::Vector4d& qvec, const Eigen::Vector3d& tvec); + void Write(const std::string& path); + template bool Estimate(const std::vector& src, const std::vector& dst); diff --git a/src/estimators/coordinate_frame.cc b/src/estimators/coordinate_frame.cc index 9f1b2e74b..23e9044ca 100644 --- a/src/estimators/coordinate_frame.cc +++ b/src/estimators/coordinate_frame.cc @@ -31,6 +31,7 @@ #include "estimators/coordinate_frame.h" +#include "base/gps.h" #include "base/line.h" #include "base/pose.h" #include "base/undistortion.h" @@ -298,4 +299,58 @@ Eigen::Matrix3d EstimateManhattanWorldFrame( return frame; } +void AlignToPrincipalPlane(Reconstruction* recon, SimilarityTransform3* tform) { + // Perform SVD on the 3D points to estimate the ground plane basis + const Eigen::Vector3d centroid = recon->ComputeCentroid(0.0, 1.0); + Eigen::MatrixXd points(3, recon->NumPoints3D()); + int pidx = 0; + for (const auto& point : recon->Points3D()) { + points.col(pidx++) = point.second.XYZ() - centroid; + } + const Eigen::Matrix3d basis = + points.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).matrixU(); + Eigen::Matrix3d rot_mat; + rot_mat << basis.col(0), basis.col(1), basis.col(0).cross(basis.col(1)); + rot_mat.transposeInPlace(); + + *tform = SimilarityTransform3(1.0, RotationMatrixToQuaternion(rot_mat), + -rot_mat * centroid); + + // if camera plane ends up below ground then flip basis vectors and create new + // transform + Image test_img = recon->Images().begin()->second; + tform->TransformPose(&test_img.Qvec(), &test_img.Tvec()); + if (test_img.ProjectionCenter().z() < 0.0) { + rot_mat << basis.col(0), -basis.col(1), basis.col(0).cross(-basis.col(1)); + rot_mat.transposeInPlace(); + *tform = SimilarityTransform3(1.0, RotationMatrixToQuaternion(rot_mat), + -rot_mat * centroid); + } + + recon->Transform(*tform); +} + +void AlignToENUPlane(Reconstruction* recon, SimilarityTransform3* tform, + bool unscaled) { + const Eigen::Vector3d centroid = recon->ComputeCentroid(0.0, 1.0); + GPSTransform gps_tform; + const Eigen::Vector3d ell_centroid = gps_tform.XYZToEll({centroid}).at(0); + + // Create rotation matrix from ECEF to ENU coordinates + const double sin_lat = sin(DegToRad(ell_centroid(0))); + const double sin_lon = sin(DegToRad(ell_centroid(1))); + const double cos_lat = cos(DegToRad(ell_centroid(0))); + const double cos_lon = cos(DegToRad(ell_centroid(1))); + + // Create ECEF to ENU rotation matrix + Eigen::Matrix3d rot_mat; + rot_mat << -sin_lon, cos_lon, 0, -cos_lon * sin_lat, -sin_lon * sin_lat, + cos_lat, cos_lon * cos_lat, sin_lon * cos_lat, sin_lat; + + const double scale = unscaled ? 1.0 / tform->Scale() : 1.0; + *tform = SimilarityTransform3(scale, RotationMatrixToQuaternion(rot_mat), + -(scale * rot_mat) * centroid); + recon->Transform(*tform); +} + } // namespace colmap diff --git a/src/estimators/coordinate_frame.h b/src/estimators/coordinate_frame.h index 19a007d74..9ce2a4732 100644 --- a/src/estimators/coordinate_frame.h +++ b/src/estimators/coordinate_frame.h @@ -70,6 +70,19 @@ Eigen::Matrix3d EstimateManhattanWorldFrame( const ManhattanWorldFrameEstimationOptions& options, const Reconstruction& reconstruction, const std::string& image_path); +// Aligns the reconstruction to the plane defined by running PCA on the 3D +// points. The model centroid is at the origin of the new coordinate system +// and the X axis is the first principal component with the Y axis being the +// second principal component +void AlignToPrincipalPlane(Reconstruction* recon, SimilarityTransform3* tform); + +// Aligns the reconstruction to the local ENU plane orientation. Rotates the +// reconstruction such that the x-y plane aligns with the ENU tangent plane at +// the point cloud centroid and translates the origin to the centroid. +// If unscaled == true, then the original scale of the model remains unchanged. +void AlignToENUPlane(Reconstruction* recon, SimilarityTransform3* tform, + bool unscaled); + } // namespace colmap #endif // COLMAP_SRC_ESTIMATORS_COORDINATE_AXES_H_ diff --git a/src/estimators/coordinate_frame_test.cc b/src/estimators/coordinate_frame_test.cc index f51cf1df9..def0706be 100644 --- a/src/estimators/coordinate_frame_test.cc +++ b/src/estimators/coordinate_frame_test.cc @@ -33,6 +33,7 @@ #include "util/testing.h" #include "estimators/coordinate_frame.h" +#include "base/gps.h" using namespace colmap; @@ -50,3 +51,107 @@ BOOST_AUTO_TEST_CASE(TestEstimateManhattanWorldFrame) { reconstruction, image_path), Eigen::Matrix3d::Zero()); } + +BOOST_AUTO_TEST_CASE(TestAlignToPrincipalPlane) { + // Start with reconstruction containing points on the Y-Z plane and cameras + // "above" the plane on the positive X axis. After alignment the points should + // be on the X-Y plane and the cameras "above" the plane on the positive Z + // axis. + SimilarityTransform3 tform; + Reconstruction reconstruction; + // Setup image with projection center at (1, 0, 0) + Image image; + image.SetImageId(1); + image.Qvec() = Eigen::Vector4d(1.0, 0.0, 0.0, 0.0); + image.Tvec() = Eigen::Vector3d(-1.0, 0.0, 0.0); + reconstruction.AddImage(image); + // Setup 4 points on the Y-Z plane + point3D_t p1 = + reconstruction.AddPoint3D(Eigen::Vector3d(0.0, -1.0, 0.0), Track()); + point3D_t p2 = + reconstruction.AddPoint3D(Eigen::Vector3d(0.0, 1.0, 0.0), Track()); + point3D_t p3 = + reconstruction.AddPoint3D(Eigen::Vector3d(0.0, 0.0, -1.0), Track()); + point3D_t p4 = + reconstruction.AddPoint3D(Eigen::Vector3d(0.0, 0.0, 1.0), Track()); + AlignToPrincipalPlane(&reconstruction, &tform); + // Note that the final X and Y axes may be inverted after alignment, so we + // need to account for both cases when checking for correctness + const bool inverted = tform.Rotation()(2) < 0; + + // Verify that points lie on the correct locations of the X-Y plane + BOOST_CHECK_LE((reconstruction.Point3D(p1).XYZ() - + Eigen::Vector3d(inverted ? 1.0 : -1.0, 0.0, 0.0)) + .norm(), + 1e-6); + BOOST_CHECK_LE((reconstruction.Point3D(p2).XYZ() - + Eigen::Vector3d(inverted ? -1.0 : 1.0, 0.0, 0.0)) + .norm(), + 1e-6); + BOOST_CHECK_LE((reconstruction.Point3D(p3).XYZ() - + Eigen::Vector3d(0.0, inverted ? 1.0 : -1.0, 0.0)) + .norm(), + 1e-6); + BOOST_CHECK_LE((reconstruction.Point3D(p4).XYZ() - + Eigen::Vector3d(0.0, inverted ? -1.0 : 1.0, 0.0)) + .norm(), + 1e-6); + // Verify that projection center is at (0, 0, 1) + BOOST_CHECK_LE((reconstruction.Image(1).ProjectionCenter() - + Eigen::Vector3d(0.0, 0.0, 1.0)) + .norm(), + 1e-6); + // Verify that transform matrix does shuffling of axes + Eigen::Matrix4d mat; + if (inverted) { + mat << 0, -1, 0, 0, 0, 0, -1, 0, 1, 0, 0, 0, 0, 0, 0, 1; + } else { + mat << 0, 1, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 0, 1; + } + std::cout << tform.Matrix() << std::endl; + BOOST_CHECK_LE((tform.Matrix() - mat).norm(), 1e-6); +} + +BOOST_AUTO_TEST_CASE(TestAlignToENUPlane) { + // Create reconstruction with 4 points with known LLA coordinates. After the + // ENU transform all 4 points should land approximately on the X-Y plane. + GPSTransform gps; + auto points = gps.EllToXYZ( + {Eigen::Vector3d(50, 10.1, 100), Eigen::Vector3d(50.1, 10, 100), + Eigen::Vector3d(50.1, 10.1, 100), Eigen::Vector3d(50, 10, 100)}); + SimilarityTransform3 tform; + Reconstruction reconstruction; + std::vector point_ids; + for (size_t i = 0; i < points.size(); ++i) { + point_ids.push_back(reconstruction.AddPoint3D(points[i], Track())); + std::cout << points[i].transpose() << std::endl; + } + AlignToENUPlane(&reconstruction, &tform, false); + // Verify final locations of points + BOOST_CHECK_LE((reconstruction.Point3D(point_ids[0]).XYZ() - + Eigen::Vector3d(3584.8565215, -5561.5336506, 0.0742643)) + .norm(), + 1e-6); + BOOST_CHECK_LE((reconstruction.Point3D(point_ids[1]).XYZ() - + Eigen::Vector3d(-3577.3888622, 5561.6397107, 0.0783761)) + .norm(), + 1e-6); + BOOST_CHECK_LE((reconstruction.Point3D(point_ids[2]).XYZ() - + Eigen::Vector3d(3577.4152111, 5561.6397283, 0.0783613)) + .norm(), + 1e-6); + BOOST_CHECK_LE((reconstruction.Point3D(point_ids[3]).XYZ() - + Eigen::Vector3d(-3584.8301178, -5561.5336683, 0.0742791)) + .norm(), + 1e-6); + + // Verify that straight line distance between points is preserved + for (size_t i = 1; i < points.size(); ++i) { + const double dist_orig = (points[i] - points[i - 1]).norm(); + const double dist_tform = (reconstruction.Point3D(point_ids[i]).XYZ() - + reconstruction.Point3D(point_ids[i - 1]).XYZ()) + .norm(); + BOOST_CHECK_LE(std::abs(dist_orig - dist_tform), 1e-6); + } + +} diff --git a/src/exe/model.cc b/src/exe/model.cc index f6a5e7fb4..bdf5bab74 100644 --- a/src/exe/model.cc +++ b/src/exe/model.cc @@ -31,6 +31,7 @@ #include "exe/model.h" +#include "base/gps.h" #include "base/pose.h" #include "base/similarity_transform.h" #include "estimators/coordinate_frame.h" @@ -40,6 +41,37 @@ namespace colmap { namespace { +void ReadFileCameraLocations(const std::string& ref_images_path, + std::vector& ref_image_names, + std::vector& ref_locations) { + const auto lines = ReadTextFileLines(ref_images_path); + for (const auto& line : lines) { + std::stringstream line_parser(line); + std::string image_name; + Eigen::Vector3d camera_position; + line_parser >> image_name >> camera_position[0] >> camera_position[1] >> + camera_position[2]; + ref_image_names.push_back(image_name); + ref_locations.push_back(camera_position); + } +} + +void ReadDatabaseCameraLocations(const std::string& database_path, + std::vector& ref_image_names, + std::vector& ref_locations) { + Database database(database_path); + auto images = database.ReadAllImages(); + std::vector gps_locations; + GPSTransform gps_transform(GPSTransform::WGS84); + for (const auto image : images) { + if (image.HasTvecPrior()) { + ref_image_names.push_back(image.Name()); + gps_locations.push_back(image.TvecPrior()); + } + } + ref_locations = gps_transform.EllToXYZ(gps_locations); +} + void WriteComparisonErrorsCSV(const std::string& path, const std::vector& rotation_errors, const std::vector& translation_errors, @@ -93,62 +125,105 @@ void PrintComparisonSummary(std::ostream& out, int RunModelAligner(int argc, char** argv) { std::string input_path; - std::string ref_images_path; std::string output_path; + std::string database_path; + std::string ref_images_path; + std::string transform_path; + std::string alignment_type = "plane"; int min_common_images = 3; bool robust_alignment = true; + bool estimate_scale = true; RANSACOptions ransac_options; OptionManager options; options.AddRequiredOption("input_path", &input_path); - options.AddRequiredOption("ref_images_path", &ref_images_path); options.AddRequiredOption("output_path", &output_path); + options.AddDefaultOption("database_path", &database_path); + options.AddDefaultOption("ref_images_path", &ref_images_path); + options.AddDefaultOption("transform_path", &transform_path); + options.AddDefaultOption("alignment_type", &alignment_type, + "{plane, ecef, enu, enu-unscaled, custom}"); options.AddDefaultOption("min_common_images", &min_common_images); options.AddDefaultOption("robust_alignment", &robust_alignment); + options.AddDefaultOption("estimate_scale", &estimate_scale); options.AddDefaultOption("robust_alignment_max_error", &ransac_options.max_error); options.Parse(argc, argv); + StringToLower(&alignment_type); + const std::unordered_set alignment_options{ + "plane", "ecef", "enu", "enu-unscaled", "custom"}; + if (alignment_options.count(alignment_type) == 0) { + std::cerr << "ERROR: Invalid `alignment_type` - supported values are " + "{'plane', 'ecef', 'enu', 'enu-unscaled', 'custom'}" + << std::endl; + return EXIT_FAILURE; + } + if (robust_alignment && ransac_options.max_error <= 0) { std::cout << "ERROR: You must provide a maximum alignment error > 0" << std::endl; return EXIT_FAILURE; } + if (alignment_type != "plane" && database_path.empty() && + ref_images_path.empty()) { + std::cerr << "ERROR: Location alignment requires either database or " + "location file path." + << std::endl; + return EXIT_FAILURE; + } + std::vector ref_image_names; std::vector ref_locations; - std::vector lines = ReadTextFileLines(ref_images_path); - for (const auto& line : lines) { - std::stringstream line_parser(line); - std::string image_name = ""; - Eigen::Vector3d camera_position; - line_parser >> image_name >> camera_position[0] >> camera_position[1] >> - camera_position[2]; - ref_image_names.push_back(image_name); - ref_locations.push_back(camera_position); + if (!ref_images_path.empty() && database_path.empty()) { + ReadFileCameraLocations(ref_images_path, ref_image_names, ref_locations); + } else if (!database_path.empty() && ref_images_path.empty()) { + ReadDatabaseCameraLocations(database_path, ref_image_names, ref_locations); + } else { + std::cerr << "ERROR: Use location file or database, not both" << std::endl; + return EXIT_FAILURE; + } + + if (alignment_type != "plane" && ref_locations.size() < min_common_images) { + std::cout << "ERROR: Cannot align with insufficient reference locations." + << std::endl; + return EXIT_FAILURE; } Reconstruction reconstruction; reconstruction.Read(input_path); + SimilarityTransform3 tform; + bool alignment_success = true; - PrintHeading2("Aligning reconstruction"); - - std::cout << StringPrintf(" => Using %d reference images", - ref_image_names.size()) - << std::endl; - - bool alignment_success; - if (robust_alignment) { - alignment_success = reconstruction.AlignRobust( - ref_image_names, ref_locations, min_common_images, ransac_options); + if (alignment_type == "plane") { + PrintHeading2("Aligning reconstruction to principal plane"); + AlignToPrincipalPlane(&reconstruction, &tform); } else { - alignment_success = - reconstruction.Align(ref_image_names, ref_locations, min_common_images); - } + PrintHeading2("Aligning reconstruction to ECEF"); + std::cout << StringPrintf(" => Using %d reference images", + ref_image_names.size()) + << std::endl; - if (alignment_success) { - std::cout << " => Alignment succeeded" << std::endl; - reconstruction.Write(output_path); + if (estimate_scale) { + if (robust_alignment) { + alignment_success = reconstruction.AlignRobust( + ref_image_names, ref_locations, min_common_images, ransac_options, + &tform); + } else { + alignment_success = reconstruction.Align(ref_image_names, ref_locations, + min_common_images, &tform); + } + } else { + if (robust_alignment) { + alignment_success = reconstruction.AlignRobust( + ref_image_names, ref_locations, min_common_images, ransac_options, + &tform); + } else { + alignment_success = reconstruction.Align( + ref_image_names, ref_locations, min_common_images, &tform); + } + } std::vector errors; errors.reserve(ref_image_names.size()); @@ -159,15 +234,27 @@ int RunModelAligner(int argc, char** argv) { errors.push_back((image->ProjectionCenter() - ref_locations[i]).norm()); } } - std::cout << StringPrintf(" => Alignment error: %f (mean), %f (median)", Mean(errors), Median(errors)) << std::endl; + + if (alignment_success && StringStartsWith(alignment_type, "enu")) { + PrintHeading2("Aligning reconstruction to ENU"); + AlignToENUPlane(&reconstruction, &tform, alignment_type == "enu-unscaled"); + } + } + + if (alignment_success) { + std::cout << " => Alignment succeeded" << std::endl; + reconstruction.Write(output_path); + if (!transform_path.empty()) { + tform.Write(transform_path); + } + return EXIT_SUCCESS; } else { std::cout << " => Alignment failed" << std::endl; + return EXIT_FAILURE; } - - return EXIT_SUCCESS; } int RunModelAnalyzer(int argc, char** argv) {