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

Merge branch 'master' of github.com:introlab/rtabmap into gtest

Browse Source
This commit is contained in:
matlabbe
2026-06-07 13:20:26 -07:00
parent 1702bb07ba 468f6e6f28
commit 41ed101462
4 changed files with 185 additions and 136 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Features2d.cpp
+50 -2
View File
@@ -2774,7 +2774,31 @@ cv::Mat SuperPointTorch::generateDescriptorsImpl(const cv::Mat & image, std::vec
{ {
#ifdef RTABMAP_TORCH #ifdef RTABMAP_TORCH
UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U); UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
return superPoint_->compute(keypoints); cv::Mat descriptors;
if(!keypoints.empty())
{
descriptors = superPoint_->compute(keypoints);
if(descriptors.empty())
{
// superpoint may have been reset between keypoint detection and now,
// re-detect features to re-inialize the descriptors matrix, then
// re-extract descriptors with original keypoints.
UWARN("Re-initializing superpoint on that image to extract descriptors");
if(!superPoint_->detect(image).empty())
{
descriptors = superPoint_->compute(keypoints);
if(descriptors.rows == (int)keypoints.size())
{
UWARN("Sucessfully re-initialized superpoint, returning %d descriptors.", descriptors.rows);
}
}
else
{
UWARN("Failed to re-initialize superpoint on that image, returning empty descriptors.");
}
}
}
return descriptors;
#else #else
UWARN("RTAB-Map is not built with Torch support so SuperPoint Torch feature cannot be used!"); UWARN("RTAB-Map is not built with Torch support so SuperPoint Torch feature cannot be used!");
return cv::Mat(); return cv::Mat();
@@ -2885,7 +2909,31 @@ cv::Mat SuperPointRpautrat::generateDescriptorsImpl(const cv::Mat & image, std::
{ {
#if defined(RTABMAP_TORCH) && defined(RTABMAP_PYTHON) #if defined(RTABMAP_TORCH) && defined(RTABMAP_PYTHON)
UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U); UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
return superPoint_->compute(keypoints); cv::Mat descriptors;
if(!keypoints.empty())
{
descriptors = superPoint_->compute(keypoints);
if(descriptors.empty())
{
// superpoint may have been reset between keypoint detection and now,
// re-detect features to re-inialize the descriptors matrix, then
// re-extract descriptors with original keypoints.
UWARN("Re-initializing superpoint on that image to extract descriptors");
if(!superPoint_->detect(image).empty())
{
descriptors = superPoint_->compute(keypoints);
if(descriptors.rows == (int)keypoints.size())
{
UWARN("Sucessfully re-initialized superpoint, returning %d descriptors.", descriptors.rows);
}
}
else
{
UWARN("Failed to re-initialize superpoint on that image, returning empty descriptors.");
}
}
}
return descriptors;
#else #else
UWARN("RTAB-Map is not built with Torch support so SuperPoint Rpautrat feature cannot be used!"); UWARN("RTAB-Map is not built with Torch support so SuperPoint Rpautrat feature cannot be used!");
return cv::Mat(); return cv::Mat();
corelib/src/Memory.cpp
+125 -132
View File
@@ -5213,6 +5213,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
// is using less features than feature2D->getMaxFeatures() // is using less features than feature2D->getMaxFeatures()
meanWordsPerLocation = 0; meanWordsPerLocation = 0;
} }
UDEBUG("ratio=%f, meanWordsPerLocation=%d", _badSignRatio, meanWordsPerLocation);
if(_parallelized && !isIntermediateNode) if(_parallelized && !isIntermediateNode)
{ {
@@ -5482,41 +5483,90 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
if(stats) stats->addStatistic(Statistics::kTimingMemDescriptors_extraction(), t*1000.0f); if(stats) stats->addStatistic(Statistics::kTimingMemDescriptors_extraction(), t*1000.0f);
UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t); UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t);
UDEBUG("ratio=%f, meanWordsPerLocation=%d", _badSignRatio, meanWordsPerLocation); if(!imagesRectified && decimatedData.cameraModels().size())
if(descriptors.rows && descriptors.rows < _badSignRatio * float(meanWordsPerLocation))
{ {
descriptors = cv::Mat(); UASSERT_MSG((int)keypoints.size() == descriptors.rows, uFormat("%d vs %d", (int)keypoints.size(), descriptors.rows).c_str());
} std::vector<cv::KeyPoint> keypointsValid;
else keypointsValid.reserve(keypoints.size());
{ cv::Mat descriptorsValid;
if(!imagesRectified && decimatedData.cameraModels().size()) descriptorsValid.reserve(descriptors.rows);
{
UASSERT_MSG((int)keypoints.size() == descriptors.rows, uFormat("%d vs %d", (int)keypoints.size(), descriptors.rows).c_str());
std::vector<cv::KeyPoint> keypointsValid;
keypointsValid.reserve(keypoints.size());
cv::Mat descriptorsValid;
descriptorsValid.reserve(descriptors.rows);
//undistort keypoints before projection (RGB-D) //undistort keypoints before projection (RGB-D)
if(decimatedData.cameraModels().size() == 1) if(decimatedData.cameraModels().size() == 1)
{
std::vector<cv::Point2f> pointsIn, pointsOut;
cv::KeyPoint::convert(keypoints,pointsIn);
if(decimatedData.cameraModels()[0].D_raw().cols == 6)
{ {
#if CV_MAJOR_VERSION > 2 or (CV_MAJOR_VERSION == 2 and (CV_MINOR_VERSION >4 or (CV_MINOR_VERSION == 4 and CV_SUBMINOR_VERSION >=10)))
// Equidistant / FishEye
// get only k parameters (k1,k2,p1,p2,k3,k4)
cv::Mat D(1, 4, CV_64FC1);
D.at<double>(0,0) = decimatedData.cameraModels()[0].D_raw().at<double>(0,0);
D.at<double>(0,1) = decimatedData.cameraModels()[0].D_raw().at<double>(0,1);
D.at<double>(0,2) = decimatedData.cameraModels()[0].D_raw().at<double>(0,4);
D.at<double>(0,3) = decimatedData.cameraModels()[0].D_raw().at<double>(0,5);
cv::fisheye::undistortPoints(pointsIn, pointsOut,
decimatedData.cameraModels()[0].K_raw(),
D,
decimatedData.cameraModels()[0].R(),
decimatedData.cameraModels()[0].P());
}
else
#else
UWARN("Too old opencv version (%d,%d,%d) to support fisheye model (min 2.4.10 required)!",
CV_MAJOR_VERSION, CV_MINOR_VERSION, CV_SUBMINOR_VERSION);
}
#endif
{
//RadialTangential
cv::undistortPoints(pointsIn, pointsOut,
decimatedData.cameraModels()[0].K_raw(),
decimatedData.cameraModels()[0].D_raw(),
decimatedData.cameraModels()[0].R(),
decimatedData.cameraModels()[0].P());
}
UASSERT(pointsOut.size() == keypoints.size());
for(unsigned int i=0; i<pointsOut.size(); ++i)
{
if(pointsOut.at(i).x>=0 && pointsOut.at(i).x<decimatedData.cameraModels()[0].imageWidth() &&
pointsOut.at(i).y>=0 && pointsOut.at(i).y<decimatedData.cameraModels()[0].imageHeight())
{
keypointsValid.push_back(keypoints.at(i));
keypointsValid.back().pt.x = pointsOut.at(i).x;
keypointsValid.back().pt.y = pointsOut.at(i).y;
descriptorsValid.push_back(descriptors.row(i));
}
}
}
else
{
UASSERT(int((decimatedData.imageRaw().cols/decimatedData.cameraModels().size())*decimatedData.cameraModels().size()) == decimatedData.imageRaw().cols);
float subImageWidth = decimatedData.imageRaw().cols/decimatedData.cameraModels().size();
for(unsigned int i=0; i<keypoints.size(); ++i)
{
int cameraIndex = int(keypoints.at(i).pt.x / subImageWidth);
UASSERT_MSG(cameraIndex >= 0 && cameraIndex < (int)decimatedData.cameraModels().size(),
uFormat("cameraIndex=%d, models=%d, kpt.x=%f, subImageWidth=%f (Camera model image width=%d)",
cameraIndex, (int)decimatedData.cameraModels().size(), keypoints[i].pt.x, subImageWidth, decimatedData.cameraModels()[0].imageWidth()).c_str());
std::vector<cv::Point2f> pointsIn, pointsOut; std::vector<cv::Point2f> pointsIn, pointsOut;
cv::KeyPoint::convert(keypoints,pointsIn); pointsIn.push_back(cv::Point2f(keypoints.at(i).pt.x-subImageWidth*cameraIndex, keypoints.at(i).pt.y));
if(decimatedData.cameraModels()[0].D_raw().cols == 6) if(decimatedData.cameraModels()[cameraIndex].D_raw().cols == 6)
{ {
#if CV_MAJOR_VERSION > 2 or (CV_MAJOR_VERSION == 2 and (CV_MINOR_VERSION >4 or (CV_MINOR_VERSION == 4 and CV_SUBMINOR_VERSION >=10))) #if CV_MAJOR_VERSION > 2 or (CV_MAJOR_VERSION == 2 and (CV_MINOR_VERSION >4 or (CV_MINOR_VERSION == 4 and CV_SUBMINOR_VERSION >=10)))
// Equidistant / FishEye // Equidistant / FishEye
// get only k parameters (k1,k2,p1,p2,k3,k4) // get only k parameters (k1,k2,p1,p2,k3,k4)
cv::Mat D(1, 4, CV_64FC1); cv::Mat D(1, 4, CV_64FC1);
D.at<double>(0,0) = decimatedData.cameraModels()[0].D_raw().at<double>(0,0); D.at<double>(0,0) = decimatedData.cameraModels()[cameraIndex].D_raw().at<double>(0,0);
D.at<double>(0,1) = decimatedData.cameraModels()[0].D_raw().at<double>(0,1); D.at<double>(0,1) = decimatedData.cameraModels()[cameraIndex].D_raw().at<double>(0,1);
D.at<double>(0,2) = decimatedData.cameraModels()[0].D_raw().at<double>(0,4); D.at<double>(0,2) = decimatedData.cameraModels()[cameraIndex].D_raw().at<double>(0,4);
D.at<double>(0,3) = decimatedData.cameraModels()[0].D_raw().at<double>(0,5); D.at<double>(0,3) = decimatedData.cameraModels()[cameraIndex].D_raw().at<double>(0,5);
cv::fisheye::undistortPoints(pointsIn, pointsOut, cv::fisheye::undistortPoints(pointsIn, pointsOut,
decimatedData.cameraModels()[0].K_raw(), decimatedData.cameraModels()[cameraIndex].K_raw(),
D, D,
decimatedData.cameraModels()[0].R(), decimatedData.cameraModels()[cameraIndex].R(),
decimatedData.cameraModels()[0].P()); decimatedData.cameraModels()[cameraIndex].P());
} }
else else
#else #else
@@ -5527,115 +5577,58 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
{ {
//RadialTangential //RadialTangential
cv::undistortPoints(pointsIn, pointsOut, cv::undistortPoints(pointsIn, pointsOut,
decimatedData.cameraModels()[0].K_raw(), decimatedData.cameraModels()[cameraIndex].K_raw(),
decimatedData.cameraModels()[0].D_raw(), decimatedData.cameraModels()[cameraIndex].D_raw(),
decimatedData.cameraModels()[0].R(), decimatedData.cameraModels()[cameraIndex].R(),
decimatedData.cameraModels()[0].P()); decimatedData.cameraModels()[cameraIndex].P());
} }
UASSERT(pointsOut.size() == keypoints.size());
for(unsigned int i=0; i<pointsOut.size(); ++i) if(pointsOut[0].x>=0 && pointsOut[0].x<decimatedData.cameraModels()[cameraIndex].imageWidth() &&
pointsOut[0].y>=0 && pointsOut[0].y<decimatedData.cameraModels()[cameraIndex].imageHeight())
{ {
if(pointsOut.at(i).x>=0 && pointsOut.at(i).x<decimatedData.cameraModels()[0].imageWidth() && keypointsValid.push_back(keypoints.at(i));
pointsOut.at(i).y>=0 && pointsOut.at(i).y<decimatedData.cameraModels()[0].imageHeight()) keypointsValid.back().pt.x = pointsOut[0].x + subImageWidth*cameraIndex;
{ keypointsValid.back().pt.y = pointsOut[0].y;
keypointsValid.push_back(keypoints.at(i)); descriptorsValid.push_back(descriptors.row(i));
keypointsValid.back().pt.x = pointsOut.at(i).x;
keypointsValid.back().pt.y = pointsOut.at(i).y;
descriptorsValid.push_back(descriptors.row(i));
}
} }
} }
else
{
UASSERT(int((decimatedData.imageRaw().cols/decimatedData.cameraModels().size())*decimatedData.cameraModels().size()) == decimatedData.imageRaw().cols);
float subImageWidth = decimatedData.imageRaw().cols/decimatedData.cameraModels().size();
for(unsigned int i=0; i<keypoints.size(); ++i)
{
int cameraIndex = int(keypoints.at(i).pt.x / subImageWidth);
UASSERT_MSG(cameraIndex >= 0 && cameraIndex < (int)decimatedData.cameraModels().size(),
uFormat("cameraIndex=%d, models=%d, kpt.x=%f, subImageWidth=%f (Camera model image width=%d)",
cameraIndex, (int)decimatedData.cameraModels().size(), keypoints[i].pt.x, subImageWidth, decimatedData.cameraModels()[0].imageWidth()).c_str());
std::vector<cv::Point2f> pointsIn, pointsOut;
pointsIn.push_back(cv::Point2f(keypoints.at(i).pt.x-subImageWidth*cameraIndex, keypoints.at(i).pt.y));
if(decimatedData.cameraModels()[cameraIndex].D_raw().cols == 6)
{
#if CV_MAJOR_VERSION > 2 or (CV_MAJOR_VERSION == 2 and (CV_MINOR_VERSION >4 or (CV_MINOR_VERSION == 4 and CV_SUBMINOR_VERSION >=10)))
// Equidistant / FishEye
// get only k parameters (k1,k2,p1,p2,k3,k4)
cv::Mat D(1, 4, CV_64FC1);
D.at<double>(0,0) = decimatedData.cameraModels()[cameraIndex].D_raw().at<double>(0,0);
D.at<double>(0,1) = decimatedData.cameraModels()[cameraIndex].D_raw().at<double>(0,1);
D.at<double>(0,2) = decimatedData.cameraModels()[cameraIndex].D_raw().at<double>(0,4);
D.at<double>(0,3) = decimatedData.cameraModels()[cameraIndex].D_raw().at<double>(0,5);
cv::fisheye::undistortPoints(pointsIn, pointsOut,
decimatedData.cameraModels()[cameraIndex].K_raw(),
D,
decimatedData.cameraModels()[cameraIndex].R(),
decimatedData.cameraModels()[cameraIndex].P());
}
else
#else
UWARN("Too old opencv version (%d,%d,%d) to support fisheye model (min 2.4.10 required)!",
CV_MAJOR_VERSION, CV_MINOR_VERSION, CV_SUBMINOR_VERSION);
}
#endif
{
//RadialTangential
cv::undistortPoints(pointsIn, pointsOut,
decimatedData.cameraModels()[cameraIndex].K_raw(),
decimatedData.cameraModels()[cameraIndex].D_raw(),
decimatedData.cameraModels()[cameraIndex].R(),
decimatedData.cameraModels()[cameraIndex].P());
}
if(pointsOut[0].x>=0 && pointsOut[0].x<decimatedData.cameraModels()[cameraIndex].imageWidth() &&
pointsOut[0].y>=0 && pointsOut[0].y<decimatedData.cameraModels()[cameraIndex].imageHeight())
{
keypointsValid.push_back(keypoints.at(i));
keypointsValid.back().pt.x = pointsOut[0].x + subImageWidth*cameraIndex;
keypointsValid.back().pt.y = pointsOut[0].y;
descriptorsValid.push_back(descriptors.row(i));
}
}
}
keypoints = keypointsValid;
descriptors = descriptorsValid;
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemRectification(), t*1000.0f);
UDEBUG("time rectification = %fs", t);
} }
if(useProvided3dPoints && keypoints.size() != data.keypoints3D().size()) keypoints = keypointsValid;
descriptors = descriptorsValid;
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemRectification(), t*1000.0f);
UDEBUG("time rectification = %fs", t);
}
if(useProvided3dPoints && keypoints.size() != data.keypoints3D().size())
{
UDEBUG("Using provided 3d points (%d->%d)", (int)data.keypoints3D().size(), (int)keypoints.size());
keypoints3D.resize(keypoints.size());
for(size_t i=0; i<keypoints.size(); ++i)
{ {
UDEBUG("Using provided 3d points (%d->%d)", (int)data.keypoints3D().size(), (int)keypoints.size()); UASSERT(keypoints[i].class_id < (int)data.keypoints3D().size());
keypoints3D.resize(keypoints.size()); keypoints3D[i] = data.keypoints3D()[keypoints[i].class_id];
for(size_t i=0; i<keypoints.size(); ++i)
{
UASSERT(keypoints[i].class_id < (int)data.keypoints3D().size());
keypoints3D[i] = data.keypoints3D()[keypoints[i].class_id];
}
}
else if(useProvided3dPoints && keypoints.size() == data.keypoints3D().size())
{
UDEBUG("Using provided 3d points (%d)", (int)data.keypoints3D().size());
keypoints3D = data.keypoints3D();
}
else if((!decimatedData.depthRaw().empty() && decimatedData.cameraModels().size() && decimatedData.cameraModels()[0].isValidForProjection()) ||
(!decimatedData.rightRaw().empty() && decimatedData.stereoCameraModels().size() && decimatedData.stereoCameraModels()[0].isValidForProjection()))
{
keypoints3D = _feature2D->generateKeypoints3D(decimatedData, keypoints);
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D.size(), t);
}
if(depthMask.empty() && (_feature2D->getMinDepth() > 0.0f || _feature2D->getMaxDepth() > 0.0f))
{
_feature2D->filterKeypointsByDepth(keypoints, descriptors, keypoints3D, _feature2D->getMinDepth(), _feature2D->getMaxDepth());
} }
} }
else if(useProvided3dPoints && keypoints.size() == data.keypoints3D().size())
{
UDEBUG("Using provided 3d points (%d)", (int)data.keypoints3D().size());
keypoints3D = data.keypoints3D();
}
else if((!decimatedData.depthRaw().empty() && decimatedData.cameraModels().size() && decimatedData.cameraModels()[0].isValidForProjection()) ||
(!decimatedData.rightRaw().empty() && decimatedData.stereoCameraModels().size() && decimatedData.stereoCameraModels()[0].isValidForProjection()))
{
keypoints3D = _feature2D->generateKeypoints3D(decimatedData, keypoints);
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D.size(), t);
}
if(depthMask.empty() && (_feature2D->getMinDepth() > 0.0f || _feature2D->getMaxDepth() > 0.0f))
{
_feature2D->filterKeypointsByDepth(keypoints, descriptors, keypoints3D, _feature2D->getMinDepth(), _feature2D->getMaxDepth());
}
} }
else if(data.imageRaw().empty()) else if(data.imageRaw().empty())
{ {
@@ -5861,12 +5854,6 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
t = timer.ticks(); t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f); if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D.size(), t); UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D.size(), t);
UDEBUG("ratio=%f, meanWordsPerLocation=%d", _badSignRatio, meanWordsPerLocation);
if(descriptors.rows && descriptors.rows < _badSignRatio * float(meanWordsPerLocation))
{
descriptors = cv::Mat();
}
} }
} }
@@ -5892,7 +5879,9 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
bool addedToDictionary = false; bool addedToDictionary = false;
if(!keypoints.empty()) if(!keypoints.empty())
{ {
if(descriptors.rows && !isIntermediateNode) if(descriptors.rows &&
!isIntermediateNode && // don't add intermediate nodes to dictionary
descriptors.rows >= int(_badSignRatio * float(meanWordsPerLocation))) // don't add bad signatures to dictionary
{ {
// In case the number of features we want to do quantization is lower // In case the number of features we want to do quantization is lower
// than extracted ones (that would be used for transform estimation) // than extracted ones (that would be used for transform estimation)
@@ -5999,7 +5988,11 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
else else
{ {
// Set all words as not used in dictionary // Set all words as not used in dictionary
wordIds.resize(keypoints.size(),-1); int negIndex = -1;
for(size_t i=0; i<keypoints.size(); ++i)
{
wordIds.push_back(negIndex--);
}
} }
t = timer.ticks(); t = timer.ticks();
corelib/src/python/PyDetector.cpp
+9 -1
View File
@@ -259,7 +259,15 @@ std::vector<cv::KeyPoint> PyDetector::generateKeypointsImpl(const cv::Mat & imag
cv::Mat PyDetector::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const cv::Mat PyDetector::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
{ {
UASSERT((int)keypoints.size() == descriptors_.rows); if(!keypoints.empty() && (int)keypoints.size() != descriptors_.rows)
{
UERROR("The number of keypoints (%ld) doesn't match the number of buffered "
"descriptors (%d). PyDetector's descriptors extraction should "
"be called right after keypoints detection, with same keypoints "
"returned by the detection. Returning empty descriptors.",
keypoints.size(), descriptors_.rows);
return cv::Mat();
}
return descriptors_; return descriptors_;
} }
corelib/src/superpoint_rpautrat/SuperpointRpautrat.cpp
+1 -1
View File
@@ -130,7 +130,7 @@ cv::Mat SPDetectorRpautrat::compute(const std::vector<cv::KeyPoint> &keypoints)
{ {
if(!detected_) if(!detected_)
{ {
UERROR("SPDetector has been reset before extracting the descriptors! detect() should be called before compute()."); UERROR("SPDetectorRpautrat has been reset before extracting the descriptors! detect() should be called before compute().");
return cv::Mat(); return cv::Mat();
} }
if(keypoints.empty()) if(keypoints.empty())