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

Fixing empty descriptors on bad signatures (#1714), (bug from #1698) (#1717)

Browse Source 
* Fixing empty descriptors on bad signatures (#1714), (bug from #1698)

* PyDetector: dont assert (just error) if descriptors/keypoints don't match. SuperPoint approaches: try re-initializing superpoint detection on the provided image if descriptors could not be fetched the first time (auto recover).
This commit is contained in:
matlabbe
2026-06-07 13:19:31 -07:00
committed by GitHub
parent f29676deb0
commit 468f6e6f28
4 changed files with 185 additions and 136 deletions
Download Patch File Download Diff File Expand all files Collapse all files

52
corelib/src/Features2d.cpp
View File

@@ -2639,7 +2639,31 @@ cv::Mat SuperPointTorch::generateDescriptorsImpl(const cv::Mat & image, std::vec
{
#ifdef RTABMAP_TORCH
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
UWARN("RTAB-Map is not built with Torch support so SuperPoint Torch feature cannot be used!");
return cv::Mat();
@@ -2741,7 +2765,31 @@ cv::Mat SuperPointRpautrat::generateDescriptorsImpl(const cv::Mat & image, std::
{
#if defined(RTABMAP_TORCH) && defined(RTABMAP_PYTHON)
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
UWARN("RTAB-Map is not built with Torch support so SuperPoint Rpautrat feature cannot be used!");
return cv::Mat();

257
corelib/src/Memory.cpp
View File

@@ -5205,6 +5205,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
// is using less features than feature2D->getMaxFeatures()
meanWordsPerLocation = 0;
}
UDEBUG("ratio=%f, meanWordsPerLocation=%d", _badSignRatio, meanWordsPerLocation);
if(_parallelized && !isIntermediateNode)
{
@@ -5474,41 +5475,90 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
if(stats) stats->addStatistic(Statistics::kTimingMemDescriptors_extraction(), t*1000.0f);
UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t);
UDEBUG("ratio=%f, meanWordsPerLocation=%d", _badSignRatio, meanWordsPerLocation);
if(descriptors.rows && descriptors.rows < _badSignRatio * float(meanWordsPerLocation))
if(!imagesRectified && decimatedData.cameraModels().size())
{
descriptors = cv::Mat();
}
else
{
if(!imagesRectified && decimatedData.cameraModels().size())
{
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);
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)
if(decimatedData.cameraModels().size() == 1)
//undistort keypoints before projection (RGB-D)
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;
cv::KeyPoint::convert(keypoints,pointsIn);
if(decimatedData.cameraModels()[0].D_raw().cols == 6)
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()[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);
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()[0].K_raw(),
decimatedData.cameraModels()[cameraIndex].K_raw(),
D,
decimatedData.cameraModels()[0].R(),
decimatedData.cameraModels()[0].P());
decimatedData.cameraModels()[cameraIndex].R(),
decimatedData.cameraModels()[cameraIndex].P());
}
else
#else
@@ -5519,115 +5569,58 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
{
//RadialTangential
cv::undistortPoints(pointsIn, pointsOut,
decimatedData.cameraModels()[0].K_raw(),
decimatedData.cameraModels()[0].D_raw(),
decimatedData.cameraModels()[0].R(),
decimatedData.cameraModels()[0].P());
decimatedData.cameraModels()[cameraIndex].K_raw(),
decimatedData.cameraModels()[cameraIndex].D_raw(),
decimatedData.cameraModels()[cameraIndex].R(),
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() &&
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));
}
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));
}
}
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());
keypoints3D.resize(keypoints.size());
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());
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(data.imageRaw().empty())
{
@@ -5853,12 +5846,6 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
t = timer.ticks();
if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
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();
}
}
}
@@ -5884,7 +5871,9 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
bool addedToDictionary = false;
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
// than extracted ones (that would be used for transform estimation)
@@ -5991,7 +5980,11 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
else
{
// 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();

10
corelib/src/python/PyDetector.cpp
View File

@@ -247,7 +247,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
{
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_;
}

2
corelib/src/superpoint_rpautrat/SuperpointRpautrat.cpp
View File

@@ -130,7 +130,7 @@ cv::Mat SPDetectorRpautrat::compute(const std::vector<cv::KeyPoint> &keypoints)
{
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();
}
if(keypoints.empty())