i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 09:30:25 +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 <mathieu@robust.ai>
This commit is contained in:
matlabbe
2022-09-14 17:13:59 -07:00
committed by GitHub
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

60
corelib/src/MarkerDetector.cpp
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())

61
corelib/src/Memory.cpp
View File

@@ -5309,52 +5309,37 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
UDEBUG("Detecting markers...");
if(landmarks.empty())
{
std::map<int, MarkerInfo> markers;
if(!data.cameraModels().empty() && data.cameraModels()[0].isValidForProjection())
std::vector<CameraModel> models = data.cameraModels();
if(models.empty())
{
if(data.cameraModels().size() > 1)
for(size_t i=0; i<data.stereoCameraModels().size(); ++i)
{
static bool warned = false;
if(!warned)
models.push_back(data.stereoCameraModels()[i].left());
}
}
if(!models.empty() && models[0].isValidForProjection())
{
std::map<int, MarkerInfo> markers = _markerDetector->detect(data.imageRaw(), models, data.depthRaw(), _landmarksSize);
for(std::map<int, MarkerInfo>::iterator iter=markers.begin(); iter!=markers.end(); ++iter)
{
if(iter->first <= 0)
{
UWARN("Detecting markers in multi-camera setup is not yet implemented, aborting marker detection. This message is only printed once.");
UERROR("Invalid marker received! IDs should be > 0 (it is %d). Ignoring this marker.", iter->first);
continue;
}
warned = true;
}
else
{
markers = _markerDetector->detect(data.imageRaw(), data.cameraModels()[0], data.depthRaw(), _landmarksSize);
cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
covariance(cv::Range(0,3), cv::Range(0,3)) *= _markerLinVariance;
covariance(cv::Range(3,6), cv::Range(3,6)) *= _markerAngVariance;
landmarks.insert(std::make_pair(iter->first, Landmark(iter->first, iter->second.length(), iter->second.pose(), covariance)));
}
UDEBUG("Markers detected = %d", (int)markers.size());
}
else if(!data.stereoCameraModels().empty() && data.stereoCameraModels()[0].isValidForProjection())
else
{
if(data.stereoCameraModels().size() > 1)
{
static bool warned = false;
if(!warned)
{
UWARN("Detecting markers in multi-camera setup is not yet implemented, aborting marker detection. This message is only printed once.");
}
warned = true;
}
else
{
markers = _markerDetector->detect(data.imageRaw(), data.stereoCameraModels()[0].left(), cv::Mat(), _landmarksSize);
}
UWARN("No valid camera calibration for marker detection");
}
for(std::map<int, MarkerInfo>::iterator iter=markers.begin(); iter!=markers.end(); ++iter)
{
if(iter->first <= 0)
{
UERROR("Invalid marker received! IDs should be > 0 (it is %d). Ignoring this marker.", iter->first);
continue;
}
cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
covariance(cv::Range(0,3), cv::Range(0,3)) *= _markerLinVariance;
covariance(cv::Range(3,6), cv::Range(3,6)) *= _markerAngVariance;
landmarks.insert(std::make_pair(iter->first, Landmark(iter->first, iter->second.length(), iter->second.pose(), covariance)));
}
UDEBUG("Markers detected = %d", (int)markers.size());
}
else
{