diff --git a/localization/localization_node/include/localization_node/localization.h b/localization/localization_node/include/localization_node/localization.h index 9278f3aa00..37acce00d5 100644 --- a/localization/localization_node/include/localization_node/localization.h +++ b/localization/localization_node/include/localization_node/localization.h @@ -48,10 +48,12 @@ class Localizer { bool ReadParams(config_reader::ConfigReader& config, bool fatal_failure = true); bool Localize(cv_bridge::CvImageConstPtr image_ptr, ff_msgs::VisualLandmarks* vl, Eigen::Matrix2Xd* image_keypoints = NULL); + const camera::CameraParameters& camera_params() const { return *cam_params_; } private: void AdjustThresholds(); sparse_mapping::SparseMap* map_; + std::shared_ptr cam_params_; // Success params for adjusting keypoint thresholds std::deque successes_; ThresholdParams params_; diff --git a/localization/localization_node/src/localization.cc b/localization/localization_node/src/localization.cc index 7ba01f1265..5c7b218da4 100644 --- a/localization/localization_node/src/localization.cc +++ b/localization/localization_node/src/localization.cc @@ -30,7 +30,7 @@ namespace localization_node { Localizer::Localizer(sparse_mapping::SparseMap* map): map_(map) {} bool Localizer::ReadParams(config_reader::ConfigReader& config, bool fatal_failure) { - camera::CameraParameters cam_params(&config, "nav_cam"); + cam_params_ = std::make_shared(&config, "nav_cam"); std::string prefix; const auto detector_name = map_->GetDetectorName(); if (detector_name == "ORGBRISK") { @@ -104,7 +104,6 @@ bool Localizer::ReadParams(config_reader::ConfigReader& config, bool fatal_failu return false; } - map_->SetCameraParameters(cam_params); map_->SetLocParams(loc_params); map_->SetDetectorParams(min_features, max_features, detection_retries, min_threshold, default_threshold, max_threshold, too_many_ratio, too_few_ratio); @@ -126,10 +125,10 @@ bool Localizer::Localize(cv_bridge::CvImageConstPtr image_ptr, ff_msgs::VisualLa vl->header.frame_id = "world"; timer_.Start(); - map_->DetectFeatures(image_ptr->image, multithreaded, &image_descriptors, image_keypoints); + map_->DetectFeatures(image_ptr->image, *cam_params_, multithreaded, &image_descriptors, image_keypoints); camera::CameraModel camera(Eigen::Vector3d(), Eigen::Matrix3d::Identity(), - map_->GetCameraParameters()); + *cam_params_); std::vector landmarks; std::vector observations; if (!map_->Localize(image_descriptors, *image_keypoints, diff --git a/localization/localization_node/src/localization_nodelet.cc b/localization/localization_node/src/localization_nodelet.cc index 5363bafadb..0f2260083c 100644 --- a/localization/localization_node/src/localization_nodelet.cc +++ b/localization/localization_node/src/localization_nodelet.cc @@ -249,7 +249,7 @@ void LocalizationNodelet::Localize(void) { Eigen::Vector2d undistorted, distorted; undistorted[0] = vl.landmarks[i].u; undistorted[1] = vl.landmarks[i].v; - (map_->GetCameraParameters()).Convert(undistorted, &distorted); + (inst_->camera_params()).Convert(undistorted, &distorted); cv::circle(used_image->image, cv::Point(distorted[0], distorted[1]), 10, CV_RGB(255, 255, 255), 3, 8); cv::circle(used_image->image, cv::Point(distorted[0], distorted[1]), 6, CV_RGB(0, 0, 0), 3, 8); } @@ -261,7 +261,7 @@ void LocalizationNodelet::Localize(void) { Eigen::Vector2d undistorted, distorted; undistorted[0] = image_keypoints.col(i)[0]; undistorted[1] = image_keypoints.col(i)[1]; - (map_->GetCameraParameters()).Convert(undistorted, &distorted); + (inst_->camera_params()).Convert(undistorted, &distorted); cv::circle(detected_image->image, cv::Point(distorted[0], distorted[1]), 10, CV_RGB(255, 255, 255), 3, 8); cv::circle(detected_image->image, cv::Point(distorted[0], distorted[1]), 6, CV_RGB(0, 0, 0), 2, 8); } diff --git a/localization/sparse_mapping/include/sparse_mapping/reprojection.h b/localization/sparse_mapping/include/sparse_mapping/reprojection.h index 3382ff9d98..00b381706b 100644 --- a/localization/sparse_mapping/include/sparse_mapping/reprojection.h +++ b/localization/sparse_mapping/include/sparse_mapping/reprojection.h @@ -30,6 +30,7 @@ namespace camera { class CameraModel; + class CameraParameters; } namespace cv { @@ -60,7 +61,8 @@ namespace sparse_mapping { **/ void BundleAdjust(std::vector > const& pid_to_cid_fid, std::vector const& cid_to_keypoint_map, - double focal_length, + std::vector const& cid_to_camera_id, + std::vector const& camera_id_to_camera_params, std::vector * cid_to_cam_t_global, std::vector * pid_to_xyz, std::vector > const& user_pid_to_cid_fid, @@ -82,7 +84,7 @@ void BundleAdjust(std::vector > const& pid_to_cid_fid, * **/ void BundleAdjustSmallSet(std::vector const& features_n, - double focal_length, + std::vector focal_length_n, std::vector * cam_t_global_n, Eigen::Matrix3Xd * pid_to_xyz, ceres::LossFunction * loss, diff --git a/localization/sparse_mapping/include/sparse_mapping/sparse_map.h b/localization/sparse_mapping/include/sparse_mapping/sparse_map.h index 370e95dbe5..72760a3217 100644 --- a/localization/sparse_mapping/include/sparse_mapping/sparse_map.h +++ b/localization/sparse_mapping/include/sparse_mapping/sparse_map.h @@ -65,14 +65,22 @@ struct SparseMap { /** * Constructs a new sparse map from a list of image files and their - * associate keypoint and descriptor files. If use_cached_features - * is set to false, it reads the image files and performs feature - * detection instead. Does not perform bundle adjustment. + * associate keypoint and descriptor files. Assumes a single camera model + * used for all the images. **/ SparseMap(const std::vector & filenames, const std::string & detector, const camera::CameraParameters & params); + /** + * Constructs a new sparse map from a list of image files and their + * associate keypoint and descriptor files. Adds camera models + * and a map associating each image with its respective camera model. + **/ + SparseMap(const std::vector& filenames, const std::string& detector, + const std::vector& cid_to_camera_id, + const std::vector& camera_id_to_camera_params); + /** * Constructs a new sparse map from a protobuf file, with specified * vocabulary tree and optional parameters. @@ -177,8 +185,17 @@ struct SparseMap { /** * Return the parameters of the camera used to construct the map. **/ - camera::CameraParameters GetCameraParameters(void) const {return camera_params_;} - void SetCameraParameters(camera::CameraParameters camera_params) {camera_params_ = camera_params;} + const camera::CameraParameters& camera_params(int cid) const { + return camera_id_to_camera_params_[cid_to_camera_id_[cid]]; + } + camera::CameraParameters GetCameraParameters(int cid) const { + return camera_id_to_camera_params_[cid_to_camera_id_[cid]]; + } + + void OverwriteCameraParameters(const camera::CameraParameters& camera_params, int camera_id = 0) { + camera_id_to_camera_params_[camera_id] = camera_params; + } + /** * Return the number of observations. Use this number to divide the final error to find the average pixel error. **/ @@ -212,10 +229,12 @@ struct SparseMap { // detect features with opencv void DetectFeaturesFromFile(std::string const& filename, + const camera::CameraParameters& camera_params, bool multithreaded, cv::Mat* descriptors, Eigen::Matrix2Xd* keypoints); void DetectFeatures(cv::Mat const& image, + const camera::CameraParameters& camera_params, bool multithreaded, cv::Mat* descriptors, Eigen::Matrix2Xd* keypoints); @@ -239,7 +258,8 @@ struct SparseMap { std::vector> cid_to_matching_cid_counts_; interest_point::FeatureDetector detector_; - camera::CameraParameters camera_params_; + std::vector cid_to_camera_id_; + std::vector camera_id_to_camera_params_; mutable sparse_mapping::VocabDB vocab_db_; // TODO(oalexan1): Mutable means someone is doing something wrong. LocalizationParameters loc_params_; std::string protobuf_file_; diff --git a/localization/sparse_mapping/include/sparse_mapping/sparse_mapping.h b/localization/sparse_mapping/include/sparse_mapping/sparse_mapping.h index c9ec62ae19..1dfd3d2d00 100644 --- a/localization/sparse_mapping/include/sparse_mapping/sparse_mapping.h +++ b/localization/sparse_mapping/include/sparse_mapping/sparse_mapping.h @@ -180,7 +180,8 @@ double ComputeRaysAngle(int pid, std::vector > const& pid_to_ std::vector const& cam_ctrs, std::vector const& pid_to_xyz); // Filter points by reprojection error and other criteria -void FilterPID(double reproj_thresh, camera::CameraParameters const& camera_params, +void FilterPID(double reproj_thresh, std::vector const& cid_to_camera_id, + std::vector const& camera_id_to_camera_params, std::vector const& cid_to_cam_t_global, std::vector const& cid_to_keypoint_map, std::vector >* pid_to_cid_fid, std::vector* pid_to_xyz, diff --git a/localization/sparse_mapping/include/sparse_mapping/tensor.h b/localization/sparse_mapping/include/sparse_mapping/tensor.h index e72cfc60ef..6159ad8a63 100644 --- a/localization/sparse_mapping/include/sparse_mapping/tensor.h +++ b/localization/sparse_mapping/include/sparse_mapping/tensor.h @@ -146,14 +146,16 @@ namespace sparse_mapping { // Filter the matches by a geometric constraint. Compute the essential matrix. void FindEssentialAndInliers(Eigen::Matrix2Xd const& keypoints1, Eigen::Matrix2Xd const& keypoints2, - std::vector const& matches, camera::CameraParameters const& camera_params, + std::vector const& matches, camera::CameraParameters const& camera_params1, + camera::CameraParameters const& camera_params2, std::vector* inlier_matches, std::vector* vec_inliers, Eigen::Matrix3d* essential_matrix, const int ransac_iterations = 4096); void BuildMapFindEssentialAndInliers(const Eigen::Matrix2Xd & keypoints1, const Eigen::Matrix2Xd & keypoints2, const std::vector & matches, - camera::CameraParameters const& camera_params, + camera::CameraParameters const& camera_params1, + camera::CameraParameters const& camera_params2, bool compute_inliers_only, size_t cam_a_idx, size_t cam_b_idx, std::mutex * match_mutex, @@ -176,7 +178,8 @@ namespace sparse_mapping { // Triangulates all points given camera positions. This is better // than what is in sparse map as it uses multiple view information. - void Triangulate(bool rm_invalid_xyz, double focal_length, + void Triangulate(bool rm_invalid_xyz, std::vector const& cid_to_camera_id, + std::vector const& camera_id_to_camera_params, std::vector const& cid_to_cam_t_global, std::vector const& cid_to_keypoint_map, std::vector > * pid_to_cid_fid, diff --git a/localization/sparse_mapping/protobuf/sparse_map.proto b/localization/sparse_mapping/protobuf/sparse_map.proto index 2f42d0998e..9735277b54 100644 --- a/localization/sparse_mapping/protobuf/sparse_map.proto +++ b/localization/sparse_mapping/protobuf/sparse_map.proto @@ -36,6 +36,7 @@ message Frame { optional string name = 1; repeated Feature feature = 2; optional Affine3d pose = 3; + optional int32 camera_id = 4; } message Vector3d { @@ -124,5 +125,6 @@ message Map { optional int32 orgbrisk_threshold = 8; // this is no longer used but remains for compatability // histogram_equalization 1 means true, 0 means false, 2 means not known optional int32 histogram_equalization = 9 [default = 2]; + optional int32 num_cameras = 10; } diff --git a/localization/sparse_mapping/src/reprojection.cc b/localization/sparse_mapping/src/reprojection.cc index 5bf243f717..2eb4cd2217 100644 --- a/localization/sparse_mapping/src/reprojection.cc +++ b/localization/sparse_mapping/src/reprojection.cc @@ -94,7 +94,8 @@ struct ReprojectionError { }; void BundleAdjust(std::vector > const& pid_to_cid_fid, - std::vector const& cid_to_keypoint_map, double focal_length, + std::vector const& cid_to_keypoint_map, std::vector const& cid_to_camera_id, + std::vector const& camera_id_to_camera_params, std::vector* cid_to_cam_t_global, std::vector* pid_to_xyz, std::vector > const& user_pid_to_cid_fid, std::vector const& user_cid_to_keypoint_map, @@ -109,12 +110,14 @@ void BundleAdjust(std::vector > const& pid_to_cid_fid, // Allocate space for the angle axis representation of rotation std::vector camera_aa_storage(3 * cid_to_cam_t_global->size()); + std::vector focal_lengths; for (size_t cid = 0; cid < cid_to_cam_t_global->size(); cid++) { Eigen::Map aa_storage(camera_aa_storage.data() + 3 * cid); Eigen::Vector3d vec; camera::RotationToRodrigues(cid_to_cam_t_global->at(cid).linear(), &vec); aa_storage = vec; + focal_lengths.emplace_back(camera_id_to_camera_params[cid_to_camera_id[cid]].GetFocalLength()); } // Build problem @@ -158,13 +161,12 @@ void BundleAdjust(std::vector > const& pid_to_cid_fid, for (std::map::value_type const& cid_fid : (*p_pid_to_cid_fid)[pid]) { ceres::CostFunction* cost_function = ReprojectionError::Create((*p_cid_to_keypoint_map)[cid_fid.first].col(cid_fid.second)); - problem.AddResidualBlock(cost_function, local_loss, &cid_to_cam_t_global->at(cid_fid.first).translation()[0], &camera_aa_storage[3 * cid_fid.first], &p_pid_to_xyz->at(pid)[0], - &focal_length); + &focal_lengths[cid_fid.first]); if (fix_all_cameras || (cid_fid.first < first || cid_fid.first > last) || fixed_cameras.find(cid_fid.first) != fixed_cameras.end()) { @@ -180,7 +182,9 @@ void BundleAdjust(std::vector > const& pid_to_cid_fid, problem.SetParameterBlockConstant(&p_pid_to_xyz->at(pid)[0]); } } - problem.SetParameterBlockConstant(&focal_length); + for (auto& focal_length : focal_lengths) { + problem.SetParameterBlockConstant(&focal_length); + } } // Solve the problem @@ -198,7 +202,7 @@ void BundleAdjust(std::vector > const& pid_to_cid_fid, // This is a very specialized function void BundleAdjustSmallSet(std::vector const& features_n, - double focal_length, + std::vector focal_length_n, std::vector * cam_t_global_n, Eigen::Matrix3Xd * pid_to_xyz, ceres::LossFunction * loss, @@ -232,10 +236,12 @@ void BundleAdjustSmallSet(std::vector const& features_n, &cam_t_global_n->at(cid).translation()[0], &aa.at(cid)[0], &pid_to_xyz->col(pid)[0], - &focal_length); + &focal_length_n[cid]); } } - problem.SetParameterBlockConstant(&focal_length); + for (size_t cid = 0; cid < n_cameras; ++cid) { + problem.SetParameterBlockConstant(&focal_length_n[cid]); + } // Solve the problem ceres::Solve(options, &problem, summary); diff --git a/localization/sparse_mapping/src/sparse_map.cc b/localization/sparse_mapping/src/sparse_map.cc index d6c758d17e..92230e0a2f 100644 --- a/localization/sparse_mapping/src/sparse_map.cc +++ b/localization/sparse_mapping/src/sparse_map.cc @@ -88,17 +88,29 @@ namespace sparse_mapping { SparseMap::SparseMap(const std::vector& filenames, const std::string& detector, const camera::CameraParameters& params) + : cid_to_filename_(filenames), + detector_(detector) { + SetDefaultLocParams(); + cid_to_descriptor_map_.resize(cid_to_filename_.size()); + cid_to_keypoint_map_.resize(cid_to_filename_.size()); + cid_to_camera_id_.resize(cid_to_filename_.size()); + camera_id_to_camera_params_.emplace_back(params); +} + +SparseMap::SparseMap(const std::vector& filenames, const std::string& detector, + const std::vector& cid_to_camera_id, + const std::vector& camera_id_to_camera_params) : cid_to_filename_(filenames), detector_(detector), - camera_params_(params) { + cid_to_camera_id_(cid_to_camera_id), + camera_id_to_camera_params_(camera_id_to_camera_params) { SetDefaultLocParams(); cid_to_descriptor_map_.resize(cid_to_filename_.size()); cid_to_keypoint_map_.resize(cid_to_filename_.size()); } SparseMap::SparseMap(const std::string& protobuf_file, bool localization) - : camera_params_(Eigen::Vector2i(-1, -1), Eigen::Vector2d::Constant(-1), Eigen::Vector2d(-1, -1)), - protobuf_file_(protobuf_file) { + : protobuf_file_(protobuf_file) { SetDefaultLocParams(); // The above camera params used bad values because we are expected to reload // later. @@ -108,8 +120,7 @@ SparseMap::SparseMap(const std::string& protobuf_file, bool localization) // Form a sparse map with given cameras/images, and no features SparseMap::SparseMap(const std::vector& cid_to_cam_t, const std::vector& filenames, const std::string& detector, const camera::CameraParameters& params) - : detector_(detector), - camera_params_(params) { + : detector_(detector) { SetDefaultLocParams(); if (filenames.size() != cid_to_cam_t.size()) LOG(FATAL) << "Expecting as many images as cameras"; @@ -127,13 +138,14 @@ SparseMap::SparseMap(const std::vector& cid_to_cam_t, const std // Initialize other data expected in the map cid_to_keypoint_map_.resize(num_cams); cid_to_descriptor_map_.resize(num_cams); + cid_to_camera_id_.resize(cid_to_filename_.size(), 0); + camera_id_to_camera_params_.emplace_back(params); } // Form a sparse map by reading a text file from disk. This is for comparing // bundler, nvm or theia maps. -SparseMap::SparseMap(bool bundler_format, std::string const& filename, std::vector const& all_image_files) - : camera_params_(Eigen::Vector2i(640, 480), Eigen::Vector2d::Constant(300), - Eigen::Vector2d(320, 240)) /* these are placeholders and must be changed */ { +SparseMap::SparseMap(bool bundler_format, std::string const& filename, + std::vector const& all_image_files) { SetDefaultLocParams(); std::string ext = ff_common::file_extension(filename); boost::to_lower(ext); @@ -276,11 +288,8 @@ void SparseMap::DetectFeatures() { for (size_t cid = 0; cid < num_files; cid++) { ff_common::PrintProgressBar(stdout, static_cast(cid) / static_cast(num_files - 1)); - pool.AddTask(&SparseMap::DetectFeaturesFromFile, this, - std::ref(cid_to_filename_[cid]), - multithreaded, - &cid_to_descriptor_map_[cid], - &cid_to_keypoint_map_[cid]); + pool.AddTask(&SparseMap::DetectFeaturesFromFile, this, std::ref(cid_to_filename_[cid]), + std::ref(camera_params(cid)), multithreaded, &cid_to_descriptor_map_[cid], &cid_to_keypoint_map_[cid]); } pool.Join(); @@ -317,30 +326,66 @@ void SparseMap::Load(const std::string & protobuf_file, bool localization) { detector_.Reset(map.detector_name()); // Check that the maps is correctly formed - assert(map.camera().focal_length_size() == 2); - assert(map.camera().optical_offset_size() == 2); - assert(map.camera().distorted_image_size_size() == 2); - assert(map.camera().undistorted_image_size_size() == 2); - typedef Eigen::Vector2d V2d; - typedef Eigen::Vector2i V2i; - camera_params_.SetFocalLength(V2d(map.camera().focal_length(0), - map.camera().focal_length(1))); - camera_params_.SetOpticalOffset(V2d(map.camera().optical_offset(0), - map.camera().optical_offset(1))); - camera_params_.SetDistortedSize(V2i(map.camera().distorted_image_size(0), - map.camera().distorted_image_size(1))); - camera_params_.SetUndistortedSize(V2i(map.camera().undistorted_image_size(0), - map.camera().undistorted_image_size(1))); - Eigen::VectorXd distortion(map.camera().distortion_size()); - for (int i = 0; i < map.camera().distortion_size(); i++) { - distortion[i] = map.camera().distortion(i); + // For backwards compatibility, if num_cameras not set just load map.camera. + if (map.has_num_cameras()) { + for (int i = 0; i < map.num_cameras(); ++i) { + sparse_mapping_protobuf::CameraModel camera; + if (!ReadProtobufFrom(input, &camera)) { + LOG(FATAL) << "Failed to parse camera."; + } + assert(camera.focal_length_size() == 2); + assert(camera.optical_offset_size() == 2); + assert(camera.distorted_image_size_size() == 2); + assert(camera.undistorted_image_size_size() == 2); + typedef Eigen::Vector2d V2d; + typedef Eigen::Vector2i V2i; + const V2d focal_length(camera.focal_length(0), + camera.focal_length(1)); + const V2d optical_center(camera.optical_offset(0), + camera.optical_offset(1)); + const V2i distorted_image_size(camera.distorted_image_size(0), + camera.distorted_image_size(1)); + Eigen::VectorXd distortion(camera.distortion_size()); + for (int i = 0; i < camera.distortion_size(); i++) { + distortion[i] = camera.distortion(i); + } + camera::CameraParameters camera_params(distorted_image_size, focal_length, optical_center, distortion); + camera_params.SetDistortedSize(V2i(camera.distorted_image_size(0), + camera.distorted_image_size(1))); + camera_params.SetUndistortedSize(V2i(camera.undistorted_image_size(0), + camera.undistorted_image_size(1))); + camera_id_to_camera_params_.emplace_back(camera_params); + } + } else { + assert(map.camera().focal_length_size() == 2); + assert(map.camera().optical_offset_size() == 2); + assert(map.camera().distorted_image_size_size() == 2); + assert(map.camera().undistorted_image_size_size() == 2); + typedef Eigen::Vector2d V2d; + typedef Eigen::Vector2i V2i; + const V2d focal_length(map.camera().focal_length(0), + map.camera().focal_length(1)); + const V2d optical_center(map.camera().optical_offset(0), + map.camera().optical_offset(1)); + const V2i distorted_image_size(map.camera().distorted_image_size(0), + map.camera().distorted_image_size(1)); + Eigen::VectorXd distortion(map.camera().distortion_size()); + for (int i = 0; i < map.camera().distortion_size(); i++) { + distortion[i] = map.camera().distortion(i); + } + camera::CameraParameters camera_params(distorted_image_size, focal_length, optical_center, distortion); + camera_params.SetDistortedSize(V2i(map.camera().distorted_image_size(0), + map.camera().distorted_image_size(1))); + camera_params.SetUndistortedSize(V2i(map.camera().undistorted_image_size(0), + map.camera().undistorted_image_size(1))); + camera_id_to_camera_params_.emplace_back(camera_params); } - camera_params_.SetDistortion(distortion); int num_frames = map.num_frames(); int num_landmarks = map.num_landmarks(); cid_to_filename_.resize(num_frames); + cid_to_camera_id_.resize(num_frames, 0); cid_to_descriptor_map_.resize(num_frames); if (!localization) { cid_to_keypoint_map_.resize(num_frames); @@ -358,6 +403,7 @@ void SparseMap::Load(const std::string & protobuf_file, bool localization) { else cid_to_filename_[cid] = ""; + if (frame.has_camera_id()) cid_to_camera_id_[cid] = frame.camera_id(); // load keypoints if (!localization) @@ -531,18 +577,7 @@ void SparseMap::Save(const std::string & protobuf_file) const { else map.set_descriptor_depth(0); - sparse_mapping_protobuf::CameraModel* camera = map.mutable_camera(); - camera->add_focal_length(camera_params_.GetFocalVector()[0]); - camera->add_focal_length(camera_params_.GetFocalVector()[1]); - camera->add_optical_offset(camera_params_.GetOpticalOffset()[0]); - camera->add_optical_offset(camera_params_.GetOpticalOffset()[1]); - camera->add_distorted_image_size(camera_params_.GetDistortedSize()[0]); - camera->add_distorted_image_size(camera_params_.GetDistortedSize()[1]); - camera->add_undistorted_image_size(camera_params_.GetUndistortedSize()[0]); - camera->add_undistorted_image_size(camera_params_.GetUndistortedSize()[1]); - for (int i = 0; i < camera_params_.GetDistortion().size(); i++) { - camera->add_distortion(camera_params_.GetDistortion()[i]); - } + map.set_num_cameras(camera_id_to_camera_params_.size()); CHECK(cid_to_filename_.size() == cid_to_keypoint_map_.size()) << "Number of CIDs in filenames and keypoint map do not match"; @@ -567,6 +602,24 @@ void SparseMap::Save(const std::string & protobuf_file) const { LOG(FATAL) << "Failed to write map to file."; } + for (const auto& camera_params : camera_id_to_camera_params_) { + sparse_mapping_protobuf::CameraModel camera; + camera.add_focal_length(camera_params.GetFocalVector()[0]); + camera.add_focal_length(camera_params.GetFocalVector()[1]); + camera.add_optical_offset(camera_params.GetOpticalOffset()[0]); + camera.add_optical_offset(camera_params.GetOpticalOffset()[1]); + camera.add_distorted_image_size(camera_params.GetDistortedSize()[0]); + camera.add_distorted_image_size(camera_params.GetDistortedSize()[1]); + camera.add_undistorted_image_size(camera_params.GetUndistortedSize()[0]); + camera.add_undistorted_image_size(camera_params.GetUndistortedSize()[1]); + for (int i = 0; i < camera_params.GetDistortion().size(); i++) { + camera.add_distortion(camera_params.GetDistortion()[i]); + } + if (!WriteProtobufTo(camera, output)) { + LOG(FATAL) << "Failed to write camera to file."; + } + } + // write the frames for (size_t cid = 0; cid < cid_to_filename_.size(); cid++) { sparse_mapping_protobuf::Frame frame; @@ -575,6 +628,7 @@ void SparseMap::Save(const std::string & protobuf_file) const { if (!cid_to_filename_[cid].empty()) { frame.set_name(cid_to_filename_[cid]); } + frame.set_camera_id(cid_to_camera_id_[cid]); // set the features, required for (int fid = 0; fid < cid_to_keypoint_map_[cid].cols(); fid++) { @@ -661,21 +715,17 @@ void SparseMap::InitializeCidFidToPid() { &cid_fid_to_pid_); } -void SparseMap::DetectFeaturesFromFile(std::string const& filename, - bool multithreaded, - cv::Mat* descriptors, - Eigen::Matrix2Xd* keypoints) { +void SparseMap::DetectFeaturesFromFile(std::string const& filename, const camera::CameraParameters& camera_params, + bool multithreaded, cv::Mat* descriptors, Eigen::Matrix2Xd* keypoints) { cv::Mat image = cv::imread(filename, cv::IMREAD_GRAYSCALE); if (image.rows == 0 || image.cols == 0) LOG(FATAL) << "Found empty image in file: " << filename; - DetectFeatures(image, multithreaded, descriptors, keypoints); + DetectFeatures(image, camera_params, multithreaded, descriptors, keypoints); } -void SparseMap::DetectFeatures(const cv::Mat& image, - bool multithreaded, - cv::Mat* descriptors, - Eigen::Matrix2Xd* keypoints) { +void SparseMap::DetectFeatures(const cv::Mat& image, const camera::CameraParameters& camera_params, bool multithreaded, + cv::Mat* descriptors, Eigen::Matrix2Xd* keypoints) { // If using histogram equalization, need an extra image to store it cv::Mat * image_ptr = const_cast(&image); cv::Mat hist_image; @@ -728,7 +778,7 @@ void SparseMap::DetectFeatures(const cv::Mat& image, Eigen::Vector2d output; for (size_t j = 0; j < storage.size(); j++) { - camera_params_.Convert + camera_params.Convert (Eigen::Vector2d(storage[j].pt.x, storage[j].pt.y), &output); keypoints->col(j) = output; } @@ -816,7 +866,7 @@ bool SparseMap::Localize(cv::Mat const& test_descriptors, Eigen::Matrix2Xd const if (map_image.empty()) { LOG(ERROR) << "Failed to load map image: " << map_filename; } else { - ViewMatches(test_keypoints, cid_to_keypoint_map_[cid], all_matches[i], camera_params_, image, map_image); + ViewMatches(test_keypoints, cid_to_keypoint_map_[cid], all_matches[i], camera_params(cid), image, map_image); } } @@ -828,8 +878,8 @@ bool SparseMap::Localize(cv::Mat const& test_descriptors, Eigen::Matrix2Xd const std::vector inlier_matches; std::vector vec_inliers; Eigen::Matrix3d essential_matrix; - FindEssentialAndInliers(test_keypoints, cid_to_keypoint_map_[cid], all_matches[i], camera_params_, - &inlier_matches, &vec_inliers, &essential_matrix, + FindEssentialAndInliers(test_keypoints, cid_to_keypoint_map_[cid], all_matches[i], pose->GetParameters(), + camera_params(cid), &inlier_matches, &vec_inliers, &essential_matrix, loc_params_.essential_ransac_iterations); all_matches[i] = inlier_matches; if (loc_params_.verbose_localization) @@ -945,7 +995,7 @@ bool SparseMap::Localize(std::string const& img_file, cv::Mat test_descriptors; Eigen::Matrix2Xd test_keypoints; bool multithreaded = false; - DetectFeaturesFromFile(img_file, multithreaded, &test_descriptors, &test_keypoints); + DetectFeaturesFromFile(img_file, pose->GetParameters(), multithreaded, &test_descriptors, &test_keypoints); return Localize(test_descriptors, test_keypoints, pose, inlier_landmarks, inlier_observations, @@ -1123,7 +1173,7 @@ bool SparseMap::Localize(const cv::Mat & image, camera::CameraModel* pose, bool multithreaded = false; cv::Mat test_descriptors; Eigen::Matrix2Xd test_keypoints; - DetectFeatures(image, multithreaded, &test_descriptors, &test_keypoints); + DetectFeatures(image, pose->GetParameters(), multithreaded, &test_descriptors, &test_keypoints); return Localize(test_descriptors, test_keypoints, pose, inlier_landmarks, inlier_observations, diff --git a/localization/sparse_mapping/src/sparse_mapping.cc b/localization/sparse_mapping/src/sparse_mapping.cc index 8cfda8af8b..93ff43f313 100644 --- a/localization/sparse_mapping/src/sparse_mapping.cc +++ b/localization/sparse_mapping/src/sparse_mapping.cc @@ -1054,7 +1054,8 @@ double sparse_mapping::ComputeRaysAngle(int pid, } void sparse_mapping::FilterPID(double reproj_thresh, - camera::CameraParameters const& camera_params, + std::vector const& cid_to_camera_id, + std::vector const& camera_id_to_camera_params, std::vector const& cid_to_cam_t_global, std::vector const& cid_to_keypoint_map, std::vector > * pid_to_cid_fid, @@ -1077,7 +1078,6 @@ void sparse_mapping::FilterPID(double reproj_thresh, s.total = (*pid_to_xyz).size(); std::vector is_bad((*pid_to_xyz).size(), false); - Eigen::Vector2d half_size = camera_params.GetUndistortedHalfSize(); for (size_t pid = 0; pid < (*pid_to_xyz).size(); pid++) { bool small_angle = false, behind_cam = false, invalid_reproj = false; @@ -1090,11 +1090,13 @@ void sparse_mapping::FilterPID(double reproj_thresh, } for (std::pair cid_fid : (*pid_to_cid_fid)[pid]) { + const auto& camera_params = camera_id_to_camera_params[cid_to_camera_id[cid_fid.first]]; Eigen::Vector2d pix = (cid_to_cam_t_global[cid_fid.first] * (*pid_to_xyz)[pid]).hnormalized() * camera_params.GetFocalLength(); errors.push_back((cid_to_keypoint_map[cid_fid.first].col(cid_fid.second) - pix).norm()); // Mark points which don't project at valid camera pixels // TODO(zmoratto) : This can probably be done with a Eigen Array reduction + Eigen::Vector2d half_size = camera_params.GetUndistortedHalfSize(); if (pix[0] < -half_size[0] || pix[0] >= half_size[0] || pix[1] < -half_size[1] || pix[1] >= half_size[1]) { invalid_reproj = true; is_bad[pid] = true; @@ -1133,6 +1135,7 @@ void sparse_mapping::FilterPID(double reproj_thresh, std::map::iterator itr = cid_fid.begin(); while (itr != cid_fid.end()) { s.num_features++; + const auto& camera_params = camera_id_to_camera_params[cid_to_camera_id[itr->first]]; Eigen::Vector2d pix = (cid_to_cam_t_global[itr->first] * (*pid_to_xyz)[pid]).hnormalized() * camera_params.GetFocalLength(); double err diff --git a/localization/sparse_mapping/src/tensor.cc b/localization/sparse_mapping/src/tensor.cc index e51fa3f94b..86906554af 100644 --- a/localization/sparse_mapping/src/tensor.cc +++ b/localization/sparse_mapping/src/tensor.cc @@ -156,13 +156,16 @@ void WriteMatches(openMVG::matching::PairWiseMatches const& match_map, void BuildMapPerformMatching(openMVG::matching::PairWiseMatches * match_map, std::vector const& cid_to_keypoint_map, std::vector const& cid_to_descriptor_map, - camera::CameraParameters const& camera_params, + std::vector const& cid_to_camera_id, + std::vector const& camera_id_to_camera_params, CIDPairAffineMap * relative_affines, std::mutex * match_mutex, int i /*query cid index*/, int j /*train cid index*/, bool compute_rays_angle, double * rays_angle) { Eigen::Matrix2Xd const& keypoints1 = cid_to_keypoint_map[i]; Eigen::Matrix2Xd const& keypoints2 = cid_to_keypoint_map[j]; + const auto& camera_params1 = camera_id_to_camera_params[cid_to_camera_id[i]]; + const auto& camera_params2 = camera_id_to_camera_params[cid_to_camera_id[j]]; std::vector matches, inlier_matches; interest_point::FindMatches(cid_to_descriptor_map[i], @@ -178,7 +181,7 @@ void BuildMapPerformMatching(openMVG::matching::PairWiseMatches * match_map, bool compute_inliers_only = false; BuildMapFindEssentialAndInliers(keypoints1, keypoints2, matches, - camera_params, compute_inliers_only, + camera_params1, camera_params2, compute_inliers_only, i, j, match_mutex, relative_affines, @@ -284,7 +287,8 @@ void MatchFeatures(const std::string & essential_file, &match_map, s->cid_to_keypoint_map_, s->cid_to_descriptor_map_, - std::cref(s->camera_params_), + s->cid_to_camera_id_, + s->camera_id_to_camera_params_, &relative_affines, &match_mutex, cid, indices[j], @@ -352,7 +356,8 @@ void BuildTracks(bool rm_invalid_xyz, // Triangulate. The results should be quite inaccurate, we'll redo this // later. This step is mostly for consistency. sparse_mapping::Triangulate(rm_invalid_xyz, - s->camera_params_.GetFocalLength(), + s->cid_to_camera_id_, + s->camera_id_to_camera_params_, s->cid_to_cam_t_global_, s->cid_to_keypoint_map_, &(s->pid_to_cid_fid_), @@ -438,7 +443,8 @@ void IncrementalBA(std::string const& essential_file, pid_to_xyz_local.clear(); std::vector > cid_fid_to_pid_local; sparse_mapping::Triangulate(rm_invalid_xyz, - s->camera_params_.GetFocalLength(), + s->cid_to_camera_id_, + s->camera_id_to_camera_params_, cid_to_cam_t_local, s->cid_to_keypoint_map_, &pid_to_cid_fid_local, @@ -472,14 +478,10 @@ void IncrementalBA(std::string const& essential_file, LOG(INFO) << "Optimizing cameras from " << start << " to " << cid << " (total: " << cid-start+1 << ")"; - sparse_mapping::BundleAdjust(pid_to_cid_fid_local, s->cid_to_keypoint_map_, - s->camera_params_.GetFocalLength(), - &cid_to_cam_t_local, &pid_to_xyz_local, - s->user_pid_to_cid_fid_, - s->user_cid_to_keypoint_map_, - &(s->user_pid_to_xyz_), - loss, options, &summary, - start, cid); + sparse_mapping::BundleAdjust(pid_to_cid_fid_local, s->cid_to_keypoint_map_, s->cid_to_camera_id_, + s->camera_id_to_camera_params_, &cid_to_cam_t_local, &pid_to_xyz_local, + s->user_pid_to_cid_fid_, s->user_cid_to_keypoint_map_, &(s->user_pid_to_xyz_), loss, + options, &summary, start, cid); // Copy back for (int c = 0; c <= cid; c++) @@ -488,7 +490,8 @@ void IncrementalBA(std::string const& essential_file, // Triangulate all points sparse_mapping::Triangulate(rm_invalid_xyz, - s->camera_params_.GetFocalLength(), + s->cid_to_camera_id_, + s->camera_id_to_camera_params_, s->cid_to_cam_t_global_, s->cid_to_keypoint_map_, &(s->pid_to_cid_fid_), @@ -600,7 +603,8 @@ void CloseLoop(sparse_mapping::SparseMap * s) { // sparse_mapping::PrintPidStats(s->pid_to_cid_fid_); bool rm_invalid_xyz = true; sparse_mapping::Triangulate(rm_invalid_xyz, - s->camera_params_.GetFocalLength(), + s->cid_to_camera_id_, + s->camera_id_to_camera_params_, s->cid_to_cam_t_global_, s->cid_to_keypoint_map_, &(s->pid_to_cid_fid_), @@ -643,7 +647,8 @@ void BundleAdjustment(sparse_mapping::SparseMap * s, int first, int last, bool fix_all_cameras, std::set const& fixed_cameras) { sparse_mapping::BundleAdjust(s->pid_to_cid_fid_, s->cid_to_keypoint_map_, - s->camera_params_.GetFocalLength(), &(s->cid_to_cam_t_global_), + s->cid_to_camera_id_, s->camera_id_to_camera_params_, + &(s->cid_to_cam_t_global_), &(s->pid_to_xyz_), s->user_pid_to_cid_fid_, s->user_cid_to_keypoint_map_, &(s->user_pid_to_xyz_), @@ -652,7 +657,7 @@ void BundleAdjustment(sparse_mapping::SparseMap * s, // First do BA, and only afterwards remove outliers. if (!FLAGS_skip_filtering) { - FilterPID(FLAGS_reproj_thresh, s->camera_params_, s->cid_to_cam_t_global_, + FilterPID(FLAGS_reproj_thresh, s->cid_to_camera_id_, s->camera_id_to_camera_params_, s->cid_to_cam_t_global_, s->cid_to_keypoint_map_, &(s->pid_to_cid_fid_), &(s->pid_to_xyz_)); s->InitializeCidFidToPid(); } @@ -1178,8 +1183,6 @@ void MergeMaps(sparse_mapping::SparseMap * A_in, sparse_mapping::SparseMap & C = *C_out; // Basic sanity checks (not exhaustive) - if ( !(A.GetCameraParameters() == B.GetCameraParameters()) ) - LOG(FATAL) << "The input maps don't have the same camera parameters."; if ( !(A.detector_ == B.detector_) ) LOG(FATAL) << "The input maps don't have the same detector and/or descriptor."; @@ -1201,18 +1204,26 @@ void MergeMaps(sparse_mapping::SparseMap * A_in, int num_acid = A.cid_to_filename_.size(); int num_bcid = B.cid_to_filename_.size(); int num_ccid = num_acid + num_bcid; + int num_A_camera_ids = A.camera_id_to_camera_params_.size(); C.cid_to_filename_ .resize(num_ccid); C.cid_to_keypoint_map_ .resize(num_ccid); C.cid_to_cam_t_global_ .resize(num_ccid); C.cid_to_descriptor_map_.resize(num_ccid); + C.cid_to_camera_id_ .resize(num_ccid); for (int cid = 0; cid < num_bcid; cid++) { // C.cid_to_filename_ already contains A.cid_to_filename_, etc. int c = num_acid + cid; C.cid_to_filename_[c] = B.cid_to_filename_[cid]; C.cid_to_keypoint_map_[c] = B.cid_to_keypoint_map_[cid]; C.cid_to_descriptor_map_[c] = B.cid_to_descriptor_map_[cid]; + C.cid_to_camera_id_[c] = B.cid_to_camera_id_[cid] + num_A_camera_ids; // We will have to deal with cid_to_cam_t_global_ later } + // Append B's camera params to A's. + // TODO(rsoussan): Check for equality of camera params and consolidate + for (const auto& camera_params : B.camera_id_to_camera_params_) { + C.camera_id_to_camera_params_.emplace_back(camera_params); + } // Create cid_fid_to_pid_ for both maps, to be able to go from cid_fid to pid. A.InitializeCidFidToPid(); @@ -1418,7 +1429,8 @@ void MergeMaps(sparse_mapping::SparseMap * A_in, std::vector > new_cid_fid_to_pid; bool rm_invalid_xyz = true; // don't remove anything, as cameras are pretty unreliable now sparse_mapping::Triangulate(rm_invalid_xyz, - C.camera_params_.GetFocalLength(), + C.cid_to_camera_id_, + C.camera_id_to_camera_params_, C.cid_to_cam_t_global_, C.cid_to_keypoint_map_, &new_pid_to_cid_fid, @@ -1700,8 +1712,9 @@ double RegistrationOrVerification(std::vector const& data_files, // Shift the keypoints. Undistort if necessary. Eigen::Vector2d output; for (size_t cid = 0; cid < map->user_cid_to_keypoint_map_.size(); cid++) { + const auto& camera_params = map->camera_params(cid); for (int i = 0; i < map->user_cid_to_keypoint_map_[cid].cols(); i++) { - map->camera_params_.Convert + camera_params.Convert (map->user_cid_to_keypoint_map_[cid].col(i), &output); map->user_cid_to_keypoint_map_[cid].col(i) = output; } @@ -1718,7 +1731,8 @@ double RegistrationOrVerification(std::vector const& data_files, std::vector > cid_fid_to_pid_local; bool rm_invalid_xyz = false; // there should be nothing to remove hopefully sparse_mapping::Triangulate(rm_invalid_xyz, - map->camera_params_.GetFocalLength(), + map->cid_to_camera_id_, + map->camera_id_to_camera_params_, map->cid_to_cam_t_global_, map->user_cid_to_keypoint_map_, &(map->user_pid_to_cid_fid_), @@ -1935,9 +1949,10 @@ void ReadAffineCSV(std::string const& input_filename, // Filter the matches by a geometric constraint. Compute the essential matrix. void FindEssentialAndInliers(Eigen::Matrix2Xd const& keypoints1, Eigen::Matrix2Xd const& keypoints2, - std::vector const& matches, camera::CameraParameters const& camera_params, - std::vector* inlier_matches, std::vector* vec_inliers, - Eigen::Matrix3d* essential_matrix, const int ransac_iterations) { + std::vector const& matches, camera::CameraParameters const& camera_params1, + camera::CameraParameters const& camera_params2, std::vector* inlier_matches, + std::vector* vec_inliers, Eigen::Matrix3d* essential_matrix, + const int ransac_iterations) { // Initialize the outputs inlier_matches->clear(); @@ -1948,18 +1963,20 @@ void FindEssentialAndInliers(Eigen::Matrix2Xd const& keypoints1, Eigen::Matrix2X observationsa.col(i) = keypoints1.col(matches[i].queryIdx); observationsb.col(i) = keypoints2.col(matches[i].trainIdx); } - - std::pair image_size(camera_params.GetUndistortedSize()[0], - camera_params.GetUndistortedSize()[1]); - Eigen::Matrix3d k = camera_params.GetIntrinsicMatrix(); + std::pair image_size1(camera_params1.GetUndistortedSize()[0], + camera_params1.GetUndistortedSize()[1]); + std::pair image_size2(camera_params2.GetUndistortedSize()[0], + camera_params2.GetUndistortedSize()[1]); + Eigen::Matrix3d k1 = camera_params1.GetIntrinsicMatrix(); + Eigen::Matrix3d k2 = camera_params2.GetIntrinsicMatrix(); // Calculate the essential matrix double error_max = std::numeric_limits::max(); double max_expected_error = 2.5; - if (!interest_point::RobustEssential(k, k, observationsa, observationsb, + if (!interest_point::RobustEssential(k1, k2, observationsa, observationsb, essential_matrix, vec_inliers, - image_size, image_size, + image_size1, image_size2, &error_max, max_expected_error, ransac_iterations)) { VLOG(2) << " | Estimation of essential matrix failed!\n"; @@ -1979,7 +1996,8 @@ void FindEssentialAndInliers(Eigen::Matrix2Xd const& keypoints1, Eigen::Matrix2X void BuildMapFindEssentialAndInliers(Eigen::Matrix2Xd const& keypoints1, Eigen::Matrix2Xd const& keypoints2, std::vector const& matches, - camera::CameraParameters const& camera_params, + camera::CameraParameters const& camera_params1, + camera::CameraParameters const& camera_params2, bool compute_inliers_only, size_t cam_a_idx, size_t cam_b_idx, std::mutex * match_mutex, @@ -1993,7 +2011,8 @@ void BuildMapFindEssentialAndInliers(Eigen::Matrix2Xd const& keypoints1, std::vector vec_inliers; Eigen::Matrix3d e; - FindEssentialAndInliers(keypoints1, keypoints2, matches, camera_params, inlier_matches, &vec_inliers, &e); + FindEssentialAndInliers(keypoints1, keypoints2, matches, camera_params1, camera_params2, inlier_matches, &vec_inliers, + &e); if (!inlier_matches) { VLOG(2) << cam_a_idx << " " << cam_b_idx << " | Estimation of essential matrix failed!\n"; @@ -2028,8 +2047,9 @@ void BuildMapFindEssentialAndInliers(Eigen::Matrix2Xd const& keypoints1, observationsa.col(i) = keypoints1.col(matches[i].queryIdx); observationsb.col(i) = keypoints2.col(matches[i].trainIdx); } - Eigen::Matrix3d k = camera_params.GetIntrinsicMatrix(); - if (!interest_point::EstimateRTFromE(k, k, observationsa, observationsb, + Eigen::Matrix3d k1 = camera_params1.GetIntrinsicMatrix(); + Eigen::Matrix3d k2 = camera_params2.GetIntrinsicMatrix(); + if (!interest_point::EstimateRTFromE(k1, k2, observationsa, observationsb, e, vec_inliers, &r, &t)) { VLOG(2) << cam_a_idx << " " << cam_b_idx @@ -2064,13 +2084,9 @@ void BuildMapFindEssentialAndInliers(Eigen::Matrix2Xd const& keypoints1, double error; int num_pts_behind_camera = 0; for (ptrdiff_t i = 0; i < observations2[0].cols(); i++) { - pid_to_xyz.col(i) = - sparse_mapping::TriangulatePoint - (Eigen::Vector3d(observations2[0](0, i), observations2[0](1, i), - camera_params.GetFocalLength()), - Eigen::Vector3d(observations2[1](0, i), observations2[1](1, i), - camera_params.GetFocalLength()), - r, t, &error); + pid_to_xyz.col(i) = sparse_mapping::TriangulatePoint + (Eigen::Vector3d(observations2[0](0, i), observations2[0](1, i), camera_params1.GetFocalLength()), + Eigen::Vector3d(observations2[1](0, i), observations2[1](1, i), camera_params2.GetFocalLength()), r, t, &error); Eigen::Vector3d P = pid_to_xyz.col(i); Eigen::Vector3d Q = r*P + t; if (P[2] <= 0 || Q[2] <= 0) { @@ -2083,7 +2099,8 @@ void BuildMapFindEssentialAndInliers(Eigen::Matrix2Xd const& keypoints1, << " (" << round((100.0*num_pts_behind_camera) / observations2[0].cols()) << "%)"; - sparse_mapping::BundleAdjustSmallSet(observations2, camera_params.GetFocalLength(), &cameras, + std::vector focal_lengths{camera_params1.GetFocalLength(), camera_params2.GetFocalLength()}; + sparse_mapping::BundleAdjustSmallSet(observations2, focal_lengths, &cameras, &pid_to_xyz, new ceres::CauchyLoss(0.5), options, &summary); @@ -2103,9 +2120,9 @@ void BuildMapFindEssentialAndInliers(Eigen::Matrix2Xd const& keypoints1, Eigen::Vector3d P = sparse_mapping::TriangulatePoint (Eigen::Vector3d(observations2[0](0, i), observations2[0](1, i), - camera_params.GetFocalLength()), + camera_params1.GetFocalLength()), Eigen::Vector3d(observations2[1](0, i), observations2[1](1, i), - camera_params.GetFocalLength()), + camera_params2.GetFocalLength()), cameras[1].linear(), cameras[1].translation(), &error); Eigen::Vector3d X0 = ctr0 - P; @@ -2181,21 +2198,21 @@ void BuildMapFindEssentialAndInliers(Eigen::Matrix2Xd const& keypoints1, } } -void Triangulate(bool rm_invalid_xyz, double focal_length, +void Triangulate(bool rm_invalid_xyz, std::vector const& cid_to_camera_id, + std::vector const& camera_id_to_camera_params, std::vector const& cid_to_cam_t_global, std::vector const& cid_to_keypoint_map, - std::vector > * pid_to_cid_fid, - std::vector * pid_to_xyz, - std::vector > * cid_fid_to_pid) { - Eigen::Matrix3d k; - k << focal_length, 0, 0, - 0, focal_length, 0, - 0, 0, 1; - + std::vector>* pid_to_cid_fid, std::vector* pid_to_xyz, + std::vector>* cid_fid_to_pid) { // Build p matrices for all of the cameras. openMVG::Triangulation // will be holding pointers to all of the cameras. std::vector cid_to_p(cid_to_cam_t_global.size()); for (size_t cid = 0; cid < cid_to_p.size(); cid++) { + const double focal_length = camera_id_to_camera_params[cid_to_camera_id[cid]].GetFocalLength(); + Eigen::Matrix3d k; + k << focal_length, 0, 0, + 0, focal_length, 0, + 0, 0, 1; openMVG::P_From_KRt(k, cid_to_cam_t_global[cid].linear(), cid_to_cam_t_global[cid].translation(), &cid_to_p[cid]); } diff --git a/localization/sparse_mapping/tools/build_map.cc b/localization/sparse_mapping/tools/build_map.cc index bfe1fe002a..50f77e4ad7 100644 --- a/localization/sparse_mapping/tools/build_map.cc +++ b/localization/sparse_mapping/tools/build_map.cc @@ -236,11 +236,20 @@ void Rebuild() { LOG(INFO) << "Rebuilding map with " << FLAGS_rebuild_detector << " detector."; sparse_mapping::SparseMap original(FLAGS_output_map); - camera::CameraParameters params = original.GetCameraParameters(); + std::vector files(original.GetNumFrames()); + for (size_t i = 0; i < original.GetNumFrames(); i++) { + files[i] = original.GetFrameFilename(i); + } + + sparse_mapping::SparseMap map(files, FLAGS_rebuild_detector, original.cid_to_camera_id_, original.camera_id_to_camera_params_); + + // Replace camera model, assumes only one is used for the map if (FLAGS_rebuild_replace_camera) { char * bot_ptr = getenv("ASTROBEE_ROBOT"); if (bot_ptr == NULL) LOG(FATAL) << "Must set ASTROBEE_ROBOT."; + if (map.camera_id_to_camera_params_.size() != 1) + LOG(FATAL) << "When replacing camera, the original map must have been build with one camera, instead is used" << map.camera_id_to_camera_params_.size(); LOG(INFO) << "Using camera for robot: " << bot_ptr << "."; config_reader::ConfigReader config; config.AddFile("cameras.config"); @@ -249,16 +258,10 @@ void Rebuild() { exit(1); return; } - params = camera::CameraParameters(&config, FLAGS_robot_camera.c_str()); + const camera::CameraParameters params(&config, FLAGS_robot_camera.c_str()); + map.OverwriteCameraParameters(params); } - std::vector files(original.GetNumFrames()); - for (size_t i = 0; i < original.GetNumFrames(); i++) { - files[i] = original.GetFrameFilename(i); - } - - sparse_mapping::SparseMap map(files, FLAGS_rebuild_detector, params); - LOG(INFO) << "Detecting features."; map.DetectFeatures(); diff --git a/localization/sparse_mapping/tools/evaluate_localization.cc b/localization/sparse_mapping/tools/evaluate_localization.cc index a2a4ec6f93..6bc5ebe20c 100644 --- a/localization/sparse_mapping/tools/evaluate_localization.cc +++ b/localization/sparse_mapping/tools/evaluate_localization.cc @@ -65,7 +65,8 @@ int main(int argc, char** argv) { Eigen::Vector3d pos(x, y, z); trials++; - camera::CameraModel camera(Eigen::Vector3d(), Eigen::Matrix3d::Identity(), map.GetCameraParameters()); + // Assume first camera model + camera::CameraModel camera(Eigen::Vector3d(), Eigen::Matrix3d::Identity(), map.GetCameraParameters(0)); struct timeval a, b; gettimeofday(&a, NULL); if (!map.Localize(name, &camera)) { diff --git a/localization/sparse_mapping/tools/export_map.cc b/localization/sparse_mapping/tools/export_map.cc index 44db27e0e1..6b37848e10 100644 --- a/localization/sparse_mapping/tools/export_map.cc +++ b/localization/sparse_mapping/tools/export_map.cc @@ -52,7 +52,9 @@ int main(int argc, char** argv) { sparse_mapping::SparseMap map(FLAGS_input_map); - camera::CameraParameters camera_params = map.GetCameraParameters(); + // Note: this assumes only one camera exists in the map, since the NVM + // format only supports one camera! + camera::CameraParameters camera_params = map.camera_id_to_camera_params_[0]; // This is very important, std::cout << "Saving the nvm file with interest point matches that "; diff --git a/localization/sparse_mapping/tools/generate_localization_test.cc b/localization/sparse_mapping/tools/generate_localization_test.cc index 93df0e9416..c5c97e3f6d 100644 --- a/localization/sparse_mapping/tools/generate_localization_test.cc +++ b/localization/sparse_mapping/tools/generate_localization_test.cc @@ -39,8 +39,9 @@ int main(int argc, char** argv) { // print results for (size_t i = 0; i < map.GetNumFrames() - 1; i++) { - camera::CameraModel m1(map.GetFrameGlobalTransform(i), map.GetCameraParameters()); - camera::CameraModel m2(map.GetFrameGlobalTransform(i), map.GetCameraParameters()); + // Assume first camera model + camera::CameraModel m1(map.GetFrameGlobalTransform(i), map.GetCameraParameters(0)); + camera::CameraModel m2(map.GetFrameGlobalTransform(i), map.GetCameraParameters(0)); std::string name1 = map.GetFrameFilename(i); std::string name2 = map.GetFrameFilename(i+1); std::string base = name1.substr(0, name1.length() - 11); diff --git a/localization/sparse_mapping/tools/import_map.cc b/localization/sparse_mapping/tools/import_map.cc index c1a204aef7..b746b94cde 100644 --- a/localization/sparse_mapping/tools/import_map.cc +++ b/localization/sparse_mapping/tools/import_map.cc @@ -78,7 +78,7 @@ namespace { // Save the camera parameters for the given robot. camera::CameraParameters camera_params(&config, "nav_cam"); - map.SetCameraParameters(camera_params); + map.OverwriteCameraParameters(camera_params); // Replace the undistorted images with distorted ones std::vector distorted_images; @@ -191,7 +191,7 @@ int main(int argc, char** argv) { = camera::CameraParameters(Eigen::Vector2i(widx, widy), Eigen::Vector2d::Constant(f), Eigen::Vector2d(cx, cy)); - map.SetCameraParameters(camera_params); + map.OverwriteCameraParameters(camera_params); } else { // Replace the images and robot camera params with distorted (original) ones diff --git a/localization/sparse_mapping/tools/localize.cc b/localization/sparse_mapping/tools/localize.cc index 728315381e..363799e9b9 100644 --- a/localization/sparse_mapping/tools/localize.cc +++ b/localization/sparse_mapping/tools/localize.cc @@ -54,8 +54,9 @@ int main(int argc, char** argv) { FLAGS_histogram_equalization); // localize frame + // Use first camera model in map camera::CameraModel camera(Eigen::Vector3d(), Eigen::Matrix3d::Identity(), - map.GetCameraParameters()); + map.GetCameraParameters(0)); if (!map.Localize(img_file, &camera)) { LOG(ERROR) << "Failed to localize image."; return 1; diff --git a/localization/sparse_mapping/tools/localize_cams.cc b/localization/sparse_mapping/tools/localize_cams.cc index f9b9b70e4c..3294b2fad6 100644 --- a/localization/sparse_mapping/tools/localize_cams.cc +++ b/localization/sparse_mapping/tools/localize_cams.cc @@ -82,20 +82,20 @@ int main(int argc, char** argv) { sparse_mapping::SparseMap source(FLAGS_source_map); - if ( !(source.GetCameraParameters() == reference.GetCameraParameters()) ) - LOG(FATAL) << "The source and reference maps don't have the same camera parameters."; - int num_good_errors = 0; size_t num_frames = source.GetNumFrames(); for (size_t cid = 0; cid < num_frames; cid++) { + if ( !(source.GetCameraParameters(cid) == reference.GetCameraParameters(cid))) + LOG(FATAL) << "The source and reference maps don't have the same camera parameters."; + std::string img_file = source.GetFrameFilename(cid); // localize frame camera::CameraModel localized_cam(Eigen::Vector3d(), Eigen::Matrix3d::Identity(), - reference.GetCameraParameters()); + reference.GetCameraParameters(cid)); camera::CameraModel source_cam(source.GetFrameGlobalTransform(cid), - source.GetCameraParameters()); + source.GetCameraParameters(cid)); std::cout << "Source map position: " << source_cam.GetPosition().transpose() << "\n"; if (!reference.Localize(img_file, &localized_cam)) { diff --git a/localization/sparse_mapping/tools/nvm_visualize.cc b/localization/sparse_mapping/tools/nvm_visualize.cc index 7bc5e2888a..2095589830 100644 --- a/localization/sparse_mapping/tools/nvm_visualize.cc +++ b/localization/sparse_mapping/tools/nvm_visualize.cc @@ -22,7 +22,6 @@ #include #include #include -#include #include #include @@ -101,7 +100,7 @@ void Draw3DFrame(cv::viz::Viz3d* window, const sparse_mapping::SparseMap & map, int cols; int rows; if (!image.empty()) { - f = map.GetCameraParameters().GetFocalLength(); + f = map.GetCameraParameters(cid).GetFocalLength(); cols = image.cols; rows = image.rows; } else { @@ -137,8 +136,9 @@ bool LocalizeFrame(MapViewerState* state, int frame) { cv::Mat image; // display localized images if (state->num_frames > 0) { + // Assume first camera model camera::CameraModel camera(Eigen::Vector3d(), Eigen::Matrix3d::Identity(), - state->map->GetCameraParameters()); + state->map->GetCameraParameters(0)); image = cv::imread(state->frames[frame], cv::IMREAD_GRAYSCALE); if (!state->map->Localize(state->frames[frame], &camera, &landmarks)) { LOG(ERROR) << "Failed to localize image."; @@ -166,7 +166,8 @@ bool LocalizeFrame(MapViewerState* state, int frame) { state->window->setViewerPose(new_pose); // state->window->getViewerPose().concatenate( // state->old_pose.inv()).concatenate(camera_pose)); } - double f = state->map->GetCameraParameters().GetFocalLength(); + // Assume first camera model + double f = state->map->GetCameraParameters(0).GetFocalLength(); cv::viz::WCameraPosition im_pose(cv::Vec2f(2 * atan(image.cols * 0.5 / f), 2 * atan(image.rows * 0.5 / f)), image, 0.5, cv::viz::Color::red()); im_pose.setRenderingProperty(cv::viz::LINE_WIDTH, 6.0); @@ -379,7 +380,7 @@ static void onMouse(int event, int x, int y, int, void*) { return; } - camera::CameraParameters camera_param = map.GetCameraParameters(); + camera::CameraParameters camera_param = map.GetCameraParameters(cid); const Eigen::Matrix2Xd & keypoint_map = map.GetFrameKeypoints(cid); // Locate that point in the image @@ -530,7 +531,9 @@ int main(int argc, char** argv) { LOG(INFO) << "\t" << map.GetNumFrames() << " cameras and " << map.GetNumLandmarks() << " points"; - camera::CameraParameters camera_param = map.GetCameraParameters(); + // Assume first camera model + // TODO(rsoussan): Add option to use robot camera parameters or pass camera parameters manually + camera::CameraParameters camera_param = map.GetCameraParameters(0); int cid = FLAGS_first; int num_images = map.GetNumFrames(); diff --git a/localization/sparse_mapping/tools/partition_image_sequences.cc b/localization/sparse_mapping/tools/partition_image_sequences.cc index 6a10500404..6d0c639800 100644 --- a/localization/sparse_mapping/tools/partition_image_sequences.cc +++ b/localization/sparse_mapping/tools/partition_image_sequences.cc @@ -108,7 +108,7 @@ Result RotationOnlyImage(const vc::FeatureMatches& matches, const camera::Camera return result; } std::vector inliers; - const auto source_T_target = sm::EstimateAffine3d(matches, camera_params, inliers); + const auto source_T_target = sm::EstimateAffine3d(matches, camera_params, camera_params, inliers); const double relative_pose_inliers_ratio = static_cast(inliers.size()) / static_cast(matches.size()); if (relative_pose_inliers_ratio < min_relative_pose_inliers_ratio) { LogDebug("Too few inliers found. Inliers: " << inliers.size() << ", total matches: " << matches.size() diff --git a/localization/sparse_mapping/tools/utilities.cc b/localization/sparse_mapping/tools/utilities.cc index aeae09c8c5..aa1c67f16b 100644 --- a/localization/sparse_mapping/tools/utilities.cc +++ b/localization/sparse_mapping/tools/utilities.cc @@ -58,7 +58,8 @@ boost::optional Matches(const vc::FeatureImage& current_imag return matches; } -Eigen::Affine3d EstimateAffine3d(const vc::FeatureMatches& matches, const camera::CameraParameters& camera_params, +Eigen::Affine3d EstimateAffine3d(const vc::FeatureMatches& matches, const camera::CameraParameters& camera_params1, + const camera::CameraParameters& camera_params2, std::vector& inliers) { Eigen::Matrix2Xd source_image_points(2, matches.size()); Eigen::Matrix2Xd target_image_points(2, matches.size()); @@ -67,8 +68,8 @@ Eigen::Affine3d EstimateAffine3d(const vc::FeatureMatches& matches, const camera const auto& match = matches[i]; Eigen::Vector2d undistorted_source_point; Eigen::Vector2d undistorted_target_point; - camera_params.Convert(match.source_point, &undistorted_source_point); - camera_params.Convert(match.target_point, &undistorted_target_point); + camera_params1.Convert(match.source_point, &undistorted_source_point); + camera_params2.Convert(match.target_point, &undistorted_target_point); source_image_points.col(i) = undistorted_source_point; target_image_points.col(i) = undistorted_target_point; cv_matches.emplace_back(cv::DMatch(i, i, i, 0)); @@ -76,8 +77,8 @@ Eigen::Affine3d EstimateAffine3d(const vc::FeatureMatches& matches, const camera std::mutex mutex; CIDPairAffineMap affines; - BuildMapFindEssentialAndInliers(source_image_points, target_image_points, cv_matches, camera_params, false, 0, 0, - &mutex, &affines, &inliers, false, nullptr); + BuildMapFindEssentialAndInliers(source_image_points, target_image_points, cv_matches, camera_params1, camera_params2, + false, 0, 0, &mutex, &affines, &inliers, false, nullptr); const Eigen::Affine3d target_T_source = affines[std::make_pair(0, 0)]; return target_T_source.inverse(); } diff --git a/localization/sparse_mapping/tools/utilities.h b/localization/sparse_mapping/tools/utilities.h index 6a019f95e4..75f1388c4b 100644 --- a/localization/sparse_mapping/tools/utilities.h +++ b/localization/sparse_mapping/tools/utilities.h @@ -38,7 +38,8 @@ boost::optional Matches( vision_common::LKOpticalFlowFeatureDetectorAndMatcher& detector_and_matcher); Eigen::Affine3d EstimateAffine3d(const vision_common::FeatureMatches& matches, - const camera::CameraParameters& camera_params, std::vector& inliers); + const camera::CameraParameters& camera_params1, + const camera::CameraParameters& camera_params2, std::vector& inliers); vision_common::FeatureImage LoadImage(const int index, const std::vector& image_names, cv::Feature2D& detector);