44 #include <visp3/core/vpConfig.h>
46 #if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020408)
50 #include <visp3/core/vpDisplay.h>
51 #include <visp3/core/vpTrackingException.h>
52 #include <visp3/klt/vpKltOpencv.h>
58 : m_gray(), m_prevGray(), m_points_id(), m_maxCount(500), m_termcrit(), m_winSize(10), m_qualityLevel(0.01),
59 m_minDistance(15), m_minEigThreshold(1e-4), m_harris_k(0.04), m_blockSize(3), m_useHarrisDetector(1),
60 m_pyrMaxLevel(3), m_next_points_id(0), m_initial_guess(false)
62 m_termcrit = cv::TermCriteria(cv::TermCriteria::COUNT | cv::TermCriteria::EPS, 20, 0.03);
69 : m_gray(), m_prevGray(), m_points_id(), m_maxCount(500), m_termcrit(), m_winSize(10), m_qualityLevel(0.01),
70 m_minDistance(15), m_minEigThreshold(1e-4), m_harris_k(0.04), m_blockSize(3), m_useHarrisDetector(1),
71 m_pyrMaxLevel(3), m_next_points_id(0), m_initial_guess(false)
121 for (
size_t i = 0; i < 2; i++) {
133 for (
size_t i = 0; i <
m_points[1].size(); i++)
148 std::vector<float> err;
154 flags |= cv::OPTFLOW_USE_INITIAL_FLOW;
167 std::vector<uchar> status;
173 for (
int i = (
int)status.size() - 1; i >= 0; i--) {
174 if (status[(
size_t)i] == 0) {
197 if ((
size_t)index >=
m_points[1].size()) {
231 const vpColor &color,
unsigned int thickness)
234 for (
size_t i = 0; i < features.size(); i++) {
254 unsigned int thickness)
257 for (
size_t i = 0; i < features.size(); i++) {
279 const std::vector<long> &featuresid,
const vpColor &color,
unsigned int thickness)
282 for (
size_t i = 0; i < features.size(); i++) {
287 std::ostringstream id;
309 const std::vector<long> &featuresid,
const vpColor &color,
unsigned int thickness)
312 for (
size_t i = 0; i < features.size(); i++) {
317 std::ostringstream id;
421 if (guess_pts.size() !=
m_points[1].size()) {
423 "Cannot set initial guess: size feature vector [%d] "
424 "and guess vector [%d] doesn't match",
425 m_points[1].size(), guess_pts.size()));
447 const std::vector<long> &fid)
449 if (guess_pts.size() != init_pts.size()) {
451 "Cannot set initial guess: size init vector [%d] and "
452 "guess vector [%d] doesn't match",
453 init_pts.size(), guess_pts.size()));
476 for (
size_t i = 0; i <
m_points[1].size(); i++) {
489 if (ids.size() != pts.size()) {
491 for (
size_t i = 0; i <
m_points[1].size(); i++)
495 for (
size_t i = 0; i <
m_points[1].size(); i++) {
560 if ((
size_t)index >=
m_points[1].size()) {
568 #elif defined(VISP_HAVE_OPENCV)
572 #include <visp3/klt/vpKltOpencv.h>
574 void vpKltOpencv::clean()
577 cvReleaseImage(&image);
579 cvReleaseImage(&prev_image);
581 cvReleaseImage(&pyramid);
583 cvReleaseImage(&prev_pyramid);
592 countPrevFeatures = 0;
595 globalcountFeatures = 0;
598 void vpKltOpencv::cleanAll()
604 cvFree(&prev_features);
608 cvFree(&lostDuringTrack);
612 cvFree(&prev_featuresid);
614 prev_features = NULL;
618 prev_featuresid = NULL;
621 void vpKltOpencv::reset() { clean(); }
627 : initialized(0), maxFeatures(50), globalcountFeatures(0), win_size(10), quality(0.01), min_distance(10),
628 harris_free_parameter(0.04), block_size(3), use_harris(1), pyramid_level(3), _tid(-1), image(NULL),
629 prev_image(NULL), pyramid(NULL), prev_pyramid(NULL), swap_temp(NULL), countFeatures(0), countPrevFeatures(0),
630 features(NULL), prev_features(NULL), featuresid(NULL), prev_featuresid(NULL), flags(0), initial_guess(false),
631 lostDuringTrack(0), status(0), OnInitialize(0), OnFeatureLost(0), OnNewFeature(0), OnMeasureFeature(0),
634 features = (CvPoint2D32f *)cvAlloc((
unsigned int)maxFeatures *
sizeof(features[0]));
635 prev_features = (CvPoint2D32f *)cvAlloc((
unsigned int)maxFeatures *
sizeof(prev_features[0]));
636 status = (
char *)cvAlloc((
size_t)maxFeatures);
637 lostDuringTrack = (
bool *)cvAlloc((
size_t)maxFeatures);
638 featuresid = (
long *)cvAlloc((
unsigned int)maxFeatures *
sizeof(long));
639 prev_featuresid = (
long *)cvAlloc((
unsigned int)maxFeatures *
sizeof(long));
646 : initialized(0), maxFeatures(50), globalcountFeatures(0), win_size(10), quality(0.01), min_distance(10),
647 harris_free_parameter(0.04), block_size(3), use_harris(1), pyramid_level(3), _tid(-1), image(NULL),
648 prev_image(NULL), pyramid(NULL), prev_pyramid(NULL), swap_temp(NULL), countFeatures(0), countPrevFeatures(0),
649 features(NULL), prev_features(NULL), featuresid(NULL), prev_featuresid(NULL), flags(0), initial_guess(false),
650 lostDuringTrack(0), status(0), OnInitialize(0), OnFeatureLost(0), OnNewFeature(0), OnMeasureFeature(0),
662 initialized = copy.initialized;
663 maxFeatures = copy.maxFeatures;
664 countFeatures = copy.countFeatures;
665 countPrevFeatures = copy.countPrevFeatures;
666 globalcountFeatures = copy.globalcountFeatures;
668 win_size = copy.win_size;
669 quality = copy.quality;
670 min_distance = copy.min_distance;
671 harris_free_parameter = copy.harris_free_parameter;
672 block_size = copy.block_size;
673 use_harris = copy.use_harris;
674 pyramid_level = copy.pyramid_level;
677 OnInitialize = copy.OnInitialize;
678 OnFeatureLost = copy.OnFeatureLost;
679 OnNewFeature = copy.OnNewFeature;
680 OnMeasureFeature = copy.OnMeasureFeature;
681 IsFeatureValid = copy.IsFeatureValid;
683 initial_guess = copy.initial_guess;
684 lostDuringTrack = copy.lostDuringTrack;
690 countPrevFeatures = 0;
693 globalcountFeatures = 0;
697 image = cvCreateImage(cvGetSize(copy.image), 8, 1);
699 cvCopy(copy.image, image, 0);
702 if (copy.prev_image) {
703 prev_image = cvCreateImage(cvGetSize(copy.prev_image), IPL_DEPTH_8U, 1);
705 cvCopy(copy.prev_image, prev_image, 0);
709 pyramid = cvCreateImage(cvGetSize(copy.pyramid), IPL_DEPTH_8U, 1);
711 cvCopy(copy.pyramid, pyramid, 0);
714 if (copy.prev_pyramid) {
715 prev_pyramid = cvCreateImage(cvGetSize(copy.prev_pyramid), IPL_DEPTH_8U, 1);
717 cvCopy(copy.prev_pyramid, prev_pyramid, 0);
722 features = (CvPoint2D32f *)cvAlloc((
unsigned int)copy.maxFeatures *
sizeof(CvPoint2D32f));
723 for (
int i = 0; i < copy.maxFeatures; i++)
724 features[i] = copy.features[i];
727 if (copy.prev_features) {
728 prev_features = (CvPoint2D32f *)cvAlloc((
unsigned int)copy.maxFeatures *
sizeof(CvPoint2D32f));
729 for (
int i = 0; i < copy.maxFeatures; i++)
730 prev_features[i] = copy.prev_features[i];
733 if (copy.featuresid) {
734 featuresid = (
long *)cvAlloc((
unsigned int)copy.maxFeatures *
sizeof(long));
735 for (
int i = 0; i < copy.maxFeatures; i++)
736 featuresid[i] = copy.featuresid[i];
739 if (copy.prev_featuresid) {
740 prev_featuresid = (
long *)cvAlloc((
unsigned int)copy.maxFeatures *
sizeof(long));
741 for (
int i = 0; i < copy.maxFeatures; i++)
742 prev_featuresid[i] = copy.prev_featuresid[i];
746 status = (
char *)cvAlloc((
unsigned int)copy.maxFeatures *
sizeof(char));
747 for (
int i = 0; i < copy.maxFeatures; i++)
748 status[i] = copy.status[i];
751 if (copy.lostDuringTrack) {
752 lostDuringTrack = (
bool *)cvAlloc((
unsigned int)copy.maxFeatures *
sizeof(bool));
753 for (
int i = 0; i < copy.maxFeatures; i++)
754 lostDuringTrack[i] = copy.lostDuringTrack[i];
777 cvFree(&prev_features);
781 cvFree(&lostDuringTrack);
785 cvFree(&prev_featuresid);
787 features = (CvPoint2D32f *)cvAlloc((
unsigned int)maxFeatures *
sizeof(CvPoint2D32f));
788 prev_features = (CvPoint2D32f *)cvAlloc((
unsigned int)maxFeatures *
sizeof(CvPoint2D32f));
789 status = (
char *)cvAlloc((
unsigned int)maxFeatures *
sizeof(char));
790 lostDuringTrack = (
bool *)cvAlloc((
unsigned int)maxFeatures *
sizeof(bool));
791 featuresid = (
long *)cvAlloc((
unsigned int)maxFeatures *
sizeof(long));
792 prev_featuresid = (
long *)cvAlloc((
unsigned int)maxFeatures *
sizeof(long));
811 if (I->depth != IPL_DEPTH_8U || I->nChannels != 1) {
816 if (mask->depth != IPL_DEPTH_8U || I->nChannels != 1) {
822 CvSize Sizeim, SizeI;
823 SizeI = cvGetSize(I);
826 Sizeim = cvGetSize(image);
827 if (SizeI.width != Sizeim.width || SizeI.height != Sizeim.height)
830 if (image == NULL || prev_image == NULL || pyramid == NULL || prev_pyramid == NULL || !b_imOK) {
832 image = cvCreateImage(cvGetSize(I), 8, 1);
833 image->origin = I->origin;
834 prev_image = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
835 pyramid = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
836 prev_pyramid = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
840 countPrevFeatures = 0;
843 globalcountFeatures = 0;
852 countFeatures = maxFeatures;
853 countPrevFeatures = 0;
854 IplImage *eig = cvCreateImage(cvGetSize(image), 32, 1);
855 IplImage *temp = cvCreateImage(cvGetSize(image), 32, 1);
856 cvGoodFeaturesToTrack(image, eig, temp, features, &countFeatures, quality, min_distance, mask, block_size, use_harris,
857 harris_free_parameter);
858 cvFindCornerSubPix(image, features, countFeatures, cvSize(win_size, win_size), cvSize(-1, -1),
859 cvTermCriteria(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03));
860 cvReleaseImage(&eig);
861 cvReleaseImage(&temp);
867 for (
int boucle = 0; boucle < countFeatures; boucle++) {
868 featuresid[boucle] = globalcountFeatures;
869 globalcountFeatures++;
872 OnNewFeature(_tid, boucle, featuresid[boucle], features[boucle].x, features[boucle].y);
887 if (size > maxFeatures)
891 CvSize Sizeim, SizeI;
892 SizeI = cvGetSize(I);
895 Sizeim = cvGetSize(image);
896 if (SizeI.width != Sizeim.width || SizeI.height != Sizeim.height)
899 if (image == NULL || prev_image == NULL || pyramid == NULL || prev_pyramid == NULL || !b_imOK) {
901 image = cvCreateImage(cvGetSize(I), 8, 1);
902 image->origin = I->origin;
903 prev_image = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
904 pyramid = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
905 prev_pyramid = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
910 countFeatures = size;
911 for (
int i = 0; i < countFeatures; i++) {
912 features[i] = pts[i];
916 globalcountFeatures = size;
924 if (size > maxFeatures)
928 CvSize Sizeim, SizeI;
929 SizeI = cvGetSize(I);
932 Sizeim = cvGetSize(image);
933 if (SizeI.width != Sizeim.width || SizeI.height != Sizeim.height)
936 if (image == NULL || prev_image == NULL || pyramid == NULL || prev_pyramid == NULL || !b_imOK) {
938 image = cvCreateImage(cvGetSize(I), 8, 1);
939 image->origin = I->origin;
940 prev_image = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
941 pyramid = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
942 prev_pyramid = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
947 countFeatures = size;
949 for (
int i = 0; i < countFeatures; i++) {
950 features[i] = pts[i];
951 featuresid[i] = fid[i];
956 globalcountFeatures = max + 1;
973 if (I->depth != IPL_DEPTH_8U || I->nChannels != 1) {
977 CV_SWAP(prev_image, image, swap_temp);
978 CV_SWAP(prev_pyramid, pyramid, swap_temp);
982 if (!initial_guess) {
984 countPrevFeatures = countFeatures;
985 for (
int boucle = 0; boucle < countFeatures; boucle++) {
986 prev_featuresid[boucle] = featuresid[boucle];
989 CvPoint2D32f *swap_features = 0;
990 CV_SWAP(prev_features, features, swap_features);
993 if (countFeatures <= 0)
996 cvCalcOpticalFlowPyrLK(prev_image, image, prev_pyramid, pyramid, prev_features, features, countFeatures,
997 cvSize(win_size, win_size), pyramid_level, status, 0,
998 cvTermCriteria(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03), flags);
1001 flags |= CV_LKFLOW_PYR_A_READY;
1003 flags = CV_LKFLOW_PYR_A_READY;
1004 initial_guess =
false;
1008 for (i = k = 0; i < countFeatures; i++) {
1010 lostDuringTrack[i] = 1;
1012 OnFeatureLost(_tid, i, featuresid[i], features[i].x, features[i].y);
1016 if (IsFeatureValid) {
1017 if (!IsFeatureValid(_tid, features[i].x, features[i].y)) {
1018 lostDuringTrack[i] = 1;
1020 OnFeatureLost(_tid, i, featuresid[i], features[i].x, features[i].y);
1024 features[k] = features[i];
1025 featuresid[k] = featuresid[i];
1027 if (OnMeasureFeature)
1028 OnMeasureFeature(_tid, k, featuresid[k], features[k].x, features[k].y);
1030 lostDuringTrack[i] = 0;
1045 if ((features == 0) || (I.
bitmap == 0) || (!initialized)) {
1068 if (index >= countFeatures) {
1073 x = features[index].x;
1074 y = features[index].y;
1075 id = featuresid[index];
1090 countPrevFeatures = countFeatures;
1091 for (
int boucle = 0; boucle < countFeatures; boucle++) {
1092 prev_featuresid[boucle] = featuresid[boucle];
1095 CvPoint2D32f *swap_features = NULL;
1096 CV_SWAP(prev_features, *guess_pts, swap_features);
1098 CV_SWAP(features, prev_features, swap_features);
1100 flags |= CV_LKFLOW_INITIAL_GUESSES;
1102 initial_guess =
true;
1122 countPrevFeatures = size;
1123 countFeatures = size;
1124 for (
int boucle = 0; boucle < size; boucle++) {
1125 prev_featuresid[boucle] = fid[boucle];
1126 featuresid[boucle] = fid[boucle];
1129 CvPoint2D32f *swap_features = NULL;
1130 CvPoint2D32f *swap_features2 = NULL;
1131 CV_SWAP(prev_features, *init_pts, swap_features);
1136 CV_SWAP(features, *guess_pts, swap_features2);
1138 flags |= CV_LKFLOW_INITIAL_GUESSES;
1140 initial_guess =
true;
1151 void vpKltOpencv::getPrevFeature(
int index,
int &
id,
float &x,
float &y)
const
1153 if (index >= countPrevFeatures) {
1158 x = prev_features[index].x;
1159 y = prev_features[index].y;
1160 id = prev_featuresid[index];
1171 if (maxFeatures == countFeatures) {
1179 features[countFeatures] = f;
1180 featuresid[countFeatures] = id;
1186 if (index >= countFeatures) {
1193 for (
int i = index; i < countFeatures; i++) {
1194 features[i] = features[i + 1];
1195 featuresid[i] = featuresid[i + 1];
1214 vpColor color,
unsigned int thickness)
1217 for (
int i = 0; i < nbFeatures; i++) {
1238 vpColor color,
unsigned int thickness)
1241 for (
int i = 0; i < nbFeatures; i++) {
1265 const long *featuresid_list,
const int &nbFeatures,
vpColor color,
unsigned int thickness)
1268 for (
int i = 0; i < nbFeatures; i++) {
1273 std::stringstream id;
1274 id << featuresid_list[i];
1297 const int &nbFeatures,
vpColor color,
unsigned int thickness)
1300 for (
int i = 0; i < nbFeatures; i++) {
1305 std::stringstream id;
1306 id << featuresid_list[i];
1315 class VISP_EXPORT dummy_vpKltOpencv
1318 dummy_vpKltOpencv(){};
1321 #if !defined(VISP_BUILD_SHARED_LIBS)
1324 void dummy_vpKltOpenCV_fct(){};
Class to define RGB colors available for display functionnalities.
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
error that can be emited by ViSP classes.
@ badValue
Used to indicate that a value is not in the allowed range.
@ memoryAllocationError
Memory allocation error.
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Type * bitmap
points toward the bitmap
Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented in OpenCV. Thus to enable this ...
std::vector< long > m_points_id
Keypoint id.
void setBlockSize(int blockSize)
void setQuality(double qualityLevel)
void track(const cv::Mat &I)
vpKltOpencv & operator=(const vpKltOpencv ©)
cv::TermCriteria m_termcrit
void setHarrisFreeParameter(double harris_k)
void getFeature(const int &index, long &id, float &x, float &y) const
std::vector< cv::Point2f > m_points[2]
Previous [0] and current [1] keypoint location.
void suppressFeature(const int &index)
void setMaxFeatures(int maxCount)
void setMinEigThreshold(double minEigThreshold)
void setInitialGuess(const std::vector< cv::Point2f > &guess_pts)
void addFeature(const float &x, const float &y)
void initTracking(const cv::Mat &I, const cv::Mat &mask=cv::Mat())
void setMinDistance(double minDistance)
void display(const vpImage< unsigned char > &I, const vpColor &color=vpColor::red, unsigned int thickness=1)
void setUseHarris(int useHarrisDetector)
void setWindowSize(int winSize)
void setPyramidLevels(int pyrMaxLevel)
static int round(double x)
Error that can be emited by the vpTracker class and its derivates.