commit 24fc2bdb465455b81519b601ad6a09375fcb261f
author Yash Chainani <yashchainani28@gmail.com> Mon Feb 14 12:43:29 2022 -0800
committer Yash Chainani <yashchainani28@gmail.com> Thu Feb 24 14:49:13 2022 -0800
tree 1ad6b71a0dbcf2bc3ce7d6b4b4d594b2913df1a8
parent de605f17b03a98361fb3a10b85dd92c885d22b49

Check camera stopped intrinsics calib

Check delta r and delta t are small enough
compared to last frame

Signed-off-by: Yash Chainani <yashchainani28@gmail.com>
Change-Id: I4116a64f4779d839404f95a89912a53ce4584ac1

y2020/vision/calibration.cc

diff --git a/y2020/vision/calibration.cc b/y2020/vision/calibration.cc
index 9293d50..0a541de 100644
--- a/y2020/vision/calibration.cc
+++ b/y2020/vision/calibration.cc
@@ -32,8 +32,8 @@
         pi_(pi),
         pi_number_(aos::network::ParsePiNumber(pi)),
         camera_id_(camera_id),
-        H_camera_board_(Eigen::Affine3d()),
         prev_H_camera_board_(Eigen::Affine3d()),
+        prev_image_H_camera_board_(Eigen::Affine3d()),
         charuco_extractor_(
             event_loop, pi,
             [this](cv::Mat rgb_image,
@@ -73,12 +73,12 @@
     }
 
     // Calibration calculates rotation and translation delta from last image
-    // captured to automatically capture next image
+    // stored to automatically capture next image
 
     Eigen::Affine3d H_board_camera =
         Eigen::Translation3d(tvec_eigen) *
         Eigen::AngleAxisd(rvec_eigen.norm(), rvec_eigen / rvec_eigen.norm());
-    H_camera_board_ = H_board_camera.inverse();
+    Eigen::Affine3d H_camera_board_ = H_board_camera.inverse();
     Eigen::Affine3d H_delta = H_board_camera * prev_H_camera_board_;
 
     Eigen::AngleAxisd delta_r = Eigen::AngleAxisd(H_delta.rotation());
@@ -88,8 +88,30 @@
     double r_norm = std::abs(delta_r.angle());
     double t_norm = delta_t.norm();
 
-    int keystroke = cv::waitKey(1);
+    bool store_image = false;
+    // Verify that camera has moved enough from last stored image
     if (r_norm > kDeltaRThreshold || t_norm > kDeltaTThreshold) {
+      // frame_ refers to deltas between current and last captured image
+      Eigen::Affine3d frame_H_delta =
+          H_board_camera * prev_image_H_camera_board_;
+
+      Eigen::AngleAxisd frame_delta_r =
+          Eigen::AngleAxisd(frame_H_delta.rotation());
+
+      Eigen::Vector3d frame_delta_t = frame_H_delta.translation();
+
+      double frame_r_norm = std::abs(frame_delta_r.angle());
+      double frame_t_norm = frame_delta_t.norm();
+
+      // Make sure camera has stopped moving before storing image
+      store_image =
+          frame_r_norm < kFrameDeltaRLimit && frame_t_norm < kFrameDeltaTLimit;
+    }
+
+    prev_image_H_camera_board_ = H_camera_board_;
+
+    int keystroke = cv::waitKey(1);
+    if (store_image) {
       if (valid) {
         prev_H_camera_board_ = H_camera_board_;
 
@@ -101,7 +123,7 @@
                     << kDeltaRThreshold;
         }
         if (t_norm > kDeltaTThreshold) {
-          LOG(INFO) << "Trigerred by translation delta = " << t_norm << " > "
+          LOG(INFO) << "Triggered by translation delta = " << t_norm << " > "
                     << kDeltaTThreshold;
         }
 
@@ -199,6 +221,9 @@
   static constexpr double kDeltaRThreshold = M_PI / 6.0;
   static constexpr double kDeltaTThreshold = 0.3;
 
+  static constexpr double kFrameDeltaRLimit = M_PI / 60;
+  static constexpr double kFrameDeltaTLimit = 0.01;
+
   aos::ShmEventLoop *event_loop_;
   std::string pi_;
   const std::optional<uint16_t> pi_number_;
@@ -209,6 +234,7 @@
 
   Eigen::Affine3d H_camera_board_;
   Eigen::Affine3d prev_H_camera_board_;
+  Eigen::Affine3d prev_image_H_camera_board_;
 
   CharucoExtractor charuco_extractor_;
 };