Improved rotation vectors averaging (#44)

* Update links to data

* Quaternion averaging implementation

* Replacing rotation averaging in all places

* Fixing quaternion to rot matrix conversion and unit tests

* Applying linters

* Correctly handling antipodal configuration

* Exposing rotation averaging method to config

McCalib/include/CameraGroupObs.hpp

@@ -31,10 +31,14 @@ class CameraGroupObs final {
   int cam_group_idx_;
   std::weak_ptr<CameraGroup> cam_group_;
 
+  bool quaternion_averaging_ =
+      true; // use Quaternion Averaging or median for average rotation
+
   // Functions
   CameraGroupObs() = delete;
   ~CameraGroupObs();
-  CameraGroupObs(std::shared_ptr<CameraGroup> new_cam_group);
+  CameraGroupObs(std::shared_ptr<CameraGroup> new_cam_group,
+                 const bool quaternion_averaging);
   void
   insertObjectObservation(std::shared_ptr<Object3DObs> new_object_observation);
   void computeObjectsPose();

McCalib/include/McCalib.hpp

@@ -63,6 +63,8 @@ class Calibration final {
   int corner_ref_max_iter_ = 20; // max iterations for corner ref
 
   // Optimization parameters
+  bool quaternion_averaging_ =
+      true; // use Quaternion Averaging or median for average rotation
   float ransac_thresh_ = 10; // threshold in pixel
   int nb_iterations_ = 1000; // max number of iteration for refinements
 

McCalib/include/geometrytools.hpp

@@ -52,3 +52,8 @@ void projectPointsWithDistortion(std::vector<cv::Point3f> object_pts,
                                  cv::Mat distortion_vector,
                                  std::vector<cv::Point2f> &repro_pts,
                                  int distortion_type);
+cv::Mat convertRotationMatrixToQuaternion(cv::Mat R);
+cv::Mat convertQuaternionToRotationMatrix(const std::array<double, 4> &q);
+cv::Mat getAverageRotation(std::vector<double> &r1, std::vector<double> &r2,
+                           std::vector<double> &r3,
+                           const bool use_quaternion_averaging = true);

McCalib/src/CameraGroupObs.cpp

@@ -13,9 +13,11 @@
  *
  * @param new_cam_group camera group to be added
  */
-CameraGroupObs::CameraGroupObs(std::shared_ptr<CameraGroup> new_cam_group) {
+CameraGroupObs::CameraGroupObs(std::shared_ptr<CameraGroup> new_cam_group,
+                               const bool quaternion_averaging) {
   cam_group_ = new_cam_group;
   cam_group_idx_ = new_cam_group->cam_group_idx_;
+  quaternion_averaging_ = quaternion_averaging;
 }
 
 /**
@@ -75,18 +77,21 @@ void CameraGroupObs::computeObjectsPose() {
     // if the reference camera has no visible observation, then take
     // the average of other observations
     if (flag_ref_cam == false) {
-      cv::Mat average_rotation = cv::Mat::zeros(3, 1, CV_64F);
+      std::vector<double> r1, r2, r3;
       cv::Mat average_translation = cv::Mat::zeros(3, 1, CV_64F);
       for (const auto &obj_obs_idx : it_obj_obs.second) {
         auto obj_obs_ptr = object_observations_[obj_obs_idx].lock();
         if (obj_obs_ptr) {
-          average_rotation += obj_obs_ptr->getRotInGroupVec();
+          const cv::Mat &R = obj_obs_ptr->getRotInGroupVec();
+          r1.push_back(R.at<double>(0));
+          r2.push_back(R.at<double>(1));
+          r3.push_back(R.at<double>(2));
           average_translation += obj_obs_ptr->getTransInGroupVec();
         }
       }
       // Average version
       group_pose_t = average_translation / it_obj_obs.second.size();
-      group_pose_r = average_rotation / it_obj_obs.second.size();
+      group_pose_r = getAverageRotation(r1, r2, r3, quaternion_averaging_);
     }
 
     // set the pose and update the observation

McCalib/src/McCalib.cpp

@@ -51,6 +51,7 @@ Calibration::Calibration(const std::string config_path) {
   fs["number_y_square"] >> nb_y_square;
   fs["root_path"] >> root_dir_;
   fs["cam_prefix"] >> cam_prefix_;
+  fs["quaternion_averaging:"] >> quaternion_averaging_;
   fs["ransac_threshold"] >> ransac_thresh_;
   fs["number_iterations"] >> nb_iterations_;
   fs["distortion_model"] >> distortion_model;
@@ -771,7 +772,6 @@ void Calibration::initInterTransform(
   for (const auto &it : pose_pairs) {
     const std::pair<int, int> &pair_idx = it.first;
     const std::vector<cv::Mat> &poses_temp = it.second;
-    cv::Mat average_rotation = cv::Mat::zeros(3, 1, CV_64F);
     cv::Mat average_translation = cv::Mat::zeros(3, 1, CV_64F);
 
     // Median
@@ -794,9 +794,9 @@ void Calibration::initInterTransform(
       t2.push_back(T.at<double>(1));
       t3.push_back(T.at<double>(2));
     }
-    average_rotation.at<double>(0) = median(r1);
-    average_rotation.at<double>(1) = median(r2);
-    average_rotation.at<double>(2) = median(r3);
+
+    cv::Mat average_rotation =
+        getAverageRotation(r1, r2, r3, quaternion_averaging_);
     average_translation.at<double>(0) = median(t1);
     average_translation.at<double>(1) = median(t2);
     average_translation.at<double>(2) = median(t3);
@@ -1142,7 +1142,8 @@ void Calibration::initCameraGroupObs(const int camera_group_idx) {
     int current_frame_id = it_frame.second->frame_idx_;
     std::shared_ptr<CameraGroupObs> new_cam_group_obs =
         std::make_shared<CameraGroupObs>(
-            cam_group_[camera_group_idx]); // declare a new observation
+            cam_group_[camera_group_idx],
+            quaternion_averaging_); // declare a new observation
 
     std::map<int, std::weak_ptr<Object3DObs>> current_object_obs =
         it_frame.second->object_observations_;

McCalib/src/geometrytools.cpp

@@ -537,11 +537,8 @@ cv::Mat handeyeBootstratpTranslationCalibration(
 
   // if enough sucess (at least 3) then compute median value
   if (nb_success > 3) {
-    cv::Mat r_he = cv::Mat::zeros(3, 1, CV_64F);
+    cv::Mat r_he = getAverageRotation(r1_he, r2_he, r3_he);
     cv::Mat t_he = cv::Mat::zeros(3, 1, CV_64F);
-    r_he.at<double>(0) = median(r1_he);
-    r_he.at<double>(1) = median(r2_he);
-    r_he.at<double>(2) = median(r3_he);
     t_he.at<double>(0) = median(t1_he);
     t_he.at<double>(1) = median(t2_he);
     t_he.at<double>(2) = median(t3_he);
@@ -626,4 +623,125 @@ void projectPointsWithDistortion(std::vector<cv::Point3f> object_pts,
     cv::fisheye::projectPoints(object_pts, repro_pts, rot, trans, camera_matrix,
                                distortion_vector, 0.0);
   }
+}
+
+cv::Mat convertRotationMatrixToQuaternion(cv::Mat R) {
+  // code is adapted from
+  // https://gist.github.com/shubh-agrawal/76754b9bfb0f4143819dbd146d15d4c8
+
+  cv::Mat Q(1, 4, CV_64F); // x y z w
+
+  double trace = R.at<double>(0, 0) + R.at<double>(1, 1) + R.at<double>(2, 2);
+
+  if (trace > 0.0) {
+    double s = std::sqrt(trace + 1.0);
+    Q.at<double>(0, 3) = (s * 0.5);
+    s = 0.5 / s;
+    Q.at<double>(0, 0) = ((R.at<double>(2, 1) - R.at<double>(1, 2)) * s);
+    Q.at<double>(0, 1) = ((R.at<double>(0, 2) - R.at<double>(2, 0)) * s);
+    Q.at<double>(0, 2) = ((R.at<double>(1, 0) - R.at<double>(0, 1)) * s);
+  }
+
+  else {
+    int i = R.at<double>(0, 0) < R.at<double>(1, 1)
+                ? (R.at<double>(1, 1) < R.at<double>(2, 2) ? 2 : 1)
+                : (R.at<double>(0, 0) < R.at<double>(2, 2) ? 2 : 0);
+    int j = (i + 1) % 3;
+    int k = (i + 2) % 3;
+
+    double s = std::sqrt(R.at<double>(i, i) - R.at<double>(j, j) -
+                         R.at<double>(k, k) + 1.0);
+    Q.at<double>(0, i) = s * 0.5;
+    s = 0.5 / s;
+
+    Q.at<double>(0, 3) = (R.at<double>(k, j) - R.at<double>(j, k)) * s;
+    Q.at<double>(0, j) = (R.at<double>(j, i) + R.at<double>(i, j)) * s;
+    Q.at<double>(0, k) = (R.at<double>(k, i) + R.at<double>(i, k)) * s;
+  }
+
+  return Q;
+}
+
+cv::Mat convertQuaternionToRotationMatrix(const std::array<double, 4> &q) {
+  // code adapted from
+  // https://automaticaddison.com/how-to-convert-a-quaternion-to-a-rotation-matrix/
+
+  const double q0 = q[3];
+  const double q1 = q[0];
+  const double q2 = q[1];
+  const double q3 = q[2];
+
+  cv::Mat rot_matrix(3, 3, CV_64F);
+  rot_matrix.at<double>(0, 0) = 2 * (q0 * q0 + q1 * q1) - 1;
+  rot_matrix.at<double>(0, 1) = 2 * (q1 * q2 - q0 * q3);
+  rot_matrix.at<double>(0, 2) = 2 * (q1 * q3 + q0 * q2);
+
+  rot_matrix.at<double>(1, 0) = 2 * (q1 * q2 + q0 * q3);
+  rot_matrix.at<double>(1, 1) = 2 * (q0 * q0 + q2 * q2) - 1;
+  rot_matrix.at<double>(1, 2) = 2 * (q2 * q3 - q0 * q1);
+
+  rot_matrix.at<double>(2, 0) = 2 * (q1 * q3 - q0 * q2);
+  rot_matrix.at<double>(2, 1) = 2 * (q2 * q3 + q0 * q1);
+  rot_matrix.at<double>(2, 2) = 2 * (q0 * q0 + q3 * q3) - 1;
+
+  return rot_matrix;
+}
+
+cv::Mat getAverageRotation(std::vector<double> &r1, std::vector<double> &r2,
+                           std::vector<double> &r3,
+                           const bool use_quaternion_averaging) {
+  cv::Mat average_rotation = cv::Mat::zeros(3, 1, CV_64F);
+  if (use_quaternion_averaging) {
+    // The Quaternion Averaging algorithm is described in
+    // https://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/20070017872.pdf
+    // implementaion references:
+    //  -
+    //  https://gist.github.com/PeteBlackerThe3rd/f73e9d569e29f23e8bd828d7886636a0
+    //  -
+    //  https://github.com/tolgabirdal/averaging_quaternions/blob/master/avg_quaternion_markley.m
+
+    assert(r1.size() == r2.size() && r2.size() == r3.size());
+
+    std::vector<cv::Mat> quaternions;
+    // convert rotation vector to quaternion through rotation matrix
+    for (unsigned int angle_idx = 0u; angle_idx < r1.size(); ++angle_idx) {
+      std::array<double, 3> angles = {r1[angle_idx], r2[angle_idx],
+                                      r3[angle_idx]};
+      const cv::Mat rot_vec = cv::Mat(1, 3, CV_64F, angles.data());
+      cv::Mat rot_matrix;
+      cv::Rodrigues(rot_vec, rot_matrix);
+      const cv::Mat quaternion = convertRotationMatrixToQuaternion(rot_matrix);
+      quaternions.push_back(quaternion);
+    }
+
+    cv::Mat A = cv::Mat::zeros(4, 4, CV_64F);
+    for (cv::Mat &q : quaternions) {
+      if (q.at<double>(0, 3) < 0) {
+        // handle the antipodal configurations
+        q = -q;
+      }
+      A += q.t() * q;
+    }
+    A /= quaternions.size();
+
+    cv::SVD svd(A, cv::SVD::FULL_UV);
+    cv::Mat U = svd.u;
+    cv::Mat singularValues = svd.w;
+
+    const unsigned int largestEigenValueIndex = 0u;
+    std::array<double, 4> average_quaternion = {
+        svd.u.at<double>(0, largestEigenValueIndex),
+        svd.u.at<double>(1, largestEigenValueIndex),
+        svd.u.at<double>(2, largestEigenValueIndex),
+        svd.u.at<double>(3, largestEigenValueIndex)};
+
+    cv::Mat rot_matrix = convertQuaternionToRotationMatrix(average_quaternion);
+    cv::Rodrigues(rot_matrix, average_rotation);
+  } else {
+    average_rotation.at<double>(0) = median(r1);
+    average_rotation.at<double>(1) = median(r2);
+    average_rotation.at<double>(2) = median(r3);
+  }
+
+  return average_rotation;
 }

README.md

@@ -202,6 +202,7 @@ cam_prefix: "Cam_"
 keypoints_path: "None"      # "path_to/detected_keypoints_data.yml" to save time on keypoint detection
 
 ######################################## Optimization Parameters #############################################
+quaternion_averaging: 1     # use Quaternion Averaging or median for average rotation
 ransac_threshold: 10        # RANSAC threshold in pixel (keep it high just to remove strong outliers)
 number_iterations: 1000     # Max number of iterations for the non linear refinement
 
@@ -310,8 +311,8 @@ The calibration toolbox automatically outputs four ```*.yml``` files. To illustr
 
 # Datasets
 The synthetic and real datasets acquired for this paper are freely available via the following links:
-- [Real Data](https://bosch.frameau.xyz/index.php/s/fqtFij4PNc9mp2a)
-- [Synthetic Data](https://bosch.frameau.xyz/index.php/s/pLc2T9bApbeLmSz)
+- [Real Data](https://drive.google.com/file/d/143jdSi5fxUGj1iEGbTIQPfSqcOyuW-MR/view?usp=sharing)
+- [Synthetic Data](https://drive.google.com/file/d/1CxaXUbO4E9WmaVrYy5aMeRLKmrFB_ARl/view?usp=sharing)
 
 
 # Contribution

configs/Blender_Images/calib_param_synth_Scenario1.yml

@@ -29,9 +29,10 @@ fix_intrinsic: 0 #if 1 then the intrinsic parameters will not be estimated nor r
 ######################################## Images Parameters ###################################################
 root_path: "../data/Blender_Images/Scenario_1/Images/"
 cam_prefix: "Cam_"
-keypoints_path: "None" #"../data/Blender_Images/Scenario_1/Results/detected_keypoints_data.yml"
+keypoints_path: "../data/Blender_Images/Scenario_1/Results/detected_keypoints_data.yml" # "None" #
 
 ######################################## Optimization Parameters #############################################
+quaternion_averaging: 1    # use Quaternion Averaging or median for average rotation
 ransac_threshold: 10       # RANSAC threshold in pixel (keep it high just to remove strong outliers)
 number_iterations: 1000    # Max number of iterations for the non linear refinement
 

configs/Blender_Images/calib_param_synth_Scenario2.yml

@@ -29,9 +29,10 @@ fix_intrinsic: 0 #if 1 then the intrinsic parameters will not be estimated nor r
 ######################################## Images Parameters ###################################################
 root_path: "../data/Blender_Images/Scenario_2/Images/"
 cam_prefix: "Cam_"
-keypoints_path: "None" #"../data/Blender_Images/Scenario_2/Results/detected_keypoints_data.yml" 
+keypoints_path: "../data/Blender_Images/Scenario_2/Results/detected_keypoints_data.yml" # "None" #
 
 ######################################## Optimization Parameters #############################################
+quaternion_averaging: 1    # use Quaternion Averaging or median for average rotation
 ransac_threshold: 10       # RANSAC threshold in pixel (keep it high just to remove strong outliers)
 number_iterations: 1000    # Max number of iterations for the non linear refinement
 

configs/Blender_Images/calib_param_synth_Scenario3.yml

@@ -30,9 +30,10 @@ fix_intrinsic: 0 #if 1 then the intrinsic parameters will not be estimated nor r
 ######################################## Images Parameters ###################################################
 root_path: "../data/Blender_Images/Scenario_3/Images/"
 cam_prefix: "Cam_"
-keypoints_path: "None" #"../data/Blender_Images/Scenario_3/Results/detected_keypoints_data.yml"  
+keypoints_path: "../data/Blender_Images/Scenario_3/Results/detected_keypoints_data.yml"  
 
 ######################################## Optimization Parameters #############################################
+quaternion_averaging: 1    # use Quaternion Averaging or median for average rotation
 ransac_threshold: 10       # RANSAC threshold in pixel (keep it high just to remove strong outliers)
 number_iterations: 1000    # Max number of iterations for the non linear refinement
 

configs/Blender_Images/calib_param_synth_Scenario4.yml

@@ -30,9 +30,10 @@ fix_intrinsic: 0 #if 1 then the intrinsic parameters will not be estimated nor r
 ######################################## Images Parameters ###################################################
 root_path: "../data/Blender_Images/Scenario_4/Images/"
 cam_prefix: "Cam_"
-keypoints_path: "None" #"../data/Blender_Images/Scenario_4/Results/detected_keypoints_data.yml" 
+keypoints_path: "../data/Blender_Images/Scenario_4/Results/detected_keypoints_data.yml" 
 
 ######################################## Optimization Parameters #############################################
+quaternion_averaging: 1    # use Quaternion Averaging or median for average rotation
 ransac_threshold: 10       # RANSAC threshold in pixel (keep it high just to remove strong outliers)
 number_iterations: 1000    # Max number of iterations for the non linear refinement
 

configs/Blender_Images/calib_param_synth_Scenario5.yml

@@ -29,9 +29,10 @@ fix_intrinsic: 0 #if 1 then the intrinsic parameters will not be estimated nor r
 ######################################## Images Parameters ###################################################
 root_path: "../data/Blender_Images/Scenario_5/Images/"
 cam_prefix: "Cam_"
-keypoints_path: "None" #"../data/Blender_Images/Scenario_5/Results/detected_keypoints_data.yml"  
+keypoints_path: "../data/Blender_Images/Scenario_5/Results/detected_keypoints_data.yml"  
 
 ######################################## Optimization Parameters #############################################
+quaternion_averaging: 1    # use Quaternion Averaging or median for average rotation
 ransac_threshold: 10       # RANSAC threshold in pixel (keep it high just to remove strong outliers)
 number_iterations: 1000    # Max number of iterations for the non linear refinement
 

configs/Real_images/calib_param_Seq00_Stereo_vision.yml

@@ -31,11 +31,12 @@ fix_intrinsic: 0 #if 1 then the intrinsic parameters will not be estimated nor r
 ######################################## Images Parameters ###################################################
 root_path: "../data/Real_Images/Seq00_Stereo_vision/" #"../../Images_Sim1Cam3Board/" # "../../Images_NonOver3/"  "../../Images_Cube/" "../../Images_Plan/" "../../Images_NonOver6Cam/"
 cam_prefix: "Cam_"
-keypoints_path: "None" #"../data/Real_Images/Seq00_Stereo_vision/detected_keypoints_data.yml" 
+keypoints_path: "../data/Real_Images/Seq00_Stereo_vision/detected_keypoints_data.yml" 
 
 ######################################## Optimization Parameters ###################################################
-ransac_threshold: 3 #RANSAC threshold in pixel (keep it high just to remove strong outliers)
-number_iterations: 1000 #Max number of iterations for the non linear refinement
+quaternion_averaging: 1    # use Quaternion Averaging or median for average rotation
+ransac_threshold: 3        # RANSAC threshold in pixel (keep it high just to remove strong outliers)
+number_iterations: 1000    # Max number of iterations for the non linear refinement
 
 ######################################## Hand-eye method #############################################
 he_approach: 0 #0: bootstrapped he technique, 1: traditional he

configs/Real_images/calib_param_Seq01_Non-overlapping.yml

@@ -31,11 +31,12 @@ fix_intrinsic: 0 #if 1 then the intrinsic parameters will not be estimated nor r
 ######################################## Images Parameters ###################################################
 root_path: "../data/Real_Images/Seq01_Non-overlapping/" #"../../Images_Sim1Cam3Board/" # "../../Images_NonOver3/"  "../../Images_Cube/" "../../Images_Plan/" "../../Images_NonOver6Cam/"
 cam_prefix: "Cam_"
-keypoints_path: "None" #"../data/Real_Images/Seq01_Non-overlapping/detected_keypoints_data.yml" 
+keypoints_path: "../data/Real_Images/Seq01_Non-overlapping/detected_keypoints_data.yml" 
 
 ######################################## Optimization Parameters ###################################################
-ransac_threshold: 3 #RANSAC threshold in pixel (keep it high just to remove strong outliers)
-number_iterations: 1000 #Max number of iterations for the non linear refinement
+quaternion_averaging: 1    # use Quaternion Averaging or median for average rotation
+ransac_threshold: 3        # RANSAC threshold in pixel (keep it high just to remove strong outliers)
+number_iterations: 1000    # Max number of iterations for the non linear refinement
 
 ######################################## Hand-eye method #############################################
 he_approach: 0 #0: bootstrapped he technique, 1: traditional he

configs/Real_images/calib_param_Seq02_Overlapping_multicamera.yml

@@ -31,11 +31,12 @@ fix_intrinsic: 0 #if 1 then the intrinsic parameters will not be estimated nor r
 ######################################## Images Parameters ###################################################
 root_path: "../data/Real_Images/Seq02_Overlapping_multicamera/" #"../../Images_Sim1Cam3Board/" # "../../Images_NonOver3/"  "../../Images_Cube/" "../../Images_Plan/" "../../Images_NonOver6Cam/"
 cam_prefix: "Cam_"
-keypoints_path: "None" #"../data/Real_Images/Seq02_Overlapping_multicamera/detected_keypoints_data.yml" 
+keypoints_path: "../data/Real_Images/Seq02_Overlapping_multicamera/detected_keypoints_data.yml" 
 
 ######################################## Optimization Parameters ###################################################
-ransac_threshold: 3 #RANSAC threshold in pixel (keep it high just to remove strong outliers)
-number_iterations: 1000 #Max number of iterations for the non linear refinement
+quaternion_averaging: 1    # use Quaternion Averaging or median for average rotation
+ransac_threshold: 3        # RANSAC threshold in pixel (keep it high just to remove strong outliers)
+number_iterations: 1000    # Max number of iterations for the non linear refinement
 
 ######################################## Hand-eye method #############################################
 he_approach: 0 #0: bootstrapped he technique, 1: traditional he