42 #include <visp3/vision/vpKeyPoint.h>
43 #include <visp3/core/vpIoTools.h>
45 #if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
47 #if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
48 # include <opencv2/calib3d/calib3d.hpp>
60 inline cv::DMatch knnToDMatch(
const std::vector<cv::DMatch> &knnMatches) {
61 if(knnMatches.size() > 0) {
74 inline vpImagePoint matchRansacToVpImage(
const std::pair<cv::KeyPoint, cv::Point3f> &pair) {
88 uint16_t reverse16bits(
const uint16_t n) {
89 unsigned char *np = (
unsigned char *) &n;
91 return ((uint16_t) np[0] << 8) | (uint16_t) np[1];
94 uint32_t reverse32bits(
const uint32_t n) {
95 unsigned char *np = (
unsigned char *) &n;
97 return ((uint32_t) np[0] << 24) | ((uint32_t) np[1] << 16)
98 | ((uint32_t) np[2] << 8) | (uint32_t) np[3];
101 float reverseFloat(
const float f) {
108 dat2.b[0] = dat1.b[3];
109 dat2.b[1] = dat1.b[2];
110 dat2.b[2] = dat1.b[1];
111 dat2.b[3] = dat1.b[0];
115 double reverseDouble(
const double d) {
122 dat2.b[0] = dat1.b[7];
123 dat2.b[1] = dat1.b[6];
124 dat2.b[2] = dat1.b[5];
125 dat2.b[3] = dat1.b[4];
126 dat2.b[4] = dat1.b[3];
127 dat2.b[5] = dat1.b[2];
128 dat2.b[6] = dat1.b[1];
129 dat2.b[7] = dat1.b[0];
133 void writeBinaryUShortLE(std::ofstream &file,
const unsigned short ushort_value) {
136 uint16_t reverse_ushort = reverse16bits(ushort_value);
137 file.write((
char *)(&reverse_ushort),
sizeof(reverse_ushort));
139 file.write((
char *)(&ushort_value),
sizeof(ushort_value));
143 void writeBinaryShortLE(std::ofstream &file,
const short short_value) {
146 uint16_t reverse_short = reverse16bits((uint16_t) short_value);
147 file.write((
char *)(&reverse_short),
sizeof(reverse_short));
149 file.write((
char *)(&short_value),
sizeof(short_value));
153 void writeBinaryUIntLE(std::ofstream &file,
const unsigned int uint_value) {
156 if(
sizeof(uint_value) == 4) {
157 uint32_t reverse_uint = reverse32bits(uint_value);
158 file.write((
char *)(&reverse_uint),
sizeof(reverse_uint));
160 uint16_t reverse_uint = reverse16bits(uint_value);
161 file.write((
char *)(&reverse_uint),
sizeof(reverse_uint));
164 file.write((
char *)(&uint_value),
sizeof(uint_value));
168 void writeBinaryIntLE(std::ofstream &file,
const int int_value) {
171 if(
sizeof(int_value) == 4) {
172 uint32_t reverse_int = reverse32bits((uint32_t) int_value);
173 file.write((
char *)(&reverse_int),
sizeof(reverse_int));
175 uint16_t reverse_int = reverse16bits((uint16_t) int_value);
176 file.write((
char *)(&reverse_int),
sizeof(reverse_int));
179 file.write((
char *)(&int_value),
sizeof(int_value));
183 void writeBinaryFloatLE(std::ofstream &file,
const float float_value) {
186 float reverse_float = reverseFloat(float_value);
187 file.write((
char *)(&reverse_float),
sizeof(reverse_float));
189 file.write((
char *)(&float_value),
sizeof(float_value));
193 void writeBinaryDoubleLE(std::ofstream &file,
const double double_value) {
196 double reverse_double = reverseDouble(double_value);
197 file.write((
char *)(&reverse_double),
sizeof(reverse_double));
199 file.write((
char *)(&double_value),
sizeof(double_value));
213 : m_computeCovariance(false), m_covarianceMatrix(), m_currentImageId(0), m_detectionMethod(detectionScore),
214 m_detectionScore(0.15), m_detectionThreshold(100.0), m_detectionTime(0.), m_detectorNames(),
215 m_detectors(), m_extractionTime(0.), m_extractorNames(), m_extractors(), m_filteredMatches(), m_filterType(filterType),
216 m_imageFormat(jpgImageFormat), m_knnMatches(), m_mapOfImageId(), m_mapOfImages(),
217 m_matcher(), m_matcherName(matcherName),
218 m_matches(), m_matchingFactorThreshold(2.0), m_matchingRatioThreshold(0.85), m_matchingTime(0.),
219 m_matchRansacKeyPointsToPoints(), m_nbRansacIterations(200), m_nbRansacMinInlierCount(100), m_objectFilteredPoints(),
220 m_poseTime(0.), m_queryDescriptors(), m_queryFilteredKeyPoints(), m_queryKeyPoints(),
221 m_ransacConsensusPercentage(20.0), m_ransacInliers(), m_ransacOutliers(), m_ransacReprojectionError(6.0),
222 m_ransacThreshold(0.01), m_trainDescriptors(), m_trainKeyPoints(), m_trainPoints(),
223 m_trainVpPoints(), m_useAffineDetection(false),
224 #if (VISP_HAVE_OPENCV_VERSION >= 0x020400 && VISP_HAVE_OPENCV_VERSION < 0x030000)
225 m_useBruteForceCrossCheck(true),
227 m_useConsensusPercentage(false),
228 m_useKnn(false), m_useMatchTrainToQuery(false), m_useRansacVVS(true), m_useSingleMatchFilter(true)
236 m_detectorNames.push_back(detectorName);
237 m_extractorNames.push_back(extractorName);
252 : m_computeCovariance(false), m_covarianceMatrix(), m_currentImageId(0), m_detectionMethod(detectionScore),
253 m_detectionScore(0.15), m_detectionThreshold(100.0), m_detectionTime(0.), m_detectorNames(detectorNames),
254 m_detectors(), m_extractionTime(0.), m_extractorNames(extractorNames), m_extractors(), m_filteredMatches(),
255 m_filterType(filterType), m_imageFormat(jpgImageFormat), m_knnMatches(), m_mapOfImageId(), m_mapOfImages(),
257 m_matcherName(matcherName), m_matches(), m_matchingFactorThreshold(2.0), m_matchingRatioThreshold(0.85), m_matchingTime(0.),
258 m_matchRansacKeyPointsToPoints(), m_nbRansacIterations(200), m_nbRansacMinInlierCount(100), m_objectFilteredPoints(),
259 m_poseTime(0.), m_queryDescriptors(), m_queryFilteredKeyPoints(), m_queryKeyPoints(),
260 m_ransacConsensusPercentage(20.0), m_ransacInliers(), m_ransacOutliers(), m_ransacReprojectionError(6.0),
261 m_ransacThreshold(0.01), m_trainDescriptors(), m_trainKeyPoints(), m_trainPoints(),
262 m_trainVpPoints(), m_useAffineDetection(false),
263 #if (VISP_HAVE_OPENCV_VERSION >= 0x020400 && VISP_HAVE_OPENCV_VERSION < 0x030000)
264 m_useBruteForceCrossCheck(true),
266 m_useConsensusPercentage(false),
267 m_useKnn(false), m_useMatchTrainToQuery(false), m_useRansacVVS(true), m_useSingleMatchFilter(true)
286 void vpKeyPoint::affineSkew(
double tilt,
double phi, cv::Mat& img,
287 cv::Mat& mask, cv::Mat& Ai) {
291 mask = cv::Mat(h, w, CV_8UC1, cv::Scalar(255));
293 cv::Mat A = cv::Mat::eye(2, 3, CV_32F);
296 if (std::fabs(phi) > std::numeric_limits<double>::epsilon()) {
301 A = (cv::Mat_<float>(2, 2) << c, -s, s, c);
303 cv::Mat corners = (cv::Mat_<float>(4, 2) << 0, 0, w, 0, w, h, 0, h);
304 cv::Mat tcorners = corners * A.t();
305 cv::Mat tcorners_x, tcorners_y;
306 tcorners.col(0).copyTo(tcorners_x);
307 tcorners.col(1).copyTo(tcorners_y);
308 std::vector<cv::Mat> channels;
309 channels.push_back(tcorners_x);
310 channels.push_back(tcorners_y);
311 cv::merge(channels, tcorners);
313 cv::Rect rect = cv::boundingRect(tcorners);
314 A = (cv::Mat_<float>(2, 3) << c, -s, -rect.x, s, c, -rect.y);
316 cv::warpAffine(img, img, A, cv::Size(rect.width, rect.height),
317 cv::INTER_LINEAR, cv::BORDER_REPLICATE);
320 if (std::fabs(tilt - 1.0) > std::numeric_limits<double>::epsilon()) {
321 double s = 0.8 * sqrt(tilt * tilt - 1);
322 cv::GaussianBlur(img, img, cv::Size(0, 0), s, 0.01);
323 cv::resize(img, img, cv::Size(0, 0), 1.0 / tilt, 1.0, cv::INTER_NEAREST);
324 A.row(0) = A.row(0) / tilt;
327 if (std::fabs(tilt - 1.0) > std::numeric_limits<double>::epsilon() || std::fabs(phi) > std::numeric_limits<double>::epsilon() ) {
330 cv::warpAffine(mask, mask, A, cv::Size(w, h), cv::INTER_NEAREST);
332 cv::invertAffineTransform(A, Ai);
356 const unsigned int height,
const unsigned int width) {
369 const vpRect &rectangle) {
372 m_trainPoints.clear();
373 m_mapOfImageId.clear();
374 m_mapOfImages.clear();
375 m_currentImageId = 1;
377 if(m_useAffineDetection) {
378 std::vector<std::vector<cv::KeyPoint> > listOfTrainKeyPoints;
379 std::vector<cv::Mat> listOfTrainDescriptors;
385 m_trainKeyPoints.clear();
386 for(std::vector<std::vector<cv::KeyPoint> >::const_iterator it = listOfTrainKeyPoints.begin();
387 it != listOfTrainKeyPoints.end(); ++it) {
388 m_trainKeyPoints.insert(m_trainKeyPoints.end(), it->begin(), it->end());
392 for(std::vector<cv::Mat>::const_iterator it = listOfTrainDescriptors.begin(); it != listOfTrainDescriptors.end(); ++it) {
395 it->copyTo(m_trainDescriptors);
397 m_trainDescriptors.push_back(*it);
401 detect(I, m_trainKeyPoints, m_detectionTime, rectangle);
402 extract(I, m_trainKeyPoints, m_trainDescriptors, m_extractionTime);
407 for(std::vector<cv::KeyPoint>::const_iterator it = m_trainKeyPoints.begin(); it != m_trainKeyPoints.end(); ++it) {
408 m_mapOfImageId[it->class_id] = m_currentImageId;
412 m_mapOfImages[m_currentImageId] = I;
421 m_matcher->add(std::vector<cv::Mat>(1, m_trainDescriptors));
423 return static_cast<unsigned int>(m_trainKeyPoints.size());
436 std::vector<cv::Point3f> &points3f,
const bool append,
const int class_id) {
437 cv::Mat trainDescriptors;
439 std::vector<cv::KeyPoint> trainKeyPoints_tmp = trainKeyPoints;
441 extract(I, trainKeyPoints, trainDescriptors, m_extractionTime, &points3f);
443 if(trainKeyPoints.size() != trainKeyPoints_tmp.size()) {
446 std::map<size_t, size_t> mapOfKeypointHashes;
448 for(std::vector<cv::KeyPoint>::const_iterator it = trainKeyPoints_tmp.begin(); it != trainKeyPoints_tmp.end(); ++it, cpt++) {
449 mapOfKeypointHashes[myKeypointHash(*it)] = cpt;
452 std::vector<cv::Point3f> trainPoints_tmp;
453 for(std::vector<cv::KeyPoint>::const_iterator it = trainKeyPoints.begin(); it != trainKeyPoints.end(); ++it) {
454 if(mapOfKeypointHashes.find(myKeypointHash(*it)) != mapOfKeypointHashes.end()) {
455 trainPoints_tmp.push_back(points3f[mapOfKeypointHashes[myKeypointHash(*it)]]);
460 points3f = trainPoints_tmp;
463 buildReference(I, trainKeyPoints, trainDescriptors, points3f, append, class_id);
477 const cv::Mat &trainDescriptors,
const std::vector<cv::Point3f> &points3f,
478 const bool append,
const int class_id) {
480 m_currentImageId = 0;
481 m_mapOfImageId.clear();
482 m_mapOfImages.clear();
483 this->m_trainKeyPoints.clear();
484 this->m_trainPoints.clear();
489 std::vector<cv::KeyPoint> trainKeyPoints_tmp = trainKeyPoints;
492 for(std::vector<cv::KeyPoint>::iterator it = trainKeyPoints_tmp.begin(); it != trainKeyPoints_tmp.end(); ++it) {
493 it->class_id = class_id;
499 for(std::vector<cv::KeyPoint>::const_iterator it = trainKeyPoints_tmp.begin(); it != trainKeyPoints_tmp.end(); ++it) {
500 m_mapOfImageId[it->class_id] = m_currentImageId;
504 m_mapOfImages[m_currentImageId] = I;
507 this->m_trainKeyPoints.insert(this->m_trainKeyPoints.end(), trainKeyPoints_tmp.begin(), trainKeyPoints_tmp.end());
509 trainDescriptors.copyTo(this->m_trainDescriptors);
511 this->m_trainDescriptors.push_back(trainDescriptors);
513 this->m_trainPoints.insert(this->m_trainPoints.end(), points3f.begin(), points3f.end());
522 m_matcher->add(std::vector<cv::Mat>(1, m_trainDescriptors));
542 std::vector<vpPoint>::const_iterator it_roi = roi.begin();
549 vpPlane Po(pts[0], pts[1], pts[2]);
550 double xc = 0.0, yc = 0.0;
561 point_obj = cMo.
inverse() * point_cam;
562 point = cv::Point3f((
float) point_obj[0], (
float) point_obj[1], (
float) point_obj[2]);
580 std::vector<vpPoint>::const_iterator it_roi = roi.begin();
587 vpPlane Po(pts[0], pts[1], pts[2]);
588 double xc = 0.0, yc = 0.0;
599 point_obj = cMo.
inverse() * point_cam;
617 std::vector<cv::KeyPoint> &candidates,
const std::vector<vpPolygon> &polygons,
618 const std::vector<std::vector<vpPoint> > &roisPt, std::vector<cv::Point3f> &points, cv::Mat *descriptors) {
620 std::vector<cv::KeyPoint> candidatesToCheck = candidates;
627 std::vector<std::pair<cv::KeyPoint, size_t> > pairOfCandidatesToCheck(candidatesToCheck.size());
628 for(
size_t i = 0; i < candidatesToCheck.size(); i++) {
629 pairOfCandidatesToCheck[i] = std::pair<cv::KeyPoint, size_t>(candidatesToCheck[i], i);
633 std::vector<vpPolygon> polygons_tmp = polygons;
634 for (std::vector<vpPolygon>::iterator it1 = polygons_tmp.begin(); it1 != polygons_tmp.end(); ++it1, cpt1++) {
635 std::vector<std::pair<cv::KeyPoint, size_t> >::iterator it2 = pairOfCandidatesToCheck.begin();
637 while(it2 != pairOfCandidatesToCheck.end()) {
638 imPt.
set_ij(it2->first.pt.y, it2->first.pt.x);
639 if (it1->isInside(imPt)) {
640 candidates.push_back(it2->first);
642 points.push_back(pt);
644 if(descriptors != NULL) {
645 desc.push_back(descriptors->row((
int) it2->second));
649 it2 = pairOfCandidatesToCheck.erase(it2);
656 if(descriptors != NULL) {
657 desc.copyTo(*descriptors);
675 std::vector<vpImagePoint> &candidates,
const std::vector<vpPolygon> &polygons,
676 const std::vector<std::vector<vpPoint> > &roisPt, std::vector<vpPoint> &points, cv::Mat *descriptors) {
678 std::vector<vpImagePoint> candidatesToCheck = candidates;
684 std::vector<std::pair<vpImagePoint, size_t> > pairOfCandidatesToCheck(candidatesToCheck.size());
685 for(
size_t i = 0; i < candidatesToCheck.size(); i++) {
686 pairOfCandidatesToCheck[i] = std::pair<vpImagePoint, size_t>(candidatesToCheck[i], i);
690 std::vector<vpPolygon> polygons_tmp = polygons;
691 for (std::vector<vpPolygon>::iterator it1 = polygons_tmp.begin(); it1 != polygons_tmp.end(); ++it1, cpt1++) {
692 std::vector<std::pair<vpImagePoint, size_t> >::iterator it2 = pairOfCandidatesToCheck.begin();
694 while(it2 != pairOfCandidatesToCheck.end()) {
695 if (it1->isInside(it2->first)) {
696 candidates.push_back(it2->first);
698 points.push_back(pt);
700 if(descriptors != NULL) {
701 desc.push_back(descriptors->row((
int) it2->second));
705 it2 = pairOfCandidatesToCheck.erase(it2);
730 if(imagePoints.size() < 4 || objectPoints.size() < 4 || imagePoints.size() != objectPoints.size()) {
732 std::cerr <<
"Not enough points to compute the pose (at least 4 points are needed)." << std::endl;
737 cv::Mat cameraMatrix =
747 cv::Mat distCoeffs = cv::Mat::zeros(1, 5, CV_64F);
750 #if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
752 cv::solvePnPRansac(objectPoints, imagePoints, cameraMatrix, distCoeffs,
753 rvec, tvec,
false, m_nbRansacIterations, (
float) m_ransacReprojectionError,
756 cv::SOLVEPNP_ITERATIVE);
771 int nbInlierToReachConsensus = m_nbRansacMinInlierCount;
772 if(m_useConsensusPercentage) {
773 nbInlierToReachConsensus = (int) (m_ransacConsensusPercentage / 100.0 * (
double) m_queryFilteredKeyPoints.size());
776 cv::solvePnPRansac(objectPoints, imagePoints, cameraMatrix, distCoeffs,
777 rvec, tvec,
false, m_nbRansacIterations,
778 (
float) m_ransacReprojectionError, nbInlierToReachConsensus,
781 }
catch (cv::Exception &e) {
782 std::cerr << e.what() << std::endl;
787 tvec.at<
double>(1), tvec.at<
double>(2));
788 vpThetaUVector thetaUVector(rvec.at<
double>(0), rvec.at<
double>(1),
818 std::vector<unsigned int> inlierIndex;
819 return computePose(objectVpPoints, cMo, inliers, inlierIndex, elapsedTime, func);
835 std::vector<vpPoint> &inliers, std::vector<unsigned int> &inlierIndex,
839 if(objectVpPoints.size() < 4) {
848 for(std::vector<vpPoint>::const_iterator it = objectVpPoints.begin(); it != objectVpPoints.end(); ++it) {
852 unsigned int nbInlierToReachConsensus = (
unsigned int) m_nbRansacMinInlierCount;
853 if(m_useConsensusPercentage) {
854 nbInlierToReachConsensus = (
unsigned int) (m_ransacConsensusPercentage / 100.0 *
855 (
double) m_queryFilteredKeyPoints.size());
862 bool isRansacPoseEstimationOk =
false;
869 if(m_computeCovariance) {
873 std::cerr <<
"e=" << e.
what() << std::endl;
889 return isRansacPoseEstimationOk;
903 double vpKeyPoint::computePoseEstimationError(
const std::vector<std::pair<cv::KeyPoint, cv::Point3f> > &matchKeyPoints,
905 if(matchKeyPoints.size() == 0) {
910 std::vector<double> errors(matchKeyPoints.size());
913 for(std::vector<std::pair<cv::KeyPoint, cv::Point3f> >::const_iterator it = matchKeyPoints.begin();
914 it != matchKeyPoints.end(); ++it, cpt++) {
919 double u = 0.0, v = 0.0;
921 errors[cpt] = std::sqrt((u-it->first.pt.x)*(u-it->first.pt.x) + (v-it->first.pt.y)*(v-it->first.pt.y));
924 return std::accumulate(errors.begin(), errors.end(), 0.0) / errors.size();
953 unsigned int nbImg = (
unsigned int) (m_mapOfImages.size() + 1);
955 if(m_mapOfImages.empty()) {
956 std::cerr <<
"There is no training image loaded !" << std::endl;
966 unsigned int nbImgSqrt = (
unsigned int)
vpMath::round(std::sqrt((
double) nbImg));
969 unsigned int nbWidth = nbImgSqrt;
971 unsigned int nbHeight = nbImgSqrt;
974 if(nbImgSqrt * nbImgSqrt < nbImg) {
978 unsigned int maxW = ICurrent.
getWidth();
979 unsigned int maxH = ICurrent.
getHeight();
980 for(std::map<
int,
vpImage<unsigned char> >::const_iterator it = m_mapOfImages.begin(); it != m_mapOfImages.end(); ++it) {
981 if(maxW < it->second.getWidth()) {
982 maxW = it->second.getWidth();
985 if(maxH < it->second.getHeight()) {
986 maxH = it->second.getHeight();
1003 const vpRect &rectangle) {
1006 cv::Mat mask = cv::Mat::zeros(matImg.rows, matImg.cols, CV_8U);
1009 cv::Point leftTop((
int) rectangle.
getLeft(), (int) rectangle.
getTop()), rightBottom((
int) rectangle.
getRight(),
1011 cv::rectangle(mask, leftTop, rightBottom, cv::Scalar(255), CV_FILLED);
1013 mask = cv::Mat::ones(matImg.rows, matImg.cols, CV_8U) * 255;
1016 detect(matImg, keyPoints, elapsedTime, mask);
1027 void vpKeyPoint::detect(
const cv::Mat &matImg, std::vector<cv::KeyPoint> &keyPoints,
double &elapsedTime,
1028 const cv::Mat &mask) {
1032 for(std::map<std::string, cv::Ptr<cv::FeatureDetector> >::const_iterator it = m_detectors.begin(); it != m_detectors.end(); ++it) {
1033 std::vector<cv::KeyPoint> kp;
1034 it->second->detect(matImg, kp, mask);
1035 keyPoints.insert(keyPoints.end(), kp.begin(), kp.end());
1050 std::vector<vpImagePoint> vpQueryImageKeyPoints;
1052 std::vector<vpImagePoint> vpTrainImageKeyPoints;
1055 for(std::vector<cv::DMatch>::const_iterator it = m_filteredMatches.begin(); it != m_filteredMatches.end(); ++it) {
1069 std::vector<vpImagePoint> vpQueryImageKeyPoints;
1072 for(std::vector<cv::DMatch>::const_iterator it = m_filteredMatches.begin(); it != m_filteredMatches.end(); ++it) {
1087 unsigned int crossSize,
unsigned int lineThickness,
const vpColor &color) {
1089 srand((
unsigned int) time(NULL));
1092 std::vector<vpImagePoint> queryImageKeyPoints;
1094 std::vector<vpImagePoint> trainImageKeyPoints;
1098 for(std::vector<cv::DMatch>::const_iterator it = m_filteredMatches.begin(); it != m_filteredMatches.end(); ++it) {
1100 currentColor =
vpColor((rand() % 256), (rand() % 256), (rand() % 256));
1103 leftPt = trainImageKeyPoints[(size_t)(it->trainIdx)];
1104 rightPt =
vpImagePoint(queryImageKeyPoints[(
size_t)(it->queryIdx)].get_i(),
1105 queryImageKeyPoints[(size_t)it->queryIdx].get_j() + IRef.
getWidth());
1123 const std::vector<vpImagePoint> &ransacInliers,
unsigned int crossSize,
unsigned int lineThickness) {
1124 if(m_mapOfImages.empty() || m_mapOfImageId.empty()) {
1126 std::cerr <<
"There is no training image loaded !" << std::endl;
1131 int nbImg = (int) (m_mapOfImages.size() + 1);
1139 int nbWidth = nbImgSqrt;
1140 int nbHeight = nbImgSqrt;
1142 if(nbImgSqrt * nbImgSqrt < nbImg) {
1146 std::map<int, int> mapOfImageIdIndex;
1149 for(std::map<
int,
vpImage<unsigned char> >::const_iterator it = m_mapOfImages.begin(); it != m_mapOfImages.end(); ++it, cpt++) {
1150 mapOfImageIdIndex[it->first] = cpt;
1152 if(maxW < it->second.getWidth()) {
1153 maxW = it->second.getWidth();
1156 if(maxH < it->second.getHeight()) {
1157 maxH = it->second.getHeight();
1162 int medianI = nbHeight / 2;
1163 int medianJ = nbWidth / 2;
1164 int medianIndex = medianI * nbWidth + medianJ;
1165 for(std::vector<cv::KeyPoint>::const_iterator it = m_trainKeyPoints.begin(); it != m_trainKeyPoints.end(); ++it) {
1167 int current_class_id_index = 0;
1168 if(mapOfImageIdIndex[m_mapOfImageId[it->class_id]] < medianIndex) {
1169 current_class_id_index = mapOfImageIdIndex[m_mapOfImageId[it->class_id]];
1172 current_class_id_index = mapOfImageIdIndex[m_mapOfImageId[it->class_id]] + 1;
1175 int indexI = current_class_id_index / nbWidth;
1176 int indexJ = current_class_id_index - (indexI * nbWidth);
1177 topLeftCorner.
set_ij((
int)maxH*indexI, (
int)maxW*indexJ);
1184 vpImagePoint topLeftCorner((
int)maxH*medianI, (
int)maxW*medianJ);
1185 for(std::vector<cv::KeyPoint>::const_iterator it = m_queryKeyPoints.begin(); it != m_queryKeyPoints.end(); ++it) {
1190 for(std::vector<vpImagePoint>::const_iterator it = ransacInliers.begin();
1191 it != ransacInliers.end(); ++it) {
1196 for(std::vector<vpImagePoint>::const_iterator it = m_ransacOutliers.begin(); it != m_ransacOutliers.end(); ++it) {
1202 for(std::vector<cv::DMatch>::const_iterator it = m_filteredMatches.begin(); it != m_filteredMatches.end(); ++it) {
1203 int current_class_id = 0;
1204 if(mapOfImageIdIndex[m_mapOfImageId[m_trainKeyPoints[(
size_t) it->trainIdx].class_id]] < medianIndex) {
1205 current_class_id = mapOfImageIdIndex[m_mapOfImageId[m_trainKeyPoints[(size_t) it->trainIdx].class_id]];
1208 current_class_id = mapOfImageIdIndex[m_mapOfImageId[m_trainKeyPoints[(size_t) it->trainIdx].class_id]] + 1;
1211 int indexI = current_class_id / nbWidth;
1212 int indexJ = current_class_id - (indexI * nbWidth);
1214 vpImagePoint end((
int)maxH*indexI + m_trainKeyPoints[(
size_t) it->trainIdx].pt.y,
1215 (
int)maxW*indexJ + m_trainKeyPoints[(
size_t) it->trainIdx].pt.x);
1216 vpImagePoint start((
int)maxH*medianI + m_queryFilteredKeyPoints[(
size_t) it->queryIdx].pt.y,
1217 (
int)maxW*medianJ + m_queryFilteredKeyPoints[(
size_t) it->queryIdx].pt.x);
1237 double &elapsedTime, std::vector<cv::Point3f> *trainPoints) {
1240 extract(matImg, keyPoints, descriptors, elapsedTime, trainPoints);
1254 double &elapsedTime, std::vector<cv::Point3f> *trainPoints) {
1258 for(std::map<std::string, cv::Ptr<cv::DescriptorExtractor> >::const_iterator itd = m_extractors.begin();
1259 itd != m_extractors.end(); ++itd) {
1263 if(trainPoints != NULL && !trainPoints->empty()) {
1265 std::vector<cv::KeyPoint> keyPoints_tmp = keyPoints;
1268 itd->second->compute(matImg, keyPoints, descriptors);
1270 if(keyPoints.size() != keyPoints_tmp.size()) {
1273 std::map<size_t, size_t> mapOfKeypointHashes;
1275 for(std::vector<cv::KeyPoint>::const_iterator it = keyPoints_tmp.begin(); it != keyPoints_tmp.end(); ++it, cpt++) {
1276 mapOfKeypointHashes[myKeypointHash(*it)] = cpt;
1279 std::vector<cv::Point3f> trainPoints_tmp;
1280 for(std::vector<cv::KeyPoint>::const_iterator it = keyPoints.begin(); it != keyPoints.end(); ++it) {
1281 if(mapOfKeypointHashes.find(myKeypointHash(*it)) != mapOfKeypointHashes.end()) {
1282 trainPoints_tmp.push_back((*trainPoints)[mapOfKeypointHashes[myKeypointHash(*it)]]);
1287 *trainPoints = trainPoints_tmp;
1291 itd->second->compute(matImg, keyPoints, descriptors);
1295 std::vector<cv::KeyPoint> keyPoints_tmp = keyPoints;
1299 itd->second->compute(matImg, keyPoints, desc);
1301 if(keyPoints.size() != keyPoints_tmp.size()) {
1304 std::map<size_t, size_t> mapOfKeypointHashes;
1306 for(std::vector<cv::KeyPoint>::const_iterator it = keyPoints_tmp.begin(); it != keyPoints_tmp.end(); ++it, cpt++) {
1307 mapOfKeypointHashes[myKeypointHash(*it)] = cpt;
1310 std::vector<cv::Point3f> trainPoints_tmp;
1311 cv::Mat descriptors_tmp;
1312 for(std::vector<cv::KeyPoint>::const_iterator it = keyPoints.begin(); it != keyPoints.end(); ++it) {
1313 if(mapOfKeypointHashes.find(myKeypointHash(*it)) != mapOfKeypointHashes.end()) {
1314 if(trainPoints != NULL && !trainPoints->empty()) {
1315 trainPoints_tmp.push_back((*trainPoints)[mapOfKeypointHashes[myKeypointHash(*it)]]);
1318 if(!descriptors.empty()) {
1319 descriptors_tmp.push_back(descriptors.row((
int) mapOfKeypointHashes[myKeypointHash(*it)]));
1324 if(trainPoints != NULL) {
1326 *trainPoints = trainPoints_tmp;
1329 descriptors_tmp.copyTo(descriptors);
1333 if(descriptors.empty()) {
1334 desc.copyTo(descriptors);
1336 cv::hconcat(descriptors, desc, descriptors);
1341 if(keyPoints.size() != (size_t) descriptors.rows) {
1342 std::cerr <<
"keyPoints.size() != (size_t) descriptors.rows" << std::endl;
1350 void vpKeyPoint::filterMatches() {
1351 std::vector<cv::KeyPoint> queryKpts;
1352 std::vector<cv::Point3f> trainPts;
1353 std::vector<cv::DMatch> m;
1356 double max_dist = 0;
1358 double min_dist = DBL_MAX;
1360 std::vector<double> distance_vec(m_knnMatches.size());
1363 for(
size_t i = 0; i < m_knnMatches.size(); i++) {
1364 double dist = m_knnMatches[i][0].distance;
1366 distance_vec[i] = dist;
1368 if (dist < min_dist) {
1371 if (dist > max_dist) {
1375 mean /= m_queryDescriptors.rows;
1378 double sq_sum = std::inner_product(distance_vec.begin(), distance_vec.end(), distance_vec.begin(), 0.0);
1379 double stdev = std::sqrt(sq_sum / distance_vec.size() - mean * mean);
1380 double threshold = min_dist + stdev;
1382 for(
size_t i = 0; i < m_knnMatches.size(); i++) {
1383 if(m_knnMatches[i].size() >= 2) {
1385 float ratio = m_knnMatches[i][0].distance / m_knnMatches[i][1].distance;
1388 double dist = m_knnMatches[i][0].distance;
1391 m.push_back(cv::DMatch((
int) queryKpts.size(), m_knnMatches[i][0].trainIdx, m_knnMatches[i][0].distance));
1393 if(!m_trainPoints.empty()) {
1394 trainPts.push_back(m_trainPoints[(
size_t)m_knnMatches[i][0].trainIdx]);
1396 queryKpts.push_back(m_queryKeyPoints[(
size_t)m_knnMatches[i][0].queryIdx]);
1401 double max_dist = 0;
1404 double min_dist = DBL_MAX;
1406 std::vector<double> distance_vec(m_matches.size());
1407 for(
size_t i = 0; i < m_matches.size(); i++) {
1408 double dist = m_matches[i].distance;
1410 distance_vec[i] = dist;
1412 if (dist < min_dist) {
1415 if (dist > max_dist) {
1419 mean /= m_queryDescriptors.rows;
1421 double sq_sum = std::inner_product(distance_vec.begin(), distance_vec.end(), distance_vec.begin(), 0.0);
1422 double stdev = std::sqrt(sq_sum / distance_vec.size() - mean * mean);
1429 for (
size_t i = 0; i < m_matches.size(); i++) {
1430 if(m_matches[i].distance <= threshold) {
1431 m.push_back(cv::DMatch((
int) queryKpts.size(), m_matches[i].trainIdx, m_matches[i].distance));
1433 if(!m_trainPoints.empty()) {
1434 trainPts.push_back(m_trainPoints[(
size_t)m_matches[i].trainIdx]);
1436 queryKpts.push_back(m_queryKeyPoints[(
size_t)m_matches[i].queryIdx]);
1441 if(m_useSingleMatchFilter) {
1443 std::vector<cv::DMatch> mTmp;
1444 std::vector<cv::Point3f> trainPtsTmp;
1445 std::vector<cv::KeyPoint> queryKptsTmp;
1447 std::map<int, int> mapOfTrainIdx;
1449 for(std::vector<cv::DMatch>::const_iterator it = m.begin(); it != m.end(); ++it) {
1450 mapOfTrainIdx[it->trainIdx]++;
1454 for(std::vector<cv::DMatch>::const_iterator it = m.begin(); it != m.end(); ++it) {
1455 if(mapOfTrainIdx[it->trainIdx] == 1) {
1456 mTmp.push_back(cv::DMatch((
int) queryKptsTmp.size(), it->trainIdx, it->distance));
1458 if(!m_trainPoints.empty()) {
1459 trainPtsTmp.push_back(m_trainPoints[(
size_t) it->trainIdx]);
1461 queryKptsTmp.push_back(queryKpts[(
size_t) it->queryIdx]);
1465 m_filteredMatches = mTmp;
1466 m_objectFilteredPoints = trainPtsTmp;
1467 m_queryFilteredKeyPoints = queryKptsTmp;
1469 m_filteredMatches = m;
1470 m_objectFilteredPoints = trainPts;
1471 m_queryFilteredKeyPoints = queryKpts;
1482 objectPoints = m_objectFilteredPoints;
1501 keyPoints = m_queryFilteredKeyPoints;
1519 keyPoints = m_trainKeyPoints;
1537 points = m_trainPoints;
1546 points = m_trainVpPoints;
1552 void vpKeyPoint::init() {
1554 #if defined(VISP_HAVE_OPENCV_NONFREE) && (VISP_HAVE_OPENCV_VERSION >= 0x020400) && (VISP_HAVE_OPENCV_VERSION < 0x030000)
1556 if (!cv::initModule_nonfree()) {
1557 std::cerr <<
"Cannot init module non free, SIFT or SURF cannot be used."
1562 initDetectors(m_detectorNames);
1563 initExtractors(m_extractorNames);
1572 void vpKeyPoint::initDetector(
const std::string &detectorName) {
1573 #if (VISP_HAVE_OPENCV_VERSION < 0x030000)
1574 m_detectors[detectorName] = cv::FeatureDetector::create(detectorName);
1576 if(m_detectors[detectorName] == NULL) {
1577 std::stringstream ss_msg;
1578 ss_msg <<
"Fail to initialize the detector: " << detectorName <<
" or it is not available in OpenCV version: "
1579 << std::hex << VISP_HAVE_OPENCV_VERSION <<
".";
1583 std::string detectorNameTmp = detectorName;
1584 std::string pyramid =
"Pyramid";
1585 std::size_t pos = detectorName.find(pyramid);
1586 bool usePyramid =
false;
1587 if(pos != std::string::npos) {
1588 detectorNameTmp = detectorName.substr(pos + pyramid.size());
1592 if(detectorNameTmp ==
"SIFT") {
1593 #ifdef VISP_HAVE_OPENCV_XFEATURES2D
1594 cv::Ptr<cv::FeatureDetector> siftDetector = cv::xfeatures2d::SIFT::create();
1596 m_detectors[detectorNameTmp] = siftDetector;
1598 std::cerr <<
"Kind of non sense to use SIFT with Pyramid !" << std::endl;
1599 m_detectors[detectorName] = cv::makePtr<PyramidAdaptedFeatureDetector>(siftDetector);
1602 std::stringstream ss_msg;
1603 ss_msg <<
"Fail to initialize the detector: SIFT. OpenCV version "
1604 << std::hex << VISP_HAVE_OPENCV_VERSION <<
" was not build with xFeatures2d module.";
1607 }
else if(detectorNameTmp ==
"SURF") {
1608 #ifdef VISP_HAVE_OPENCV_XFEATURES2D
1609 cv::Ptr<cv::FeatureDetector> surfDetector = cv::xfeatures2d::SURF::create();
1611 m_detectors[detectorNameTmp] = surfDetector;
1613 std::cerr <<
"Kind of non sense to use SURF with Pyramid !" << std::endl;
1614 m_detectors[detectorName] = cv::makePtr<PyramidAdaptedFeatureDetector>(surfDetector);
1617 std::stringstream ss_msg;
1618 ss_msg <<
"Fail to initialize the detector: SURF. OpenCV version "
1619 << std::hex << VISP_HAVE_OPENCV_VERSION <<
" was not build with xFeatures2d module.";
1622 }
else if(detectorNameTmp ==
"FAST") {
1623 cv::Ptr<cv::FeatureDetector> fastDetector = cv::FastFeatureDetector::create();
1625 m_detectors[detectorNameTmp] = fastDetector;
1628 m_detectors[detectorName] = cv::makePtr<PyramidAdaptedFeatureDetector>(fastDetector);
1630 }
else if(detectorNameTmp ==
"MSER") {
1631 cv::Ptr<cv::FeatureDetector> fastDetector = cv::MSER::create();
1633 m_detectors[detectorNameTmp] = fastDetector;
1636 m_detectors[detectorName] = cv::makePtr<PyramidAdaptedFeatureDetector>(fastDetector);
1638 }
else if(detectorNameTmp ==
"ORB") {
1639 cv::Ptr<cv::FeatureDetector> orbDetector = cv::ORB::create();
1641 m_detectors[detectorNameTmp] = orbDetector;
1643 std::cerr <<
"Kind of non sense to use ORB with Pyramid !" << std::endl;
1644 m_detectors[detectorName] = cv::makePtr<PyramidAdaptedFeatureDetector>(orbDetector);
1646 }
else if(detectorNameTmp ==
"BRISK") {
1647 cv::Ptr<cv::FeatureDetector> briskDetector = cv::BRISK::create();
1649 m_detectors[detectorNameTmp] = briskDetector;
1651 std::cerr <<
"Kind of non sense to use BRISK with Pyramid !" << std::endl;
1652 m_detectors[detectorName] = cv::makePtr<PyramidAdaptedFeatureDetector>(briskDetector);
1654 }
else if(detectorNameTmp ==
"KAZE") {
1655 cv::Ptr<cv::FeatureDetector> kazeDetector = cv::KAZE::create();
1657 m_detectors[detectorNameTmp] = kazeDetector;
1659 std::cerr <<
"Kind of non sense to use KAZE with Pyramid !" << std::endl;
1660 m_detectors[detectorName] = cv::makePtr<PyramidAdaptedFeatureDetector>(kazeDetector);
1662 }
else if(detectorNameTmp ==
"AKAZE") {
1663 cv::Ptr<cv::FeatureDetector> akazeDetector = cv::AKAZE::create();
1665 m_detectors[detectorNameTmp] = akazeDetector;
1667 std::cerr <<
"Kind of non sense to use AKAZE with Pyramid !" << std::endl;
1668 m_detectors[detectorName] = cv::makePtr<PyramidAdaptedFeatureDetector>(akazeDetector);
1670 }
else if(detectorNameTmp ==
"GFTT") {
1671 cv::Ptr<cv::FeatureDetector> gfttDetector = cv::GFTTDetector::create();
1673 m_detectors[detectorNameTmp] = gfttDetector;
1676 m_detectors[detectorName] = cv::makePtr<PyramidAdaptedFeatureDetector>(gfttDetector);
1678 }
else if(detectorNameTmp ==
"SimpleBlob") {
1679 cv::Ptr<cv::FeatureDetector> simpleBlobDetector = cv::SimpleBlobDetector::create();
1681 m_detectors[detectorNameTmp] = simpleBlobDetector;
1684 m_detectors[detectorName] = cv::makePtr<PyramidAdaptedFeatureDetector>(simpleBlobDetector);
1686 }
else if(detectorNameTmp ==
"STAR") {
1687 #ifdef VISP_HAVE_OPENCV_XFEATURES2D
1688 cv::Ptr<cv::FeatureDetector> starDetector = cv::xfeatures2d::StarDetector::create();
1690 m_detectors[detectorNameTmp] = starDetector;
1693 m_detectors[detectorName] = cv::makePtr<PyramidAdaptedFeatureDetector>(starDetector);
1696 std::stringstream ss_msg;
1697 ss_msg <<
"Fail to initialize the detector: STAR. OpenCV version "
1698 << std::hex << VISP_HAVE_OPENCV_VERSION <<
" was not build with xFeatures2d module.";
1701 }
else if(detectorNameTmp ==
"AGAST") {
1702 cv::Ptr<cv::FeatureDetector> agastDetector = cv::AgastFeatureDetector::create();
1704 m_detectors[detectorNameTmp] = agastDetector;
1706 m_detectors[detectorName] = cv::makePtr<PyramidAdaptedFeatureDetector>(agastDetector);
1709 std::cerr <<
"The detector:" << detectorNameTmp <<
" is not available." << std::endl;
1712 bool detectorInitialized =
false;
1714 detectorInitialized = (m_detectors[detectorNameTmp] != NULL);
1716 detectorInitialized = (m_detectors[detectorName] != NULL);
1719 if(!detectorInitialized) {
1720 std::stringstream ss_msg;
1721 ss_msg <<
"Fail to initialize the detector: " << detectorNameTmp <<
" or it is not available in OpenCV version: "
1722 << std::hex << VISP_HAVE_OPENCV_VERSION <<
".";
1734 void vpKeyPoint::initDetectors(
const std::vector<std::string> &detectorNames) {
1735 for(std::vector<std::string>::const_iterator it = detectorNames.begin(); it != detectorNames.end(); ++it) {
1745 void vpKeyPoint::initExtractor(
const std::string &extractorName) {
1746 #if (VISP_HAVE_OPENCV_VERSION < 0x030000)
1747 m_extractors[extractorName] = cv::DescriptorExtractor::create(extractorName);
1749 if(extractorName ==
"SIFT") {
1750 #ifdef VISP_HAVE_OPENCV_XFEATURES2D
1751 m_extractors[extractorName] = cv::xfeatures2d::SIFT::create();
1753 std::stringstream ss_msg;
1754 ss_msg <<
"Fail to initialize the extractor: SIFT. OpenCV version "
1755 << std::hex << VISP_HAVE_OPENCV_VERSION <<
" was not build with xFeatures2d module.";
1758 }
else if(extractorName ==
"SURF") {
1759 #ifdef VISP_HAVE_OPENCV_XFEATURES2D
1761 m_extractors[extractorName] = cv::xfeatures2d::SURF::create(100, 4, 3,
true);
1763 std::stringstream ss_msg;
1764 ss_msg <<
"Fail to initialize the extractor: SURF. OpenCV version "
1765 << std::hex << VISP_HAVE_OPENCV_VERSION <<
" was not build with xFeatures2d module.";
1768 }
else if(extractorName ==
"ORB") {
1769 m_extractors[extractorName] = cv::ORB::create();
1770 }
else if(extractorName ==
"BRISK") {
1771 m_extractors[extractorName] = cv::BRISK::create();
1772 }
else if(extractorName ==
"FREAK") {
1773 #ifdef VISP_HAVE_OPENCV_XFEATURES2D
1774 m_extractors[extractorName] = cv::xfeatures2d::FREAK::create();
1776 std::stringstream ss_msg;
1777 ss_msg <<
"Fail to initialize the extractor: FREAK. OpenCV version "
1778 << std::hex << VISP_HAVE_OPENCV_VERSION <<
" was not build with xFeatures2d module.";
1781 }
else if(extractorName ==
"BRIEF") {
1782 #ifdef VISP_HAVE_OPENCV_XFEATURES2D
1783 m_extractors[extractorName] = cv::xfeatures2d::BriefDescriptorExtractor::create();
1785 std::stringstream ss_msg;
1786 ss_msg <<
"Fail to initialize the extractor: BRIEF. OpenCV version "
1787 << std::hex << VISP_HAVE_OPENCV_VERSION <<
" was not build with xFeatures2d module.";
1790 }
else if(extractorName ==
"KAZE") {
1791 m_extractors[extractorName] = cv::KAZE::create();
1792 }
else if(extractorName ==
"AKAZE") {
1793 m_extractors[extractorName] = cv::AKAZE::create();
1794 }
else if(extractorName ==
"DAISY") {
1795 #ifdef VISP_HAVE_OPENCV_XFEATURES2D
1796 m_extractors[extractorName] = cv::xfeatures2d::DAISY::create();
1798 std::stringstream ss_msg;
1799 ss_msg <<
"Fail to initialize the extractor: DAISY. OpenCV version "
1800 << std::hex << VISP_HAVE_OPENCV_VERSION <<
" was not build with xFeatures2d module.";
1803 }
else if(extractorName ==
"LATCH") {
1804 #ifdef VISP_HAVE_OPENCV_XFEATURES2D
1805 m_extractors[extractorName] = cv::xfeatures2d::LATCH::create();
1807 std::stringstream ss_msg;
1808 ss_msg <<
"Fail to initialize the extractor: LATCH. OpenCV version "
1809 << std::hex << VISP_HAVE_OPENCV_VERSION <<
" was not build with xFeatures2d module.";
1812 }
else if(extractorName ==
"LUCID") {
1813 #ifdef VISP_HAVE_OPENCV_XFEATURES2D
1814 m_extractors[extractorName] = cv::xfeatures2d::LUCID::create(1, 2);
1816 std::stringstream ss_msg;
1817 ss_msg <<
"Fail to initialize the extractor: LUCID. OpenCV version "
1818 << std::hex << VISP_HAVE_OPENCV_VERSION <<
" was not build with xFeatures2d module.";
1822 std::cerr <<
"The extractor:" << extractorName <<
" is not available." << std::endl;
1826 if(m_extractors[extractorName] == NULL) {
1827 std::stringstream ss_msg;
1828 ss_msg <<
"Fail to initialize the extractor: " << extractorName <<
" or it is not available in OpenCV version: "
1829 << std::hex << VISP_HAVE_OPENCV_VERSION <<
".";
1833 #if (VISP_HAVE_OPENCV_VERSION >= 0x020400 && VISP_HAVE_OPENCV_VERSION < 0x030000)
1834 if(extractorName ==
"SURF") {
1836 m_extractors[extractorName]->set(
"extended", 1);
1846 void vpKeyPoint::initExtractors(
const std::vector<std::string> &extractorNames) {
1847 for(std::vector<std::string>::const_iterator it = extractorNames.begin(); it != extractorNames.end(); ++it) {
1851 int descriptorType = CV_32F;
1852 bool firstIteration =
true;
1853 for(std::map<std::string, cv::Ptr<cv::DescriptorExtractor> >::const_iterator it = m_extractors.begin();
1854 it != m_extractors.end(); ++it) {
1855 if(firstIteration) {
1856 firstIteration =
false;
1857 descriptorType = it->second->descriptorType();
1859 if(descriptorType != it->second->descriptorType()) {
1872 int descriptorType = CV_32F;
1873 bool firstIteration =
true;
1874 for(std::map<std::string, cv::Ptr<cv::DescriptorExtractor> >::const_iterator it = m_extractors.begin();
1875 it != m_extractors.end(); ++it) {
1876 if(firstIteration) {
1877 firstIteration =
false;
1878 descriptorType = it->second->descriptorType();
1880 if(descriptorType != it->second->descriptorType()) {
1886 if(matcherName ==
"FlannBased") {
1887 if(m_extractors.empty()) {
1888 std::cout <<
"Warning: No extractor initialized, by default use floating values (CV_32F) "
1889 "for descriptor type !" << std::endl;
1892 if(descriptorType == CV_8U) {
1893 #if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
1894 m_matcher = cv::makePtr<cv::FlannBasedMatcher>(cv::makePtr<cv::flann::LshIndexParams>(12, 20, 2));
1896 m_matcher =
new cv::FlannBasedMatcher(
new cv::flann::LshIndexParams(12, 20, 2));
1899 #if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
1900 m_matcher = cv::makePtr<cv::FlannBasedMatcher>(cv::makePtr<cv::flann::KDTreeIndexParams>());
1902 m_matcher =
new cv::FlannBasedMatcher(
new cv::flann::KDTreeIndexParams());
1906 m_matcher = cv::DescriptorMatcher::create(matcherName);
1909 #if (VISP_HAVE_OPENCV_VERSION >= 0x020400 && VISP_HAVE_OPENCV_VERSION < 0x030000)
1910 if(m_matcher != NULL && !m_useKnn && matcherName ==
"BruteForce") {
1911 m_matcher->set(
"crossCheck", m_useBruteForceCrossCheck);
1915 if(m_matcher == NULL) {
1916 std::stringstream ss_msg;
1917 ss_msg <<
"Fail to initialize the matcher: " << matcherName <<
" or it is not available in OpenCV version: "
1918 << std::hex << VISP_HAVE_OPENCV_VERSION <<
".";
1934 IMatching.
insert(IRef, topLeftCorner);
1936 IMatching.
insert(ICurrent, topLeftCorner);
1948 int nbImg = (int) (m_mapOfImages.size() + 1);
1950 if(m_mapOfImages.empty()) {
1951 std::cerr <<
"There is no training image loaded !" << std::endl;
1961 int nbWidth = nbImgSqrt;
1962 int nbHeight = nbImgSqrt;
1964 if(nbImgSqrt * nbImgSqrt < nbImg) {
1969 for(std::map<
int,
vpImage<unsigned char> >::const_iterator it = m_mapOfImages.begin(); it != m_mapOfImages.end(); ++it) {
1970 if(maxW < it->second.getWidth()) {
1971 maxW = it->second.getWidth();
1974 if(maxH < it->second.getHeight()) {
1975 maxH = it->second.getHeight();
1980 int medianI = nbHeight / 2;
1981 int medianJ = nbWidth / 2;
1982 int medianIndex = medianI * nbWidth + medianJ;
1985 for(std::map<
int,
vpImage<unsigned char> >::const_iterator it = m_mapOfImages.begin(); it != m_mapOfImages.end(); ++it, cpt++) {
1986 int local_cpt = cpt;
1987 if(cpt >= medianIndex) {
1991 int indexI = local_cpt / nbWidth;
1992 int indexJ = local_cpt - (indexI * nbWidth);
1993 vpImagePoint topLeftCorner((
int)maxH*indexI, (
int)maxW*indexJ);
1995 IMatching.
insert(it->second, topLeftCorner);
1998 vpImagePoint topLeftCorner((
int)maxH*medianI, (
int)maxW*medianJ);
1999 IMatching.
insert(ICurrent, topLeftCorner);
2003 #ifdef VISP_HAVE_XML2
2014 m_detectorNames.clear();
2015 m_extractorNames.clear();
2016 m_detectors.clear();
2017 m_extractors.clear();
2019 std::cout <<
" *********** Parsing XML for configuration for vpKeyPoint ************ " << std::endl;
2020 xmlp.
parse(configFile);
2084 int startClassId = 0;
2085 int startImageId = 0;
2087 m_trainKeyPoints.clear();
2088 m_trainPoints.clear();
2089 m_mapOfImageId.clear();
2090 m_mapOfImages.clear();
2093 for(std::map<int, int>::const_iterator it = m_mapOfImageId.begin(); it != m_mapOfImageId.end(); ++it) {
2094 if(startClassId < it->first) {
2095 startClassId = it->first;
2100 for(std::map<
int,
vpImage<unsigned char> >::const_iterator it = m_mapOfImages.begin(); it != m_mapOfImages.end(); ++it) {
2101 if(startImageId < it->first) {
2102 startImageId = it->first;
2109 if(!parent.empty()) {
2114 std::ifstream file(filename.c_str(), std::ifstream::binary);
2115 if(!file.is_open()){
2121 file.read((
char *)(&nbImgs),
sizeof(nbImgs));
2123 #if !defined(VISP_HAVE_MODULE_IO)
2125 std::cout <<
"Warning: The learning file contains image data that will not be loaded as visp_io module "
2126 "is not available !" << std::endl;
2130 for(
int i = 0; i < nbImgs; i++) {
2133 file.read((
char *)(&
id),
sizeof(
id));
2136 file.read((
char *)(&length),
sizeof(length));
2138 char* path =
new char[length + 1];
2140 for(
int cpt = 0; cpt < length; cpt++) {
2142 file.read((
char *)(&c),
sizeof(c));
2145 path[length] =
'\0';
2148 #ifdef VISP_HAVE_MODULE_IO
2156 m_mapOfImages[
id + startImageId] = I;
2164 int have3DInfoInt = 0;
2165 file.read((
char *)(&have3DInfoInt),
sizeof(have3DInfoInt));
2166 bool have3DInfo = have3DInfoInt != 0;
2170 file.read((
char *)(&nRows),
sizeof(nRows));
2174 file.read((
char *)(&nCols),
sizeof(nCols));
2177 int descriptorType = 5;
2178 file.read((
char *)(&descriptorType),
sizeof(descriptorType));
2180 cv::Mat trainDescriptorsTmp = cv::Mat(nRows, nCols, descriptorType);
2181 for(
int i = 0; i < nRows; i++) {
2183 float u, v, size, angle, response;
2184 int octave, class_id, image_id;
2185 file.read((
char *)(&u),
sizeof(u));
2186 file.read((
char *)(&v),
sizeof(v));
2187 file.read((
char *)(&size),
sizeof(size));
2188 file.read((
char *)(&angle),
sizeof(angle));
2189 file.read((
char *)(&response),
sizeof(response));
2190 file.read((
char *)(&octave),
sizeof(octave));
2191 file.read((
char *)(&class_id),
sizeof(class_id));
2192 file.read((
char *)(&image_id),
sizeof(image_id));
2193 cv::KeyPoint keyPoint(cv::Point2f(u, v), size, angle, response, octave, (class_id + startClassId));
2194 m_trainKeyPoints.push_back(keyPoint);
2196 if(image_id != -1) {
2197 #ifdef VISP_HAVE_MODULE_IO
2199 m_mapOfImageId[class_id] = image_id + startImageId;
2206 file.read((
char *)(&oX),
sizeof(oX));
2207 file.read((
char *)(&oY),
sizeof(oY));
2208 file.read((
char *)(&oZ),
sizeof(oZ));
2209 m_trainPoints.push_back(cv::Point3f(oX, oY, oZ));
2212 for(
int j = 0; j < nCols; j++) {
2214 switch(descriptorType) {
2217 unsigned char value;
2218 file.read((
char *)(&value),
sizeof(value));
2219 trainDescriptorsTmp.at<
unsigned char>(i, j) = value;
2226 file.read((
char *)(&value),
sizeof(value));
2227 trainDescriptorsTmp.at<
char>(i, j) = value;
2233 unsigned short int value;
2234 file.read((
char *)(&value),
sizeof(value));
2235 trainDescriptorsTmp.at<
unsigned short int>(i, j) = value;
2242 file.read((
char *)(&value),
sizeof(value));
2243 trainDescriptorsTmp.at<
short int>(i, j) = value;
2250 file.read((
char *)(&value),
sizeof(value));
2251 trainDescriptorsTmp.at<
int>(i, j) = value;
2258 file.read((
char *)(&value),
sizeof(value));
2259 trainDescriptorsTmp.at<
float>(i, j) = value;
2266 file.read((
char *)(&value),
sizeof(value));
2267 trainDescriptorsTmp.at<
double>(i, j) = value;
2274 file.read((
char *)(&value),
sizeof(value));
2275 trainDescriptorsTmp.at<
float>(i, j) = value;
2282 if(!append || m_trainDescriptors.empty()) {
2283 trainDescriptorsTmp.copyTo(m_trainDescriptors);
2285 cv::vconcat(m_trainDescriptors, trainDescriptorsTmp, m_trainDescriptors);
2290 #ifdef VISP_HAVE_XML2
2291 xmlDocPtr doc = NULL;
2292 xmlNodePtr root_element = NULL;
2302 doc = xmlReadFile(filename.c_str(), NULL, 0);
2308 root_element = xmlDocGetRootElement(doc);
2310 xmlNodePtr first_level_node = NULL;
2313 int descriptorType = CV_32F;
2314 int nRows = 0, nCols = 0;
2317 cv::Mat trainDescriptorsTmp;
2319 for (first_level_node = root_element->children; first_level_node;
2320 first_level_node = first_level_node->next) {
2322 std::string name((
char *) first_level_node->name);
2323 if (first_level_node->type == XML_ELEMENT_NODE && name ==
"TrainingImageInfo") {
2324 xmlNodePtr image_info_node = NULL;
2326 for (image_info_node = first_level_node->children; image_info_node; image_info_node =
2327 image_info_node->next) {
2328 name = std::string ((
char *) image_info_node->name);
2330 if(name ==
"trainImg") {
2332 xmlChar *image_id_property = xmlGetProp(image_info_node, BAD_CAST
"image_id");
2334 if(image_id_property) {
2335 id = std::atoi((
char *) image_id_property);
2337 xmlFree(image_id_property);
2340 #ifdef VISP_HAVE_MODULE_IO
2341 std::string path((
char *) image_info_node->children->content);
2350 m_mapOfImages[
id + startImageId] = I;
2354 }
else if(first_level_node->type == XML_ELEMENT_NODE && name ==
"DescriptorsInfo") {
2355 xmlNodePtr descriptors_info_node = NULL;
2356 for (descriptors_info_node = first_level_node->children; descriptors_info_node; descriptors_info_node =
2357 descriptors_info_node->next) {
2358 if (descriptors_info_node->type == XML_ELEMENT_NODE) {
2359 name = std::string ((
char *) descriptors_info_node->name);
2361 if(name ==
"nrows") {
2362 nRows = std::atoi((
char *) descriptors_info_node->children->content);
2363 }
else if(name ==
"ncols") {
2364 nCols = std::atoi((
char *) descriptors_info_node->children->content);
2365 }
else if(name ==
"type") {
2366 descriptorType = std::atoi((
char *) descriptors_info_node->children->content);
2371 trainDescriptorsTmp = cv::Mat(nRows, nCols, descriptorType);
2372 }
else if (first_level_node->type == XML_ELEMENT_NODE && name ==
"DescriptorInfo") {
2373 xmlNodePtr point_node = NULL;
2374 double u = 0.0, v = 0.0, size = 0.0, angle = 0.0, response = 0.0;
2375 int octave = 0, class_id = 0, image_id = 0;
2376 double oX = 0.0, oY = 0.0, oZ = 0.0;
2378 std::stringstream ss;
2380 for (point_node = first_level_node->children; point_node; point_node =
2382 if (point_node->type == XML_ELEMENT_NODE) {
2383 name = std::string ((
char *) point_node->name);
2387 u = std::strtod((
char *) point_node->children->content, &pEnd);
2388 }
else if(name ==
"v") {
2389 v = std::strtod((
char *) point_node->children->content, &pEnd);
2390 }
else if(name ==
"size") {
2391 size = std::strtod((
char *) point_node->children->content, &pEnd);
2392 }
else if(name ==
"angle") {
2393 angle = std::strtod((
char *) point_node->children->content, &pEnd);
2394 }
else if(name ==
"response") {
2395 response = std::strtod((
char *) point_node->children->content, &pEnd);
2396 }
else if(name ==
"octave") {
2397 octave = std::atoi((
char *) point_node->children->content);
2398 }
else if(name ==
"class_id") {
2399 class_id = std::atoi((
char *) point_node->children->content);
2400 cv::KeyPoint keyPoint(cv::Point2f((
float) u, (
float) v), (
float) size,
2401 (
float) angle, (
float) response, octave, (class_id + startClassId));
2402 m_trainKeyPoints.push_back(keyPoint);
2403 }
else if(name ==
"image_id") {
2404 image_id = std::atoi((
char *) point_node->children->content);
2405 if(image_id != -1) {
2406 #ifdef VISP_HAVE_MODULE_IO
2408 m_mapOfImageId[m_trainKeyPoints.back().class_id] = image_id + startImageId;
2411 }
else if (name ==
"oX") {
2412 oX = std::atof((
char *) point_node->children->content);
2413 }
else if (name ==
"oY") {
2414 oY = std::atof((
char *) point_node->children->content);
2415 }
else if (name ==
"oZ") {
2416 oZ = std::atof((
char *) point_node->children->content);
2417 m_trainPoints.push_back(cv::Point3f((
float) oX, (
float) oY, (
float) oZ));
2418 }
else if (name ==
"desc") {
2419 xmlNodePtr descriptor_value_node = NULL;
2422 for (descriptor_value_node = point_node->children;
2423 descriptor_value_node; descriptor_value_node =
2424 descriptor_value_node->next) {
2426 if (descriptor_value_node->type == XML_ELEMENT_NODE) {
2428 std::string parseStr((
char *) descriptor_value_node->children->content);
2433 switch(descriptorType) {
2439 trainDescriptorsTmp.at<
unsigned char>(i, j) = (
unsigned char) parseValue;
2447 trainDescriptorsTmp.at<
char>(i, j) = (
char) parseValue;
2451 ss >> trainDescriptorsTmp.at<
unsigned short int>(i, j);
2455 ss >> trainDescriptorsTmp.at<
short int>(i, j);
2459 ss >> trainDescriptorsTmp.at<
int>(i, j);
2463 ss >> trainDescriptorsTmp.at<
float>(i, j);
2467 ss >> trainDescriptorsTmp.at<
double>(i, j);
2471 ss >> trainDescriptorsTmp.at<
float>(i, j);
2475 std::cerr <<
"Error when converting:" << ss.str() << std::endl;
2488 if(!append || m_trainDescriptors.empty()) {
2489 trainDescriptorsTmp.copyTo(m_trainDescriptors);
2491 cv::vconcat(m_trainDescriptors, trainDescriptorsTmp, m_trainDescriptors);
2503 std::cout <<
"Error: libxml2 is required !" << std::endl;
2513 m_matcher->add(std::vector<cv::Mat>(1, m_trainDescriptors));
2528 std::vector<cv::DMatch> &matches,
double &elapsedTime) {
2532 m_knnMatches.clear();
2534 if(m_useMatchTrainToQuery) {
2535 std::vector<std::vector<cv::DMatch> > knnMatchesTmp;
2538 cv::Ptr<cv::DescriptorMatcher> matcherTmp = m_matcher->clone(
true);
2539 matcherTmp->knnMatch(trainDescriptors, queryDescriptors, knnMatchesTmp, 2);
2541 for(std::vector<std::vector<cv::DMatch> >::const_iterator it1 = knnMatchesTmp.begin(); it1 != knnMatchesTmp.end(); ++it1) {
2542 std::vector<cv::DMatch> tmp;
2543 for(std::vector<cv::DMatch>::const_iterator it2 = it1->begin(); it2 != it1->end(); ++it2) {
2544 tmp.push_back(cv::DMatch(it2->trainIdx, it2->queryIdx, it2->distance));
2546 m_knnMatches.push_back(tmp);
2549 matches.resize(m_knnMatches.size());
2550 std::transform(m_knnMatches.begin(), m_knnMatches.end(), matches.begin(), knnToDMatch);
2553 m_matcher->knnMatch(queryDescriptors, m_knnMatches, 2);
2554 matches.resize(m_knnMatches.size());
2555 std::transform(m_knnMatches.begin(), m_knnMatches.end(), matches.begin(), knnToDMatch);
2560 if(m_useMatchTrainToQuery) {
2561 std::vector<cv::DMatch> matchesTmp;
2563 cv::Ptr<cv::DescriptorMatcher> matcherTmp = m_matcher->clone(
true);
2564 matcherTmp->match(trainDescriptors, queryDescriptors, matchesTmp);
2566 for(std::vector<cv::DMatch>::const_iterator it = matchesTmp.begin(); it != matchesTmp.end(); ++it) {
2567 matches.push_back(cv::DMatch(it->trainIdx, it->queryIdx, it->distance));
2571 m_matcher->match(queryDescriptors, matches);
2599 const unsigned int height,
const unsigned int width) {
2612 const vpRect& rectangle) {
2613 if(m_trainDescriptors.empty()) {
2614 std::cerr <<
"Reference is empty." << std::endl;
2616 std::cerr <<
"Reference is not computed." << std::endl;
2618 std::cerr <<
"Matching is not possible." << std::endl;
2623 if(m_useAffineDetection) {
2624 std::vector<std::vector<cv::KeyPoint> > listOfQueryKeyPoints;
2625 std::vector<cv::Mat> listOfQueryDescriptors;
2631 m_queryKeyPoints.clear();
2632 for(std::vector<std::vector<cv::KeyPoint> >::const_iterator it = listOfQueryKeyPoints.begin();
2633 it != listOfQueryKeyPoints.end(); ++it) {
2634 m_queryKeyPoints.insert(m_queryKeyPoints.end(), it->begin(), it->end());
2638 for(std::vector<cv::Mat>::const_iterator it = listOfQueryDescriptors.begin(); it != listOfQueryDescriptors.end(); ++it) {
2641 it->copyTo(m_queryDescriptors);
2643 m_queryDescriptors.push_back(*it);
2647 detect(I, m_queryKeyPoints, m_detectionTime, rectangle);
2648 extract(I, m_queryKeyPoints, m_queryDescriptors, m_extractionTime);
2651 match(m_trainDescriptors, m_queryDescriptors, m_matches, m_matchingTime);
2654 m_queryFilteredKeyPoints.clear();
2655 m_objectFilteredPoints.clear();
2656 m_filteredMatches.clear();
2660 if(m_useMatchTrainToQuery) {
2662 m_queryFilteredKeyPoints.clear();
2663 m_filteredMatches.clear();
2664 for(std::vector<cv::DMatch>::const_iterator it = m_matches.begin(); it != m_matches.end(); ++it) {
2665 m_filteredMatches.push_back(cv::DMatch((
int) m_queryFilteredKeyPoints.size(), it->trainIdx, it->distance));
2666 m_queryFilteredKeyPoints.push_back(m_queryKeyPoints[(
size_t) it->queryIdx]);
2669 m_queryFilteredKeyPoints = m_queryKeyPoints;
2670 m_filteredMatches = m_matches;
2673 if(!m_trainPoints.empty()) {
2674 m_objectFilteredPoints.clear();
2677 for(std::vector<cv::DMatch>::const_iterator it = m_matches.begin(); it != m_matches.end(); ++it) {
2679 m_objectFilteredPoints.push_back(m_trainPoints[(
size_t) it->trainIdx]);
2688 return static_cast<unsigned int>(m_filteredMatches.size());
2708 if(m_trainDescriptors.empty()) {
2709 std::cerr <<
"Reference is empty." << std::endl;
2711 std::cerr <<
"Reference is not computed." << std::endl;
2713 std::cerr <<
"Matching is not possible." << std::endl;
2718 if(m_useAffineDetection) {
2719 std::vector<std::vector<cv::KeyPoint> > listOfQueryKeyPoints;
2720 std::vector<cv::Mat> listOfQueryDescriptors;
2726 m_queryKeyPoints.clear();
2727 for(std::vector<std::vector<cv::KeyPoint> >::const_iterator it = listOfQueryKeyPoints.begin();
2728 it != listOfQueryKeyPoints.end(); ++it) {
2729 m_queryKeyPoints.insert(m_queryKeyPoints.end(), it->begin(), it->end());
2733 for(std::vector<cv::Mat>::const_iterator it = listOfQueryDescriptors.begin(); it != listOfQueryDescriptors.end(); ++it) {
2736 it->copyTo(m_queryDescriptors);
2738 m_queryDescriptors.push_back(*it);
2742 detect(I, m_queryKeyPoints, m_detectionTime, rectangle);
2743 extract(I, m_queryKeyPoints, m_queryDescriptors, m_extractionTime);
2746 match(m_trainDescriptors, m_queryDescriptors, m_matches, m_matchingTime);
2748 elapsedTime = m_detectionTime + m_extractionTime + m_matchingTime;
2751 m_queryFilteredKeyPoints.clear();
2752 m_objectFilteredPoints.clear();
2753 m_filteredMatches.clear();
2757 if(m_useMatchTrainToQuery) {
2759 m_queryFilteredKeyPoints.clear();
2760 m_filteredMatches.clear();
2761 for(std::vector<cv::DMatch>::const_iterator it = m_matches.begin(); it != m_matches.end(); ++it) {
2762 m_filteredMatches.push_back(cv::DMatch((
int) m_queryFilteredKeyPoints.size(), it->trainIdx, it->distance));
2763 m_queryFilteredKeyPoints.push_back(m_queryKeyPoints[(
size_t) it->queryIdx]);
2766 m_queryFilteredKeyPoints = m_queryKeyPoints;
2767 m_filteredMatches = m_matches;
2770 if(!m_trainPoints.empty()) {
2771 m_objectFilteredPoints.clear();
2774 for(std::vector<cv::DMatch>::const_iterator it = m_matches.begin(); it != m_matches.end(); ++it) {
2776 m_objectFilteredPoints.push_back(m_trainPoints[(
size_t) it->trainIdx]);
2787 m_ransacInliers.clear();
2788 m_ransacOutliers.clear();
2790 if(m_useRansacVVS) {
2791 std::vector<vpPoint> objectVpPoints(m_objectFilteredPoints.size());
2794 for(std::vector<cv::Point3f>::const_iterator it = m_objectFilteredPoints.begin(); it != m_objectFilteredPoints.end();
2799 vpImagePoint imP(m_queryFilteredKeyPoints[cpt].pt.y, m_queryFilteredKeyPoints[cpt].pt.x);
2801 double x = 0.0, y = 0.0;
2806 objectVpPoints[cpt] = pt;
2809 std::vector<vpPoint> inliers;
2810 std::vector<unsigned int> inlierIndex;
2812 bool res =
computePose(objectVpPoints, cMo, inliers, inlierIndex, m_poseTime, func);
2814 std::map<unsigned int, bool> mapOfInlierIndex;
2815 m_matchRansacKeyPointsToPoints.clear();
2817 for (std::vector<unsigned int>::const_iterator it = inlierIndex.begin(); it != inlierIndex.end(); ++it) {
2818 m_matchRansacKeyPointsToPoints.push_back(std::pair<cv::KeyPoint, cv::Point3f>(m_queryFilteredKeyPoints[(
size_t)(*it)],
2819 m_objectFilteredPoints[(
size_t)(*it)]));
2820 mapOfInlierIndex[*it] =
true;
2823 for(
size_t i = 0; i < m_queryFilteredKeyPoints.size(); i++) {
2824 if(mapOfInlierIndex.find((
unsigned int) i) == mapOfInlierIndex.end()) {
2825 m_ransacOutliers.push_back(
vpImagePoint(m_queryFilteredKeyPoints[i].pt.y, m_queryFilteredKeyPoints[i].pt.x));
2829 error = computePoseEstimationError(m_matchRansacKeyPointsToPoints, cam, cMo);
2831 m_ransacInliers.resize(m_matchRansacKeyPointsToPoints.size());
2832 std::transform(m_matchRansacKeyPointsToPoints.begin(), m_matchRansacKeyPointsToPoints.end(), m_ransacInliers.begin(),
2833 matchRansacToVpImage);
2835 elapsedTime += m_poseTime;
2839 std::vector<cv::Point2f> imageFilteredPoints;
2840 cv::KeyPoint::convert(m_queryFilteredKeyPoints, imageFilteredPoints);
2841 std::vector<int> inlierIndex;
2842 bool res =
computePose(imageFilteredPoints, m_objectFilteredPoints, cam, cMo, inlierIndex, m_poseTime);
2844 std::map<int, bool> mapOfInlierIndex;
2845 m_matchRansacKeyPointsToPoints.clear();
2847 for (std::vector<int>::const_iterator it = inlierIndex.begin(); it != inlierIndex.end(); ++it) {
2848 m_matchRansacKeyPointsToPoints.push_back(std::pair<cv::KeyPoint, cv::Point3f>(m_queryFilteredKeyPoints[(
size_t)(*it)],
2849 m_objectFilteredPoints[(
size_t)(*it)]));
2850 mapOfInlierIndex[*it] =
true;
2853 for(
size_t i = 0; i < m_queryFilteredKeyPoints.size(); i++) {
2854 if(mapOfInlierIndex.find((
int) i) == mapOfInlierIndex.end()) {
2855 m_ransacOutliers.push_back(
vpImagePoint(m_queryFilteredKeyPoints[i].pt.y, m_queryFilteredKeyPoints[i].pt.x));
2859 error = computePoseEstimationError(m_matchRansacKeyPointsToPoints, cam, cMo);
2861 m_ransacInliers.resize(m_matchRansacKeyPointsToPoints.size());
2862 std::transform(m_matchRansacKeyPointsToPoints.begin(), m_matchRansacKeyPointsToPoints.end(), m_ransacInliers.begin(),
2863 matchRansacToVpImage);
2865 elapsedTime += m_poseTime;
2889 const bool isPlanarObject, std::vector<vpImagePoint> *imPts1, std::vector<vpImagePoint> *imPts2,
2890 double *meanDescriptorDistance,
double *detection_score,
const vpRect& rectangle) {
2891 if(imPts1 != NULL && imPts2 != NULL) {
2898 double meanDescriptorDistanceTmp = 0.0;
2899 for(std::vector<cv::DMatch>::const_iterator it = m_filteredMatches.begin(); it != m_filteredMatches.end(); ++it) {
2900 meanDescriptorDistanceTmp += (double) it->distance;
2903 meanDescriptorDistanceTmp /= (double) m_filteredMatches.size();
2904 double score = (double) m_filteredMatches.size() / meanDescriptorDistanceTmp;
2906 if(meanDescriptorDistance != NULL) {
2907 *meanDescriptorDistance = meanDescriptorDistanceTmp;
2909 if(detection_score != NULL) {
2910 *detection_score = score;
2913 if(m_filteredMatches.size() >= 4) {
2915 std::vector<cv::Point2f> points1(m_filteredMatches.size());
2917 std::vector<cv::Point2f> points2(m_filteredMatches.size());
2919 for(
size_t i = 0; i < m_filteredMatches.size(); i++) {
2920 points1[i] = cv::Point2f(m_trainKeyPoints[(
size_t)m_filteredMatches[i].trainIdx].pt);
2921 points2[i] = cv::Point2f(m_queryFilteredKeyPoints[(
size_t)m_filteredMatches[i].queryIdx].pt);
2924 std::vector<vpImagePoint> inliers;
2925 if(isPlanarObject) {
2926 #if (VISP_HAVE_OPENCV_VERSION < 0x030000)
2927 cv::Mat homographyMatrix = cv::findHomography(points1, points2, CV_RANSAC);
2929 cv::Mat homographyMatrix = cv::findHomography(points1, points2, cv::RANSAC);
2932 for(
size_t i = 0; i < m_filteredMatches.size(); i++ ) {
2934 cv::Mat realPoint = cv::Mat(3, 1, CV_64F);
2935 realPoint.at<
double>(0,0) = points1[i].x;
2936 realPoint.at<
double>(1,0) = points1[i].y;
2937 realPoint.at<
double>(2,0) = 1.f;
2939 cv::Mat reprojectedPoint = homographyMatrix * realPoint;
2940 double err_x = (reprojectedPoint.at<
double>(0,0) / reprojectedPoint.at<
double>(2,0)) - points2[i].x;
2941 double err_y = (reprojectedPoint.at<
double>(1,0) / reprojectedPoint.at<
double>(2,0)) - points2[i].y;
2942 double reprojectionError = std::sqrt(err_x*err_x + err_y*err_y);
2944 if(reprojectionError < 6.0) {
2945 inliers.push_back(
vpImagePoint((
double) points2[i].y, (
double) points2[i].x));
2946 if(imPts1 != NULL) {
2947 imPts1->push_back(
vpImagePoint((
double) points1[i].y, (
double) points1[i].x));
2950 if(imPts2 != NULL) {
2951 imPts2->push_back(
vpImagePoint((
double) points2[i].y, (
double) points2[i].x));
2955 }
else if(m_filteredMatches.size() >= 8) {
2956 cv::Mat fundamentalInliers;
2957 cv::Mat fundamentalMatrix = cv::findFundamentalMat(points1, points2, cv::FM_RANSAC, 3, 0.99, fundamentalInliers);
2959 for(
size_t i = 0; i < (size_t) fundamentalInliers.rows; i++) {
2960 if(fundamentalInliers.at<uchar>((
int) i, 0)) {
2961 inliers.push_back(
vpImagePoint((
double) points2[i].y, (
double) points2[i].x));
2963 if(imPts1 != NULL) {
2964 imPts1->push_back(
vpImagePoint((
double) points1[i].y, (
double) points1[i].x));
2967 if(imPts2 != NULL) {
2968 imPts2->push_back(
vpImagePoint((
double) points2[i].y, (
double) points2[i].x));
2974 if(!inliers.empty()) {
2980 double meanU = 0.0, meanV = 0.0;
2981 for(std::vector<vpImagePoint>::const_iterator it = inliers.begin(); it != inliers.end(); ++it) {
2982 meanU += it->get_u();
2983 meanV += it->get_v();
2986 meanU /= (double) inliers.size();
2987 meanV /= (double) inliers.size();
2989 centerOfGravity.
set_u(meanU);
2990 centerOfGravity.
set_v(meanV);
2998 return meanDescriptorDistanceTmp < m_detectionThreshold;
3000 return score > m_detectionScore;
3023 double &error,
double &elapsedTime,
vpRect &boundingBox,
vpImagePoint ¢erOfGravity,
3025 bool isMatchOk =
matchPoint(I, cam, cMo, error, elapsedTime, func, rectangle);
3030 std::vector<vpImagePoint> modelImagePoints(m_trainVpPoints.size());
3032 for(std::vector<vpPoint>::const_iterator it = m_trainVpPoints.begin(); it != m_trainVpPoints.end(); ++it, cpt++) {
3036 modelImagePoints[cpt] = imPt;
3044 double meanU = 0.0, meanV = 0.0;
3045 for(std::vector<vpImagePoint>::const_iterator it = m_ransacInliers.begin(); it != m_ransacInliers.end();
3047 meanU += it->get_u();
3048 meanV += it->get_v();
3051 meanU /= (double) m_ransacInliers.size();
3052 meanV /= (double) m_ransacInliers.size();
3054 centerOfGravity.
set_u(meanU);
3055 centerOfGravity.
set_v(meanV);
3077 listOfKeypoints.clear();
3078 listOfDescriptors.clear();
3080 for (
int tl = 1; tl < 6; tl++) {
3081 double t = pow(2, 0.5 * tl);
3082 for (
int phi = 0; phi < 180; phi += (int)(72.0 / t)) {
3083 std::vector<cv::KeyPoint> keypoints;
3084 cv::Mat descriptors;
3086 cv::Mat timg, mask, Ai;
3089 affineSkew(t, phi, timg, mask, Ai);
3092 if(listOfAffineI != NULL) {
3094 bitwise_and(mask, timg, img_disp);
3097 listOfAffineI->push_back(tI);
3101 cv::bitwise_and(mask, timg, img_disp);
3102 cv::namedWindow(
"Skew", cv::WINDOW_AUTOSIZE );
3103 cv::imshow(
"Skew", img_disp );
3107 for(std::map<std::string, cv::Ptr<cv::FeatureDetector> >::const_iterator it = m_detectors.begin();
3108 it != m_detectors.end(); ++it) {
3109 std::vector<cv::KeyPoint> kp;
3110 it->second->detect(timg, kp, mask);
3111 keypoints.insert(keypoints.end(), kp.begin(), kp.end());
3115 extract(timg, keypoints, descriptors, elapsedTime);
3117 for(
unsigned int i = 0; i < keypoints.size(); i++) {
3118 cv::Point3f kpt(keypoints[i].pt.x, keypoints[i].pt.y, 1.f);
3119 cv::Mat kpt_t = Ai * cv::Mat(kpt);
3120 keypoints[i].pt.x = kpt_t.at<
float>(0, 0);
3121 keypoints[i].pt.y = kpt_t.at<
float>(1, 0);
3124 listOfKeypoints.push_back(keypoints);
3125 listOfDescriptors.push_back(descriptors);
3134 std::vector<std::pair<double, int> > listOfAffineParams;
3135 for (
int tl = 1; tl < 6; tl++) {
3136 double t = pow(2, 0.5 * tl);
3137 for (
int phi = 0; phi < 180; phi += (int)(72.0 / t)) {
3138 listOfAffineParams.push_back(std::pair<double, int>(t, phi));
3142 listOfKeypoints.resize(listOfAffineParams.size());
3143 listOfDescriptors.resize(listOfAffineParams.size());
3145 if(listOfAffineI != NULL) {
3146 listOfAffineI->resize(listOfAffineParams.size());
3149 #ifdef VISP_HAVE_OPENMP
3150 #pragma omp parallel for
3152 for(
int cpt = 0; cpt < static_cast<int>(listOfAffineParams.size()); cpt++) {
3153 std::vector<cv::KeyPoint> keypoints;
3154 cv::Mat descriptors;
3156 cv::Mat timg, mask, Ai;
3159 affineSkew(listOfAffineParams[(
size_t) cpt].first, listOfAffineParams[(
size_t) cpt].second, timg, mask, Ai);
3162 if(listOfAffineI != NULL) {
3164 bitwise_and(mask, timg, img_disp);
3167 (*listOfAffineI)[(size_t) cpt] = tI;
3172 cv::bitwise_and(mask, timg, img_disp);
3173 cv::namedWindow(
"Skew", cv::WINDOW_AUTOSIZE );
3174 cv::imshow(
"Skew", img_disp );
3178 for(std::map<std::string, cv::Ptr<cv::FeatureDetector> >::const_iterator it = m_detectors.begin();
3179 it != m_detectors.end(); ++it) {
3180 std::vector<cv::KeyPoint> kp;
3181 it->second->detect(timg, kp, mask);
3182 keypoints.insert(keypoints.end(), kp.begin(), kp.end());
3186 extract(timg, keypoints, descriptors, elapsedTime);
3188 for(
size_t i = 0; i < keypoints.size(); i++) {
3189 cv::Point3f kpt(keypoints[i].pt.x, keypoints[i].pt.y, 1.f);
3190 cv::Mat kpt_t = Ai * cv::Mat(kpt);
3191 keypoints[i].pt.x = kpt_t.at<
float>(0, 0);
3192 keypoints[i].pt.y = kpt_t.at<
float>(1, 0);
3195 listOfKeypoints[(size_t) cpt] = keypoints;
3196 listOfDescriptors[(size_t) cpt] = descriptors;
3209 m_computeCovariance =
false; m_covarianceMatrix =
vpMatrix(); m_currentImageId = 0; m_detectionMethod =
detectionScore;
3210 m_detectionScore = 0.15; m_detectionThreshold = 100.0; m_detectionTime = 0.0; m_detectorNames.clear();
3211 m_detectors.clear(); m_extractionTime = 0.0; m_extractorNames.clear(); m_extractors.clear(); m_filteredMatches.clear();
3213 m_imageFormat =
jpgImageFormat; m_knnMatches.clear(); m_mapOfImageId.clear(); m_mapOfImages.clear();
3214 m_matcher = cv::Ptr<cv::DescriptorMatcher>(); m_matcherName =
"BruteForce-Hamming";
3215 m_matches.clear(); m_matchingFactorThreshold = 2.0; m_matchingRatioThreshold = 0.85; m_matchingTime = 0.0;
3216 m_matchRansacKeyPointsToPoints.clear(); m_nbRansacIterations = 200; m_nbRansacMinInlierCount = 100;
3217 m_objectFilteredPoints.clear();
3218 m_poseTime = 0.0; m_queryDescriptors = cv::Mat(); m_queryFilteredKeyPoints.clear(); m_queryKeyPoints.clear();
3219 m_ransacConsensusPercentage = 20.0; m_ransacInliers.clear(); m_ransacOutliers.clear(); m_ransacReprojectionError = 6.0;
3220 m_ransacThreshold = 0.01; m_trainDescriptors = cv::Mat(); m_trainKeyPoints.clear(); m_trainPoints.clear();
3221 m_trainVpPoints.clear(); m_useAffineDetection =
false;
3222 #if (VISP_HAVE_OPENCV_VERSION >= 0x020400 && VISP_HAVE_OPENCV_VERSION < 0x030000)
3223 m_useBruteForceCrossCheck =
true;
3225 m_useConsensusPercentage =
false;
3227 m_useMatchTrainToQuery =
false; m_useRansacVVS =
true; m_useSingleMatchFilter =
true;
3229 m_detectorNames.push_back(
"ORB");
3230 m_extractorNames.push_back(
"ORB");
3244 if(!parent.empty()) {
3248 std::map<int, std::string> mapOfImgPath;
3249 if(saveTrainingImages) {
3250 #ifdef VISP_HAVE_MODULE_IO
3254 for(std::map<
int,
vpImage<unsigned char> >::const_iterator it = m_mapOfImages.begin(); it != m_mapOfImages.end(); ++it, cpt++) {
3260 sprintf(buffer,
"%03d", cpt);
3261 std::stringstream ss;
3262 ss <<
"train_image_" << buffer;
3264 switch(m_imageFormat) {
3286 std::string imgFilename = ss.str();
3287 mapOfImgPath[it->first] = imgFilename;
3288 vpImageIo::write(it->second, parent + (!parent.empty() ?
"/" :
"") + imgFilename);
3291 std::cout <<
"Warning: in vpKeyPoint::saveLearningData() training images are not saved because "
3292 "visp_io module is not available !" << std::endl;
3296 bool have3DInfo = m_trainPoints.size() > 0;
3297 if(have3DInfo && m_trainPoints.size() != m_trainKeyPoints.size()) {
3303 std::ofstream file(filename.c_str(), std::ofstream::binary);
3304 if(!file.is_open()) {
3309 int nbImgs = (int) mapOfImgPath.size();
3311 writeBinaryIntLE(file, nbImgs);
3313 #ifdef VISP_HAVE_MODULE_IO
3314 for(std::map<int, std::string>::const_iterator it = mapOfImgPath.begin(); it != mapOfImgPath.end(); ++it) {
3318 writeBinaryIntLE(file,
id);
3321 std::string path = it->second;
3322 int length = (int) path.length();
3324 writeBinaryIntLE(file, length);
3326 for(
int cpt = 0; cpt < length; cpt++) {
3327 file.write((
char *) (&path[(
size_t)cpt]),
sizeof(path[(
size_t)cpt]));
3333 int have3DInfoInt = have3DInfo ? 1 : 0;
3335 writeBinaryIntLE(file, have3DInfoInt);
3338 int nRows = m_trainDescriptors.rows,
3339 nCols = m_trainDescriptors.cols;
3340 int descriptorType = m_trainDescriptors.type();
3344 writeBinaryIntLE(file, nRows);
3348 writeBinaryIntLE(file, nCols);
3352 writeBinaryIntLE(file, descriptorType);
3354 for (
int i = 0; i < nRows; i++) {
3355 unsigned int i_ = (
unsigned int) i;
3357 float u = m_trainKeyPoints[i_].pt.x;
3359 writeBinaryFloatLE(file, u);
3362 float v = m_trainKeyPoints[i_].pt.y;
3364 writeBinaryFloatLE(file, v);
3367 float size = m_trainKeyPoints[i_].size;
3369 writeBinaryFloatLE(file, size);
3372 float angle = m_trainKeyPoints[i_].angle;
3374 writeBinaryFloatLE(file, angle);
3377 float response = m_trainKeyPoints[i_].response;
3379 writeBinaryFloatLE(file, response);
3382 int octave = m_trainKeyPoints[i_].octave;
3384 writeBinaryIntLE(file, octave);
3387 int class_id = m_trainKeyPoints[i_].class_id;
3389 writeBinaryIntLE(file, class_id);
3392 #ifdef VISP_HAVE_MODULE_IO
3393 std::map<int, int>::const_iterator it_findImgId = m_mapOfImageId.find(m_trainKeyPoints[i_].class_id);
3394 int image_id = (saveTrainingImages && it_findImgId != m_mapOfImageId.end()) ? it_findImgId->second : -1;
3396 writeBinaryIntLE(file, image_id);
3400 writeBinaryIntLE(file, image_id);
3404 float oX = m_trainPoints[i_].x, oY = m_trainPoints[i_].y, oZ = m_trainPoints[i_].z;
3407 writeBinaryFloatLE(file, oX);
3411 writeBinaryFloatLE(file, oY);
3415 writeBinaryFloatLE(file, oZ);
3418 for (
int j = 0; j < nCols; j++) {
3420 switch(descriptorType) {
3422 file.write((
char *)(&m_trainDescriptors.at<
unsigned char>(i, j)),
sizeof(m_trainDescriptors.at<
unsigned char>(i, j)));
3426 file.write((
char *)(&m_trainDescriptors.at<
char>(i, j)),
sizeof(m_trainDescriptors.at<
char>(i, j)));
3431 writeBinaryUShortLE(file, m_trainDescriptors.at<
unsigned short int>(i, j));
3436 writeBinaryShortLE(file, m_trainDescriptors.at<
short int>(i, j));
3441 writeBinaryIntLE(file, m_trainDescriptors.at<
int>(i, j));
3446 writeBinaryFloatLE(file, m_trainDescriptors.at<
float>(i, j));
3451 writeBinaryDoubleLE(file, m_trainDescriptors.at<
double>(i, j));
3464 #ifdef VISP_HAVE_XML2
3465 xmlDocPtr doc = NULL;
3466 xmlNodePtr root_node = NULL, image_node = NULL, image_info_node = NULL, descriptors_info_node = NULL,
3467 descriptor_node = NULL, desc_node = NULL;
3476 doc = xmlNewDoc(BAD_CAST
"1.0");
3481 root_node = xmlNewNode(NULL, BAD_CAST
"LearningData");
3482 xmlDocSetRootElement(doc, root_node);
3484 std::stringstream ss;
3487 image_node = xmlNewChild(root_node, NULL, BAD_CAST
"TrainingImageInfo", NULL);
3489 #ifdef VISP_HAVE_MODULE_IO
3490 for(std::map<int, std::string>::const_iterator it = mapOfImgPath.begin(); it != mapOfImgPath.end(); ++it) {
3491 image_info_node = xmlNewChild(image_node, NULL, BAD_CAST
"trainImg",
3492 BAD_CAST it->second.c_str());
3495 xmlNewProp(image_info_node, BAD_CAST
"image_id", BAD_CAST ss.str().c_str());
3500 descriptors_info_node = xmlNewChild(root_node, NULL, BAD_CAST
"DescriptorsInfo", NULL);
3502 int nRows = m_trainDescriptors.rows,
3503 nCols = m_trainDescriptors.cols;
3504 int descriptorType = m_trainDescriptors.type();
3509 xmlNewChild(descriptors_info_node, NULL, BAD_CAST
"nrows", BAD_CAST ss.str().c_str());
3514 xmlNewChild(descriptors_info_node, NULL, BAD_CAST
"ncols", BAD_CAST ss.str().c_str());
3518 ss << descriptorType;
3519 xmlNewChild(descriptors_info_node, NULL, BAD_CAST
"type", BAD_CAST ss.str().c_str());
3521 for (
int i = 0; i < nRows; i++) {
3522 unsigned int i_ = (
unsigned int) i;
3523 descriptor_node = xmlNewChild(root_node, NULL, BAD_CAST
"DescriptorInfo",
3528 ss << std::fixed << std::setprecision(9) << m_trainKeyPoints[i_].pt.x;
3529 xmlNewChild(descriptor_node, NULL, BAD_CAST
"u",
3530 BAD_CAST ss.str().c_str());
3534 ss << std::fixed << std::setprecision(9) << m_trainKeyPoints[i_].pt.y;
3535 xmlNewChild(descriptor_node, NULL, BAD_CAST
"v",
3536 BAD_CAST ss.str().c_str());
3540 ss << std::fixed << std::setprecision(9) << m_trainKeyPoints[i_].size;
3541 xmlNewChild(descriptor_node, NULL, BAD_CAST
"size",
3542 BAD_CAST ss.str().c_str());
3546 ss << std::fixed << std::setprecision(9) << m_trainKeyPoints[i_].angle;
3547 xmlNewChild(descriptor_node, NULL, BAD_CAST
"angle",
3548 BAD_CAST ss.str().c_str());
3552 ss << std::fixed << std::setprecision(9) << m_trainKeyPoints[i_].response;
3553 xmlNewChild(descriptor_node, NULL, BAD_CAST
"response",
3554 BAD_CAST ss.str().c_str());
3557 ss << m_trainKeyPoints[i_].octave;
3558 xmlNewChild(descriptor_node, NULL, BAD_CAST
"octave",
3559 BAD_CAST ss.str().c_str());
3562 ss << m_trainKeyPoints[i_].class_id;
3563 xmlNewChild(descriptor_node, NULL, BAD_CAST
"class_id",
3564 BAD_CAST ss.str().c_str());
3567 #ifdef VISP_HAVE_MODULE_IO
3568 std::map<int, int>::const_iterator it_findImgId = m_mapOfImageId.find(m_trainKeyPoints[i_].class_id);
3569 ss << ((saveTrainingImages && it_findImgId != m_mapOfImageId.end()) ? it_findImgId->second : -1);
3570 xmlNewChild(descriptor_node, NULL, BAD_CAST
"image_id",
3571 BAD_CAST ss.str().c_str());
3574 xmlNewChild(descriptor_node, NULL, BAD_CAST
"image_id",
3575 BAD_CAST ss.str().c_str());
3581 ss << std::fixed << std::setprecision(9) << m_trainPoints[i_].x;
3582 xmlNewChild(descriptor_node, NULL, BAD_CAST
"oX",
3583 BAD_CAST ss.str().c_str());
3587 ss << std::fixed << std::setprecision(9) << m_trainPoints[i_].y;
3588 xmlNewChild(descriptor_node, NULL, BAD_CAST
"oY",
3589 BAD_CAST ss.str().c_str());
3593 ss << std::fixed << std::setprecision(9) << m_trainPoints[i_].z;
3594 xmlNewChild(descriptor_node, NULL, BAD_CAST
"oZ",
3595 BAD_CAST ss.str().c_str());
3598 desc_node = xmlNewChild(descriptor_node, NULL, BAD_CAST
"desc", NULL);
3600 for (
int j = 0; j < nCols; j++) {
3603 switch(descriptorType) {
3610 int val_tmp = m_trainDescriptors.at<
unsigned char>(i, j);
3621 int val_tmp = m_trainDescriptors.at<
char>(i, j);
3627 ss << m_trainDescriptors.at<
unsigned short int>(i, j);
3631 ss << m_trainDescriptors.at<
short int>(i, j);
3635 ss << m_trainDescriptors.at<
int>(i, j);
3640 ss << std::fixed << std::setprecision(9) << m_trainDescriptors.at<
float>(i, j);
3645 ss << std::fixed << std::setprecision(17) << m_trainDescriptors.at<
double>(i, j);
3652 xmlNewChild(desc_node, NULL, BAD_CAST
"val",
3653 BAD_CAST ss.str().c_str());
3657 xmlSaveFormatFileEnc(filename.c_str(), doc,
"UTF-8", 1);
3668 std::cerr <<
"Error: libxml2 is required !" << std::endl;
3673 #if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x030000)
3675 struct KeypointResponseGreaterThanThreshold {
3676 KeypointResponseGreaterThanThreshold(
float _value) :
3679 inline bool operator()(
const cv::KeyPoint& kpt)
const {
3680 return kpt.response >= value;
3685 struct KeypointResponseGreater {
3686 inline bool operator()(
const cv::KeyPoint& kp1,
3687 const cv::KeyPoint& kp2)
const {
3688 return kp1.response > kp2.response;
3693 void vpKeyPoint::KeyPointsFilter::retainBest(
3694 std::vector<cv::KeyPoint>& keypoints,
int n_points) {
3696 if (n_points >= 0 && keypoints.size() > (size_t) n_points) {
3697 if (n_points == 0) {
3702 std::nth_element(keypoints.begin(), keypoints.begin() + n_points,
3703 keypoints.end(), KeypointResponseGreater());
3705 float ambiguous_response = keypoints[(size_t) (n_points - 1)].response;
3707 std::vector<cv::KeyPoint>::const_iterator new_end = std::partition(
3708 keypoints.begin() + n_points, keypoints.end(),
3709 KeypointResponseGreaterThanThreshold(ambiguous_response));
3711 keypoints.resize((
size_t) (new_end - keypoints.begin()));
3715 struct RoiPredicate {
3716 RoiPredicate(
const cv::Rect& _r) :
3720 bool operator()(
const cv::KeyPoint& keyPt)
const {
3721 return !r.contains(keyPt.pt);
3727 void vpKeyPoint::KeyPointsFilter::runByImageBorder(
3728 std::vector<cv::KeyPoint>& keypoints, cv::Size imageSize,
int borderSize) {
3729 if (borderSize > 0) {
3730 if (imageSize.height <= borderSize * 2 || imageSize.width <= borderSize * 2)
3734 std::remove_if(keypoints.begin(), keypoints.end(),
3736 cv::Rect(cv::Point(borderSize, borderSize),
3737 cv::Point(imageSize.width - borderSize,
3738 imageSize.height - borderSize)))), keypoints.end());
3742 struct SizePredicate {
3743 SizePredicate(
float _minSize,
float _maxSize) :
3744 minSize(_minSize), maxSize(_maxSize) {
3747 bool operator()(
const cv::KeyPoint& keyPt)
const {
3748 float size = keyPt.size;
3749 return (size < minSize) || (size > maxSize);
3752 float minSize, maxSize;
3755 void vpKeyPoint::KeyPointsFilter::runByKeypointSize(
3756 std::vector<cv::KeyPoint>& keypoints,
float minSize,
float maxSize) {
3757 CV_Assert(minSize >= 0);
3758 CV_Assert(maxSize >= 0);
3759 CV_Assert(minSize <= maxSize);
3762 std::remove_if(keypoints.begin(), keypoints.end(),
3763 SizePredicate(minSize, maxSize)), keypoints.end());
3766 class MaskPredicate {
3768 MaskPredicate(
const cv::Mat& _mask) :
3771 bool operator()(
const cv::KeyPoint& key_pt)
const {
3772 return mask.at<uchar>((int) (key_pt.pt.y + 0.5f),
3773 (int) (key_pt.pt.x + 0.5f)) == 0;
3778 MaskPredicate& operator=(
const MaskPredicate&);
3781 void vpKeyPoint::KeyPointsFilter::runByPixelsMask(
3782 std::vector<cv::KeyPoint>& keypoints,
const cv::Mat& mask) {
3787 std::remove_if(keypoints.begin(), keypoints.end(), MaskPredicate(mask)),
3791 struct KeyPoint_LessThan {
3792 KeyPoint_LessThan(
const std::vector<cv::KeyPoint>& _kp) :
3795 bool operator()(
size_t i,
size_t j)
const {
3796 const cv::KeyPoint& kp1 = (*kp)[ i];
3797 const cv::KeyPoint& kp2 = (*kp)[ j];
3798 if (!
vpMath::equal(kp1.pt.x, kp2.pt.x, std::numeric_limits<float>::epsilon())) {
3799 return kp1.pt.x < kp2.pt.x;
3802 if (!
vpMath::equal(kp1.pt.y, kp2.pt.y, std::numeric_limits<float>::epsilon())) {
3803 return kp1.pt.y < kp2.pt.y;
3806 if (!
vpMath::equal(kp1.size, kp2.size, std::numeric_limits<float>::epsilon())) {
3807 return kp1.size > kp2.size;
3810 if (!
vpMath::equal(kp1.angle, kp2.angle, std::numeric_limits<float>::epsilon())) {
3811 return kp1.angle < kp2.angle;
3814 if (!
vpMath::equal(kp1.response, kp2.response, std::numeric_limits<float>::epsilon())) {
3815 return kp1.response > kp2.response;
3818 if (kp1.octave != kp2.octave) {
3819 return kp1.octave > kp2.octave;
3822 if (kp1.class_id != kp2.class_id) {
3823 return kp1.class_id > kp2.class_id;
3828 const std::vector<cv::KeyPoint>* kp;
3831 void vpKeyPoint::KeyPointsFilter::removeDuplicated(
3832 std::vector<cv::KeyPoint>& keypoints) {
3833 size_t i, j, n = keypoints.size();
3834 std::vector<size_t> kpidx(n);
3835 std::vector<uchar> mask(n, (uchar) 1);
3837 for (i = 0; i < n; i++) {
3840 std::sort(kpidx.begin(), kpidx.end(), KeyPoint_LessThan(keypoints));
3841 for (i = 1, j = 0; i < n; i++) {
3842 cv::KeyPoint& kp1 = keypoints[kpidx[i]];
3843 cv::KeyPoint& kp2 = keypoints[kpidx[j]];
3845 if (!
vpMath::equal(kp1.pt.x, kp2.pt.x, std::numeric_limits<float>::epsilon()) ||
3846 !
vpMath::equal(kp1.pt.y, kp2.pt.y, std::numeric_limits<float>::epsilon()) ||
3847 !
vpMath::equal(kp1.size, kp2.size, std::numeric_limits<float>::epsilon()) ||
3848 !
vpMath::equal(kp1.angle, kp2.angle, std::numeric_limits<float>::epsilon())) {
3855 for (i = j = 0; i < n; i++) {
3858 keypoints[j] = keypoints[i];
3863 keypoints.resize(j);
3869 vpKeyPoint::PyramidAdaptedFeatureDetector::PyramidAdaptedFeatureDetector(
3870 const cv::Ptr<cv::FeatureDetector>& _detector,
int _maxLevel) :
3871 detector(_detector), maxLevel(_maxLevel) {
3874 bool vpKeyPoint::PyramidAdaptedFeatureDetector::empty()
const {
3875 return detector.empty() || (cv::FeatureDetector*) detector->empty();
3878 void vpKeyPoint::PyramidAdaptedFeatureDetector::detect( cv::InputArray image, CV_OUT std::vector<cv::KeyPoint>& keypoints,
3879 cv::InputArray mask ) {
3880 detectImpl(image.getMat(), keypoints, mask.getMat());
3883 void vpKeyPoint::PyramidAdaptedFeatureDetector::detectImpl(
const cv::Mat& image,
3884 std::vector<cv::KeyPoint>& keypoints,
const cv::Mat& mask)
const {
3885 cv::Mat src = image;
3886 cv::Mat src_mask = mask;
3888 cv::Mat dilated_mask;
3889 if (!mask.empty()) {
3890 cv::dilate(mask, dilated_mask, cv::Mat());
3891 cv::Mat mask255(mask.size(), CV_8UC1, cv::Scalar(0));
3892 mask255.setTo(cv::Scalar(255), dilated_mask != 0);
3893 dilated_mask = mask255;
3896 for (
int l = 0, multiplier = 1; l <= maxLevel; ++l, multiplier *= 2) {
3898 std::vector<cv::KeyPoint> new_pts;
3899 detector->detect(src, new_pts, src_mask);
3900 std::vector<cv::KeyPoint>::iterator it = new_pts.begin(), end =
3902 for (; it != end; ++it) {
3903 it->pt.x *= multiplier;
3904 it->pt.y *= multiplier;
3905 it->size *= multiplier;
3908 keypoints.insert(keypoints.end(), new_pts.begin(), new_pts.end());
3917 resize(dilated_mask, src_mask, src.size(), 0, 0, CV_INTER_AREA);
3922 vpKeyPoint::KeyPointsFilter::runByPixelsMask(keypoints, mask);
3926 #elif !defined(VISP_BUILD_SHARED_LIBS)
3928 void dummy_vpKeyPoint() {};
virtual void displayCircle(const vpImagePoint ¢er, unsigned int radius, const vpColor &color, bool fill=false, unsigned int thickness=1)=0
static void write(const vpImage< unsigned char > &I, const char *filename)
void getTrainKeyPoints(std::vector< cv::KeyPoint > &keyPoints) const
Implementation of a matrix and operations on matrices.
void displayMatching(const vpImage< unsigned char > &IRef, vpImage< unsigned char > &IMatching, unsigned int crossSize, unsigned int lineThickness=1, const vpColor &color=vpColor::green)
static void compute3D(const cv::KeyPoint &candidate, const std::vector< vpPoint > &roi, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, cv::Point3f &point)
void set_oZ(const double oZ)
Set the point Z coordinate in the object frame.
void detectExtractAffine(const vpImage< unsigned char > &I, std::vector< std::vector< cv::KeyPoint > > &listOfKeypoints, std::vector< cv::Mat > &listOfDescriptors, std::vector< vpImage< unsigned char > > *listOfAffineI=NULL)
void getQueryKeyPoints(std::vector< cv::KeyPoint > &keyPoints) const
std::string getMatcherName() const
unsigned int getWidth() const
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
void createImageMatching(vpImage< unsigned char > &IRef, vpImage< unsigned char > &ICurrent, vpImage< unsigned char > &IMatching)
Implementation of an homogeneous matrix and operations on such kind of matrices.
void detect(const vpImage< unsigned char > &I, std::vector< cv::KeyPoint > &keyPoints, double &elapsedTime, const vpRect &rectangle=vpRect())
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
Point coordinates conversion from normalized coordinates in meter to pixel coordinates ...
Class to define colors available for display functionnalities.
static bool equal(double x, double y, double s=0.001)
double getMatchingFactorThreshold() const
bool getUseRansacVVSPoseEstimation() const
static const vpColor none
error that can be emited by ViSP classes.
double getRansacConsensusPercentage() const
void setRansacThreshold(const double &t)
void set_x(const double x)
Set the point x coordinate in the image plane.
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
Point coordinates conversion from pixel coordinates to normalized coordinates in meter...
std::vector< unsigned int > getRansacInlierIndex() const
int getNbRansacMinInlierCount() const
double get_y() const
Get the point y coordinate in the image plane.
static const vpColor green
void match(const cv::Mat &trainDescriptors, const cv::Mat &queryDescriptors, std::vector< cv::DMatch > &matches, double &elapsedTime)
static int round(const double x)
bool getUseRansacConsensusPercentage() const
void insertImageMatching(const vpImage< unsigned char > &IRef, const vpImage< unsigned char > &ICurrent, vpImage< unsigned char > &IMatching)
Class that defines what is a point.
Defines a generic 2D polygon.
const char * what() const
vpRect getBoundingBox() const
double getRansacReprojectionError() const
void set_u(const double u)
static void compute3DForPointsInPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, std::vector< cv::KeyPoint > &candidate, const std::vector< vpPolygon > &polygons, const std::vector< std::vector< vpPoint > > &roisPt, std::vector< cv::Point3f > &points, cv::Mat *descriptors=NULL)
bool computePose(vpPoseMethodType methode, vpHomogeneousMatrix &cMo, bool(*func)(vpHomogeneousMatrix *)=NULL)
compute the pose for a given method
bool _reference_computed
flag to indicate if the reference has been built.
virtual void displayCross(const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)=0
void set_v(const double v)
Class used for pose computation from N points (pose from point only).
Generic class defining intrinsic camera parameters.
int getNbRansacIterations() const
double getMatchingRatioThreshold() const
void set_y(const double y)
Set the point y coordinate in the image plane.
std::string getDetectorName() const
void set_oX(const double oX)
Set the point X coordinate in the object frame.
void display(const vpImage< unsigned char > &IRef, const vpImage< unsigned char > &ICurrent, unsigned int size=3)
unsigned int buildReference(const vpImage< unsigned char > &I)
double get_x() const
Get the point x coordinate in the image plane.
vpMatrix getCovarianceMatrix() const
vpKeyPoint(const std::string &detectorName="ORB", const std::string &extractorName="ORB", const std::string &matcherName="BruteForce-Hamming", const vpFilterMatchingType &filterType=ratioDistanceThreshold)
std::vector< vpImagePoint > referenceImagePointsList
void getObjectPoints(std::vector< cv::Point3f > &objectPoints) const
bool computePose(const std::vector< cv::Point2f > &imagePoints, const std::vector< cv::Point3f > &objectPoints, const vpCameraParameters &cam, vpHomogeneousMatrix &cMo, std::vector< int > &inlierIndex, double &elapsedTime, bool(*func)(vpHomogeneousMatrix *)=NULL)
unsigned int matchPoint(const vpImage< unsigned char > &I)
void setRansacMaxTrials(const int &rM)
void setRansacNbInliersToReachConsensus(const unsigned int &nbC)
VISP_EXPORT double measureTimeMs()
void loadLearningData(const std::string &filename, const bool binaryMode=false, const bool append=false)
void setWorldCoordinates(const double oX, const double oY, const double oZ)
double getRansacThreshold() const
Implementation of column vector and the associated operations.
void set_oY(const double oY)
Set the point Y coordinate in the object frame.
void saveLearningData(const std::string &filename, const bool binaryMode=false, const bool saveTrainingImages=true)
void getTrainPoints(std::vector< cv::Point3f > &points) const
void insert(const vpImage< Type > &src, const vpImagePoint topLeft)
vpHomogeneousMatrix inverse() const
void extract(const vpImage< unsigned char > &I, std::vector< cv::KeyPoint > &keyPoints, cv::Mat &descriptors, double &elapsedTime, std::vector< cv::Point3f > *trainPoints=NULL)
bool matchPointAndDetect(const vpImage< unsigned char > &I, vpRect &boundingBox, vpImagePoint ¢erOfGravity, const bool isPlanarObject=true, std::vector< vpImagePoint > *imPts1=NULL, std::vector< vpImagePoint > *imPts2=NULL, double *meanDescriptorDistance=NULL, double *detectionScore=NULL, const vpRect &rectangle=vpRect())
std::vector< unsigned int > matchedReferencePoints
unsigned int getHeight() const
Defines a rectangle in the plane.
std::vector< vpImagePoint > currentImagePointsList
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
virtual void displayLine(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1)=0
This class defines the container for a plane geometrical structure.
void loadConfigFile(const std::string &configFile)
static void read(vpImage< unsigned char > &I, const char *filename)
void addPoint(const vpPoint &P)
Add a new point in this array.
static void convertFromOpenCV(const cv::KeyPoint &from, vpImagePoint &to)
Class that consider the case of a translation vector.
Implementation of a rotation vector as axis-angle minimal representation.
std::vector< vpPoint > getRansacInliers() const
void setCovarianceComputation(const bool &flag)
std::string getExtractorName() const
void set_ij(const double ii, const double jj)
vpMatchingMethodEnum getMatchingMethod() const
void initMatcher(const std::string &matcherName)
void parse(const std::string &filename)