Allow for n distortion params

include/kimera-vio/frontend/CameraParams.h

@@ -57,7 +57,6 @@ class CameraParams : public PipelineParams {
         body_Pose_cam_(),
         frame_rate_(),
         image_size_(),
-        calibration_(),
         distortion_model_(),
         distortion_coeff_(),
         distortion_coeff_mat_(),
@@ -93,7 +92,8 @@ class CameraParams : public PipelineParams {
   Intrinsics intrinsics_;
   // OpenCV structures: needed to compute the undistortion map.
   // 3x3 camera matrix K (last row is {0,0,1})
-  cv::Mat K_;
+  cv::Mat 
+    ;
 
   // Sensor extrinsics wrt body-frame
   gtsam::Pose3 body_Pose_cam_;
@@ -102,10 +102,6 @@ class CameraParams : public PipelineParams {
   double frame_rate_;
   cv::Size image_size_;
 
-  // GTSAM structures to calibrate points:
-  // Contains intrinsics and distortion parameters.
-  gtsam::Cal3DS2 calibration_;
-
   // Distortion parameters
   DistortionModel distortion_model_;
   std::vector<double> distortion_coeff_;
@@ -132,7 +128,7 @@ class CameraParams : public PipelineParams {
       cv::Mat* distortion_coeffs_mat);
   static void convertIntrinsicsVectorToMatrix(const Intrinsics& intrinsics,
                                               cv::Mat* camera_matrix);
-  static void createGtsamCalibration(const std::vector<double>& distortion,
+  static void createGtsamCalibration(const cv::Mat& distortion,
                                      const Intrinsics& intrinsics,
                                      gtsam::Cal3DS2* calibration);
 

src/frontend/CameraParams.cpp

@@ -21,7 +21,6 @@
 
 namespace VIO {
 
-/* -------------------------------------------------------------------------- */
 // Parse YAML file describing camera parameters.
 bool CameraParams::parseYAML(const std::string& filepath) {
   YamlParser yaml_parser(filepath);
@@ -48,25 +47,20 @@ bool CameraParams::parseYAML(const std::string& filepath) {
   // Convert intrinsics to cv::Mat format.
   convertIntrinsicsVectorToMatrix(intrinsics_, &K_);
 
-  // Create gtsam calibration object.
-  // Calibration of a camera with radial distortion that also supports
-  createGtsamCalibration(distortion_coeff_mat_, intrinsics_, &calibration_);
-
   // P_ = R_rectify_ * camera_matrix_;
   return true;
 }
 
-/* -------------------------------------------------------------------------- */
 void CameraParams::parseDistortion(const YamlParser& yaml_parser) {
   std::string distortion_model;
   yaml_parser.getYamlParam("distortion_model", &distortion_model);
   yaml_parser.getYamlParam("camera_model", &camera_model_);
   distortion_model_ = stringToDistortion(distortion_model, camera_model_);
-  // 4 parameters (read from file)
   CHECK(distortion_model_ == DistortionModel::RADTAN ||
         distortion_model_ == DistortionModel::EQUIDISTANT)
       << "Unsupported distortion model. Expected: radtan or equidistant.";
   yaml_parser.getYamlParam("distortion_coefficients", &distortion_coeff_);
+  CHECK_GE(distortion_coeff_.size(), 4u);
   convertDistortionVectorToMatrix(distortion_coeff_, &distortion_coeff_mat_);
 }
 
@@ -107,20 +101,18 @@ const DistortionModel CameraParams::stringToDistortion(
   }
 }
 
-/* -------------------------------------------------------------------------- */
 // Convert distortion coefficients to OpenCV Format
 void CameraParams::convertDistortionVectorToMatrix(
     const std::vector<double>& distortion_coeffs,
     cv::Mat* distortion_coeffs_mat) {
   CHECK_NOTNULL(distortion_coeffs_mat);
-  CHECK_EQ(distortion_coeffs.size(), 4);
+  CHECK_GE(distortion_coeffs.size(), 4u);
   *distortion_coeffs_mat = cv::Mat::zeros(1, distortion_coeffs.size(), CV_64F);
   for (int k = 0; k < distortion_coeffs_mat->cols; k++) {
     distortion_coeffs_mat->at<double>(0, k) = distortion_coeffs[k];
   }
 }
 
-/* -------------------------------------------------------------------------- */
 void CameraParams::parseImgSize(const YamlParser& yaml_parser,
                                 cv::Size* image_size) {
   CHECK_NOTNULL(image_size);
@@ -130,7 +122,6 @@ void CameraParams::parseImgSize(const YamlParser& yaml_parser,
   *image_size = cv::Size(resolution[0], resolution[1]);
 }
 
-/* -------------------------------------------------------------------------- */
 void CameraParams::parseFrameRate(const YamlParser& yaml_parser,
                                   double* frame_rate) {
   CHECK_NOTNULL(frame_rate);
@@ -140,22 +131,14 @@ void CameraParams::parseFrameRate(const YamlParser& yaml_parser,
   *frame_rate = 1 / static_cast<double>(rate);
 }
 
-/* -------------------------------------------------------------------------- */
 void CameraParams::parseBodyPoseCam(const YamlParser& yaml_parser,
                                     gtsam::Pose3* body_Pose_cam) {
   CHECK_NOTNULL(body_Pose_cam);
-  // int n_rows = 0;
-  // yaml_parser.getNestedYamlParam("T_BS", "rows", &n_rows);
-  // CHECK_EQ(n_rows, 4);
-  // int n_cols = 0;
-  // yaml_parser.getNestedYamlParam("T_BS", "cols", &n_cols);
-  // CHECK_EQ(n_cols, 4);
   std::vector<double> vector_pose;
   yaml_parser.getNestedYamlParam("T_BS", "data", &vector_pose);
   *body_Pose_cam = UtilsOpenCV::poseVectorToGtsamPose3(vector_pose);
 }
 
-/* -------------------------------------------------------------------------- */
 void CameraParams::parseCameraIntrinsics(const YamlParser& yaml_parser,
                                          Intrinsics* intrinsics_) {
   CHECK_NOTNULL(intrinsics_);
@@ -168,7 +151,6 @@ void CameraParams::parseCameraIntrinsics(const YamlParser& yaml_parser,
               intrinsics_->begin());
 }
 
-/* -------------------------------------------------------------------------- */
 void CameraParams::convertIntrinsicsVectorToMatrix(const Intrinsics& intrinsics,
                                                    cv::Mat* camera_matrix) {
   CHECK_NOTNULL(camera_matrix);
@@ -180,26 +162,27 @@ void CameraParams::convertIntrinsicsVectorToMatrix(const Intrinsics& intrinsics,
   camera_matrix->at<double>(1, 2) = intrinsics[3];
 }
 
-/* -------------------------------------------------------------------------- */
-// TODO(Toni) : Check if equidistant distortion is supported as well in gtsam.
-void CameraParams::createGtsamCalibration(const std::vector<double>& distortion,
+// TODO(Toni): Check if equidistant distortion is supported as well in gtsam.
+// TODO(Toni): rather remove this function as it is only used in tests for
+// uncalibrating the keypoints.. Use instead opencv.
+void CameraParams::createGtsamCalibration(const cv::Mat& distortion,
                                           const Intrinsics& intrinsics,
                                           gtsam::Cal3DS2* calibration) {
   CHECK_NOTNULL(calibration);
   CHECK_GE(intrinsics.size(), 4);
-  CHECK_GE(distortion.size(), 4);
-  *calibration = gtsam::Cal3DS2(intrinsics[0],   // fx
-                                intrinsics[1],   // fy
-                                0.0,             // skew
-                                intrinsics[2],   // u0
-                                intrinsics[3],   // v0
-                                distortion[0],   // k1
-                                distortion[1],   // k2
-                                distortion[2],   // p1 (k3)
-                                distortion[3]);  // p2 (k4)
+  CHECK_GE(distortion.cols, 4);
+  CHECK_EQ(distortion.rows, 1);
+  *calibration = gtsam::Cal3DS2(intrinsics[0],                 // fx
+                                intrinsics[1],                 // fy
+                                0.0,                           // skew
+                                intrinsics[2],                 // u0
+                                intrinsics[3],                 // v0
+                                distortion.at<double>(0, 0),   // k1
+                                distortion.at<double>(0, 1),   // k2
+                                distortion.at<double>(0, 2),   // p1 (k3)
+                                distortion.at<double>(0, 3));  // p2 (k4)
 }
 
-/* -------------------------------------------------------------------------- */
 // Display all params.
 void CameraParams::print() const {
   std::stringstream out;
@@ -228,27 +211,17 @@ void CameraParams::print() const {
             << "- distortion_coeff: " << distortion_coeff_mat_ << '\n'
             << "- R_rectify: " << R_rectify_ << '\n'
             << "- P: " << P_;
-
-  if (FLAGS_minloglevel < 1) calibration_.print("\n gtsam calibration:\n");
 }
 
-/* -------------------------------------------------------------------------- */
 // Assert equality up to a tolerance.
 bool CameraParams::equals(const CameraParams& cam_par,
                           const double& tol) const {
-  bool areIntrinsicEqual = true;
-  for (size_t i = 0; i < intrinsics_.size(); i++) {
-    if (std::fabs(intrinsics_[i] - cam_par.intrinsics_[i]) > tol) {
-      areIntrinsicEqual = false;
-      break;
-    }
-  }
-  return camera_id_ == cam_par.camera_id_ && areIntrinsicEqual &&
+  return camera_id_ == cam_par.camera_id_ &&
+         intrinsics_ == cam_par.intrinsics_ &&
          body_Pose_cam_.equals(cam_par.body_Pose_cam_, tol) &&
-         (std::fabs(frame_rate_ - cam_par.frame_rate_) < tol) &&
-         (image_size_.width == cam_par.image_size_.width) &&
-         (image_size_.height == cam_par.image_size_.height) &&
-         calibration_.equals(cam_par.calibration_, tol) &&
+         frame_rate_ == cam_par.frame_rate_ &&
+         image_size_.width == cam_par.image_size_.width &&
+         image_size_.height == cam_par.image_size_.height &&
          UtilsOpenCV::compareCvMatsUpToTol(K_, cam_par.K_) &&
          UtilsOpenCV::compareCvMatsUpToTol(distortion_coeff_mat_,
                                            cam_par.distortion_coeff_mat_) &&

tests/data/ForStereoFrame/sensorLeft5thDistNeg.yaml

@@ -0,0 +1,21 @@
+%YAML:1.0
+# General sensor definitions.
+camera_id: camera
+
+# Sensor extrinsics wrt. the body-frame.
+T_BS:
+  cols: 4
+  rows: 4
+  data: [0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975,
+         0.999557249008, 0.0149672133247, 0.025715529948, -0.064676986768,
+        -0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949,
+         0.0, 0.0, 0.0, 1.0]
+
+# Camera specific definitions.
+rate_hz: 20
+resolution: [752, 480]
+camera_model: pinhole
+intrinsics: [458.654, 457.296, 367.215, 248.375] #fu, fv, cu, cv
+distortion_model: radial-tangential
+distortion_coefficients: [-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05, -0.04]
+

tests/data/ForStereoFrame/sensorLeft5thDistPos.yaml

@@ -0,0 +1,21 @@
+%YAML:1.0
+# General sensor definitions.
+camera_id: camera
+
+# Sensor extrinsics wrt. the body-frame.
+T_BS:
+  cols: 4
+  rows: 4
+  data: [0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975,
+         0.999557249008, 0.0149672133247, 0.025715529948, -0.064676986768,
+        -0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949,
+         0.0, 0.0, 0.0, 1.0]
+
+# Camera specific definitions.
+rate_hz: 20
+resolution: [752, 480]
+camera_model: pinhole
+intrinsics: [458.654, 457.296, 367.215, 248.375] #fu, fv, cu, cv
+distortion_model: radial-tangential
+distortion_coefficients: [-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05, 0.4]
+

tests/data/ForStereoFrame/sensorLeft5thDistZero.yaml

@@ -0,0 +1,21 @@
+%YAML:1.0
+# General sensor definitions.
+camera_id: camera
+
+# Sensor extrinsics wrt. the body-frame.
+T_BS:
+  cols: 4
+  rows: 4
+  data: [0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975,
+         0.999557249008, 0.0149672133247, 0.025715529948, -0.064676986768,
+        -0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949,
+         0.0, 0.0, 0.0, 1.0]
+
+# Camera specific definitions.
+rate_hz: 20
+resolution: [752, 480]
+camera_model: pinhole
+intrinsics: [458.654, 457.296, 367.215, 248.375] #fu, fv, cu, cv
+distortion_model: radial-tangential
+distortion_coefficients: [-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05, 0.0]
+

tests/data/ForStereoFrame/sensorRight5thDistNeg.yaml

@@ -0,0 +1,20 @@
+%YAML:1.0
+# General sensor definitions.
+camera_id: camera
+
+# Sensor extrinsics wrt. the body-frame.
+T_BS:
+  cols: 4
+  rows: 4
+  data: [0.0125552670891, -0.999755099723, 0.0182237714554, -0.0198435579556,
+         0.999598781151, 0.0130119051815, 0.0251588363115, 0.0453689425024,
+        -0.0253898008918, 0.0179005838253, 0.999517347078, 0.00786212447038,
+         0.0, 0.0, 0.0, 1.0]
+
+# Camera specific definitions.
+rate_hz: 20
+resolution: [752, 480]
+camera_model: pinhole
+intrinsics: [457.587, 456.134, 379.999, 255.238] #fu, fv, cu, cv
+distortion_model: radial-tangential
+distortion_coefficients: [-0.28368365,  0.07451284, -0.00010473, -3.55590700e-05, -0.04]

tests/data/ForStereoFrame/sensorRight5thDistPos.yaml

@@ -0,0 +1,20 @@
+%YAML:1.0
+# General sensor definitions.
+camera_id: camera
+
+# Sensor extrinsics wrt. the body-frame.
+T_BS:
+  cols: 4
+  rows: 4
+  data: [0.0125552670891, -0.999755099723, 0.0182237714554, -0.0198435579556,
+         0.999598781151, 0.0130119051815, 0.0251588363115, 0.0453689425024,
+        -0.0253898008918, 0.0179005838253, 0.999517347078, 0.00786212447038,
+         0.0, 0.0, 0.0, 1.0]
+
+# Camera specific definitions.
+rate_hz: 20
+resolution: [752, 480]
+camera_model: pinhole
+intrinsics: [457.587, 456.134, 379.999, 255.238] #fu, fv, cu, cv
+distortion_model: radial-tangential
+distortion_coefficients: [-0.28368365,  0.07451284, -0.00010473, -3.55590700e-05, 0.4]

tests/data/ForStereoFrame/sensorRight5thDistZero.yaml

@@ -0,0 +1,20 @@
+%YAML:1.0
+# General sensor definitions.
+camera_id: camera
+
+# Sensor extrinsics wrt. the body-frame.
+T_BS:
+  cols: 4
+  rows: 4
+  data: [0.0125552670891, -0.999755099723, 0.0182237714554, -0.0198435579556,
+         0.999598781151, 0.0130119051815, 0.0251588363115, 0.0453689425024,
+        -0.0253898008918, 0.0179005838253, 0.999517347078, 0.00786212447038,
+         0.0, 0.0, 0.0, 1.0]
+
+# Camera specific definitions.
+rate_hz: 20
+resolution: [752, 480]
+camera_model: pinhole
+intrinsics: [457.587, 456.134, 379.999, 255.238] #fu, fv, cu, cv
+distortion_model: radial-tangential
+distortion_coefficients: [-0.28368365,  0.07451284, -0.00010473, -3.55590700e-05, 0.0]