52 _minInliers(
Parameters::defaultVisMinInliers()),
53 _inlierDistance(
Parameters::defaultVisInlierDistance()),
54 _iterations(
Parameters::defaultVisIterations()),
55 _refineIterations(
Parameters::defaultVisRefineIterations()),
56 _epipolarGeometryVar(
Parameters::defaultVisEpipolarGeometryVar()),
57 _estimationType(
Parameters::defaultVisEstimationType()),
58 _forwardEstimateOnly(
Parameters::defaultVisForwardEstOnly()),
59 _PnPReprojError(
Parameters::defaultVisPnPReprojError()),
61 _PnPRefineIterations(
Parameters::defaultVisPnPRefineIterations()),
62 _correspondencesApproach(
Parameters::defaultVisCorType()),
63 _flowWinSize(
Parameters::defaultVisCorFlowWinSize()),
64 _flowIterations(
Parameters::defaultVisCorFlowIterations()),
66 _flowMaxLevel(
Parameters::defaultVisCorFlowMaxLevel()),
68 _guessWinSize(
Parameters::defaultVisCorGuessWinSize()),
69 _guessMatchToProjection(
Parameters::defaultVisCorGuessMatchToProjection()),
70 _bundleAdjustment(
Parameters::defaultVisBundleAdjustment()),
71 _depthAsMask(
Parameters::defaultVisDepthAsMask())
122 for(ParametersMap::const_iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
124 std::string group =
uSplit(iter->first,
'/').front();
131 if(
uContains(parameters, Parameters::kVisCorNNType()))
135 if(
uContains(parameters, Parameters::kVisCorNNDR()))
139 if(
uContains(parameters, Parameters::kVisFeatureType()))
143 if(
uContains(parameters, Parameters::kVisMaxFeatures()))
147 if(
uContains(parameters, Parameters::kVisMaxDepth()))
151 if(
uContains(parameters, Parameters::kVisMinDepth()))
155 if(
uContains(parameters, Parameters::kVisRoiRatios()))
159 if(
uContains(parameters, Parameters::kVisSubPixEps()))
163 if(
uContains(parameters, Parameters::kVisSubPixIterations()))
167 if(
uContains(parameters, Parameters::kVisSubPixWinSize()))
171 if(
uContains(parameters, Parameters::kVisGridRows()))
175 if(
uContains(parameters, Parameters::kVisGridCols()))
211 UDEBUG(
"Input(%d): from=%d words, %d 3D words, %d words descriptors, %d kpts, %d kpts3D, %d descriptors, image=%dx%d",
213 (int)fromSignature.
getWords().size(),
222 UDEBUG(
"Input(%d): to=%d words, %d 3D words, %d words descriptors, %d kpts, %d kpts3D, %d descriptors, image=%dx%d",
275 std::vector<cv::KeyPoint> kptsFrom;
279 std::vector<int> orignalWordsFromIds;
280 if(fromSignature.
getWords().empty())
284 if(!imageFrom.empty())
286 if(imageFrom.channels() > 1)
289 cv::cvtColor(imageFrom, tmp, cv::COLOR_BGR2GRAY);
304 kptsFrom = detector->generateKeypoints(
316 kptsFrom.resize(fromSignature.
getWords().size());
317 orignalWordsFromIds.resize(fromSignature.
getWords().size());
319 bool allUniques =
true;
320 for(std::multimap<int, cv::KeyPoint>::const_iterator iter=fromSignature.
getWords().begin(); iter!=fromSignature.
getWords().end(); ++iter)
322 kptsFrom[i] = iter->second;
323 orignalWordsFromIds[i] = iter->first;
324 if(i>0 && iter->first==orignalWordsFromIds[i-1])
332 UDEBUG(
"IDs are not unique, IDs will be regenerated!");
333 orignalWordsFromIds.clear();
337 std::multimap<int, cv::KeyPoint> wordsFrom;
338 std::multimap<int, cv::KeyPoint> wordsTo;
339 std::multimap<int, cv::Point3f> words3From;
340 std::multimap<int, cv::Point3f> words3To;
341 std::multimap<int, cv::Mat> wordsDescFrom;
342 std::multimap<int, cv::Mat> wordsDescTo;
347 if(imageFrom.channels() > 1)
350 cv::cvtColor(imageFrom, tmp, cv::COLOR_BGR2GRAY);
353 if(imageTo.channels() > 1)
356 cv::cvtColor(imageTo, tmp, cv::COLOR_BGR2GRAY);
360 std::vector<cv::Point3f> kptsFrom3D;
361 if(kptsFrom.size() == fromSignature.
getWords3().size())
371 kptsFrom3D = detector->generateKeypoints3D(fromSignature.
sensorData(), kptsFrom);
374 if(!imageFrom.empty() && !imageTo.empty())
376 std::vector<cv::Point2f> cornersFrom;
377 cv::KeyPoint::convert(kptsFrom, cornersFrom);
378 std::vector<cv::Point2f> cornersTo;
384 cv::Mat R = (cv::Mat_<double>(3,3) <<
385 (
double)guessCameraRef.
r11(), (double)guessCameraRef.
r12(), (double)guessCameraRef.
r13(),
386 (double)guessCameraRef.
r21(), (double)guessCameraRef.
r22(), (double)guessCameraRef.
r23(),
387 (double)guessCameraRef.
r31(), (double)guessCameraRef.
r32(), (double)guessCameraRef.
r33());
388 cv::Mat rvec(1,3, CV_64FC1);
389 cv::Rodrigues(R, rvec);
390 cv::Mat tvec = (cv::Mat_<double>(1,3) << (
double)guessCameraRef.
x(), (double)guessCameraRef.
y(), (double)guessCameraRef.
z());
392 cv::projectPoints(kptsFrom3D, rvec, tvec, K, cv::Mat(), cornersTo);
396 UDEBUG(
"guessSet = %d", guessSet?1:0);
397 std::vector<unsigned char> status;
398 std::vector<float> err;
399 UDEBUG(
"cv::calcOpticalFlowPyrLK() begin");
400 cv::calcOpticalFlowPyrLK(
410 cv::OPTFLOW_LK_GET_MIN_EIGENVALS | (guessSet?cv::OPTFLOW_USE_INITIAL_FLOW:0), 1
e-4);
411 UDEBUG(
"cv::calcOpticalFlowPyrLK() end");
413 UASSERT(kptsFrom.size() == kptsFrom3D.size());
414 std::vector<cv::KeyPoint> kptsTo(kptsFrom.size());
415 std::vector<cv::Point3f> kptsFrom3DKept(kptsFrom3D.size());
416 std::vector<int> orignalWordsFromIdsCpy = orignalWordsFromIds;
418 for(
unsigned int i=0; i<status.size(); ++i)
421 uIsInBounds(cornersTo[i].x, 0.0
f,
float(imageTo.cols)) &&
424 if(orignalWordsFromIdsCpy.size())
426 orignalWordsFromIds[ki] = orignalWordsFromIdsCpy[i];
428 kptsFrom[ki] = cv::KeyPoint(cornersFrom[i], 1);
429 kptsFrom3DKept[ki] = kptsFrom3D[i];
430 kptsTo[ki++] = cv::KeyPoint(cornersTo[i], 1);
433 if(orignalWordsFromIds.size())
435 orignalWordsFromIds.resize(ki);
439 kptsFrom3DKept.resize(ki);
440 kptsFrom3D = kptsFrom3DKept;
442 std::vector<cv::Point3f> kptsTo3D;
445 kptsTo3D = detector->generateKeypoints3D(toSignature.
sensorData(), kptsTo);
448 UASSERT(kptsFrom.size() == kptsFrom3DKept.size());
449 UASSERT(kptsFrom.size() == kptsTo.size());
450 UASSERT(kptsTo3D.size() == 0 || kptsTo.size() == kptsTo3D.size());
451 for(
unsigned int i=0; i< kptsFrom3DKept.size(); ++i)
453 int id = orignalWordsFromIds.size()?orignalWordsFromIds[i]:i;
454 wordsFrom.insert(std::make_pair(
id, kptsFrom[i]));
455 words3From.insert(std::make_pair(
id, kptsFrom3DKept[i]));
456 wordsTo.insert(std::make_pair(
id, kptsTo[i]));
459 words3To.insert(std::make_pair(
id, kptsTo3D[i]));
466 if(imageFrom.empty())
468 UERROR(
"Optical flow correspondences requires images in data!");
470 UASSERT(kptsFrom.size() == kptsFrom3D.size());
471 for(
unsigned int i=0; i< kptsFrom3D.size(); ++i)
475 int id = orignalWordsFromIds.size()?orignalWordsFromIds[i]:i;
476 wordsFrom.insert(std::make_pair(
id, kptsFrom[i]));
477 words3From.insert(std::make_pair(
id, kptsFrom3D[i]));
480 toSignature.
sensorData().
setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
488 std::vector<cv::KeyPoint> kptsTo;
494 if(imageTo.channels() > 1)
497 cv::cvtColor(imageTo, tmp, cv::COLOR_BGR2GRAY);
512 kptsTo = detector->generateKeypoints(
527 UDEBUG(
"kptsFrom=%d", (
int)kptsFrom.size());
528 UDEBUG(
"kptsTo=%d", (
int)kptsTo.size());
529 cv::Mat descriptorsFrom;
538 for(std::multimap<int, cv::Mat>::const_iterator iter=fromSignature.
getWordsDescriptors().begin();
542 iter->second.copyTo(descriptorsFrom.row(i));
549 else if(!imageFrom.empty())
551 if(imageFrom.channels() > 1)
554 cv::cvtColor(imageFrom, tmp, cv::COLOR_BGR2GRAY);
557 orignalWordsFromIds.clear();
558 descriptorsFrom = detector->generateDescriptors(imageFrom, kptsFrom);
561 cv::Mat descriptorsTo;
570 for(std::multimap<int, cv::Mat>::const_iterator iter=toSignature.
getWordsDescriptors().begin();
574 iter->second.copyTo(descriptorsTo.row(i));
581 else if(!imageTo.empty())
583 if(imageTo.channels() > 1)
586 cv::cvtColor(imageTo, tmp, cv::COLOR_BGR2GRAY);
590 descriptorsTo = detector->generateDescriptors(imageTo, kptsTo);
595 std::vector<cv::Point3f> kptsFrom3D;
596 std::vector<cv::Point3f> kptsTo3D;
597 if(kptsFrom.size() == fromSignature.
getWords3().size())
607 if(fromSignature.
getWords3().size() && kptsFrom.size() != fromSignature.
getWords3().size())
609 UWARN(
"kptsFrom (%d) is not the same size as fromSignature.getWords3() (%d), there " 610 "is maybe a problem with the logic above (getWords3() should be null or equal to kptsfrom). Regenerating kptsFrom3D...",
616 UWARN(
"kptsFrom (%d) is not the same size as fromSignature.sensorData().keypoints3D() (%d), there " 617 "is maybe a problem with the logic above (keypoints3D should be null or equal to kptsfrom). Regenerating kptsFrom3D...",
621 kptsFrom3D = detector->generateKeypoints3D(fromSignature.
sensorData(), kptsFrom);
622 UDEBUG(
"generated kptsFrom3D=%d", (
int)kptsFrom3D.size());
623 if(detector->getMinDepth() > 0.0f || detector->getMaxDepth() > 0.0f)
626 UASSERT((
int)kptsFrom.size() == descriptorsFrom.rows &&
627 kptsFrom3D.size() == kptsFrom.size());
628 std::vector<cv::KeyPoint> validKeypoints(kptsFrom.size());
629 std::vector<cv::Point3f> validKeypoints3D(kptsFrom.size());
630 cv::Mat validDescriptors(descriptorsFrom.size(), descriptorsFrom.type());
631 std::vector<int> validKeypointsIds;
632 if(orignalWordsFromIds.size())
634 validKeypointsIds.resize(kptsFrom.size());
638 for(
unsigned int i=0; i<kptsFrom3D.size(); ++i)
642 validKeypoints[oi] = kptsFrom[i];
643 validKeypoints3D[oi] = kptsFrom3D[i];
644 if(orignalWordsFromIds.size())
646 validKeypointsIds[oi] = orignalWordsFromIds[i];
648 descriptorsFrom.row(i).copyTo(validDescriptors.row(oi));
652 UDEBUG(
"Removed %d invalid 3D points", (
int)kptsFrom3D.size()-oi);
653 validKeypoints.resize(oi);
654 validKeypoints3D.resize(oi);
655 kptsFrom = validKeypoints;
656 kptsFrom3D = validKeypoints3D;
658 if(orignalWordsFromIds.size())
660 validKeypointsIds.resize(oi);
661 orignalWordsFromIds = validKeypointsIds;
663 descriptorsFrom = validDescriptors.rowRange(0, oi).clone();
667 if(kptsTo.size() == toSignature.
getWords3().size())
679 UWARN(
"kptsTo (%d) is not the same size as toSignature.getWords3() (%d), there " 680 "is maybe a problem with the logic above (getWords3() should be null or equal to kptsTo). Regenerating kptsTo3D...",
686 UWARN(
"kptsTo (%d) is not the same size as toSignature.sensorData().keypoints3D() (%d), there " 687 "is maybe a problem with the logic above (keypoints3D() should be null or equal to kptsTo). Regenerating kptsTo3D...",
691 kptsTo3D = detector->generateKeypoints3D(toSignature.
sensorData(), kptsTo);
692 if(kptsTo3D.size() && (detector->getMinDepth() > 0.0f || detector->getMaxDepth() > 0.0f))
696 UASSERT((
int)kptsTo.size() == descriptorsTo.rows &&
697 kptsTo3D.size() == kptsTo.size());
698 std::vector<cv::KeyPoint> validKeypoints(kptsTo.size());
699 std::vector<cv::Point3f> validKeypoints3D(kptsTo.size());
700 cv::Mat validDescriptors(descriptorsTo.size(), descriptorsTo.type());
703 for(
unsigned int i=0; i<kptsTo3D.size(); ++i)
707 validKeypoints[oi] = kptsTo[i];
708 validKeypoints3D[oi] = kptsTo3D[i];
709 descriptorsTo.row(i).copyTo(validDescriptors.row(oi));
713 UDEBUG(
"Removed %d invalid 3D points", (
int)kptsTo3D.size()-oi);
714 validKeypoints.resize(oi);
715 validKeypoints3D.resize(oi);
716 kptsTo = validKeypoints;
717 kptsTo3D = validKeypoints3D;
718 descriptorsTo = validDescriptors.rowRange(0, oi).clone();
722 UASSERT(kptsFrom.empty() || descriptorsFrom.rows == 0 || int(kptsFrom.size()) == descriptorsFrom.rows);
727 UDEBUG(
"descriptorsFrom=%d", descriptorsFrom.rows);
728 UDEBUG(
"descriptorsTo=%d", descriptorsTo.rows);
731 if(descriptorsFrom.rows > 0 && descriptorsTo.rows > 0)
733 cv::Size imageSize = imageTo.size();
734 bool isCalibrated =
false;
735 if(imageSize.height == 0 || imageSize.width == 0)
740 isCalibrated = imageSize.height != 0 && imageSize.width != 0 &&
749 UASSERT((
int)kptsTo.size() == descriptorsTo.rows);
750 UASSERT((
int)kptsFrom3D.size() == descriptorsFrom.rows);
755 UFATAL(
"Guess reprojection feature matching is not supported for multiple cameras.");
760 cv::Mat R = (cv::Mat_<double>(3,3) <<
761 (
double)guessCameraRef.
r11(), (double)guessCameraRef.
r12(), (double)guessCameraRef.
r13(),
762 (double)guessCameraRef.
r21(), (double)guessCameraRef.
r22(), (double)guessCameraRef.
r23(),
763 (double)guessCameraRef.
r31(), (double)guessCameraRef.
r32(), (double)guessCameraRef.
r33());
764 cv::Mat rvec(1,3, CV_64FC1);
765 cv::Rodrigues(R, rvec);
766 cv::Mat tvec = (cv::Mat_<double>(1,3) << (
double)guessCameraRef.
x(), (double)guessCameraRef.
y(), (double)guessCameraRef.
z());
768 std::vector<cv::Point2f> projected;
769 cv::projectPoints(kptsFrom3D, rvec, tvec, K, cv::Mat(), projected);
770 UDEBUG(
"Projected points=%d", (
int)projected.size());
772 UASSERT((
int)projected.size() == descriptorsFrom.rows);
773 std::vector<cv::Point2f> cornersProjected(projected.size());
774 std::vector<int> projectedIndexToDescIndex(projected.size());
776 for(
unsigned int i=0; i<projected.size(); ++i)
778 if(
uIsInBounds(projected[i].x, 0.0
f,
float(imageSize.width-1)) &&
779 uIsInBounds(projected[i].y, 0.0
f,
float(imageSize.height-1)) &&
782 projectedIndexToDescIndex[oi] = i;
783 cornersProjected[oi++] = projected[i];
786 projectedIndexToDescIndex.resize(oi);
787 cornersProjected.resize(oi);
788 UDEBUG(
"corners in frame=%d", (
int)cornersProjected.size());
793 if(cornersProjected.size())
803 std::vector< std::vector<size_t> > indices;
804 std::vector<std::vector<float> > dists;
806 std::vector<cv::Point2f> pointsTo;
807 cv::KeyPoint::convert(kptsTo, pointsTo);
811 UASSERT(indices.size() == pointsToMat.rows);
812 UASSERT(descriptorsFrom.cols == descriptorsTo.cols);
813 UASSERT(descriptorsFrom.rows == (
int)kptsFrom.size());
814 UASSERT((
int)pointsToMat.rows == descriptorsTo.rows);
815 UASSERT(pointsToMat.rows == kptsTo.size());
816 UDEBUG(
"radius search done for guess");
819 int newToId = orignalWordsFromIds.size()?orignalWordsFromIds.back():descriptorsFrom.rows;
820 std::map<int,int> addedWordsFrom;
821 std::map<int, int> duplicates;
823 cv::Mat descriptors(10, descriptorsTo.cols, descriptorsTo.type());
824 for(
unsigned int i = 0; i < pointsToMat.rows; ++i)
828 int octave = kptsTo[i].octave;
829 int matchedIndex = -1;
830 if(indices[i].size() >= 2)
832 std::vector<int> descriptorsIndices(indices[i].size());
834 if((
int)indices[i].size() > descriptors.rows)
836 descriptors.resize(indices[i].size());
838 for(
unsigned int j=0; j<indices[i].size(); ++j)
840 if(kptsFrom.at(projectedIndexToDescIndex[indices[i].at(j)]).octave==octave)
842 descriptorsFrom.row(projectedIndexToDescIndex[indices[i].at(j)]).copyTo(descriptors.row(oi));
843 descriptorsIndices[oi++] = indices[i].at(j);
846 descriptorsIndices.resize(oi);
849 std::vector<std::vector<cv::DMatch> > matches;
850 cv::BFMatcher matcher(descriptors.type()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR);
851 matcher.knnMatch(descriptorsTo.row(i), cv::Mat(descriptors, cv::Range(0, oi)), matches, 2);
853 UASSERT(matches[0].size() == 2);
856 matchedIndex = descriptorsIndices.at(matches[0].at(0).trainIdx);
861 matchedIndex = descriptorsIndices[0];
864 else if(indices[i].size() == 1 &&
865 kptsFrom.at(projectedIndexToDescIndex[indices[i].at(0)]).octave == octave)
867 matchedIndex = indices[i].at(0);
870 if(matchedIndex >= 0)
872 matchedIndex = projectedIndexToDescIndex[matchedIndex];
873 int id = orignalWordsFromIds.size()?orignalWordsFromIds[matchedIndex]:matchedIndex;
875 if(addedWordsFrom.find(matchedIndex) != addedWordsFrom.end())
877 id = addedWordsFrom.at(matchedIndex);
878 duplicates.insert(std::make_pair(matchedIndex,
id));
882 addedWordsFrom.insert(std::make_pair(matchedIndex,
id));
886 wordsFrom.insert(std::make_pair(
id, kptsFrom[matchedIndex]));
888 words3From.insert(std::make_pair(
id, kptsFrom3D[matchedIndex]));
889 wordsDescFrom.insert(std::make_pair(
id, descriptorsFrom.row(matchedIndex)));
892 wordsTo.insert(std::make_pair(
id, kptsTo[i]));
893 wordsDescTo.insert(std::make_pair(
id, descriptorsTo.row(i)));
896 words3To.insert(std::make_pair(
id, kptsTo3D[i]));
902 wordsTo.insert(wordsTo.end(), std::make_pair(newToId, kptsTo[i]));
903 wordsDescTo.insert(wordsDescTo.end(), std::make_pair(newToId, descriptorsTo.row(i)));
906 words3To.insert(words3To.end(), std::make_pair(newToId, kptsTo3D[i]));
914 UDEBUG(
"addedWordsFrom=%d/%d (duplicates=%d, newWords=%d), kptsTo=%d, wordsTo=%d, words3From=%d",
915 (
int)addedWordsFrom.size(), (int)cornersProjected.size(), (int)duplicates.size(), newWords,
916 (int)kptsTo.size(), (int)wordsTo.size(), (int)words3From.size());
919 int addWordsFromNotMatched = 0;
920 for(
unsigned int i=0; i<kptsFrom3D.size(); ++i)
922 if(
util3d::isFinite(kptsFrom3D[i]) && addedWordsFrom.find(i) == addedWordsFrom.end())
924 int id = orignalWordsFromIds.size()?orignalWordsFromIds[i]:i;
925 wordsFrom.insert(wordsFrom.end(), std::make_pair(
id, kptsFrom[i]));
926 wordsDescFrom.insert(wordsDescFrom.end(), std::make_pair(
id, descriptorsFrom.row(i)));
927 words3From.insert(words3From.end(), std::make_pair(
id, kptsFrom3D[i]));
929 ++addWordsFromNotMatched;
932 UDEBUG(
"addWordsFromNotMatched=%d -> words3From=%d", addWordsFromNotMatched, (
int)words3From.size());
937 std::vector<cv::Point2f> pointsTo;
938 cv::KeyPoint::convert(kptsTo, pointsTo);
943 std::vector< std::vector<size_t> > indices;
944 std::vector<std::vector<float> > dists;
949 UASSERT(indices.size() == cornersProjectedMat.rows);
950 UASSERT(descriptorsFrom.cols == descriptorsTo.cols);
951 UASSERT(descriptorsFrom.rows == (
int)kptsFrom.size());
952 UASSERT((
int)pointsToMat.rows == descriptorsTo.rows);
953 UASSERT(pointsToMat.rows == kptsTo.size());
954 UDEBUG(
"radius search done for guess");
957 std::set<int> addedWordsTo;
958 std::set<int> addedWordsFrom;
959 std::set<int> indicesToIgnore;
960 double bruteForceTotalTime = 0.0;
961 double bruteForceDescCopy = 0.0;
963 cv::Mat descriptors(10, descriptorsTo.cols, descriptorsTo.type());
964 for(
unsigned int i = 0; i < cornersProjectedMat.rows; ++i)
966 int matchedIndexFrom = projectedIndexToDescIndex[i];
968 if(indices[i].size())
970 info.
projectedIDs.push_back(orignalWordsFromIds.size()?orignalWordsFromIds[matchedIndexFrom]:matchedIndexFrom);
975 int matchedIndexTo = -1;
976 if(indices[i].size() >= 2)
979 std::vector<int> descriptorsIndices(indices[i].size());
981 if((
int)indices[i].size() > descriptors.rows)
983 descriptors.resize(indices[i].size());
985 std::list<int> indicesToIgnoretmp;
986 for(
unsigned int j=0; j<indices[i].size(); ++j)
988 int octave = kptsTo[indices[i].at(j)].octave;
989 if(kptsFrom.at(matchedIndexFrom).octave==octave)
991 descriptorsTo.row(indices[i].at(j)).copyTo(descriptors.row(oi));
992 descriptorsIndices[oi++] = indices[i].at(j);
994 indicesToIgnoretmp.push_back(indices[i].at(j));
997 bruteForceDescCopy += bruteForceTimer.
ticks();
1000 std::vector<std::vector<cv::DMatch> > matches;
1001 cv::BFMatcher matcher(descriptors.type()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR);
1002 matcher.knnMatch(descriptorsFrom.row(matchedIndexFrom), cv::Mat(descriptors, cv::Range(0, oi)), matches, 2);
1004 UASSERT(matches[0].size() == 2);
1005 bruteForceTotalTime+=bruteForceTimer.
elapsed();
1008 matchedIndexTo = descriptorsIndices.at(matches[0].at(0).trainIdx);
1010 indicesToIgnore.insert(indicesToIgnore.begin(), indicesToIgnore.end());
1015 matchedIndexTo = descriptorsIndices[0];
1018 else if(indices[i].size() == 1)
1020 int octave = kptsTo[indices[i].at(0)].octave;
1021 if(kptsFrom.at(matchedIndexFrom).octave == octave)
1023 matchedIndexTo = indices[i].at(0);
1027 int id = orignalWordsFromIds.size()?orignalWordsFromIds[matchedIndexFrom]:matchedIndexFrom;
1028 addedWordsFrom.insert(addedWordsFrom.end(), matchedIndexFrom);
1032 wordsFrom.insert(wordsFrom.end(), std::make_pair(
id, kptsFrom[matchedIndexFrom]));
1034 words3From.insert(words3From.end(), std::make_pair(
id, kptsFrom3D[matchedIndexFrom]));
1035 wordsDescFrom.insert(wordsDescFrom.end(), std::make_pair(
id, descriptorsFrom.row(matchedIndexFrom)));
1038 matchedIndexTo >= 0 &&
1039 addedWordsTo.find(matchedIndexTo) == addedWordsTo.end())
1041 addedWordsTo.insert(matchedIndexTo);
1043 wordsTo.insert(wordsTo.end(), std::make_pair(
id, kptsTo[matchedIndexTo]));
1044 wordsDescTo.insert(wordsDescTo.end(), std::make_pair(
id, descriptorsTo.row(matchedIndexTo)));
1047 words3To.insert(words3To.end(), std::make_pair(
id, kptsTo3D[matchedIndexTo]));
1052 UDEBUG(
"bruteForceDescCopy=%fs, bruteForceTotalTime=%fs", bruteForceDescCopy, bruteForceTotalTime);
1055 for(
unsigned int i=0; i<kptsFrom3D.size(); ++i)
1057 if(
util3d::isFinite(kptsFrom3D[i]) && addedWordsFrom.find(i) == addedWordsFrom.end())
1059 int id = orignalWordsFromIds.size()?orignalWordsFromIds[i]:i;
1060 wordsFrom.insert(wordsFrom.end(), std::make_pair(
id, kptsFrom[i]));
1061 wordsDescFrom.insert(wordsDescFrom.end(), std::make_pair(
id, descriptorsFrom.row(i)));
1062 words3From.insert(words3From.end(), std::make_pair(
id, kptsFrom3D[i]));
1066 int newToId = orignalWordsFromIds.size()?orignalWordsFromIds.back():descriptorsFrom.rows;
1067 for(
unsigned int i = 0; i < kptsTo.size(); ++i)
1069 if(addedWordsTo.find(i) == addedWordsTo.end() && indicesToIgnore.find(i) == indicesToIgnore.end())
1071 wordsTo.insert(wordsTo.end(), std::make_pair(newToId, kptsTo[i]));
1072 wordsDescTo.insert(wordsDescTo.end(), std::make_pair(newToId, descriptorsTo.row(i)));
1075 words3To.insert(words3To.end(), std::make_pair(newToId, kptsTo3D[i]));
1084 UWARN(
"All projected points are outside the camera. Guess (%s) is wrong or images are not overlapping.", guess.
prettyPrint().c_str());
1090 if(guessSet &&
_guessWinSize > 0 && kptsFrom3D.size() && !isCalibrated)
1094 UWARN(
"Finding correspondences with the guess cannot " 1095 "be done with multiple cameras, global matching is " 1096 "done instead. Please set \"%s\" to 0 to avoid this warning.",
1097 Parameters::kVisCorGuessWinSize().c_str());
1101 UWARN(
"Calibration not found! Finding correspondences " 1102 "with the guess cannot be done, global matching is " 1110 std::list<int> fromWordIds;
1111 for (
int i = 0; i < descriptorsFrom.rows; ++i)
1113 int id = orignalWordsFromIds.size() ? orignalWordsFromIds[i] : i;
1115 fromWordIds.push_back(
id);
1118 std::list<int> toWordIds;
1119 if(descriptorsTo.rows)
1122 toWordIds = dictionary.
addNewWords(descriptorsTo, 2);
1124 dictionary.
clear(
false);
1126 std::multiset<int> fromWordIdsSet(fromWordIds.begin(), fromWordIds.end());
1127 std::multiset<int> toWordIdsSet(toWordIds.begin(), toWordIds.end());
1129 UASSERT(kptsFrom3D.empty() || fromWordIds.size() == kptsFrom3D.size());
1130 UASSERT(
int(fromWordIds.size()) == descriptorsFrom.rows);
1132 for(std::list<int>::iterator iter=fromWordIds.begin(); iter!=fromWordIds.end(); ++iter)
1134 if(fromWordIdsSet.count(*iter) == 1)
1136 if (kptsFrom.size())
1138 wordsFrom.insert(std::make_pair(*iter, kptsFrom[i]));
1140 if(kptsFrom3D.size())
1142 words3From.insert(std::make_pair(*iter, kptsFrom3D[i]));
1144 wordsDescFrom.insert(std::make_pair(*iter, descriptorsFrom.row(i)));
1148 UASSERT(kptsTo3D.size() == 0 || kptsTo3D.size() == kptsTo.size());
1149 UASSERT(toWordIds.size() == kptsTo.size());
1150 UASSERT(
int(toWordIds.size()) == descriptorsTo.rows);
1152 for(std::list<int>::iterator iter=toWordIds.begin(); iter!=toWordIds.end(); ++iter)
1154 if(toWordIdsSet.count(*iter) == 1)
1156 wordsTo.insert(std::make_pair(*iter, kptsTo[i]));
1157 wordsDescTo.insert(std::make_pair(*iter, descriptorsTo.row(i)));
1160 words3To.insert(std::make_pair(*iter, kptsTo3D[i]));
1167 else if(descriptorsFrom.rows)
1170 UASSERT(kptsFrom3D.empty() || int(kptsFrom3D.size()) == descriptorsFrom.rows);
1171 for(
int i=0; i<descriptorsFrom.rows; ++i)
1173 wordsFrom.insert(std::make_pair(i, kptsFrom[i]));
1174 wordsDescFrom.insert(std::make_pair(i, descriptorsFrom.row(i)));
1175 if(kptsFrom3D.size())
1177 words3From.insert(std::make_pair(i, kptsFrom3D[i]));
1195 cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
1196 int inliersCount = 0;
1197 int matchesCount = 0;
1203 std::vector<int> inliers[2];
1204 std::vector<int> matches[2];
1205 cv::Mat covariances[2];
1206 covariances[0] = cv::Mat::eye(6,6,CV_64FC1);
1207 covariances[1] = cv::Mat::eye(6,6,CV_64FC1);
1215 signatureA = &fromSignature;
1216 signatureB = &toSignature;
1220 signatureA = &toSignature;
1221 signatureB = &fromSignature;
1230 UERROR(
"Calibrated camera required (multi-cameras not supported).");
1240 double variance = 1.0f;
1254 covariances[dir] *= variance;
1255 inliers[dir] =
uKeys(inliers3D);
1257 if(!cameraTransform.
isNull())
1265 transforms[dir] = cameraTransform.
to3DoF();
1269 transforms[dir] = cameraTransform;
1286 msg =
uFormat(
"No camera transform found");
1290 else if(signatureA->
getWords().size() == 0)
1292 msg =
uFormat(
"No enough features (%d)", (
int)signatureA->
getWords().size());
1297 msg =
uFormat(
"No camera model");
1308 UERROR(
"Calibrated camera required (multi-cameras not supported). Id=%d Models=%d StereoModel=%d weight=%d",
1324 std::vector<int> inliersV;
1325 std::vector<int> matchesV;
1340 inliers[dir] = inliersV;
1341 matches[dir] = matchesV;
1342 if(transforms[dir].
isNull())
1344 msg =
uFormat(
"Not enough inliers %d/%d (matches=%d) between %d and %d",
1345 (
int)inliers[dir].size(),
_minInliers, (
int)matches[dir].size(), signatureA->
id(), signatureB->
id());
1350 transforms[dir] = transforms[dir].
to3DoF();
1355 msg =
uFormat(
"Not enough features in images (old=%d, new=%d, min=%d)",
1369 std::vector<int> inliersV;
1370 std::vector<int> matchesV;
1381 inliers[dir] = inliersV;
1382 matches[dir] = matchesV;
1383 if(transforms[dir].
isNull())
1385 msg =
uFormat(
"Not enough inliers %d/%d (matches=%d) between %d and %d",
1386 (
int)inliers[dir].size(),
_minInliers, (
int)matches[dir].size(), signatureA->
id(), signatureB->
id());
1391 transforms[dir] = transforms[dir].
to3DoF();
1396 msg =
uFormat(
"Not enough 3D features in images (old=%d, new=%d, min=%d)",
1405 UDEBUG(
"from->to=%s", transforms[0].prettyPrint().c_str());
1406 UDEBUG(
"to->from=%s", transforms[1].prettyPrint().c_str());
1409 std::vector<int> allInliers = inliers[0];
1410 if(inliers[1].size())
1412 std::set<int> allInliersSet(allInliers.begin(), allInliers.end());
1413 unsigned int oi = allInliers.size();
1414 allInliers.resize(allInliers.size() + inliers[1].size());
1415 for(
unsigned int i=0; i<inliers[1].size(); ++i)
1417 if(allInliersSet.find(inliers[1][i]) == allInliersSet.end())
1419 allInliers[oi++] = inliers[1][i];
1422 allInliers.resize(oi);
1424 std::vector<int> allMatches = matches[0];
1425 if(matches[1].size())
1427 std::set<int> allMatchesSet(allMatches.begin(), allMatches.end());
1428 unsigned int oi = allMatches.size();
1429 allMatches.resize(allMatches.size() + matches[1].size());
1430 for(
unsigned int i=0; i<matches[1].size(); ++i)
1432 if(allMatchesSet.find(matches[1][i]) == allMatchesSet.end())
1434 allMatches[oi++] = matches[1][i];
1437 allMatches.resize(oi);
1442 !transforms[0].
isNull() &&
1443 allInliers.size() &&
1451 std::map<int, Transform> poses;
1452 std::multimap<int, Link> links;
1453 std::map<int, cv::Point3f> points3DMap;
1456 poses.insert(std::make_pair(2, transforms[0]));
1458 for(
int i=0;i<2;++i)
1460 UASSERT(covariances[i].cols==6 && covariances[i].rows == 6 && covariances[i].type() == CV_64FC1);
1475 cv::Mat cov = covariances[0].clone();
1478 if(!transforms[1].
isNull() && inliers[1].size())
1480 cov = covariances[1].clone();
1484 std::map<int, Transform> optimizedPoses;
1489 std::map<int, CameraModel> models;
1498 cameraModelFrom.
fy(),
1499 cameraModelFrom.
cx(),
1500 cameraModelFrom.
cy(),
1530 if(invLocalTransformFrom.
isNull())
1532 invLocalTransformFrom = invLocalTransformTo;
1535 models.insert(std::make_pair(1, cameraModelFrom.
isValidForProjection()?cameraModelFrom:cameraModelTo));
1536 models.insert(std::make_pair(2, cameraModelTo));
1538 std::map<int, std::map<int, cv::Point3f> > wordReferences;
1539 for(
unsigned int i=0; i<allInliers.size(); ++i)
1541 int wordId = allInliers[i];
1542 const cv::Point3f & pt3D = fromSignature.
getWords3().find(wordId)->second;
1543 points3DMap.insert(std::make_pair(wordId, pt3D));
1545 std::map<int, cv::Point3f> ptMap;
1549 const cv::Point2f & kpt = fromSignature.
getWords().find(wordId)->second.pt;
1550 ptMap.insert(std::make_pair(1,cv::Point3f(kpt.x, kpt.y, depthFrom)));
1555 const cv::Point2f & kpt = toSignature.
getWords().find(wordId)->second.pt;
1557 ptMap.insert(std::make_pair(2,cv::Point3f(kpt.x, kpt.y, depthTo)));
1560 wordReferences.insert(std::make_pair(wordId, ptMap));
1569 std::set<int> sbaOutliers;
1570 optimizedPoses = sba->
optimizeBA(1, poses, links, models, points3DMap, wordReferences, &sbaOutliers);
1574 if(optimizedPoses.size() == 2 &&
1575 !optimizedPoses.begin()->second.isNull() &&
1576 !optimizedPoses.rbegin()->second.isNull())
1578 UDEBUG(
"Pose optimization: %s -> %s", transforms[0].prettyPrint().c_str(), optimizedPoses.rbegin()->second.
prettyPrint().c_str());
1580 if(sbaOutliers.size())
1582 std::vector<int> newInliers(allInliers.size());
1584 for(
unsigned int i=0; i<allInliers.size(); ++i)
1586 if(sbaOutliers.find(allInliers[i]) == sbaOutliers.end())
1588 newInliers[oi++] = allInliers[i];
1591 newInliers.resize(oi);
1592 UDEBUG(
"BA outliers ratio %f",
float(sbaOutliers.size())/
float(allInliers.size()));
1593 allInliers = newInliers;
1597 msg =
uFormat(
"Not enough inliers after bundle adjustment %d/%d (matches=%d) between %d and %d",
1598 (
int)allInliers.size(),
_minInliers, fromSignature.
id(), toSignature.
id());
1603 transforms[0] = optimizedPoses.rbegin()->second;
1629 inliersCount = (int)allInliers.size();
1630 matchesCount = (int)allMatches.size();
1631 if(!transforms[1].
isNull())
1633 transforms[1] = transforms[1].
inverse();
1634 if(transforms[0].
isNull())
1636 transform = transforms[1];
1637 covariance = covariances[1];
1641 transform = transforms[0].
interpolate(0.5
f, transforms[1]);
1642 covariance = (covariances[0]+covariances[1])/2.0
f;
1647 transform = transforms[0];
1648 covariance = covariances[0];
1653 UWARN(
"Missing correspondences for registration (%d->%d). fromWords = %d fromImageEmpty=%d toWords = %d toImageEmpty=%d",
1654 fromSignature.
id(), toSignature.
id(),
static bool isFeatureParameter(const std::string ¶m)
const std::multimap< int, cv::Point3f > & getWords3() const
bool RTABMAP_EXP isFinite(const cv::Point3f &pt)
static bool parse(const ParametersMap ¶meters, const std::string &key, bool &value)
int _correspondencesApproach
ParametersMap _bundleParameters
std::map< K, V > uMultimapToMapUnique(const std::multimap< K, V > &m)
Transform RTABMAP_EXP estimateMotion3DTo2D(const std::map< int, cv::Point3f > &words3A, const std::map< int, cv::KeyPoint > &words2B, const CameraModel &cameraModel, int minInliers=10, int iterations=100, double reprojError=5., int flagsPnP=0, int pnpRefineIterations=1, const Transform &guess=Transform::getIdentity(), const std::map< int, cv::Point3f > &words3B=std::map< int, cv::Point3f >(), cv::Mat *covariance=0, std::vector< int > *matchesOut=0, std::vector< int > *inliersOut=0)
std::map< int, cv::Point3f > RTABMAP_EXP generateWords3DMono(const std::map< int, cv::KeyPoint > &kpts, const std::map< int, cv::KeyPoint > &previousKpts, const CameraModel &cameraModel, Transform &cameraTransform, int pnpIterations=100, float pnpReprojError=8.0f, int pnpFlags=0, int pnpRefineIterations=1, float ransacParam1=3.0f, float ransacParam2=0.99f, const std::map< int, cv::Point3f > &refGuess3D=std::map< int, cv::Point3f >(), double *variance=0)
const cv::Size & imageSize() const
cv::Point3f RTABMAP_EXP transformPoint(const cv::Point3f &pt, const Transform &transform)
std::vector< K > uKeys(const std::multimap< K, V > &mm)
bool _guessMatchToProjection
const std::multimap< int, cv::KeyPoint > & getWords() const
void setWords3(const std::multimap< int, cv::Point3f > &words3)
bool uIsInBounds(const T &value, const T &low, const T &high)
void setWords(const std::multimap< int, cv::KeyPoint > &words)
virtual ~RegistrationVis()
bool _forwardEstimateOnly
static double COVARIANCE_EPSILON
std::pair< std::string, std::string > ParametersPair
float _epipolarGeometryVar
GLM_FUNC_DECL genType e()
Feature2D * createFeatureDetector() const
const cv::Mat & descriptors() const
virtual std::list< int > addNewWords(const cv::Mat &descriptors, int signatureId)
std::map< std::string, std::string > ParametersMap
GLM_FUNC_DECL genType::value_type distance(genType const &p0, genType const &p1)
const std::vector< cv::KeyPoint > & keypoints() const
const std::multimap< int, cv::Mat > & getWordsDescriptors() const
Basic mathematics functions.
std::vector< int > inliersIDs
Some conversion functions.
const cv::Mat & imageRaw() const
std::list< std::string > uSplit(const std::string &str, char separator= ' ')
GLM_FUNC_DECL bool isNull(detail::tmat2x2< T, P > const &m, T const &epsilon)
cv::Mat RTABMAP_EXP interpolate(const cv::Mat &image, int factor, float depthErrorRatio=0.02f)
#define UASSERT(condition)
Wrappers of STL for convenient functions.
bool isValidForProjection() const
const CameraModel & left() const
static Feature2D * create(const ParametersMap ¶meters=ParametersMap())
#define UASSERT_MSG(condition, msg_str)
std::vector< int > projectedIDs
virtual void parseParameters(const ParametersMap ¶meters)
void setWordsDescriptors(const std::multimap< int, cv::Mat > &descriptors)
void clear(bool printWarningsIfNotEmpty=true)
Transform RTABMAP_EXP estimateMotion3DTo3D(const std::map< int, cv::Point3f > &words3A, const std::map< int, cv::Point3f > &words3B, int minInliers=10, double inliersDistance=0.1, int iterations=100, int refineIterations=5, cv::Mat *covariance=0, std::vector< int > *matchesOut=0, std::vector< int > *inliersOut=0)
virtual Transform computeTransformationImpl(Signature &from, Signature &to, Transform guess, RegistrationInfo &info) const
bool uContains(const std::list< V > &list, const V &value)
bool isValidForProjection() const
static const ParametersMap & getDefaultParameters()
const std::vector< CameraModel > & cameraModels() const
std::vector< V > uValues(const std::multimap< K, V > &mm)
virtual std::map< int, Transform > optimizeBA(int rootId, const std::map< int, Transform > &poses, const std::multimap< int, Link > &links, const std::map< int, CameraModel > &models, std::map< int, cv::Point3f > &points3DMap, const std::map< int, std::map< int, cv::Point3f > > &wordReferences, std::set< int > *outliers=0)
SensorData & sensorData()
virtual void parseParameters(const ParametersMap ¶meters)
ULogger class and convenient macros.
const StereoCameraModel & stereoCameraModel() const
ParametersMap _featureParameters
std::vector< int > matchesIDs
virtual void addWord(VisualWord *vw)
const Transform & localTransform() const
std::string UTILITE_EXP uFormat(const char *fmt,...)
static Optimizer * create(const ParametersMap ¶meters)
void setFeatures(const std::vector< cv::KeyPoint > &keypoints, const std::vector< cv::Point3f > &keypoints3D, const cv::Mat &descriptors)
RegistrationVis(const ParametersMap ¶meters=ParametersMap(), Registration *child=0)
const std::vector< cv::Point3f > & keypoints3D() const
void uInsert(std::map< K, V > &map, const std::pair< K, V > &pair)
const Transform & localTransform() const
GLM_FUNC_DECL matType< T, P > inverse(matType< T, P > const &m)