mirror of
https://github.com/introlab/rtabmap.git
synced 2026-10-06 01:57:45 +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
+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