i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-06 10:07:47 +08:00
Code Issues Packages Projects Releases Wiki Activity

implement multi camera marker detection #898 (#899)

Browse Source 
* Added multicamera marker detection support

* Fixed marker detection on camera index> 0

Co-authored-by: mathieu86 <[email protected]>
This commit is contained in:
matlabbe
2022-09-14 17:13:59 -07:00
committed by GitHub
co-authored by mathieu86
parent adfb250d4e
commit 4d6bc78e3d
3 changed files with 96 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/MarkerDetector.cpp
+59 -1
View File
@@ -130,12 +130,70 @@ std::map<int, Transform> MarkerDetector::detect(const cv::Mat & image, const Cam
return detections;
}
std::map<int, MarkerInfo> MarkerDetector::detect(const cv::Mat & image,
const std::vector<CameraModel> & models,
const cv::Mat & depth,
const std::map<int, float> & markerLengths,
cv::Mat * imageWithDetections)
{
UASSERT(!models.empty() && !image.empty());
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)
{
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)
{
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 = image.clone();
}
if(!subImageWithDetections.empty())
{
subImageWithDetections.copyTo(cv::Mat(*imageWithDetections, cv::Rect(subRGBWidth*i, 0, subRGBWidth, image.rows)));
}
}
}
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;
#ifdef HAVE_OPENCV_ARUCO
@@ -257,7 +315,7 @@ std::map<int, MarkerInfo> MarkerDetector::detect(const cv::Mat & image,
R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvecs[i].val[2]);
Transform pose = model.localTransform() * t;
detections.insert(std::make_pair(ids[i], MarkerInfo(ids[i], length, pose)));
UDEBUG("Marker %d detected at %s (%s)", ids[i], pose.prettyPrint().c_str(), t.prettyPrint().c_str());
UDEBUG("Marker %d detected in base_link: %s, optical_link=%s, local transform=%s", ids[i], pose.prettyPrint().c_str(), t.prettyPrint().c_str(), model.localTransform().prettyPrint().c_str());
}
}
if(markerLength_ == 0 && !scales.empty())