diff --git a/glomap/estimators/global_rotation_averaging.cc b/glomap/estimators/global_rotation_averaging.cc index 3f7d031..a968fa1 100644 --- a/glomap/estimators/global_rotation_averaging.cc +++ b/glomap/estimators/global_rotation_averaging.cc @@ -56,6 +56,9 @@ bool RotationEstimator::EstimateRotations( image.cam_from_world.rotation = Eigen::Quaterniond(AngleAxisToRotation( rotation_estimated_.segment(image_id_to_idx_[image_id], 3))); } + // Restore the prior position (t = -Rc = R * R_ori * t_ori = R * t_ori) + image.cam_from_world.translation = + (image.cam_from_world.rotation * image.cam_from_world.translation); } return true; diff --git a/glomap/io/colmap_converter.cc b/glomap/io/colmap_converter.cc index eceaead..3747308 100644 --- a/glomap/io/colmap_converter.cc +++ b/glomap/io/colmap_converter.cc @@ -184,14 +184,15 @@ void ConvertDatabaseToGlomap(const colmap::Database& database, << images_colmap.size() << std::flush; counter++; - image_t image_id = image.ImageId(); + const image_t image_id = image.ImageId(); if (image_id == colmap::kInvalidImageId) continue; auto ite = images.insert(std::make_pair( image_id, Image(image_id, image.CameraId(), image.Name()))); const colmap::PosePrior prior = database.ReadPosePrior(image_id); if (prior.IsValid()) { - ite.first->second.cam_from_world = Rigid3d( - colmap::Rigid3d(Eigen::Quaterniond::Identity(), prior.position)); + const colmap::Rigid3d world_from_cam_prior(Eigen::Quaterniond::Identity(), + prior.position); + ite.first->second.cam_from_world = Rigid3d(Inverse(world_from_cam_prior)); } else { ite.first->second.cam_from_world = Rigid3d(); } @@ -200,20 +201,18 @@ void ConvertDatabaseToGlomap(const colmap::Database& database, // Read keypoints for (auto& [image_id, image] : images) { - colmap::FeatureKeypoints keypoints = database.ReadKeypoints(image_id); - - image.features.reserve(keypoints.size()); - for (int i = 0; i < keypoints.size(); i++) { - image.features.emplace_back( - Eigen::Vector2d(keypoints[i].x, keypoints[i].y)); + const colmap::FeatureKeypoints keypoints = database.ReadKeypoints(image_id); + const int num_keypoints = keypoints.size(); + image.features.resize(num_keypoints); + for (int i = 0; i < num_keypoints; i++) { + image.features[i] = Eigen::Vector2d(keypoints[i].x, keypoints[i].y); } } // Add the cameras std::vector cameras_colmap = database.ReadAllCameras(); for (auto& camera : cameras_colmap) { - camera_t camera_id = camera.camera_id; - cameras[camera_id] = camera; + cameras[camera.camera_id] = camera; } // Add the matches