Skip to content
Snippets Groups Projects
Commit 68c4cd76 authored by Sebastian Hahta's avatar Sebastian Hahta Committed by Nicolas Pope
Browse files

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.
parent a9a5d51e
No related branches found
No related tags found
No related merge requests found
......@@ -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);
};
};
......
......@@ -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);
}
......
......@@ -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;
......
......@@ -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";
}
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment