46 #include <opencv2/core/core_c.h>
48 #if defined(HAVE_OPENCV_XFEATURES2D) && (CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION==3 && CV_MINOR_VERSION >=4 && CV_SUBMINOR_VERSION >= 1))
49 #include <opencv2/xfeatures2d.hpp>
63 _minInliers(
Parameters::defaultVisMinInliers()),
64 _inlierDistance(
Parameters::defaultVisInlierDistance()),
65 _iterations(
Parameters::defaultVisIterations()),
66 _refineIterations(
Parameters::defaultVisRefineIterations()),
67 _epipolarGeometryVar(
Parameters::defaultVisEpipolarGeometryVar()),
68 _estimationType(
Parameters::defaultVisEstimationType()),
69 _forwardEstimateOnly(
Parameters::defaultVisForwardEstOnly()),
70 _PnPReprojError(
Parameters::defaultVisPnPReprojError()),
72 _PnPRefineIterations(
Parameters::defaultVisPnPRefineIterations()),
73 _PnPVarMedianRatio(
Parameters::defaultVisPnPVarianceMedianRatio()),
74 _PnPMaxVar(
Parameters::defaultVisPnPMaxVariance()),
75 _PnPSplitLinearCovarianceComponents(
Parameters::defaultVisPnPSplitLinearCovComponents()),
76 _multiSamplingPolicy(
Parameters::defaultVisPnPSamplingPolicy()),
77 _correspondencesApproach(
Parameters::defaultVisCorType()),
78 _flowWinSize(
Parameters::defaultVisCorFlowWinSize()),
79 _flowIterations(
Parameters::defaultVisCorFlowIterations()),
81 _flowMaxLevel(
Parameters::defaultVisCorFlowMaxLevel()),
84 _gmsWithRotation(
Parameters::defaultGMSWithRotation()),
85 _gmsWithScale(
Parameters::defaultGMSWithScale()),
86 _gmsThresholdFactor(
Parameters::defaultGMSThresholdFactor()),
87 _guessWinSize(
Parameters::defaultVisCorGuessWinSize()),
88 _guessMatchToProjection(
Parameters::defaultVisCorGuessMatchToProjection()),
89 _bundleAdjustment(
Parameters::defaultVisBundleAdjustment()),
90 _depthAsMask(
Parameters::defaultVisDepthAsMask()),
91 _minInliersDistributionThr(
Parameters::defaultVisMinInliersDistribution()),
92 _maxInliersMeanDistance(
Parameters::defaultVisMeanInliersDistance()),
157 UWARN(
"%s should be >= 6 but it is set to %d, setting to 6.", Parameters::kVisMinInliers().
c_str(),
_minInliers);
166 #ifndef RTABMAP_PYTHON
167 UWARN(
"%s is set to 6 but RTAB-Map is not built with Python3 support, using default %d.",
168 Parameters::kVisCorNNType().
c_str(), Parameters::defaultVisCorNNType());
169 _nnType = Parameters::defaultVisCorNNType();
171 int iterations = _pyMatcher?_pyMatcher->iterations():Parameters::defaultPyMatcherIterations();
172 float matchThr = _pyMatcher?_pyMatcher->matchThreshold():Parameters::defaultPyMatcherThreshold();
173 std::string
path = _pyMatcher?_pyMatcher->path():Parameters::defaultPyMatcherPath();
174 bool cuda = _pyMatcher?_pyMatcher->cuda():Parameters::defaultPyMatcherCuda();
175 std::string
model = _pyMatcher?_pyMatcher->model():Parameters::defaultPyMatcherModel();
183 UERROR(
"%s parameter should be set to use Python3 matching (%s=6), using default %d.",
184 Parameters::kPyMatcherPath().
c_str(),
185 Parameters::kVisCorNNType().
c_str(),
186 Parameters::defaultVisCorNNType());
187 _nnType = Parameters::defaultVisCorNNType();
196 #if !defined(HAVE_OPENCV_XFEATURES2D) || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION<4 || CV_MINOR_VERSION==4 && CV_SUBMINOR_VERSION<1))
199 UWARN(
"%s is set to 7 but RTAB-Map is not built with OpenCV's xfeatures2d support (OpenCV >= 3.4.1 also required), using default %d.",
200 Parameters::kVisCorNNType().
c_str(), Parameters::defaultVisCorNNType());
201 _nnType = Parameters::defaultVisCorNNType();
206 for(ParametersMap::const_iterator
iter=parameters.begin();
iter!=parameters.end(); ++
iter)
208 std::string group =
uSplit(
iter->first,
'/').front();
215 if(
uContains(parameters, Parameters::kVisCorNNType()))
222 if(
uContains(parameters, Parameters::kVisCorNNDR()))
226 if(
uContains(parameters, Parameters::kKpByteToFloat()))
230 if(
uContains(parameters, Parameters::kVisFeatureType()))
234 if(
uContains(parameters, Parameters::kVisMaxFeatures()))
238 if(
uContains(parameters, Parameters::kVisSSC()))
242 if(
uContains(parameters, Parameters::kVisMaxDepth()))
246 if(
uContains(parameters, Parameters::kVisMinDepth()))
250 if(
uContains(parameters, Parameters::kVisRoiRatios()))
254 if(
uContains(parameters, Parameters::kVisSubPixEps()))
258 if(
uContains(parameters, Parameters::kVisSubPixIterations()))
262 if(
uContains(parameters, Parameters::kVisSubPixWinSize()))
266 if(
uContains(parameters, Parameters::kVisGridRows()))
270 if(
uContains(parameters, Parameters::kVisGridCols()))
285 #ifdef RTABMAP_PYTHON
318 UDEBUG(
"Input(%d): from=%d words, %d 3D words, %d words descriptors, %d kpts, %d kpts3D, %d descriptors, image=%dx%d models=%d stereo=%d",
320 (
int)fromSignature.
getWords().size(),
331 UDEBUG(
"Input(%d): to=%d words, %d 3D words, %d words descriptors, %d kpts, %d kpts3D, %d descriptors, image=%dx%d models=%d stereo=%d",
345 info.projectedIDs.clear();
358 UDEBUG(
"Bypassing feature matching as descriptors and images are empty. We assume features are already matched.");
385 std::vector<cv::KeyPoint> kptsFrom;
389 std::vector<int> orignalWordsFromIds;
390 int kptsFromSource = 0;
391 if(fromSignature.
getWords().empty())
395 if(!imageFrom.empty())
397 if(imageFrom.channels() > 1)
400 cv::cvtColor(imageFrom, tmp, cv::COLOR_BGR2GRAY);
415 UWARN(
"%s is true, but RGB size (%dx%d) modulo depth size (%dx%d) is not 0. Ignoring depth mask for feature detection.",
416 Parameters::kVisDepthAsMask().
c_str(),
436 orignalWordsFromIds.resize(fromSignature.
getWords().size());
438 bool allUniques =
true;
439 int previousIdAdded = 0;
443 UASSERT(
iter->second>=0 &&
iter->second<(
int)orignalWordsFromIds.size());
444 orignalWordsFromIds[
iter->second] =
iter->first;
445 if(
i>0 &&
iter->first==previousIdAdded)
449 previousIdAdded =
iter->first;
454 UDEBUG(
"IDs are not unique, IDs will be regenerated!");
455 orignalWordsFromIds.clear();
459 std::multimap<int, int> wordsFrom;
460 std::multimap<int, int> wordsTo;
461 std::vector<cv::KeyPoint> wordsKptsFrom;
462 std::vector<cv::KeyPoint> wordsKptsTo;
463 std::vector<cv::Point3f> words3From;
464 std::vector<cv::Point3f> words3To;
465 cv::Mat wordsDescFrom;
471 if(imageFrom.channels() > 1)
474 cv::cvtColor(imageFrom, tmp, cv::COLOR_BGR2GRAY);
477 if(imageTo.channels() > 1)
480 cv::cvtColor(imageTo, tmp, cv::COLOR_BGR2GRAY);
484 std::vector<cv::Point3f> kptsFrom3D;
485 if(kptsFrom.size() == fromSignature.
getWords3().size())
498 if(!imageFrom.empty() && !imageTo.empty())
501 std::vector<cv::Point2f> cornersFrom;
502 cv::KeyPoint::convert(kptsFrom, cornersFrom);
503 std::vector<cv::Point2f> cornersTo;
511 cv::Mat
R = (cv::Mat_<double>(3,3) <<
512 (double)guessCameraRef.
r11(), (double)guessCameraRef.
r12(), (double)guessCameraRef.
r13(),
513 (double)guessCameraRef.
r21(), (double)guessCameraRef.
r22(), (double)guessCameraRef.
r23(),
514 (double)guessCameraRef.
r31(), (double)guessCameraRef.
r32(), (double)guessCameraRef.
r33());
515 cv::Mat rvec(1,3, CV_64FC1);
516 cv::Rodrigues(
R, rvec);
517 cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)guessCameraRef.
x(), (double)guessCameraRef.
y(), (double)guessCameraRef.
z());
519 cv::projectPoints(kptsFrom3D, rvec, tvec,
K, cv::Mat(), cornersTo);
525 cornersTo = cornersFrom;
527 std::vector<Transform> inverseTransforms(nCameras);
528 for(
int c=0;
c<nCameras; ++
c)
531 inverseTransforms[
c] = (guess * localTransform).
inverse();
532 UDEBUG(
"inverse transforms: cam %d -> %s",
c, inverseTransforms[
c].prettyPrint().
c_str());
536 UASSERT(kptsFrom3D.size() == cornersTo.size());
539 for(
size_t i=0;
i<kptsFrom3D.size(); ++
i)
542 int startIndex = cornersFrom[
i].x/subImageWidth;
543 UASSERT(startIndex < nCameras);
544 for(
int c=startIndex; (
c+1)%nCameras != 0; ++
c)
548 if(ptsInCamFrame.z > 0)
551 model.reproject(ptsInCamFrame.x, ptsInCamFrame.y, ptsInCamFrame.z, u,
v);
554 cornersTo[
i].x = u+
model.imageWidth()*
c;
563 UDEBUG(
"Projected %d/%ld points inside %d cameras (time=%fs)",
564 inFrame, cornersTo.size(), nCameras,
t.ticks());
569 UDEBUG(
"guessSet = %d", guessSet?1:0);
570 std::vector<unsigned char> status;
571 std::vector<float> err;
572 UDEBUG(
"cv::calcOpticalFlowPyrLK() begin");
573 cv::calcOpticalFlowPyrLK(
583 cv::OPTFLOW_LK_GET_MIN_EIGENVALS | (guessSet?cv::OPTFLOW_USE_INITIAL_FLOW:0), 1
e-4);
584 UDEBUG(
"cv::calcOpticalFlowPyrLK() end");
586 UASSERT(kptsFrom.size() == kptsFrom3D.size());
587 std::vector<cv::KeyPoint> kptsTo(kptsFrom.size());
588 std::vector<cv::Point3f> kptsFrom3DKept(kptsFrom3D.size());
589 std::vector<int> orignalWordsFromIdsCpy = orignalWordsFromIds;
591 for(
unsigned int i=0;
i<status.size(); ++
i)
597 if(orignalWordsFromIdsCpy.size())
599 orignalWordsFromIds[ki] = orignalWordsFromIdsCpy[
i];
601 kptsFrom[ki] = cv::KeyPoint(cornersFrom[
i], 1);
602 kptsFrom3DKept[ki] = kptsFrom3D[
i];
603 kptsTo[ki++] = cv::KeyPoint(cornersTo[
i], 1);
606 if(orignalWordsFromIds.size())
608 orignalWordsFromIds.resize(ki);
612 kptsFrom3DKept.resize(ki);
613 kptsFrom3D = kptsFrom3DKept;
615 std::vector<cv::Point3f> kptsTo3D;
621 UASSERT(kptsFrom.size() == kptsFrom3DKept.size());
622 UASSERT(kptsFrom.size() == kptsTo.size());
623 UASSERT(kptsTo3D.size() == 0 || kptsTo.size() == kptsTo3D.size());
624 for(
unsigned int i=0;
i< kptsFrom3DKept.size(); ++
i)
626 int id = !orignalWordsFromIds.empty()?orignalWordsFromIds[
i]:
i;
627 wordsFrom.insert(wordsFrom.end(), std::make_pair(
id, wordsFrom.size()));
628 wordsKptsFrom.push_back(kptsFrom[
i]);
629 words3From.push_back(kptsFrom3DKept[
i]);
631 wordsTo.insert(wordsTo.end(), std::make_pair(
id, wordsTo.size()));
632 wordsKptsTo.push_back(kptsTo[
i]);
633 if(!kptsTo3D.empty())
635 words3To.push_back(kptsTo3D[
i]);
642 if(imageFrom.empty())
644 UERROR(
"Optical flow correspondences requires images in data!");
646 UASSERT(kptsFrom.size() == kptsFrom3D.size());
647 for(
unsigned int i=0;
i< kptsFrom3D.size(); ++
i)
651 int id = !orignalWordsFromIds.empty()?orignalWordsFromIds[
i]:
i;
652 wordsFrom.insert(wordsFrom.end(), std::make_pair(
id, wordsFrom.size()));
653 wordsKptsFrom.push_back(kptsFrom[
i]);
654 words3From.push_back(kptsFrom3D[
i]);
657 toSignature.
sensorData().
setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
665 std::vector<cv::KeyPoint> kptsTo;
666 int kptsToSource = 0;
672 if(imageTo.channels() > 1)
675 cv::cvtColor(imageTo, tmp, cv::COLOR_BGR2GRAY);
690 UWARN(
"%s is true, but RGB size (%dx%d) modulo depth size (%dx%d) is not 0. Ignoring depth mask for feature detection.",
691 Parameters::kVisDepthAsMask().
c_str(),
714 UDEBUG(
"kptsFrom=%d kptsFromSource=%d", (
int)kptsFrom.size(), kptsFromSource);
715 UDEBUG(
"kptsTo=%d kptsToSource=%d", (
int)kptsTo.size(), kptsToSource);
716 cv::Mat descriptorsFrom;
717 if(kptsFromSource == 2 &&
724 else if(kptsFromSource == 1 &&
729 else if(!imageFrom.empty())
731 if(imageFrom.channels() > 1)
734 cv::cvtColor(imageFrom, tmp, cv::COLOR_BGR2GRAY);
737 UDEBUG(
"cleared orignalWordsFromIds");
738 orignalWordsFromIds.clear();
742 cv::Mat descriptorsTo;
745 if(kptsToSource == 2 &&
750 else if(kptsToSource == 1 &&
755 else if(!imageTo.empty())
757 if(imageTo.channels() > 1)
760 cv::cvtColor(imageTo, tmp, cv::COLOR_BGR2GRAY);
769 std::vector<cv::Point3f> kptsFrom3D;
770 std::vector<cv::Point3f> kptsTo3D;
771 if(kptsFromSource == 2 &&
772 kptsFrom.size() == fromSignature.
getWords3().size())
776 else if(kptsFromSource == 1 &&
783 if(fromSignature.
getWords3().size() && kptsFrom.size() != fromSignature.
getWords3().size())
785 UWARN(
"kptsFrom (%d) is not the same size as fromSignature.getWords3() (%d), there "
786 "is maybe a problem with the logic above (getWords3() should be null or equal to kptsfrom). Regenerating kptsFrom3D...",
792 UWARN(
"kptsFrom (%d) is not the same size as fromSignature.sensorData().keypoints3D() (%d), there "
793 "is maybe a problem with the logic above (keypoints3D should be null or equal to kptsfrom). Regenerating kptsFrom3D...",
798 UDEBUG(
"generated kptsFrom3D=%d", (
int)kptsFrom3D.size());
801 if(!kptsFrom3D.empty() &&
808 if(kptsToSource == 2 && kptsTo.size() == toSignature.
getWords3().size())
812 else if(kptsToSource == 1 &&
821 UWARN(
"kptsTo (%d) is not the same size as toSignature.getWords3() (%d), there "
822 "is maybe a problem with the logic above (getWords3() should be null or equal to kptsTo). Regenerating kptsTo3D...",
828 UWARN(
"kptsTo (%d) is not the same size as toSignature.sensorData().keypoints3D() (%d), there "
829 "is maybe a problem with the logic above (keypoints3D() should be null or equal to kptsTo). Regenerating kptsTo3D...",
836 if(kptsTo3D.size() &&
843 UASSERT(kptsFrom.empty() || descriptorsFrom.rows == 0 ||
int(kptsFrom.size()) == descriptorsFrom.rows);
848 UDEBUG(
"descriptorsFrom=%d", descriptorsFrom.rows);
849 UDEBUG(
"descriptorsTo=%d", descriptorsTo.rows);
850 UDEBUG(
"orignalWordsFromIds=%d", (
int)orignalWordsFromIds.size());
853 if(descriptorsFrom.rows > 0 && descriptorsTo.rows > 0)
855 std::vector<CameraModel> models;
868 bool isCalibrated = !models.empty();
869 for(
size_t i=0;
i<models.size() && isCalibrated; ++
i)
871 isCalibrated = models[
i].isValidForProjection();
874 if(isCalibrated && (models[
i].imageWidth()==0 || models[
i].imageHeight()==0))
882 isCalibrated =
false;
895 UASSERT((
int)kptsTo.size() == descriptorsTo.rows);
896 UASSERT((
int)kptsFrom3D.size() == descriptorsFrom.rows);
898 std::vector<cv::Point2f> cornersProjected;
899 std::vector<int> projectedIndexToDescIndex;
900 float subImageWidth = models[0].imageWidth();
903 for(
size_t m=0;
m<models.size(); ++
m)
906 cv::Mat
R = (cv::Mat_<double>(3,3) <<
907 (double)guessCameraRef.
r11(), (double)guessCameraRef.
r12(), (double)guessCameraRef.
r13(),
908 (double)guessCameraRef.
r21(), (double)guessCameraRef.
r22(), (double)guessCameraRef.
r23(),
909 (double)guessCameraRef.
r31(), (double)guessCameraRef.
r32(), (double)guessCameraRef.
r33());
910 cv::Mat rvec(1,3, CV_64FC1);
911 cv::Rodrigues(
R, rvec);
912 cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)guessCameraRef.
x(), (double)guessCameraRef.
y(), (double)guessCameraRef.
z());
913 cv::Mat
K = models[
m].K();
914 std::vector<cv::Point2f> projected;
915 cv::projectPoints(kptsFrom3D, rvec, tvec,
K, cv::Mat(), projected);
916 UDEBUG(
"Projected points=%d", (
int)projected.size());
919 UASSERT((
int)projected.size() == descriptorsFrom.rows);
920 int cornersInFrame = 0;
921 for(
unsigned int i=0;
i<projected.size(); ++
i)
923 if(
uIsInBounds(projected[
i].
x, 0.0
f,
float(models[
m].imageWidth()-1)) &&
927 if(added.find(
i) != added.end())
933 projectedIndexToDescIndex.push_back(
i);
934 projected[
i].x += subImageWidth*
float(
m);
935 cornersProjected.push_back(projected[
i]);
940 UDEBUG(
"corners in frame=%d (camera index=%ld)", cornersInFrame,
m);
946 UDEBUG(
"guessMatchToProjection=%d, cornersProjected=%d orignalWordsFromIds=%d (added=%ld, duplicates=%d)",
948 added.size(), duplicates);
949 if(cornersProjected.size())
953 UDEBUG(
"match frame to projected");
959 std::vector< std::vector<size_t> >
indices;
960 std::vector<std::vector<float> > dists;
962 std::vector<cv::Point2f> pointsTo;
963 cv::KeyPoint::convert(kptsTo, pointsTo);
968 UASSERT(descriptorsFrom.cols == descriptorsTo.cols);
969 UASSERT(descriptorsFrom.rows == (
int)kptsFrom.size());
970 UASSERT((
int)pointsToMat.
rows == descriptorsTo.rows);
972 UDEBUG(
"radius search done for guess");
975 int newToId = !orignalWordsFromIds.empty()?fromSignature.
getWords().rbegin()->first+1:descriptorsFrom.rows;
976 std::map<int,int> addedWordsFrom;
977 std::map<int, int> duplicates;
979 cv::Mat descriptors(10, descriptorsTo.cols, descriptorsTo.type());
980 for(
unsigned int i = 0;
i < pointsToMat.
rows; ++
i)
982 int matchedIndex = -1;
993 descriptorsFrom.row(projectedIndexToDescIndex[
indices[
i].at(
j)]).copyTo(descriptors.row(oi));
994 descriptorsIndices[oi++] =
indices[
i].at(
j);
996 descriptorsIndices.resize(oi);
999 cv::BFMatcher
matcher(descriptors.type()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR,
_nnType == 5);
1002 std::vector<cv::DMatch>
matches;
1003 matcher.match(descriptorsTo.row(
i), cv::Mat(descriptors, cv::Range(0, oi)),
matches);
1006 matchedIndex = descriptorsIndices.at(
matches.at(0).trainIdx);
1011 std::vector<std::vector<cv::DMatch> >
matches;
1012 matcher.knnMatch(descriptorsTo.row(
i), cv::Mat(descriptors, cv::Range(0, oi)),
matches, 2);
1017 matchedIndex = descriptorsIndices.at(
matches[0].at(0).trainIdx);
1026 if(matchedIndex >= 0)
1028 matchedIndex = projectedIndexToDescIndex[matchedIndex];
1029 int id = !orignalWordsFromIds.empty()?orignalWordsFromIds[matchedIndex]:matchedIndex;
1031 if(addedWordsFrom.find(matchedIndex) != addedWordsFrom.end())
1033 id = addedWordsFrom.at(matchedIndex);
1034 duplicates.insert(std::make_pair(matchedIndex,
id));
1038 addedWordsFrom.insert(std::make_pair(matchedIndex,
id));
1040 wordsFrom.insert(wordsFrom.end(), std::make_pair(
id, wordsFrom.size()));
1041 if(!kptsFrom.empty())
1043 wordsKptsFrom.push_back(kptsFrom[matchedIndex]);
1045 words3From.push_back(kptsFrom3D[matchedIndex]);
1046 wordsDescFrom.push_back(descriptorsFrom.row(matchedIndex));
1049 wordsTo.insert(wordsTo.end(), std::make_pair(
id, wordsTo.size()));
1050 wordsKptsTo.push_back(kptsTo[
i]);
1051 wordsDescTo.push_back(descriptorsTo.row(
i));
1052 if(!kptsTo3D.empty())
1054 words3To.push_back(kptsTo3D[
i]);
1060 wordsTo.insert(wordsTo.end(), std::make_pair(newToId, wordsTo.size()));
1061 wordsKptsTo.push_back(kptsTo[
i]);
1062 wordsDescTo.push_back(descriptorsTo.row(
i));
1063 if(!kptsTo3D.empty())
1065 words3To.push_back(kptsTo3D[
i]);
1072 UDEBUG(
"addedWordsFrom=%d/%d (duplicates=%d, newWords=%d), kptsTo=%d, wordsTo=%d, words3From=%d",
1073 (
int)addedWordsFrom.size(), (
int)cornersProjected.size(), (
int)duplicates.size(), newWords,
1074 (
int)kptsTo.size(), (
int)wordsTo.size(), (
int)words3From.size());
1077 int addWordsFromNotMatched = 0;
1078 for(
unsigned int i=0;
i<kptsFrom3D.size(); ++
i)
1080 if(
util3d::isFinite(kptsFrom3D[
i]) && addedWordsFrom.find(
i) == addedWordsFrom.end())
1082 int id = !orignalWordsFromIds.empty()?orignalWordsFromIds[
i]:
i;
1083 wordsFrom.insert(wordsFrom.end(), std::make_pair(
id, wordsFrom.size()));
1084 wordsKptsFrom.push_back(kptsFrom[
i]);
1085 wordsDescFrom.push_back(descriptorsFrom.row(
i));
1086 words3From.push_back(kptsFrom3D[
i]);
1088 ++addWordsFromNotMatched;
1091 UDEBUG(
"addWordsFromNotMatched=%d -> words3From=%d", addWordsFromNotMatched, (
int)words3From.size());
1095 UDEBUG(
"match projected to frame");
1096 std::vector<cv::Point2f> pointsTo;
1097 cv::KeyPoint::convert(kptsTo, pointsTo);
1102 std::vector< std::vector<size_t> >
indices;
1103 std::vector<std::vector<float> > dists;
1109 UASSERT(descriptorsFrom.cols == descriptorsTo.cols);
1110 UASSERT(descriptorsFrom.rows == (
int)kptsFrom.size());
1111 UASSERT((
int)pointsToMat.
rows == descriptorsTo.rows);
1113 UDEBUG(
"radius search done for guess");
1116 std::set<int> addedWordsTo;
1117 std::set<int> addedWordsFrom;
1118 double bruteForceTotalTime = 0.0;
1119 double bruteForceDescCopy = 0.0;
1121 cv::Mat descriptors(10, descriptorsTo.cols, descriptorsTo.type());
1122 for(
unsigned int i = 0;
i < cornersProjectedMat.
rows; ++
i)
1124 int matchedIndexFrom = projectedIndexToDescIndex[
i];
1128 info.projectedIDs.push_back(!orignalWordsFromIds.empty()?orignalWordsFromIds[matchedIndexFrom]:matchedIndexFrom);
1133 int matchedIndexTo = -1;
1137 std::vector<int> descriptorsIndices(
indices[
i].
size());
1145 descriptorsTo.row(
indices[
i].at(
j)).copyTo(descriptors.row(oi));
1146 descriptorsIndices[oi++] =
indices[
i].at(
j);
1148 bruteForceDescCopy += bruteForceTimer.
ticks();
1151 cv::BFMatcher
matcher(descriptors.type()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR,
_nnType==5);
1154 std::vector<cv::DMatch>
matches;
1155 matcher.match(descriptorsFrom.row(matchedIndexFrom), cv::Mat(descriptors, cv::Range(0, oi)),
matches);
1158 matchedIndexTo = descriptorsIndices.at(
matches.at(0).trainIdx);
1163 std::vector<std::vector<cv::DMatch> >
matches;
1164 matcher.knnMatch(descriptorsFrom.row(matchedIndexFrom), cv::Mat(descriptors, cv::Range(0, oi)),
matches, 2);
1167 bruteForceTotalTime+=bruteForceTimer.
elapsed();
1170 matchedIndexTo = descriptorsIndices.at(
matches[0].at(0).trainIdx);
1179 int id = !orignalWordsFromIds.empty()?orignalWordsFromIds[matchedIndexFrom]:matchedIndexFrom;
1180 addedWordsFrom.insert(addedWordsFrom.end(), matchedIndexFrom);
1182 wordsFrom.insert(wordsFrom.end(), std::make_pair(
id, wordsFrom.size()));
1183 if(!kptsFrom.empty())
1185 wordsKptsFrom.push_back(kptsFrom[matchedIndexFrom]);
1187 words3From.push_back(kptsFrom3D[matchedIndexFrom]);
1188 wordsDescFrom.push_back(descriptorsFrom.row(matchedIndexFrom));
1190 if( matchedIndexTo >= 0 &&
1191 addedWordsTo.find(matchedIndexTo) == addedWordsTo.end())
1193 addedWordsTo.insert(matchedIndexTo);
1195 wordsTo.insert(wordsTo.end(), std::make_pair(
id, wordsTo.size()));
1196 wordsKptsTo.push_back(kptsTo[matchedIndexTo]);
1197 wordsDescTo.push_back(descriptorsTo.row(matchedIndexTo));
1198 if(!kptsTo3D.empty())
1200 words3To.push_back(kptsTo3D[matchedIndexTo]);
1205 UDEBUG(
"bruteForceDescCopy=%fs, bruteForceTotalTime=%fs", bruteForceDescCopy, bruteForceTotalTime);
1208 for(
unsigned int i=0;
i<kptsFrom3D.size(); ++
i)
1210 if(
util3d::isFinite(kptsFrom3D[
i]) && addedWordsFrom.find(
i) == addedWordsFrom.end())
1212 int id = !orignalWordsFromIds.empty()?orignalWordsFromIds[
i]:
i;
1213 wordsFrom.insert(wordsFrom.end(), std::make_pair(
id, wordsFrom.size()));
1214 wordsKptsFrom.push_back(kptsFrom[
i]);
1215 wordsDescFrom.push_back(descriptorsFrom.row(
i));
1216 words3From.push_back(kptsFrom3D[
i]);
1220 int newToId = !orignalWordsFromIds.empty()?fromSignature.
getWords().rbegin()->first+1:descriptorsFrom.rows;
1221 for(
unsigned int i = 0;
i < kptsTo.size(); ++
i)
1223 if(addedWordsTo.find(
i) == addedWordsTo.end())
1225 wordsTo.insert(wordsTo.end(), std::make_pair(newToId, wordsTo.size()));
1226 wordsKptsTo.push_back(kptsTo[
i]);
1227 wordsDescTo.push_back(descriptorsTo.row(
i));
1228 if(!kptsTo3D.empty())
1230 words3To.push_back(kptsTo3D[
i]);
1239 UWARN(
"All projected points are outside the camera. Guess (%s) is wrong or images are not overlapping.", guess.
prettyPrint().c_str());
1245 if(guessSet &&
_guessWinSize > 0 && kptsFrom3D.size() && !isCalibrated)
1247 UWARN(
"Calibration not found! Finding correspondences "
1248 "with the guess cannot be done, global matching is "
1254 std::list<int> fromWordIds;
1255 std::list<int> toWordIds;
1256 #ifdef RTABMAP_PYTHON
1262 std::vector<int> fromWordIdsV(descriptorsFrom.rows);
1263 for (
int i = 0;
i < descriptorsFrom.rows; ++
i)
1266 if(!orignalWordsFromIds.empty())
1268 id = orignalWordsFromIds[
i];
1270 fromWordIds.push_back(
id);
1271 fromWordIdsV[
i] =
id;
1273 if(descriptorsTo.rows)
1275 std::vector<int> toWordIdsV(descriptorsTo.rows, 0);
1276 std::vector<cv::DMatch>
matches;
1277 #ifdef RTABMAP_PYTHON
1278 if(
_nnType == 6 && _pyMatcher &&
1279 descriptorsTo.cols == descriptorsFrom.cols &&
1280 descriptorsTo.rows == (
int)kptsTo.size() &&
1281 descriptorsTo.type() == CV_32F &&
1282 descriptorsFrom.type() == CV_32F &&
1283 descriptorsFrom.rows == (
int)kptsFrom.size() &&
1286 UDEBUG(
"Python matching");
1287 matches = _pyMatcher->match(descriptorsTo, descriptorsFrom, kptsTo, kptsFrom, models[0].imageSize());
1291 if(
_nnType == 6 && _pyMatcher)
1293 UDEBUG(
"Invalid inputs for Python matching (desc type=%d, only float descriptors supported, multicam not supported), doing bruteforce matching instead.", descriptorsFrom.type());
1298 bool doCrossCheck =
true;
1299 #ifdef HAVE_OPENCV_XFEATURES2D
1300 #if CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION==3 && CV_MINOR_VERSION >=4 && CV_SUBMINOR_VERSION >= 1)
1301 cv::Size imageSizeFrom;
1304 imageSizeFrom = imageFrom.size();
1309 if(!models.empty() && models[0].imageSize().height > 0 && models[0].imageSize().width > 0 &&
1310 imageSizeFrom.height > 0 && imageSizeFrom.width > 0)
1312 doCrossCheck =
false;
1316 UDEBUG(
"Invalid inputs for GMS matching, image size should be set for both inputs, doing bruteforce matching instead.");
1322 UDEBUG(
"BruteForce matching%s",
_nnType!=7?
" with crosscheck":
" with GMS");
1323 cv::BFMatcher
matcher(descriptorsFrom.type()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR, doCrossCheck);
1326 #if defined(HAVE_OPENCV_XFEATURES2D) && (CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION==3 && CV_MINOR_VERSION >=4 && CV_SUBMINOR_VERSION >= 1))
1330 std::vector<cv::DMatch> matchesGMS;
1340 for(
size_t i=0;
i<toWordIdsV.size(); ++
i)
1342 int toId = toWordIdsV[
i];
1345 toId = fromWordIds.back()+
i+1;
1347 toWordIds.push_back(toId);
1353 UDEBUG(
"VWDictionary knn matching");
1355 if(orignalWordsFromIds.empty())
1357 fromWordIds = dictionary.
addNewWords(descriptorsFrom, 1);
1361 for (
int i = 0;
i < descriptorsFrom.rows; ++
i)
1363 int id = orignalWordsFromIds[
i];
1365 fromWordIds.push_back(
id);
1369 if(descriptorsTo.rows)
1372 toWordIds = dictionary.
addNewWords(descriptorsTo, 2);
1374 dictionary.
clear(
false);
1377 std::multiset<int> fromWordIdsSet(fromWordIds.begin(), fromWordIds.end());
1378 std::multiset<int> toWordIdsSet(toWordIds.begin(), toWordIds.end());
1380 UASSERT(kptsFrom3D.empty() || fromWordIds.size() == kptsFrom3D.size());
1381 UASSERT(
int(fromWordIds.size()) == descriptorsFrom.rows);
1383 for(std::list<int>::iterator
iter=fromWordIds.begin();
iter!=fromWordIds.end(); ++
iter)
1385 if(fromWordIdsSet.count(*
iter) == 1)
1387 wordsFrom.insert(wordsFrom.end(), std::make_pair(*
iter, wordsFrom.size()));
1388 if (!kptsFrom.empty())
1390 wordsKptsFrom.push_back(kptsFrom[
i]);
1392 if(!kptsFrom3D.empty())
1394 words3From.push_back(kptsFrom3D[
i]);
1396 wordsDescFrom.push_back(descriptorsFrom.row(
i));
1400 UASSERT(kptsTo3D.size() == 0 || kptsTo3D.size() == kptsTo.size());
1401 UASSERT(toWordIds.size() == kptsTo.size());
1402 UASSERT(
int(toWordIds.size()) == descriptorsTo.rows);
1404 for(std::list<int>::iterator
iter=toWordIds.begin();
iter!=toWordIds.end(); ++
iter)
1406 if(toWordIdsSet.count(*
iter) == 1)
1408 wordsTo.insert(wordsTo.end(), std::make_pair(*
iter, wordsTo.size()));
1409 wordsKptsTo.push_back(kptsTo[
i]);
1410 wordsDescTo.push_back(descriptorsTo.row(
i));
1411 if(!kptsTo3D.empty())
1413 words3To.push_back(kptsTo3D[
i]);
1420 else if(descriptorsFrom.rows)
1423 UASSERT(kptsFrom3D.empty() ||
int(kptsFrom3D.size()) == descriptorsFrom.rows);
1424 for(
int i=0;
i<descriptorsFrom.rows; ++
i)
1426 wordsFrom.insert(wordsFrom.end(), std::make_pair(
i, wordsFrom.size()));
1427 wordsKptsFrom.push_back(kptsFrom[
i]);
1428 wordsDescFrom.push_back(descriptorsFrom.row(
i));
1429 if(!kptsFrom3D.empty())
1431 words3From.push_back(kptsFrom3D[
i]);
1437 fromSignature.
setWords(wordsFrom, wordsKptsFrom, words3From, wordsDescFrom);
1438 toSignature.
setWords(wordsTo, wordsKptsTo, words3To, wordsDescTo);
1445 cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
1446 int inliersCount = 0;
1447 int matchesCount = 0;
1448 info.inliersIDs.clear();
1449 info.matchesIDs.clear();
1453 std::vector<int> inliers[2];
1455 cv::Mat covariances[2];
1456 covariances[0] = cv::Mat::eye(6,6,CV_64FC1);
1457 covariances[1] = cv::Mat::eye(6,6,CV_64FC1);
1465 signatureA = &fromSignature;
1466 signatureB = &toSignature;
1470 signatureA = &toSignature;
1471 signatureB = &fromSignature;
1481 UERROR(
"Calibrated camera required (multi-cameras not supported).");
1491 double variance = 1.0f;
1492 std::vector<int> matchesV;
1495 std::map<int, cv::KeyPoint> wordsA;
1496 std::map<int, cv::Point3f> words3A;
1497 std::map<int, cv::KeyPoint> wordsB;
1498 for(std::map<int, int>::iterator
iter=uniqueWordsA.begin();
iter!=uniqueWordsA.end(); ++
iter)
1503 words3A.insert(std::make_pair(
iter->first, signatureA->
getWords3()[
iter->second]));
1506 for(std::map<int, int>::iterator
iter=uniqueWordsB.begin();
iter!=uniqueWordsB.end(); ++
iter)
1520 covariances[dir] *= variance;
1521 inliers[dir] =
uKeys(inliers3D);
1524 if(!cameraTransform.
isNull())
1532 transforms[dir] = cameraTransform.
to3DoF();
1536 transforms[dir] = cameraTransform;
1557 else if(signatureA->
getWords().size() == 0)
1574 UERROR(
"Calibrated camera required. Id=%d Models=%d StereoModels=%d weight=%d",
1580 #ifndef RTABMAP_OPENGV
1583 UERROR(
"Multi-camera 2D-3D PnP registration is only available if rtabmap is built "
1584 "with OpenGV dependency. Use 3D-3D registration approach instead for multi-camera.");
1594 std::vector<int> inliersV;
1595 std::vector<int> matchesV;
1598 std::map<int, cv::Point3f> words3A;
1599 std::map<int, cv::Point3f> words3B;
1600 std::map<int, cv::KeyPoint> wordsB;
1601 for(std::map<int, int>::iterator
iter=uniqueWordsA.begin();
iter!=uniqueWordsA.end(); ++
iter)
1603 words3A.insert(std::make_pair(
iter->first, signatureA->
getWords3()[
iter->second]));
1605 for(std::map<int, int>::iterator
iter=uniqueWordsB.begin();
iter!=uniqueWordsB.end(); ++
iter)
1610 words3B.insert(std::make_pair(
iter->first, signatureB->
getWords3()[
iter->second]));
1614 std::vector<CameraModel> models;
1630 UASSERT(models[0].isValidForProjection());
1650 inliers[dir] = inliersV;
1655 UASSERT(models.size() == 1 && models[0].isValidForProjection());
1674 inliers[dir] = inliersV;
1677 UDEBUG(
"inliers: %d/%d", (
int)inliersV.size(), (
int)matchesV.size());
1678 if(transforms[dir].
isNull())
1680 msg =
uFormat(
"Not enough inliers %d/%d (matches=%d) between %d and %d",
1686 transforms[dir] = transforms[dir].
to3DoF();
1691 msg =
uFormat(
"Not enough features in images (old=%d, new=%d, min=%d)",
1705 std::vector<int> inliersV;
1706 std::vector<int> matchesV;
1709 std::map<int, cv::Point3f> words3A;
1710 std::map<int, cv::Point3f> words3B;
1711 for(std::map<int, int>::iterator
iter=uniqueWordsA.begin();
iter!=uniqueWordsA.end(); ++
iter)
1713 words3A.insert(std::make_pair(
iter->first, signatureA->
getWords3()[
iter->second]));
1715 for(std::map<int, int>::iterator
iter=uniqueWordsB.begin();
iter!=uniqueWordsB.end(); ++
iter)
1717 words3B.insert(std::make_pair(
iter->first, signatureB->
getWords3()[
iter->second]));
1729 inliers[dir] = inliersV;
1731 UDEBUG(
"inliers: %d/%d", (
int)inliersV.size(), (
int)matchesV.size());
1732 if(transforms[dir].
isNull())
1734 msg =
uFormat(
"Not enough inliers %d/%d (matches=%d) between %d and %d",
1740 transforms[dir] = transforms[dir].
to3DoF();
1745 msg =
uFormat(
"Not enough 3D features in images (old=%d, new=%d, min=%d)",
1754 UDEBUG(
"from->to=%s", transforms[0].prettyPrint().
c_str());
1755 UDEBUG(
"to->from=%s", transforms[1].prettyPrint().
c_str());
1758 std::vector<int> allInliers = inliers[0];
1759 if(inliers[1].
size())
1761 std::set<int> allInliersSet(allInliers.begin(), allInliers.end());
1762 unsigned int oi = allInliers.size();
1763 allInliers.resize(allInliers.size() + inliers[1].size());
1764 for(
unsigned int i=0;
i<inliers[1].size(); ++
i)
1766 if(allInliersSet.find(inliers[1][
i]) == allInliersSet.end())
1768 allInliers[oi++] = inliers[1][
i];
1771 allInliers.resize(oi);
1773 std::vector<int> allMatches =
matches[0];
1776 std::set<int> allMatchesSet(allMatches.begin(), allMatches.end());
1777 unsigned int oi = allMatches.size();
1778 allMatches.resize(allMatches.size() +
matches[1].size());
1779 for(
unsigned int i=0;
i<
matches[1].size(); ++
i)
1781 if(allMatchesSet.find(
matches[1][
i]) == allMatchesSet.end())
1786 allMatches.resize(oi);
1791 !transforms[0].
isNull() &&
1792 allInliers.size() &&
1798 UDEBUG(
"Refine with bundle adjustment");
1801 std::map<int, Transform> poses;
1802 std::multimap<int, Link> links;
1803 std::map<int, cv::Point3f> points3DMap;
1806 poses.insert(std::make_pair(2, transforms[0]));
1808 for(
int i=0;
i<2;++
i)
1825 cv::Mat cov = covariances[0].clone();
1828 if(!transforms[1].
isNull() && inliers[1].size())
1830 cov = covariances[1].clone();
1834 std::map<int, Transform> optimizedPoses;
1839 std::map<int, std::vector<CameraModel> > models;
1841 std::vector<CameraModel> cameraModelsFrom;
1855 cameraModelsFrom.push_back(cameraModel);
1863 std::vector<CameraModel> cameraModelsTo;
1877 cameraModelsTo.push_back(cameraModel);
1885 models.insert(std::make_pair(1, cameraModelsFrom));
1886 models.insert(std::make_pair(2, cameraModelsTo));
1888 std::map<int, std::map<int, FeatureBA> > wordReferences;
1889 std::set<int> sbaOutliers;
1891 for(
unsigned int i=0;
i<allInliers.size(); ++
i)
1893 int wordId = allInliers[
i];
1894 int indexFrom = fromSignature.
getWords().find(wordId)->second;
1895 const cv::Point3f & pt3D = fromSignature.
getWords3()[indexFrom];
1899 sbaOutliers.insert(wordId);
1903 points3DMap.insert(std::make_pair(wordId, pt3D));
1905 std::map<int, FeatureBA> ptMap;
1908 cv::KeyPoint kpt = fromSignature.
getWordsKpts()[indexFrom];
1910 int cameraIndex = 0;
1911 const std::vector<CameraModel> &
cam = models.at(1);
1915 float subImageWidth =
cam[0].imageWidth();
1916 cameraIndex =
int(kpt.pt.x / subImageWidth);
1917 kpt.pt.x = kpt.pt.x - (subImageWidth*
float(cameraIndex));
1920 UASSERT(
cam[cameraIndex].isValidForProjection());
1923 ptMap.insert(std::make_pair(1,
FeatureBA(kpt, depthFrom, cv::Mat(), cameraIndex)));
1928 int indexTo = toSignature.
getWords().find(wordId)->second;
1929 cv::KeyPoint kpt = toSignature.
getWordsKpts()[indexTo];
1931 int cameraIndex = 0;
1932 const std::vector<CameraModel> &
cam = models.at(2);
1936 float subImageWidth =
cam[0].imageWidth();
1937 cameraIndex =
int(kpt.pt.x / subImageWidth);
1938 kpt.pt.x = kpt.pt.x - (subImageWidth*
float(cameraIndex));
1941 UASSERT(
cam[cameraIndex].isValidForProjection());
1943 float depthTo = 0.0f;
1949 ptMap.insert(std::make_pair(2,
FeatureBA(kpt, depthTo, cv::Mat(), cameraIndex)));
1952 wordReferences.insert(std::make_pair(wordId, ptMap));
1961 optimizedPoses = sba->
optimizeBA(1, poses, links, models, points3DMap, wordReferences, &sbaOutliers);
1965 if(optimizedPoses.size() == 2 &&
1966 !optimizedPoses.begin()->second.isNull() &&
1967 !optimizedPoses.rbegin()->second.isNull())
1969 UDEBUG(
"Pose optimization: %s -> %s", transforms[0].prettyPrint().
c_str(), optimizedPoses.rbegin()->second.prettyPrint().c_str());
1971 if(sbaOutliers.size())
1973 std::vector<int> newInliers(allInliers.size());
1975 for(
unsigned int i=0;
i<allInliers.size(); ++
i)
1977 if(sbaOutliers.find(allInliers[
i]) == sbaOutliers.end())
1979 newInliers[oi++] = allInliers[
i];
1982 newInliers.resize(oi);
1983 UDEBUG(
"BA outliers ratio %f",
float(sbaOutliers.size())/
float(allInliers.size()));
1984 allInliers = newInliers;
1988 msg =
uFormat(
"Not enough inliers after bundle adjustment %d/%d (matches=%d) between %d and %d",
1989 (
int)allInliers.size(),
_minInliers, (
int)allInliers.size()+sbaOutliers.size(), fromSignature.
id(), toSignature.
id());
1994 transforms[0] = optimizedPoses.rbegin()->second;
2018 info.inliersIDs = allInliers;
2019 info.matchesIDs = allMatches;
2020 inliersCount = (
int)allInliers.size();
2021 matchesCount = (
int)allMatches.size();
2022 if(!transforms[1].
isNull())
2024 transforms[1] = transforms[1].
inverse();
2025 if(transforms[0].
isNull())
2028 covariance = covariances[1];
2033 covariance = (covariances[0]+covariances[1])/2.0
f;
2039 covariance = covariances[0];
2045 std::vector<CameraModel> cameraModelsTo;
2059 if(cameraModelsTo.size() >= 1 && cameraModelsTo[0].isValidForReprojection())
2061 if(cameraModelsTo[0].imageWidth()>0 && cameraModelsTo[0].imageHeight()>0)
2063 pcaData = cv::Mat(allInliers.size(), 2, CV_32FC1);
2067 UERROR(
"Invalid calibration image size (%dx%d), cannot compute inliers distribution! (see %s=%f)", cameraModelsTo[0].imageWidth(), cameraModelsTo[0].imageHeight(), Parameters::kVisMinInliersDistribution().
c_str(),
_minInliersDistributionThr);
2077 std::vector<float> distances;
2080 distances.reserve(allInliers.size());
2082 for(
unsigned int i=0;
i<allInliers.size(); ++
i)
2084 std::multimap<int, int>::const_iterator wordsIter = toSignature.
getWords().find(allInliers[
i]);
2087 const cv::KeyPoint & kpt = toSignature.
getWordsKpts()[wordsIter->second];
2088 int cameraIndex = (
int)(kpt.pt.x / cameraModelsTo[0].imageWidth());
2089 UASSERT_MSG(cameraIndex < (
int)cameraModelsTo.size(),
uFormat(
"cameraIndex=%d (x=%f models=%d camera width = %d)", cameraIndex, kpt.pt.x, (
int)cameraModelsTo.size(), cameraModelsTo[0].imageWidth()).c_str());
2093 const cv::Point3f & pt = toSignature.
getWords3()[wordsIter->second];
2096 UASSERT(cameraModelsTo[cameraIndex].isValidForProjection());
2099 distances.push_back(depth);
2103 if(!pcaData.empty())
2105 float * ptr = pcaData.ptr<
float>(
i, 0);
2106 ptr[0] = (kpt.pt.x-cameraIndex*cameraModelsTo[cameraIndex].imageWidth()-cameraModelsTo[cameraIndex].cx()) / cameraModelsTo[cameraIndex].imageWidth();
2107 ptr[1] = (kpt.pt.y-cameraModelsTo[cameraIndex].cy()) / cameraModelsTo[cameraIndex].imageHeight();
2112 if(!distances.empty())
2114 info.inliersMeanDistance =
uMean(distances);
2118 msg =
uFormat(
"The mean distance of the inliers is over %s threshold (%f)",
2124 if(!
transform.isNull() && !pcaData.empty())
2126 cv::Mat pcaEigenVectors, pcaEigenValues;
2127 cv::PCA pca_analysis(pcaData, cv::Mat(), CV_PCA_DATA_AS_ROW);
2129 info.inliersDistribution = pca_analysis.eigenvalues.at<
float>(0, 1);
2133 msg =
uFormat(
"The distribution (%f) of inliers is under %s threshold (%f)",
2142 msg =
uFormat(
"Missing correspondences for registration (%d->%d). fromWords = %d fromImageEmpty=%d toWords = %d toImageEmpty=%d",
2143 fromSignature.
id(), toSignature.
id(),
2148 info.inliers = inliersCount;
2150 info.matches = matchesCount;
2152 info.covariance = covariance;