Add additional error calculation to calibration pattern in registration. Use...

Add additional error calculation to calibration pattern in registration. Use findChessboardCornersSB if OpenCV version > 4.

applications/reconstruct/include/ftl/registration.hpp

@@ -26,7 +26,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cornersToPointCloud(const std::vector<cv::Po
 
 /* Find chessboard corners from image.
  */
-bool findChessboardCorners(cv::Mat &rgb, const cv::Mat &disp, const ftl::rgbd::CameraParameters &p, const cv::Size pattern_size, pcl::PointCloud<pcl::PointXYZ>::Ptr &out);
+bool findChessboardCorners(cv::Mat &rgb, const cv::Mat &disp, const ftl::rgbd::CameraParameters &p, const cv::Size pattern_size, pcl::PointCloud<pcl::PointXYZ>::Ptr &out, float error_threshold);
 
 };
 };

applications/reconstruct/src/main.cpp

@@ -195,6 +195,7 @@ std::map<string, Eigen::Matrix4f> runRegistration(ftl::net::Universe &net, Conta
 	
 	int iter = (int) config["registration"]["calibration"]["iterations"];
 	int delay = (int) config["registration"]["calibration"]["delay"];
+	float max_error = (int) config["registration"]["calibration"]["max_error"];
 	string ref_uri = (string) config["registration"]["reference-source"];
 	cv::Size pattern_size(	(int) config["registration"]["calibration"]["patternsize"][0],
 							(int) config["registration"]["calibration"]["patternsize"][1]);
@@ -233,7 +234,7 @@ std::map<string, Eigen::Matrix4f> runRegistration(ftl::net::Universe &net, Conta
 			
 			if ((rgb.cols == 0) || (depth.cols == 0)) { retval = false; break; }
 			
-			retval &= ftl::registration::findChessboardCorners(rgb, depth, input->getParameters(), pattern_size, patterns_iter[i]);
+			retval &= ftl::registration::findChessboardCorners(rgb, depth, input->getParameters(), pattern_size, patterns_iter[i], max_error);
 			
 			cv::imshow("Registration: " + (string)input->getConfig()["uri"], rgb);
 		}

applications/reconstruct/src/registration.cpp

@@ -18,7 +18,7 @@
 
 #include <pcl/registration/transformation_validation.h>
 #include <pcl/registration/transformation_validation_euclidean.h>
-
+#include <pcl/common/geometry.h>
 //#include <pcl/registration/icp_nl.h>
 
 namespace ftl {
@@ -36,7 +36,7 @@ using cv::Mat;
 //
 // Fit calibration pattern into plane using RANSAC + project points
 //
-void fitPlane(PointCloud<PointXYZ>::Ptr cloud_in, PointCloud<PointXYZ>::Ptr cloud_out) {
+pcl::ModelCoefficients::Ptr fitPlane(PointCloud<PointXYZ>::Ptr cloud_in, float distance_threshold=5.0) {
 	// TODO: include pattern in model (find best alignment of found points and return transformed reference?)
 	
 	pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
@@ -48,16 +48,34 @@ void fitPlane(PointCloud<PointXYZ>::Ptr cloud_in, PointCloud<PointXYZ>::Ptr clou
 	seg.setOptimizeCoefficients(true);
 	seg.setModelType(pcl::SACMODEL_PLANE);
 	seg.setMethodType(pcl::SAC_RANSAC);
-	seg.setDistanceThreshold(0.01);
+	seg.setDistanceThreshold(distance_threshold);
 	seg.setInputCloud(cloud_in);
 	seg.segment(*inliers, *coefficients);
 	
+	return coefficients;
+}
+
+float fitPlaneError(PointCloud<PointXYZ>::Ptr cloud_in, float distance_threshold=5.0) {
+	auto coefficients = fitPlane(cloud_in, distance_threshold);
+	PointCloud<PointXYZ> cloud_proj;
+	
 	// Project points into plane
 	pcl::ProjectInliers<PointXYZ> proj;
 	proj.setModelType(pcl::SACMODEL_PLANE);
 	proj.setInputCloud(cloud_in);
 	proj.setModelCoefficients(coefficients);
-	proj.filter(*cloud_out);
+	proj.filter(cloud_proj); 
+	
+	LOG_ASSERT(cloud_in->size() == cloud_proj.size());
+	
+	// todo: which error score is suitable? (using MSE)
+	float score = 0.0;
+	for(size_t i = 0; i < cloud_proj.size(); i++) {
+		float d = pcl::geometry::distance(cloud_in->points[i], cloud_proj.points[i]);
+		score += d * d;
+	}
+	
+	return score / cloud_proj.size();
 }
 
 //template<typename T = PointXYZ> typename
@@ -93,8 +111,6 @@ PointCloud<PointXYZ>::Ptr cornersToPointCloud(const vector<cv::Point2f> &corners
 		point.y = (((double)y + CY) / FY) * d; // / 1000.0f;
 		point.z = d;
 		
-		if (d > 6000.0f) LOG(ERROR) << "Bad corner : " << i;
-		
 		// no check since disparities assumed to be good in the calibration pattern
 		// if (fabs(d-minDisparity) <= FLT_EPSILON)
 		
@@ -104,13 +120,28 @@ PointCloud<PointXYZ>::Ptr cornersToPointCloud(const vector<cv::Point2f> &corners
 	return cloud;
 }
 
-bool findChessboardCorners(Mat &rgb, const Mat &disp, const CameraParameters &p, const cv::Size pattern_size, PointCloud<PointXYZ>::Ptr &out) {
+bool findChessboardCorners(Mat &rgb, const Mat &disp, const CameraParameters &p, const cv::Size pattern_size, PointCloud<PointXYZ>::Ptr &out, float error_threshold) {
 	vector<cv::Point2f> corners(pattern_size.width * pattern_size.height);
-	bool retval = cv::findChessboardCorners(rgb, pattern_size, corners); // (todo) change to findChessboardCornersSB (OpenCV > 4)
-	// todo: verify that disparities are good
+
+#if CV_VERSION_MAJOR >= 4
+	bool retval = cv::findChessboardCornersSB(rgb, pattern_size, corners);
+#else
+	bool retval = cv::findChessboardCorners(rgb, pattern_size, corners);
+#endif
+
 	cv::drawChessboardCorners(rgb, pattern_size, Mat(corners), retval);
 	if (!retval) { return false; }
+	
 	auto corners_cloud = cornersToPointCloud(corners, disp, p);
+	// simple check that the values make some sense
+	float error = fitPlaneError(corners_cloud, error_threshold); // should use different parameter?
+	LOG(INFO) << "MSE against estimated plane: " << error;
+	
+	if (error > error_threshold) {
+		LOG(WARNING) << "too high error score for calibration pattern, threshold " << error_threshold;
+		return false;
+	}
+	
 	if (out) { *out += *corners_cloud; } // if cloud is valid, add the points
 	else { out = corners_cloud; }
 	return true;

components/rgbd-sources/src/calibrate.cpp

@@ -391,12 +391,16 @@ bool Calibrate::_recalibrate(vector<vector<Point2f>> *imagePoints,
 	double prevTimestamp = 0.0;
 	const Scalar RED(0, 0, 255), GREEN(0, 255, 0);
 
+#if CV_VERSION_MAJOR >= 4
+	int chessBoardFlags = CALIB_CB_NORMALIZE_IMAGE;
+#else
 	int chessBoardFlags = CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_NORMALIZE_IMAGE;
 
 	if (!settings_.useFisheye) {
 		// fast check erroneously fails with high distortions like fisheye
 		chessBoardFlags |= CALIB_CB_FAST_CHECK;
 	}
+#endif
 
 	for (;;) {
 		Mat view[2];
@@ -432,35 +436,47 @@ bool Calibrate::_recalibrate(vector<vector<Point2f>> *imagePoints,
 
 		vector<Point2f> pointBuf[2];
 		bool found1, found2;
+#if CV_VERSION_MAJOR >= 4
+		LOG(INFO) << "Using findChessboardCornersSB()";
+		found1 = findChessboardCornersSB(view[0], settings_.boardSize,
+				pointBuf[0], chessBoardFlags);
+		found2 = !local_->isStereo() || findChessboardCornersSB(view[1],
+			settings_.boardSize, pointBuf[1], chessBoardFlags);
+#else
+		LOG(INFO) << "Using findChessboardCorners()";
 		found1 = findChessboardCorners(view[0], settings_.boardSize,
 				pointBuf[0], chessBoardFlags);
 		found2 = !local_->isStereo() || findChessboardCorners(view[1],
 				settings_.boardSize, pointBuf[1], chessBoardFlags);
-
+#endif
 		if (found1 && found2 &&
 				local_->getTimestamp()-prevTimestamp > settings_.delay) {
 			prevTimestamp = local_->getTimestamp();
-            // improve the found corners' coordinate accuracy for chessboard
+#if CV_VERSION_MAJOR >=4
+			// findChessboardCornersSB() does not require subpixel step.
+#else
+			// improve the found corners' coordinate accuracy for chessboard.
 			Mat viewGray;
 			cvtColor(view[0], viewGray, COLOR_BGR2GRAY);
 			cornerSubPix(viewGray, pointBuf[0], Size(winSize, winSize),
 				Size(-1, -1), TermCriteria(
 					TermCriteria::EPS+TermCriteria::COUNT, 30, 0.0001));
-                imagePoints[0].push_back(pointBuf[0]);
 			
 			if (local_->isStereo()) {
 				cvtColor(view[1], viewGray, COLOR_BGR2GRAY);
 				cornerSubPix(viewGray, pointBuf[1], Size(winSize, winSize),
 					Size(-1, -1), TermCriteria(
 						TermCriteria::EPS+TermCriteria::COUNT, 30, 0.0001));
-					imagePoints[1].push_back(pointBuf[1]);
 			}
-
+#endif
+			imagePoints[0].push_back(pointBuf[0]);
+			if (local_->isStereo()) { imagePoints[1].push_back(pointBuf[1]); }
 			// Draw the corners.
+			
 			drawChessboardCorners(view[0], settings_.boardSize,
 					Mat(pointBuf[0]), found1);
-                if (local_->isStereo()) drawChessboardCorners(view[1],
-                		settings_.boardSize, Mat(pointBuf[1]), found2);
+			if (local_->isStereo())
+				drawChessboardCorners(view[1],settings_.boardSize, Mat(pointBuf[1]), found2);
 		} else {
 			LOG(WARNING) << "No calibration pattern found";
 		}