mirror of
https://github.com/introlab/rtabmap.git
synced 2026-10-07 02:27:47 +08:00
New parameter: Marker/Strategy (default opencv-aruco as before). Optimized multicameras marker detection (do only once with stitched image)
This commit is contained in:
matlabbe
@@ -919,7 +919,7 @@ class RTABMAP_CORE_EXPORT Parameters
|
||||
|
||||
RTABMAP_PARAM(Marker, Strategy, int, 0, "Marker detection implementation: 0=OpenCV, 1=AprilTag");
|
||||
RTABMAP_PARAM(Marker, Dictionary, int, 0, "Dictionary to use: DICT_ARUCO_4X4_50=0, DICT_ARUCO_4X4_100=1, DICT_ARUCO_4X4_250=2, DICT_ARUCO_4X4_1000=3, DICT_ARUCO_5X5_50=4, DICT_ARUCO_5X5_100=5, DICT_ARUCO_5X5_250=6, DICT_ARUCO_5X5_1000=7, DICT_ARUCO_6X6_50=8, DICT_ARUCO_6X6_100=9, DICT_ARUCO_6X6_250=10, DICT_ARUCO_6X6_1000=11, DICT_ARUCO_7X7_50=12, DICT_ARUCO_7X7_100=13, DICT_ARUCO_7X7_250=14, DICT_ARUCO_7X7_1000=15, DICT_ARUCO_ORIGINAL = 16, DICT_APRILTAG_16h5=17, DICT_APRILTAG_25h9=18, DICT_APRILTAG_36h10=19, DICT_APRILTAG_36h11=20");
|
||||
RTABMAP_PARAM(Marker, Length, float, 0, "The length (m) of the markers' side. 0 means automatic marker length estimation using the depth image (the camera should look at the marker perpendicularly for initialization).");
|
||||
RTABMAP_PARAM(Marker, Length, float, 0, "The length (m) of the markers' side. Value <=0 means automatic marker length estimation using the depth image (the camera should look at the marker perpendicularly for initialization). If 0, the length is estimated only on the first marker detected, then re-used for all next detections (i.e., this assumes that markers have all the same length). With <0, the length is estimated once for each unique marker, then re-used for next detections with the same marker ID.");
|
||||
RTABMAP_PARAM(Marker, MaxDepthError, float, 0.01, uFormat("Maximum depth error between all corners of a marker when estimating the marker length (when %s is 0). The smaller it is, the more perpendicular the camera should be toward the marker to initialize the length.", kMarkerLength().c_str()));
|
||||
RTABMAP_PARAM(Marker, VarianceLinear, float, 0.001, uFormat("Linear variance to set on marker detections. If %s is enabled and %s=2 (GTSAM): it is the variance of the range factor, with 9999 to disable range factor and to do only bearing.", kMarkerVarianceOrientationIgnored().c_str(), kOptimizerStrategy().c_str()));
|
||||
RTABMAP_PARAM(Marker, VarianceAngular, float, 0.01, uFormat("Angular variance to set on marker detections. If %s is enabled, it is ignored with %s=1 (g2o) and it corresponds to bearing variance with %s=2 (GTSAM).", kMarkerVarianceOrientationIgnored().c_str(), kOptimizerStrategy().c_str(), kOptimizerStrategy().c_str()));
|
||||
|
||||
+248
-197
@@ -171,6 +171,7 @@ void MarkerDetector::parseParameters(const ParametersMap & parameters)
|
||||
#endif
|
||||
}
|
||||
|
||||
// deprecated
|
||||
std::map<int, Transform> MarkerDetector::detect(const cv::Mat & image, const CameraModel & model, const cv::Mat & depth, float * markerLengthOut, cv::Mat * imageWithDetections)
|
||||
{
|
||||
std::map<int, Transform> detections;
|
||||
@@ -199,88 +200,77 @@ std::map<int, MarkerInfo> MarkerDetector::detect(const cv::Mat & image,
|
||||
UASSERT(int((image.cols/models.size())*models.size()) == image.cols);
|
||||
UASSERT(int((depth.cols/models.size())*models.size()) == depth.cols);
|
||||
int subRGBWidth = image.cols/models.size();
|
||||
int subDepthWidth = depth.cols/models.size();
|
||||
|
||||
std::map<int, MarkerInfo> allInfo;
|
||||
for(size_t i=0; i<models.size(); ++i)
|
||||
float rgbToDepthFactorX = 1.0f;
|
||||
float rgbToDepthFactorY = 1.0f;
|
||||
if(!depth.empty())
|
||||
{
|
||||
cv::Mat subImage(image, cv::Rect(subRGBWidth*i, 0, subRGBWidth, image.rows));
|
||||
cv::Mat subDepth;
|
||||
if(!depth.empty())
|
||||
subDepth = cv::Mat(depth, cv::Rect(subDepthWidth*i, 0, subDepthWidth, depth.rows));
|
||||
CameraModel model = models[i];
|
||||
cv::Mat subImageWithDetections;
|
||||
std::map<int, MarkerInfo> subInfo = detect(subImage, model, subDepth, markerLengths, imageWithDetections?&subImageWithDetections:0);
|
||||
if(ULogger::level() >= ULogger::kWarning)
|
||||
rgbToDepthFactorX = float(depth.cols) / float(image.cols);
|
||||
rgbToDepthFactorY = float(depth.rows) / float(image.rows);
|
||||
}
|
||||
else if(markerLength_ == 0)
|
||||
{
|
||||
if(depth.empty())
|
||||
{
|
||||
for(std::map<int, MarkerInfo>::iterator iter=subInfo.begin(); iter!=subInfo.end(); ++iter)
|
||||
{
|
||||
std::pair<std::map<int, MarkerInfo>::iterator, bool> inserted = allInfo.insert(*iter);
|
||||
if(!inserted.second)
|
||||
{
|
||||
UWARN("Marker %d already added by another camera, ignoring detection from camera %d", iter->first, i);
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
allInfo.insert(subInfo.begin(), subInfo.end());
|
||||
}
|
||||
if(imageWithDetections)
|
||||
{
|
||||
if(i==0)
|
||||
{
|
||||
*imageWithDetections = cv::Mat(image.size(), subImageWithDetections.type());
|
||||
}
|
||||
if(!subImageWithDetections.empty())
|
||||
{
|
||||
subImageWithDetections.copyTo(cv::Mat(*imageWithDetections, cv::Rect(subRGBWidth*i, 0, subRGBWidth, image.rows)));
|
||||
}
|
||||
UERROR("Depth image is empty, please set %s parameter to non-null.", Parameters::kMarkerLength().c_str());
|
||||
return std::map<int, MarkerInfo>();
|
||||
}
|
||||
}
|
||||
return allInfo;
|
||||
}
|
||||
|
||||
std::map<int, MarkerInfo> MarkerDetector::detect(const cv::Mat & image,
|
||||
const CameraModel & model,
|
||||
const cv::Mat & depth,
|
||||
const std::map<int, float> & markerLengths,
|
||||
cv::Mat * imageWithDetections)
|
||||
{
|
||||
if(!image.empty() && image.cols != model.imageWidth())
|
||||
{
|
||||
UERROR("This method cannot handle multi-camera marker detection, use the other function version supporting it.");
|
||||
return std::map<int, MarkerInfo>();
|
||||
}
|
||||
|
||||
std::map<int, MarkerInfo> detections;
|
||||
|
||||
std::vector< int > ids;
|
||||
std::vector< std::vector< cv::Point2f > > corners, rejected;
|
||||
std::vector< int > cams;
|
||||
std::vector< std::vector< cv::Point2f > > corners; // in stitched image
|
||||
std::vector<Transform> poses;
|
||||
|
||||
// detect markers and estimate pose
|
||||
if(strategy_ == kStrategyApriltag)
|
||||
{
|
||||
#ifdef RTABMAP_APRILTAG
|
||||
// Make an image_u8_t header for the Mat data
|
||||
UASSERT(image.type() == CV_8UC1);
|
||||
image_u8_t im = {image.cols, image.rows, (int)image.step, image.data};
|
||||
|
||||
zarray_t *apriltagDetections = apriltag_detector_detect(((apriltag_detector_t*)apriltagLibDetector_), &im);
|
||||
|
||||
if (errno == EAGAIN) {
|
||||
UFATAL("Unable to create the %d threads requested.", ((apriltag_detector_t*)apriltagLibDetector_)->nthreads);
|
||||
exit(-1);
|
||||
UERROR("Unable to create the %d threads requested.", ((apriltag_detector_t*)apriltagLibDetector_)->nthreads);
|
||||
if(apriltagDetections)
|
||||
{
|
||||
apriltag_detections_destroy(apriltagDetections);
|
||||
}
|
||||
return std::map<int, MarkerInfo>();
|
||||
}
|
||||
|
||||
for (int i = 0; i < zarray_size(apriltagDetections); i++) {
|
||||
std::set<int> idsAdded;
|
||||
for (int i = 0; i < zarray_size(apriltagDetections); i++)
|
||||
{
|
||||
apriltag_detection_t *det;
|
||||
zarray_get(apriltagDetections, i, &det);
|
||||
|
||||
// Which camera?
|
||||
int cameraIndex = int(det->c[0]) / subRGBWidth;
|
||||
UASSERT(cameraIndex>=0 && cameraIndex<(int)models.size());
|
||||
|
||||
if(idsAdded.find(det->id)!=idsAdded.end())
|
||||
{
|
||||
UWARN("Marker %d already added by another camera, ignoring detection from camera %d", det->id, cameraIndex);
|
||||
continue;
|
||||
}
|
||||
|
||||
const CameraModel & model = models[cameraIndex];
|
||||
float offsetX = cameraIndex*subRGBWidth;
|
||||
|
||||
// Convert detection in local camera
|
||||
det->c[0] -= offsetX;
|
||||
for(int i=0; i<4; ++i)
|
||||
{
|
||||
det->p[i][0] -= offsetX;
|
||||
}
|
||||
|
||||
// FIXME, homography needs to be updated
|
||||
|
||||
apriltag_detection_info_t info;
|
||||
info.det = det;
|
||||
info.tagsize = markerLength_<=0.0?1.0f:markerLength_;
|
||||
info.tagsize = 1.0f;
|
||||
info.fx = model.fx();
|
||||
info.fy = model.fy();
|
||||
info.cx = model.cx();
|
||||
@@ -291,20 +281,22 @@ std::map<int, MarkerInfo> MarkerDetector::detect(const cv::Mat & image,
|
||||
double err = estimate_tag_pose(&info, &pose);
|
||||
if (pose.R && pose.t)
|
||||
{
|
||||
Transform t(MATD_EL(pose.R, 0, 0), MATD_EL(pose.R, 0, 1), MATD_EL(pose.R, 0, 2), MATD_EL(pose.t, 0, 0),
|
||||
MATD_EL(pose.R, 1, 0), MATD_EL(pose.R, 1, 1), MATD_EL(pose.R, 1, 2), MATD_EL(pose.t, 1, 0),
|
||||
MATD_EL(pose.R, 2, 0), MATD_EL(pose.R, 2, 1), MATD_EL(pose.R, 2, 2), MATD_EL(pose.t, 2, 0));
|
||||
Transform t(MATD_EL(pose.R, 0, 0), MATD_EL(pose.R, 1, 0), MATD_EL(pose.R, 2, 0), MATD_EL(pose.t, 0, 0),
|
||||
MATD_EL(pose.R, 0, 1), MATD_EL(pose.R, 1, 1), MATD_EL(pose.R, 2, 1), MATD_EL(pose.t, 1, 0),
|
||||
MATD_EL(pose.R, 0, 2), MATD_EL(pose.R, 1, 2), MATD_EL(pose.R, 2, 2), MATD_EL(pose.t, 2, 0));
|
||||
poses.push_back(t);
|
||||
|
||||
corners.push_back(std::vector<cv::Point2f>(4));
|
||||
for(int i=0; i<4; ++i)
|
||||
{
|
||||
corners.back()[i].x = det->p[i][0];
|
||||
// reconvert in original stitched image
|
||||
corners.back()[i].x = det->p[i][0]+offsetX;
|
||||
corners.back()[i].y = det->p[i][1];
|
||||
UDEBUG("Marker %d corner %d : %f %f", det->id, i, corners.back()[i].x, corners.back()[i].y);
|
||||
}
|
||||
ids.push_back(det->id);
|
||||
cams.push_back(cameraIndex);
|
||||
UDEBUG("Add marker %d (err = %f)", det->id, err);
|
||||
idsAdded.insert(det->id);
|
||||
}
|
||||
else
|
||||
{
|
||||
@@ -323,168 +315,206 @@ std::map<int, MarkerInfo> MarkerDetector::detect(const cv::Mat & image,
|
||||
}
|
||||
else // opencv
|
||||
{
|
||||
std::vector< cv::Vec3d > rvecs, tvecs;
|
||||
std::vector< int > cvIds;
|
||||
std::vector< std::vector< cv::Point2f > > cvCorners, cvRejected;
|
||||
#ifdef HAVE_OPENCV_ARUCO
|
||||
#if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION == 3 && CV_MINOR_VERSION >=2)
|
||||
cv::aruco::detectMarkers(image, dictionary_, corners, ids, detectorParams_, rejected);
|
||||
cv::aruco::detectMarkers(image, dictionary_, cvCorners, cvIds, detectorParams_, cvRejected);
|
||||
#else
|
||||
cv::aruco::detectMarkers(image, *dictionary_, corners, ids, *detectorParams_, rejected);
|
||||
cv::aruco::detectMarkers(image, *dictionary_, cvCorners, cvIds, *detectorParams_, cvRejected);
|
||||
#endif
|
||||
cv::aruco::estimatePoseSingleMarkers(corners, markerLength_<=0.0?1.0f:markerLength_, model.K(), model.D(), rvecs, tvecs);
|
||||
UDEBUG("Markers detected=%d rejected=%d", (int)cvIds.size(), (int)cvRejected.size());
|
||||
|
||||
for(size_t i=0; i<ids.size(); ++i)
|
||||
// split detections per camera
|
||||
std::set<int> idsAdded;
|
||||
std::vector< std::vector< std::vector< cv::Point2f > > > cvCornersPerCam(models.size());
|
||||
std::vector< std::vector< int > > cvIdsPerCam(models.size());
|
||||
for(size_t i=0; i<cvIds.size(); ++i)
|
||||
{
|
||||
cv::Mat R;
|
||||
cv::Rodrigues(rvecs[i], R);
|
||||
Transform t(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), tvecs[i].val[0],
|
||||
R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), tvecs[i].val[1],
|
||||
R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvecs[i].val[2]);
|
||||
poses.push_back(t);
|
||||
int id = cvIds[i];
|
||||
|
||||
// Which camera?
|
||||
int cameraIndex = int(cvCorners[i][0].x) / subRGBWidth;
|
||||
UASSERT(cameraIndex>=0 && cameraIndex<(int)models.size());
|
||||
|
||||
if(idsAdded.find(id) != idsAdded.end())
|
||||
{
|
||||
UWARN("Marker %d already added by another camera, ignoring detection from camera %d", id, cameraIndex);
|
||||
continue;
|
||||
}
|
||||
|
||||
float offsetX = cameraIndex*subRGBWidth;
|
||||
for(size_t j=0; j<cvCorners[i].size(); ++j)
|
||||
{
|
||||
cvCorners[i][j].x -= offsetX;
|
||||
}
|
||||
|
||||
cvCornersPerCam[cameraIndex].push_back(cvCorners[i]);
|
||||
cvIdsPerCam[cameraIndex].push_back(id);
|
||||
idsAdded.insert(id);
|
||||
UDEBUG("Detected marker %d on camera %d", id, cameraIndex);
|
||||
}
|
||||
|
||||
for(size_t cam=0; cam < cvCornersPerCam.size(); ++cam)
|
||||
{
|
||||
std::vector< cv::Vec3d > rvecs, tvecs;
|
||||
const CameraModel & model = models[cam];
|
||||
cv::aruco::estimatePoseSingleMarkers(cvCornersPerCam[cam], 1.0f, model.K(), model.D(), rvecs, tvecs);
|
||||
float offsetX = cam*subRGBWidth;
|
||||
for(size_t i=0; i<cvIdsPerCam[cam].size(); ++i)
|
||||
{
|
||||
cv::Mat R;
|
||||
cv::Rodrigues(rvecs[i], R);
|
||||
Transform t(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), tvecs[i].val[0],
|
||||
R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), tvecs[i].val[1],
|
||||
R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvecs[i].val[2]);
|
||||
poses.push_back(t);
|
||||
ids.push_back(cvIdsPerCam[cam][i]);
|
||||
cams.push_back(cam);
|
||||
// reconvert in original stitched image
|
||||
for(size_t j=0; j<cvCornersPerCam[cam][i].size(); ++j)
|
||||
{
|
||||
cvCornersPerCam[cam][i][j].x += offsetX;
|
||||
}
|
||||
corners.push_back(cvCornersPerCam[cam][i]);
|
||||
}
|
||||
}
|
||||
#else
|
||||
UERROR("RTAB-Map is not built with \"aruco\" module from OpenCV.");
|
||||
#endif
|
||||
}
|
||||
|
||||
UDEBUG("Markers detected=%d rejected=%d", (int)ids.size(), (int)rejected.size());
|
||||
if(ids.size() > 0)
|
||||
// Estimate scale and fill up output detections
|
||||
std::vector<float> scales;
|
||||
std::map<int, MarkerInfo> detections;
|
||||
for(size_t i=0; i<ids.size(); ++i)
|
||||
{
|
||||
float rgbToDepthFactorX = 1.0f;
|
||||
float rgbToDepthFactorY = 1.0f;
|
||||
if(!depth.empty())
|
||||
float length = 0.0f;
|
||||
std::map<int, float>::const_iterator findIter = markerLengths.find(ids[i]);
|
||||
if(!depth.empty() && (markerLength_ == 0 || (markerLength_<0 && findIter==markerLengths.end())))
|
||||
{
|
||||
rgbToDepthFactorX = 1.0f/(model.imageWidth()>0?float(model.imageWidth())/float(depth.cols):1.0f);
|
||||
rgbToDepthFactorY = 1.0f/(model.imageHeight()>0?float(model.imageHeight())/float(depth.rows):1.0f);
|
||||
}
|
||||
else if(markerLength_ == 0)
|
||||
{
|
||||
if(depth.empty())
|
||||
{
|
||||
UERROR("Depth image is empty, please set %s parameter to non-null.", Parameters::kMarkerLength().c_str());
|
||||
return detections;
|
||||
}
|
||||
}
|
||||
|
||||
std::vector<float> scales;
|
||||
for(size_t i=0; i<ids.size(); ++i)
|
||||
{
|
||||
float length = 0.0f;
|
||||
std::map<int, float>::const_iterator findIter = markerLengths.find(ids[i]);
|
||||
if(!depth.empty() && (markerLength_ == 0 || (markerLength_<0 && findIter==markerLengths.end())))
|
||||
float d = util2d::getDepth(depth, (corners[i][0].x + (corners[i][2].x-corners[i][0].x)/2.0f)*rgbToDepthFactorX, (corners[i][0].y + (corners[i][2].y-corners[i][0].y)/2.0f)*rgbToDepthFactorY, true, 0.02f, true);
|
||||
float d1 = util2d::getDepth(depth, corners[i][0].x*rgbToDepthFactorX, corners[i][0].y*rgbToDepthFactorY, true, 0.02f, true);
|
||||
float d2 = util2d::getDepth(depth, corners[i][1].x*rgbToDepthFactorX, corners[i][1].y*rgbToDepthFactorY, true, 0.02f, true);
|
||||
float d3 = util2d::getDepth(depth, corners[i][2].x*rgbToDepthFactorX, corners[i][2].y*rgbToDepthFactorY, true, 0.02f, true);
|
||||
float d4 = util2d::getDepth(depth, corners[i][3].x*rgbToDepthFactorX, corners[i][3].y*rgbToDepthFactorY, true, 0.02f, true);
|
||||
// Accept measurement only if all 4 depth values are valid and
|
||||
// they are at the same depth (camera should be perpendicular to marker for
|
||||
// best depth estimation)
|
||||
if(d>0 && d1>0 && d2>0 && d3>0 && d4>0)
|
||||
{
|
||||
float d = util2d::getDepth(depth, (corners[i][0].x + (corners[i][2].x-corners[i][0].x)/2.0f)*rgbToDepthFactorX, (corners[i][0].y + (corners[i][2].y-corners[i][0].y)/2.0f)*rgbToDepthFactorY, true, 0.02f, true);
|
||||
float d1 = util2d::getDepth(depth, corners[i][0].x*rgbToDepthFactorX, corners[i][0].y*rgbToDepthFactorY, true, 0.02f, true);
|
||||
float d2 = util2d::getDepth(depth, corners[i][1].x*rgbToDepthFactorX, corners[i][1].y*rgbToDepthFactorY, true, 0.02f, true);
|
||||
float d3 = util2d::getDepth(depth, corners[i][2].x*rgbToDepthFactorX, corners[i][2].y*rgbToDepthFactorY, true, 0.02f, true);
|
||||
float d4 = util2d::getDepth(depth, corners[i][3].x*rgbToDepthFactorX, corners[i][3].y*rgbToDepthFactorY, true, 0.02f, true);
|
||||
// Accept measurement only if all 4 depth values are valid and
|
||||
// they are at the same depth (camera should be perpendicular to marker for
|
||||
// best depth estimation)
|
||||
if(d>0 && d1>0 && d2>0 && d3>0 && d4>0)
|
||||
float scale = d / poses[i].z();
|
||||
|
||||
if( fabs(d-d1) < maxDepthError_ &&
|
||||
fabs(d-d2) < maxDepthError_ &&
|
||||
fabs(d-d3) < maxDepthError_ &&
|
||||
fabs(d-d4) < maxDepthError_)
|
||||
{
|
||||
float scale = d / poses[i].z();
|
||||
|
||||
if( fabs(d-d1) < maxDepthError_ &&
|
||||
fabs(d-d2) < maxDepthError_ &&
|
||||
fabs(d-d3) < maxDepthError_ &&
|
||||
fabs(d-d4) < maxDepthError_)
|
||||
{
|
||||
length = scale;
|
||||
scales.push_back(length);
|
||||
poses[i].x() *= length;
|
||||
poses[i].y() *= length;
|
||||
poses[i].z() *= length;
|
||||
UWARN("Automatic marker length estimation: id=%d depth=%fm length=%fm", ids[i], d, length);
|
||||
}
|
||||
else
|
||||
{
|
||||
UWARN("The four marker's corners should be "
|
||||
"perpendicular to camera to estimate correctly "
|
||||
"the marker's length. Errors: %f, %f, %f > %fm (%s). Four corners: %f %f %f %f (middle=%f). "
|
||||
"Parameter %s can be set to non-null to skip automatic "
|
||||
"marker length estimation. Detections are ignored.",
|
||||
fabs(d1-d2), fabs(d1-d3), fabs(d1-d4), maxDepthError_, Parameters::kMarkerMaxDepthError().c_str(),
|
||||
d1, d2, d3, d4, d,
|
||||
Parameters::kMarkerLength().c_str());
|
||||
continue;
|
||||
}
|
||||
length = scale;
|
||||
scales.push_back(length);
|
||||
UWARN("Automatic marker length estimation: id=%d depth=%fm length=%fm", ids[i], d, length);
|
||||
}
|
||||
else
|
||||
{
|
||||
UWARN("Some depth values (%f,%f,%f,%f, middle=%f) cannot be detected on the "
|
||||
"marker's corners, cannot initialize marker length. "
|
||||
"Parameter %s can be set to non-null to skip automatic "
|
||||
"marker length estimation. Detections are ignored.",
|
||||
d1,d2,d3,d4,d,
|
||||
Parameters::kMarkerLength().c_str());
|
||||
continue;
|
||||
UWARN("The four marker's corners should be "
|
||||
"perpendicular to camera to estimate correctly "
|
||||
"the marker's length. Errors: %f, %f, %f > %fm (%s). Four corners: %f %f %f %f (middle=%f). "
|
||||
"Parameter %s can be set to non-null to skip automatic "
|
||||
"marker length estimation. Detections are ignored.",
|
||||
fabs(d1-d2), fabs(d1-d3), fabs(d1-d4), maxDepthError_, Parameters::kMarkerMaxDepthError().c_str(),
|
||||
d1, d2, d3, d4, d,
|
||||
Parameters::kMarkerLength().c_str());
|
||||
continue;
|
||||
}
|
||||
}
|
||||
else if(markerLength_ < 0)
|
||||
{
|
||||
if(findIter!=markerLengths.end())
|
||||
{
|
||||
length = findIter->second;
|
||||
poses[i].x() *= length;
|
||||
poses[i].y() *= length;
|
||||
poses[i].z() *= length;
|
||||
}
|
||||
else
|
||||
{
|
||||
UWARN("Cannot find marker length for marker %d, ignoring this marker (count=%d)", ids[i], (int)markerLengths.size());
|
||||
continue;
|
||||
}
|
||||
}
|
||||
else if(markerLength_ > 0)
|
||||
{
|
||||
length = markerLength_;
|
||||
}
|
||||
else
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
// Limit the detection range to be between the min / max range.
|
||||
// If the ranges are -1, allow any detection within that direction.
|
||||
if((maxRange_ <= 0 || poses[i].z() < maxRange_) &&
|
||||
(minRange_ <= 0 || poses[i].z() > minRange_))
|
||||
else
|
||||
{
|
||||
Transform pose = model.localTransform() * poses[i];
|
||||
detections.insert(std::make_pair(ids[i], MarkerInfo(ids[i], length, pose)));
|
||||
UDEBUG("Marker %d detected in base_link: %s, optical_link=%s, local transform=%s",
|
||||
ids[i], pose.prettyPrint().c_str(), poses[i].prettyPrint().c_str(), model.localTransform().prettyPrint().c_str());
|
||||
UWARN("Some depth values (%f,%f,%f,%f, middle=%f) cannot be detected on the "
|
||||
"marker's corners, cannot initialize marker length. "
|
||||
"Parameter %s can be set to non-null to skip automatic "
|
||||
"marker length estimation. Detections are ignored.",
|
||||
d1,d2,d3,d4,d,
|
||||
Parameters::kMarkerLength().c_str());
|
||||
continue;
|
||||
}
|
||||
}
|
||||
if(markerLength_ == 0 && !scales.empty())
|
||||
else if(markerLength_ < 0)
|
||||
{
|
||||
float sum = 0.0f;
|
||||
float maxError = 0.0f;
|
||||
for(size_t i=0; i<scales.size(); ++i)
|
||||
if(findIter!=markerLengths.end())
|
||||
{
|
||||
if(i>0)
|
||||
{
|
||||
float error = fabs(scales[i]-scales[0]);
|
||||
if(error > 0.001f)
|
||||
{
|
||||
UWARN("The marker's length detected between 2 of the "
|
||||
"markers doesn't match (%fm vs %fm)."
|
||||
"Parameter %s can be set to non-null to skip automatic "
|
||||
"marker length estimation. Detections are ignored.",
|
||||
scales[i], scales[0],
|
||||
Parameters::kMarkerLength().c_str());
|
||||
detections.clear();
|
||||
return detections;
|
||||
}
|
||||
if(error > maxError)
|
||||
{
|
||||
maxError = error;
|
||||
}
|
||||
}
|
||||
sum += scales[i];
|
||||
length = findIter->second;
|
||||
}
|
||||
else
|
||||
{
|
||||
UWARN("Cannot find marker length for marker %d, ignoring this marker (count=%d)", ids[i], (int)markerLengths.size());
|
||||
continue;
|
||||
}
|
||||
markerLength_ = sum/float(scales.size());
|
||||
UWARN("Final marker length estimated = %fm, max error=%fm (used for subsequent detections)", markerLength_, maxError);
|
||||
}
|
||||
else if(markerLength_ > 0)
|
||||
{
|
||||
length = markerLength_;
|
||||
}
|
||||
else
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
UASSERT(length > 0.0f);
|
||||
poses[i].x() *= length;
|
||||
poses[i].y() *= length;
|
||||
poses[i].z() *= length;
|
||||
|
||||
// Limit the detection range to be between the min / max range.
|
||||
// If the ranges are -1, allow any detection within that direction.
|
||||
if((maxRange_ <= 0 || poses[i].z() < maxRange_) &&
|
||||
(minRange_ <= 0 || poses[i].z() > minRange_))
|
||||
{
|
||||
Transform pose = models[cams[i]].localTransform() * poses[i];
|
||||
detections.insert(std::make_pair(ids[i], MarkerInfo(ids[i], length, pose)));
|
||||
UDEBUG("Marker %d detected in base_link: %s, optical_link=%s, local transform=%s",
|
||||
ids[i],
|
||||
pose.prettyPrint().c_str(),
|
||||
poses[i].prettyPrint().c_str(),
|
||||
models[cams[i]].localTransform().prettyPrint().c_str());
|
||||
}
|
||||
else
|
||||
{
|
||||
UDEBUG("Filtered marker %d by distance (min=%f max=%f value=%f) from the camera %d",
|
||||
ids[i], minRange_, maxRange_, poses[i].z(), cams[i]);
|
||||
}
|
||||
}
|
||||
if(markerLength_ == 0 && !scales.empty())
|
||||
{
|
||||
float sum = 0.0f;
|
||||
float maxError = 0.0f;
|
||||
for(size_t i=0; i<scales.size(); ++i)
|
||||
{
|
||||
if(i>0)
|
||||
{
|
||||
float error = fabs(scales[i]-scales[0]);
|
||||
if(error > 0.001f)
|
||||
{
|
||||
UWARN("The marker's length detected between 2 of the "
|
||||
"markers doesn't match (%fm vs %fm)."
|
||||
"Parameter %s can be set to non-null to skip automatic "
|
||||
"marker length estimation or set to a negative value to "
|
||||
"estimate length for each marker. The current %ld detections "
|
||||
"are ignored!",
|
||||
scales[i], scales[0],
|
||||
Parameters::kMarkerLength().c_str(),
|
||||
detections.size());
|
||||
detections.clear();
|
||||
return detections;
|
||||
}
|
||||
if(error > maxError)
|
||||
{
|
||||
maxError = error;
|
||||
}
|
||||
}
|
||||
sum += scales[i];
|
||||
}
|
||||
markerLength_ = sum/float(scales.size());
|
||||
UWARN("Final marker length estimated = %fm, max error=%fm (used for subsequent detections)", markerLength_, maxError);
|
||||
}
|
||||
|
||||
if(imageWithDetections)
|
||||
@@ -507,13 +537,16 @@ std::map<int, MarkerInfo> MarkerDetector::detect(const cv::Mat & image,
|
||||
std::map<int, MarkerInfo>::iterator iter = detections.find(ids[i]);
|
||||
if(iter!=detections.end())
|
||||
{
|
||||
int cam = cams[i];
|
||||
const CameraModel & model = models[cam];
|
||||
cv::Mat subImage(*imageWithDetections, cv::Rect(subRGBWidth*i, 0, subRGBWidth, imageWithDetections->rows));
|
||||
cv::Vec3d rvec;
|
||||
cv::Vec3d tvec(poses[i].x(), poses[i].y(), poses[i].z());
|
||||
cv::Rodrigues(poses[i].rotationMatrix(), rvec);
|
||||
#if CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && (CV_MINOR_VERSION >1 || (CV_MINOR_VERSION==1 && CV_PATCH_VERSION>=1)))
|
||||
cv::drawFrameAxes(*imageWithDetections, model.K(), model.D(), rvec, tvec, iter->second.length() * 0.5f);
|
||||
cv::drawFrameAxes(subImage, model.K(), model.D(), rvec, tvec, iter->second.length() * 0.5f);
|
||||
#else
|
||||
cv::aruco::drawAxis(*imageWithDetections, model.K(), model.D(), rvec, tvec, iter->second.length() * 0.5f);
|
||||
cv::aruco::drawAxis(subImage, model.K(), model.D(), rvec, tvec, iter->second.length() * 0.5f);
|
||||
#endif
|
||||
}
|
||||
}
|
||||
@@ -526,5 +559,23 @@ std::map<int, MarkerInfo> MarkerDetector::detect(const cv::Mat & image,
|
||||
return detections;
|
||||
}
|
||||
|
||||
std::map<int, MarkerInfo> MarkerDetector::detect(const cv::Mat & image,
|
||||
const CameraModel & model,
|
||||
const cv::Mat & depth,
|
||||
const std::map<int, float> & markerLengths,
|
||||
cv::Mat * imageWithDetections)
|
||||
{
|
||||
if(!image.empty() && image.cols != model.imageWidth())
|
||||
{
|
||||
UERROR("This method cannot handle multi-camera marker detection, use the other function version supporting it.");
|
||||
return std::map<int, MarkerInfo>();
|
||||
}
|
||||
|
||||
std::vector<CameraModel> models;
|
||||
models.push_back(model);
|
||||
|
||||
return detect(image, models, depth, markerLengths, imageWithDetections);
|
||||
}
|
||||
|
||||
|
||||
} /* namespace rtabmap */
|
||||
|
||||
Reference in New Issue
Block a user