kalibr
kalibr copied to clipboard
ethz-asl
How does Kalibr detect apriltags on fisheye images?
I am noticing that when I use the source code from http://people.csail.mit.edu/kaess/apriltags/, the software can not detect the apriltags on wide angle fisheye images. However Kalibr can do it quite well, since Kalibr does not know the intrinsic paramters beforehand, it can not do undistortion. I am wondering if there is some trick for detection?
thx Yukan
I cannot clone the original svn repo right now to check, but what we found to be quite important for successful detections under larger distortions compared to the version used in Kalibr are two things:
- Most importantly you should comment out the define for
INTERPOLATE(https://github.com/ethz-asl/kalibr/blob/694ca2058fac2560ece5bfdf4a1195f438005937/aslam_offline_calibration/ethz_apriltag2/include/apriltags/Homography33.h#L12) to ensure that homography is used. - With distorted tags, you might need to tweak the subpixel refinement to allow for larger displacements. We adopt a simple scheme based on tag size.
This is the diff. It is certainly not perfect and has some false positive corners, but many more detections that Kalibr for distorted images. Note that our apriltag.cpp corresponds to a modified https://github.com/ethz-asl/kalibr/blob/master/aslam_cv/aslam_cameras_april/src/GridCalibrationTargetAprilgrid.cpp, so you might have to adapt it a bit.
diff --git a/thirdparty/apriltag/ethz_apriltag2/include/apriltags/Homography33.h b/thirdparty/apriltag/ethz_apriltag2/include/apriltags/Homography33.h
index 841cea5..055f74d 100644
--- a/thirdparty/apriltag/ethz_apriltag2/include/apriltags/Homography33.h
+++ b/thirdparty/apriltag/ethz_apriltag2/include/apriltags/Homography33.h
@@ -8,8 +8,13 @@
#include <Eigen/Dense>
+// NOTE: In Basalt we use homography, since for fisheye-distorted
+// images, interpolation is not good enough, resulting in a lot less
+// valid detections. At the same time we don't case about speed too
+// much.
+
// interpolate points instead of using homography
-#define INTERPOLATE
+//#define INTERPOLATE
// use stable version of homography recover (opencv, includes refinement step)
#define STABLE_H
diff --git a/thirdparty/apriltag/src/apriltag.cpp b/thirdparty/apriltag/src/apriltag.cpp
index 19b8c96..ee63cc1 100644
--- a/thirdparty/apriltag/src/apriltag.cpp
+++ b/thirdparty/apriltag/src/apriltag.cpp
@@ -10,7 +10,7 @@ namespace basalt {
struct ApriltagDetectorData {
ApriltagDetectorData()
: doSubpixRefinement(true),
- maxSubpixDisplacement2(1.5),
+ maxSubpixDisplacement(0),
minTagsForValidObs(4),
minBorderDistance(4.0),
blackTagBorder(2),
@@ -20,7 +20,7 @@ struct ApriltagDetectorData {
}
bool doSubpixRefinement;
- double maxSubpixDisplacement2;
+ double maxSubpixDisplacement; //!< maximum displacement for subpixel refinement. If 0, only base it on tag size.
unsigned int minTagsForValidObs;
double minBorderDistance;
unsigned int blackTagBorder;
@@ -37,9 +37,11 @@ ApriltagDetector::~ApriltagDetector() { delete data; }
void ApriltagDetector::detectTags(basalt::ManagedImage<uint16_t>& img_raw,
Eigen::vector<Eigen::Vector2d>& corners,
- std::vector<int>& ids) {
+ std::vector<int>& ids,
+ std::vector<double>& radii) {
corners.clear();
ids.clear();
+ radii.clear();
cv::Mat image16(img_raw.h, img_raw.w, CV_16U, img_raw.ptr);
cv::Mat image;
@@ -107,6 +109,17 @@ void ApriltagDetector::detectTags(basalt::ManagedImage<uint16_t>& img_raw,
}
}
+ // compute search radius for sub-pixel refinement depending on size of tag in image
+ std::vector<double> radiiRaw;
+ for (unsigned i = 0; i < detections.size(); i++) {
+ const double minimalRadius = 2.0;
+ const double percentOfSideLength = 7.5;
+ const double avgSideLength = static_cast<double>(detections[i].observedPerimeter) / 4.0;
+ // use certain percentage of average side length as radius
+ // subtract 1.0 since this radius is for displacement threshold; Search region is slightly larger
+ radiiRaw.emplace_back(std::max(minimalRadius, (percentOfSideLength/100.0 * avgSideLength) - 1.0));
+ }
+
// convert corners to cv::Mat (4 consecutive corners form one tag)
/// point ordering here
/// 11-----10 15-----14
@@ -130,9 +143,26 @@ void ApriltagDetector::detectTags(basalt::ManagedImage<uint16_t>& img_raw,
// optional subpixel refinement on all tag corners (four corners each tag)
if (data->doSubpixRefinement)
- cv::cornerSubPix(
- image, tagCorners, cv::Size(4, 4), cv::Size(-1, -1),
- cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));
+ {
+ for (size_t i = 0; i < detections.size(); i++) {
+ cv::Mat currentCorners(4, 2, CV_32F);
+ for (unsigned j = 0; j < 4; j++) {
+ currentCorners.at<float>(j, 0) = tagCorners.at<float>(4*i+j, 0);
+ currentCorners.at<float>(j, 1) = tagCorners.at<float>(4*i+j, 1);
+ }
+
+ const int radius = static_cast<int>(std::ceil(radiiRaw[i] + 1.0));
+ cv::cornerSubPix(
+ image, currentCorners, cv::Size(radius, radius), cv::Size(-1, -1),
+ cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 100, 0.01));
+
+ for (unsigned j = 0; j < 4; j++) {
+ tagCorners.at<float>(4*i+j, 0) = currentCorners.at<float>(j, 0);
+ tagCorners.at<float>(4*i+j, 1) = currentCorners.at<float>(j, 1);
+ }
+
+ }
+ }
// insert the observed points into the correct location of the grid point
// array
@@ -149,6 +179,12 @@ void ApriltagDetector::detectTags(basalt::ManagedImage<uint16_t>& img_raw,
// get the tag id
int tagId = detections[i].id;
+ // check maximum displacement from subpixel refinement
+ const double radius = radiiRaw[i];
+ const double tagMaxDispl2 = radius * radius;
+ const double globalMaxDispl2 = data->maxSubpixDisplacement * data->maxSubpixDisplacement;
+ const double subpixRefinementThreshold2 = globalMaxDispl2 > 0 ? std::min(globalMaxDispl2, tagMaxDispl2) : tagMaxDispl2;
+
// add four points per tag
for (int j = 0; j < 4; j++) {
int pointId = (tagId << 2) + j;
@@ -170,9 +206,16 @@ void ApriltagDetector::detectTags(basalt::ManagedImage<uint16_t>& img_raw,
// add all points, but only set active if the point has not moved to far
// in the subpix refinement
- if (subpix_displacement_squarred <= data->maxSubpixDisplacement2) {
+ // TODO: We still get a few false positives here, e.g. when the whole search region lies on an edge but the actual corner is not included.
+ // Maybe what we would need to do is actually checking a "corner score" vs "edge score" after refinement and discard all corners that
+ // are not more "cornery" than "edgy". Another possible issue might be corners, where (due to fisheye distortion), neighboring corners
+ // are in the search radius. For those we should check if in the radius there is really a clear single maximum in the corner score
+ // and otherwise discard the corner.
+
+ if (subpix_displacement_squarred <= subpixRefinementThreshold2) {
corners.emplace_back(corner_x, corner_y);
ids.emplace_back(pointId);
+ radii.emplace_back(radius);
}
}
}
＠luyukan Is this method helpful to you? Can you share the result with me? Thanks!
@NikolausDemmel Thank for your great sharing! "This is the diff. It is certainly not perfect and has some false positive corners, but many more detections that Kalibr for distorted images." From your above conclusion, why not commit it to the kalibr repository since the method is good?
Thanks again！
@luyukan Great! Thanks! Which camera/distortion model fit your fisheye camera well?
Just for the record, you can find the above code changes released as part of basalt: https://gitlab.com/VladyslavUsenko/basalt/tree/master/thirdparty/apriltag
From your above conclusion, why not commit it to the kalibr repository since the method is good?
Sure, would be great to get it into Kalibr, but its a lot of effort to ensure it doesn't break anything for other cases (e.g. non-fisheye). In fact, I wouldn't even know where to start with systematically testing this, since I don't think there is a unit test suite for the detection and I don't have the time for that. If someone else wants to do it, go ahead.
Just for the record, you can find the above code changes released as part of basalt: https://gitlab.com/VladyslavUsenko/basalt/tree/master/thirdparty/apriltag
From your above conclusion, why not commit it to the kalibr repository since the method is good?
Sure, would be great to get it into Kalibr, but its a lot of effort to ensure it doesn't break anything for other cases (e.g. non-fisheye). In fact, I wouldn't even know where to start with systematically testing this, since I don't think there is a unit test suite for the detection and I don't have the time for that. If someone else wants to do it, go ahead.
Can you briefly talk about the idea of fisheye detection ? I couldn't find the point in the code, thanks a lot！
