Show reprojection stats, disable reprojection when doing camera extra…

…ction (target pose is not recovered so can't viz to user, only IMU-to-CAM calib can)
alejandrofontan · Nov 24, 2022 · 3df0aa7 · 3df0aa7
1 parent 50001eb
commit 3df0aa7
Show file tree

Hide file tree

Showing 2 changed files with 42 additions and 14 deletions.
diff --git a/aslam_cv/aslam_cameras/src/GridDetector.cpp b/aslam_cv/aslam_cameras/src/GridDetector.cpp
@@ -130,19 +130,17 @@ bool GridDetector::findTarget(const cv::Mat & image, const aslam::Time & stamp,
       SM_DEBUG_STREAM("estimateTransformation() failed");
   }
 
-  //remove corners with a reprojection error above a threshold
-  //(remove detection outliers)
-  if(_options.filterCornerOutliers && success)
-  {
-    //calculate reprojection errors
-    std::vector<cv::Point2f> corners_reproj;
-    std::vector<cv::Point2f> corners_detected;
+  //calculate reprojection errors
+  auto compute_stats = [&](double &mean, double &std, Eigen::MatrixXd &reprojection_errors_norm,
+                           std::vector<cv::Point2f> &corners_reproj, std::vector<cv::Point2f> &corners_detected) {
+
+    corners_reproj.clear();
+    corners_detected.clear();
     outObservation.getCornerReprojection(_geometry, corners_reproj);
     unsigned int numCorners = outObservation.getCornersImageFrame(corners_detected);
 
     //calculate error norm
-    Eigen::MatrixXd reprojection_errors_norm = Eigen::MatrixXd::Zero(numCorners,1);
-
+    reprojection_errors_norm = Eigen::MatrixXd::Zero(numCorners,1);
     for(unsigned int i=0; i<numCorners; i++ )
     {
       cv::Point2f reprojection_err = corners_detected[i] - corners_reproj[i];
@@ -152,8 +150,8 @@ bool GridDetector::findTarget(const cv::Mat & image, const aslam::Time & stamp,
     }
 
     //calculate statistics
-    double mean = reprojection_errors_norm.mean();
-    double std = 0.0;
+    mean = reprojection_errors_norm.mean();
+    std = 0.0;
     for(unsigned int i=0; i<numCorners; i++)
     {
       double temp = reprojection_errors_norm(i,0)-mean;
@@ -162,6 +160,18 @@ bool GridDetector::findTarget(const cv::Mat & image, const aslam::Time & stamp,
     std /= (double)numCorners;
     std = sqrt(std);
 
+  };
+
+  //remove corners with a reprojection error above a threshold
+  //(remove detection outliers)
+  if(_options.filterCornerOutliers && success)
+  {
+    //calculate reprojection errors
+    double mean, std;
+    Eigen::MatrixXd reprojection_errors_norm;
+    std::vector<cv::Point2f> corners_reproj, corners_detected;
+    compute_stats(mean, std, reprojection_errors_norm, corners_reproj, corners_detected);
+
     //disable outlier corners
     std::vector<unsigned int> cornerIdx;
     outObservation.getCornersIdx(cornerIdx);
@@ -179,7 +189,7 @@ bool GridDetector::findTarget(const cv::Mat & image, const aslam::Time & stamp,
     }
 
     if(removeCount>0)
-      SM_DEBUG_STREAM("removed " << removeCount << " of " << numCorners << " calibration target corner outliers\n";);
+      SM_DEBUG_STREAM("removed " << removeCount << " of " << reprojection_errors_norm.rows() << " calibration target corner outliers\n";);
   }
 
 
@@ -196,6 +206,24 @@ bool GridDetector::findTarget(const cv::Mat & image, const aslam::Time & stamp,
       for (unsigned int i = 0; i < reprojs.size(); i++)
         cv::circle(imageCopy1, reprojs[i], 3, CV_RGB(255,0,0), 1);
 
+      //calculate reprojection errors
+      double mean, std;
+      Eigen::MatrixXd reprojection_errors_norm;
+      std::vector<cv::Point2f> corners_reproj, corners_detected;
+      compute_stats(mean, std, reprojection_errors_norm, corners_reproj, corners_detected);
+
+      // show the on the rendered image
+      auto format_str = [](double data) {
+        std::ostringstream ss;
+        ss << std::setprecision(3) << data;
+        return ss.str();
+      };
+      cv::putText(imageCopy1, "reproj err mean: " + format_str(mean), 
+                  cv::Point(50, 50), cv::FONT_HERSHEY_SIMPLEX, 0.8,
+                  CV_RGB(0,255,0), 3, 8, false);
+      cv::putText(imageCopy1, "reproj err std: " + format_str(std), 
+                  cv::Point(50, 100), cv::FONT_HERSHEY_SIMPLEX, 0.8,
+                  CV_RGB(0,255,0), 3, 8, false);
 
     } else {
       cv::putText(imageCopy1, "Detection failed! (frame not used)",

diff --git a/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/CameraCalibrator.py b/aslam_offline_calibration/kalibr/python/kalibr_camera_calibration/CameraCalibrator.py
@@ -43,9 +43,9 @@ def __init__(self, cameraModel, targetConfig, dataset, geometry=None, verbose=Fa
         self.dv = cameraModel.designVariable(self.geometry)
         self.setDvActiveStatus(True, True, False)
         self.isGeometryInitialized = False
-        
+
         #create target detector
-        self.ctarget = TargetDetector(targetConfig, self.geometry, showCorners=verbose, showReproj=verbose)
+        self.ctarget = TargetDetector(targetConfig, self.geometry, showCorners=verbose)
 
     def setDvActiveStatus(self, projectionActive, distortionActive, shutterActice):
         self.dv.projectionDesignVariable().setActive(projectionActive)