Improved rotation vectors averaging

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

@@ -75,18 +75,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);
     }
 
     // set the pose and update the observation

McCalib/src/McCalib.cpp

@@ -771,7 +771,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 +793,8 @@ 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);
     average_translation.at<double>(0) = median(t1);
     average_translation.at<double>(1) = median(t2);
     average_translation.at<double>(2) = median(t3);

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

@@ -310,8 +310,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

tests/CMakeLists.txt

@@ -1,7 +1,7 @@
 find_package (Boost REQUIRED COMPONENTS unit_test_framework log_setup log filesystem REQUIRED)
 
 set(SOURCES
-    main.cpp test_graph.cpp test_calibration.cpp simple_unit_tests_example.cpp
+    main.cpp test_graph.cpp test_calibration.cpp test_geometrytools.cpp simple_unit_tests_example.cpp
 )
 
 add_executable(boost_tests_run ${SOURCES})

tests/test_geometrytools.cpp

@@ -0,0 +1,100 @@
+#include <boost/test/unit_test.hpp>
+
+#include <geometrytools.hpp>
+
+BOOST_AUTO_TEST_SUITE(CheckGeometryTools)
+
+double INTRINSICS_TOLERANCE = 1.0; // in percentage
+
+BOOST_AUTO_TEST_CASE(CheckRotationMatrixToQuaternionConversion1) {
+  std::array<double, 9> rot_matrix_data = {1, 0, 0, 0, 1, 0, 0, 0, 1};
+  const cv::Mat rot_matrix = cv::Mat(3, 3, CV_64F, rot_matrix_data.data());
+  const cv::Mat quaternion_pred = convertRotationMatrixToQuaternion(rot_matrix);
+
+  std::array<double, 4> quaternion_gt_data = {0, 0, 0, 1};
+  const cv::Mat quaternion_gt =
+      cv::Mat(1, 4, CV_64F, quaternion_gt_data.data());
+
+  BOOST_REQUIRE_EQUAL(quaternion_pred.size[0], quaternion_gt.size[0]);
+  BOOST_REQUIRE_EQUAL(quaternion_pred.size[1], quaternion_gt.size[1]);
+  for (int i = 0; i < quaternion_pred.size[1]; ++i) {
+    BOOST_CHECK_CLOSE(quaternion_pred.at<double>(i),
+                      quaternion_gt.at<double>(i), INTRINSICS_TOLERANCE);
+  }
+}
+
+BOOST_AUTO_TEST_CASE(CheckRotationMatrixToQuaternionConversion2) {
+  std::array<double, 9> rot_matrix_data = {-0.9999, -0.1998, -0.3996,
+                                           0.1998,  0.6000,  -0.8000,
+                                           0.3996,  -0.8000, -0.5999};
+  const cv::Mat rot_matrix = cv::Mat(3, 3, CV_64F, rot_matrix_data.data());
+  const cv::Mat quaternion_pred = convertRotationMatrixToQuaternion(rot_matrix);
+
+  std::array<double, 4> quaternion_gt_data = {0, 0.8944, -0.4472,
+                                              -0.2234}; // x y z w
+  const cv::Mat quaternion_gt =
+      cv::Mat(1, 4, CV_64F, quaternion_gt_data.data());
+
+  BOOST_REQUIRE_EQUAL(quaternion_pred.size[0], quaternion_gt.size[0]);
+  BOOST_REQUIRE_EQUAL(quaternion_pred.size[1], quaternion_gt.size[1]);
+  for (int i = 0; i < quaternion_pred.size[1]; ++i) {
+    BOOST_CHECK_CLOSE(quaternion_pred.at<double>(i),
+                      quaternion_gt.at<double>(i), INTRINSICS_TOLERANCE);
+  }
+}
+
+BOOST_AUTO_TEST_CASE(CheckQuaternionToRotationMatrixConversion) {
+  std::array<double, 4> quaternion_data = {0.2809946, 0.8377387, -0.4188693,
+                                           -0.2092473}; // x y z w
+  const cv::Mat quaternion = cv::Mat(1, 4, CV_64F, quaternion_data.data());
+
+  cv::Mat rot_matrix_pred = convertQuaternionToRotationMatrix(quaternion);
+
+  std::array<double, 9> rot_matrix_gt_data = {
+      -0.7545152, 0.2955056, -0.5859892, 0.6460947, 0.4911810,
+      -0.5842113, 0.1151891, -0.8194008, -0.5615281};
+  const cv::Mat rot_matrix_gt =
+      cv::Mat(3, 3, CV_64F, rot_matrix_gt_data.data());
+
+  BOOST_REQUIRE_EQUAL(rot_matrix_pred.size[0], rot_matrix_gt.size[0]);
+  BOOST_REQUIRE_EQUAL(rot_matrix_pred.size[1], rot_matrix_gt.size[1]);
+  for (int i = 0; i < rot_matrix_pred.size[0]; ++i) {
+    for (int j = 0; j < rot_matrix_pred.size[1]; ++j) {
+      BOOST_CHECK_CLOSE(rot_matrix_pred.at<double>(i, j),
+                        rot_matrix_gt.at<double>(i, j), INTRINSICS_TOLERANCE);
+    }
+  }
+}
+
+BOOST_AUTO_TEST_CASE(CheckRotationMatrixToQuaternionAndBackConversion) {
+  for (int angle_x = -20; angle_x <= 20; ++angle_x) {
+    for (int angle_y = -45; angle_y <= 90; ++angle_y) {
+      for (int angle_z = -270; angle_z <= -180; ++angle_z) {
+        std::array<double, 3> angles = {static_cast<double>(angle_x),
+                                        static_cast<double>(angle_y),
+                                        static_cast<double>(angle_z)};
+        const cv::Mat rot_vec = cv::Mat(1, 3, CV_64F, angles.data());
+        cv::Mat rot_matrix_before;
+        cv::Rodrigues(rot_vec, rot_matrix_before);
+        const cv::Mat quaternion =
+            convertRotationMatrixToQuaternion(rot_matrix_before);
+        cv::Mat rot_matrix_after =
+            convertQuaternionToRotationMatrix(quaternion);
+
+        BOOST_REQUIRE_EQUAL(rot_matrix_after.size[0],
+                            rot_matrix_before.size[0]);
+        BOOST_REQUIRE_EQUAL(rot_matrix_after.size[1],
+                            rot_matrix_before.size[1]);
+        for (int i = 0; i < rot_matrix_after.size[0]; ++i) {
+          for (int j = 0; j < rot_matrix_after.size[1]; ++j) {
+            BOOST_CHECK_CLOSE(rot_matrix_after.at<double>(i, j),
+                              rot_matrix_before.at<double>(i, j),
+                              INTRINSICS_TOLERANCE);
+          }
+        }
+      }
+    }
+  }
+}
+
+BOOST_AUTO_TEST_SUITE_END()