Sparse map multiple cameras

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<camera::CameraParameters> cam_params_;
   // Success params for adjusting keypoint thresholds
   std::deque<int> successes_;
   ThresholdParams params_;

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<camera::CameraParameters>(&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<Eigen::Vector3d> landmarks;
   std::vector<Eigen::Vector2d> observations;
   if (!map_->Localize(image_descriptors, *image_keypoints,

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<camera::UNDISTORTED_C, camera::DISTORTED>(undistorted, &distorted);
+      (inst_->camera_params()).Convert<camera::UNDISTORTED_C, camera::DISTORTED>(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<camera::UNDISTORTED_C, camera::DISTORTED>(undistorted, &distorted);
+      (inst_->camera_params()).Convert<camera::UNDISTORTED_C, camera::DISTORTED>(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);
     }

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<std::map<int, int> > const& pid_to_cid_fid,
                   std::vector<Eigen::Matrix2Xd > const& cid_to_keypoint_map,
-                  double focal_length,
+                  std::vector<int> const& cid_to_camera_id,
+                  std::vector<camera::CameraParameters> const& camera_id_to_camera_params,
                   std::vector<Eigen::Affine3d> * cid_to_cam_t_global,
                   std::vector<Eigen::Vector3d> * pid_to_xyz,
                   std::vector<std::map<int, int> > const& user_pid_to_cid_fid,
@@ -82,7 +84,7 @@ void BundleAdjust(std::vector<std::map<int, int> > const& pid_to_cid_fid,
  *
  **/
 void BundleAdjustSmallSet(std::vector<Eigen::Matrix2Xd> const& features_n,
-                          double focal_length,
+                          std::vector<double> focal_length_n,
                           std::vector<Eigen::Affine3d> * cam_t_global_n,
                           Eigen::Matrix3Xd * pid_to_xyz,
                           ceres::LossFunction * loss,

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<std::string> & 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<std::string>& filenames, const std::string& detector,
+            const std::vector<int>& cid_to_camera_id,
+            const std::vector<camera::CameraParameters>& 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<std::map<int, int>> cid_to_matching_cid_counts_;
 
   interest_point::FeatureDetector detector_;
-  camera::CameraParameters camera_params_;
+  std::vector<int> cid_to_camera_id_;
+  std::vector<camera::CameraParameters> 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_;

localization/sparse_mapping/include/sparse_mapping/sparse_mapping.h

@@ -180,7 +180,8 @@ double ComputeRaysAngle(int pid, std::vector<std::map<int, int> > const& pid_to_
                         std::vector<Eigen::Vector3d> const& cam_ctrs, std::vector<Eigen::Vector3d> 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<int> const& cid_to_camera_id,
+               std::vector<camera::CameraParameters> const& camera_id_to_camera_params,
                std::vector<Eigen::Affine3d> const& cid_to_cam_t_global,
                std::vector<Eigen::Matrix2Xd> const& cid_to_keypoint_map,
                std::vector<std::map<int, int> >* pid_to_cid_fid, std::vector<Eigen::Vector3d>* pid_to_xyz,

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<cv::DMatch> const& matches, camera::CameraParameters const& camera_params,
+                               std::vector<cv::DMatch> const& matches, camera::CameraParameters const& camera_params1,
+                               camera::CameraParameters const& camera_params2,
                                std::vector<cv::DMatch>* inlier_matches, std::vector<size_t>* vec_inliers,
                                Eigen::Matrix3d* essential_matrix, const int ransac_iterations = 4096);
 
   void BuildMapFindEssentialAndInliers(const Eigen::Matrix2Xd & keypoints1,
                                        const Eigen::Matrix2Xd & keypoints2,
                                        const std::vector<cv::DMatch> & 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<int> const& cid_to_camera_id,
+                   std::vector<camera::CameraParameters> const& camera_id_to_camera_params,
                    std::vector<Eigen::Affine3d> const& cid_to_cam_t_global,
                    std::vector<Eigen::Matrix2Xd> const& cid_to_keypoint_map,
                    std::vector<std::map<int, int> > * pid_to_cid_fid,

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;
 }
 

localization/sparse_mapping/src/reprojection.cc

@@ -94,7 +94,8 @@ struct ReprojectionError {
 };
 
 void BundleAdjust(std::vector<std::map<int, int> > const& pid_to_cid_fid,
-                  std::vector<Eigen::Matrix2Xd> const& cid_to_keypoint_map, double focal_length,
+                  std::vector<Eigen::Matrix2Xd> const& cid_to_keypoint_map, std::vector<int> const& cid_to_camera_id,
+                  std::vector<camera::CameraParameters> const& camera_id_to_camera_params,
                   std::vector<Eigen::Affine3d>* cid_to_cam_t_global, std::vector<Eigen::Vector3d>* pid_to_xyz,
                   std::vector<std::map<int, int> > const& user_pid_to_cid_fid,
                   std::vector<Eigen::Matrix2Xd> const& user_cid_to_keypoint_map,
@@ -109,12 +110,14 @@ void BundleAdjust(std::vector<std::map<int, int> > const& pid_to_cid_fid,
 
   // Allocate space for the angle axis representation of rotation
   std::vector<double> camera_aa_storage(3 * cid_to_cam_t_global->size());
+  std::vector<double> focal_lengths;
   for (size_t cid = 0; cid < cid_to_cam_t_global->size(); cid++) {
     Eigen::Map<Eigen::Vector3d> 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<std::map<int, int> > const& pid_to_cid_fid,
       for (std::map<int, int>::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<std::map<int, int> > 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<std::map<int, int> > const& pid_to_cid_fid,
 
 // This is a very specialized function
 void BundleAdjustSmallSet(std::vector<Eigen::Matrix2Xd> const& features_n,
-                          double focal_length,
+                          std::vector<double> focal_length_n,
                           std::vector<Eigen::Affine3d> * cam_t_global_n,
                           Eigen::Matrix3Xd * pid_to_xyz,
                           ceres::LossFunction * loss,
@@ -232,10 +236,12 @@ void BundleAdjustSmallSet(std::vector<Eigen::Matrix2Xd> 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);