diff --git a/aslam_cv/aslam_cameras/src/GridDetector.cpp b/aslam_cv/aslam_cameras/src/GridDetector.cpp index 31d4fe9ac..7aba6de03 100644 --- 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 corners_reproj; - std::vector corners_detected; + //calculate reprojection errors + auto compute_stats = [&](double &mean, double &std, Eigen::MatrixXd &reprojection_errors_norm, + std::vector &corners_reproj, std::vector &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 corners_reproj, corners_detected; + compute_stats(mean, std, reprojection_errors_norm, corners_reproj, corners_detected); + //disable outlier corners std::vector 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 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 index a503fc4da..1b01515d0 100644 --- 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)