48 #include <visp3/core/vpConfig.h> 50 #if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020408) 54 #include <visp3/core/vpDisplay.h> 55 #include <visp3/core/vpTrackingException.h> 56 #include <visp3/klt/vpKltOpencv.h> 62 : m_gray(), m_prevGray(), m_points_id(), m_maxCount(500), m_termcrit(), m_winSize(10), m_qualityLevel(0.01),
63 m_minDistance(15), m_minEigThreshold(1e-4), m_harris_k(0.04), m_blockSize(3), m_useHarrisDetector(1),
64 m_pyrMaxLevel(3), m_next_points_id(0), m_initial_guess(false)
66 m_termcrit = cv::TermCriteria(cv::TermCriteria::COUNT | cv::TermCriteria::EPS, 20, 0.03);
125 for (
size_t i = 0; i < 2; i++) {
137 for (
size_t i = 0; i <
m_points[1].size(); i++)
152 std::vector<float> err;
158 flags |= cv::OPTFLOW_USE_INITIAL_FLOW;
171 std::vector<uchar> status;
177 for (
int i = (
int)status.size() - 1; i >= 0; i--) {
178 if (status[(
size_t)i] == 0) {
201 if ((
size_t)index >=
m_points[1].size()) {
235 const vpColor &color,
unsigned int thickness)
238 for (
size_t i = 0; i < features.size(); i++) {
258 unsigned int thickness)
261 for (
size_t i = 0; i < features.size(); i++) {
283 const std::vector<long> &featuresid,
const vpColor &color,
unsigned int thickness)
286 for (
size_t i = 0; i < features.size(); i++) {
291 std::ostringstream id;
313 const std::vector<long> &featuresid,
const vpColor &color,
unsigned int thickness)
316 for (
size_t i = 0; i < features.size(); i++) {
321 std::ostringstream id;
425 if (guess_pts.size() !=
m_points[1].size()) {
427 "Cannot set initial guess: size feature vector [%d] " 428 "and guess vector [%d] doesn't match",
429 m_points[1].size(), guess_pts.size()));
451 const std::vector<long> &fid)
453 if (guess_pts.size() != init_pts.size()) {
455 "Cannot set initial guess: size init vector [%d] and " 456 "guess vector [%d] doesn't match",
457 init_pts.size(), guess_pts.size()));
480 for (
size_t i = 0; i <
m_points[1].size(); i++) {
493 if (ids.size() != pts.size()) {
495 for (
size_t i = 0; i <
m_points[1].size(); i++)
499 for (
size_t i = 0; i <
m_points[1].size(); i++) {
564 if ((
size_t)index >=
m_points[1].size()) {
572 #elif defined(VISP_HAVE_OPENCV) 576 #include <visp3/klt/vpKltOpencv.h> 578 void vpKltOpencv::clean()
581 cvReleaseImage(&image);
583 cvReleaseImage(&prev_image);
585 cvReleaseImage(&pyramid);
587 cvReleaseImage(&prev_pyramid);
596 countPrevFeatures = 0;
599 globalcountFeatures = 0;
602 void vpKltOpencv::cleanAll()
608 cvFree(&prev_features);
612 cvFree(&lostDuringTrack);
616 cvFree(&prev_featuresid);
618 prev_features = NULL;
622 prev_featuresid = NULL;
625 void vpKltOpencv::reset() { clean(); }
631 : initialized(0), maxFeatures(50), globalcountFeatures(0), win_size(10), quality(0.01), min_distance(10),
632 harris_free_parameter(0.04), block_size(3), use_harris(1), pyramid_level(3), _tid(-1), image(NULL),
633 prev_image(NULL), pyramid(NULL), prev_pyramid(NULL), swap_temp(NULL), countFeatures(0), countPrevFeatures(0),
634 features(NULL), prev_features(NULL), featuresid(NULL), prev_featuresid(NULL), flags(0), initial_guess(
false),
635 lostDuringTrack(0), status(0), OnInitialize(0), OnFeatureLost(0), OnNewFeature(0), OnMeasureFeature(0),
638 features = (CvPoint2D32f *)cvAlloc((
unsigned int)maxFeatures *
sizeof(features[0]));
639 prev_features = (CvPoint2D32f *)cvAlloc((
unsigned int)maxFeatures *
sizeof(prev_features[0]));
640 status = (
char *)cvAlloc((
size_t)maxFeatures);
641 lostDuringTrack = (
bool *)cvAlloc((
size_t)maxFeatures);
642 featuresid = (
long *)cvAlloc((
unsigned int)maxFeatures *
sizeof(long));
643 prev_featuresid = (
long *)cvAlloc((
unsigned int)maxFeatures *
sizeof(long));
650 : initialized(0), maxFeatures(50), globalcountFeatures(0), win_size(10), quality(0.01), min_distance(10),
651 harris_free_parameter(0.04), block_size(3), use_harris(1), pyramid_level(3), _tid(-1), image(NULL),
652 prev_image(NULL), pyramid(NULL), prev_pyramid(NULL), swap_temp(NULL), countFeatures(0), countPrevFeatures(0),
653 features(NULL), prev_features(NULL), featuresid(NULL), prev_featuresid(NULL), flags(0), initial_guess(
false),
654 lostDuringTrack(0), status(0), OnInitialize(0), OnFeatureLost(0), OnNewFeature(0), OnMeasureFeature(0),
666 initialized = copy.initialized;
667 maxFeatures = copy.maxFeatures;
668 countFeatures = copy.countFeatures;
669 countPrevFeatures = copy.countPrevFeatures;
670 globalcountFeatures = copy.globalcountFeatures;
672 win_size = copy.win_size;
673 quality = copy.quality;
674 min_distance = copy.min_distance;
675 harris_free_parameter = copy.harris_free_parameter;
676 block_size = copy.block_size;
677 use_harris = copy.use_harris;
678 pyramid_level = copy.pyramid_level;
681 OnInitialize = copy.OnInitialize;
682 OnFeatureLost = copy.OnFeatureLost;
683 OnNewFeature = copy.OnNewFeature;
684 OnMeasureFeature = copy.OnMeasureFeature;
685 IsFeatureValid = copy.IsFeatureValid;
687 initial_guess = copy.initial_guess;
688 lostDuringTrack = copy.lostDuringTrack;
694 countPrevFeatures = 0;
697 globalcountFeatures = 0;
701 image = cvCreateImage(cvGetSize(copy.image), 8, 1);
703 cvCopy(copy.image, image, 0);
706 if (copy.prev_image) {
707 prev_image = cvCreateImage(cvGetSize(copy.prev_image), IPL_DEPTH_8U, 1);
709 cvCopy(copy.prev_image, prev_image, 0);
713 pyramid = cvCreateImage(cvGetSize(copy.pyramid), IPL_DEPTH_8U, 1);
715 cvCopy(copy.pyramid, pyramid, 0);
718 if (copy.prev_pyramid) {
719 prev_pyramid = cvCreateImage(cvGetSize(copy.prev_pyramid), IPL_DEPTH_8U, 1);
721 cvCopy(copy.prev_pyramid, prev_pyramid, 0);
726 features = (CvPoint2D32f *)cvAlloc((
unsigned int)copy.maxFeatures *
sizeof(CvPoint2D32f));
727 for (
int i = 0; i < copy.maxFeatures; i++)
728 features[i] = copy.features[i];
731 if (copy.prev_features) {
732 prev_features = (CvPoint2D32f *)cvAlloc((
unsigned int)copy.maxFeatures *
sizeof(CvPoint2D32f));
733 for (
int i = 0; i < copy.maxFeatures; i++)
734 prev_features[i] = copy.prev_features[i];
737 if (copy.featuresid) {
738 featuresid = (
long *)cvAlloc((
unsigned int)copy.maxFeatures *
sizeof(long));
739 for (
int i = 0; i < copy.maxFeatures; i++)
740 featuresid[i] = copy.featuresid[i];
743 if (copy.prev_featuresid) {
744 prev_featuresid = (
long *)cvAlloc((
unsigned int)copy.maxFeatures *
sizeof(long));
745 for (
int i = 0; i < copy.maxFeatures; i++)
746 prev_featuresid[i] = copy.prev_featuresid[i];
750 status = (
char *)cvAlloc((
unsigned int)copy.maxFeatures *
sizeof(char));
751 for (
int i = 0; i < copy.maxFeatures; i++)
752 status[i] = copy.status[i];
755 if (copy.lostDuringTrack) {
756 lostDuringTrack = (
bool *)cvAlloc((
unsigned int)copy.maxFeatures *
sizeof(bool));
757 for (
int i = 0; i < copy.maxFeatures; i++)
758 lostDuringTrack[i] = copy.lostDuringTrack[i];
781 cvFree(&prev_features);
785 cvFree(&lostDuringTrack);
789 cvFree(&prev_featuresid);
791 features = (CvPoint2D32f *)cvAlloc((
unsigned int)maxFeatures *
sizeof(CvPoint2D32f));
792 prev_features = (CvPoint2D32f *)cvAlloc((
unsigned int)maxFeatures *
sizeof(CvPoint2D32f));
793 status = (
char *)cvAlloc((
unsigned int)maxFeatures *
sizeof(char));
794 lostDuringTrack = (
bool *)cvAlloc((
unsigned int)maxFeatures *
sizeof(bool));
795 featuresid = (
long *)cvAlloc((
unsigned int)maxFeatures *
sizeof(long));
796 prev_featuresid = (
long *)cvAlloc((
unsigned int)maxFeatures *
sizeof(long));
815 if (I->depth != IPL_DEPTH_8U || I->nChannels != 1) {
820 if (mask->depth != IPL_DEPTH_8U || I->nChannels != 1) {
826 CvSize Sizeim, SizeI;
827 SizeI = cvGetSize(I);
830 Sizeim = cvGetSize(image);
831 if (SizeI.width != Sizeim.width || SizeI.height != Sizeim.height)
834 if (image == NULL || prev_image == NULL || pyramid == NULL || prev_pyramid == NULL || !b_imOK) {
836 image = cvCreateImage(cvGetSize(I), 8, 1);
837 image->origin = I->origin;
838 prev_image = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
839 pyramid = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
840 prev_pyramid = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
844 countPrevFeatures = 0;
847 globalcountFeatures = 0;
856 countFeatures = maxFeatures;
857 countPrevFeatures = 0;
858 IplImage *eig = cvCreateImage(cvGetSize(image), 32, 1);
859 IplImage *temp = cvCreateImage(cvGetSize(image), 32, 1);
860 cvGoodFeaturesToTrack(image, eig, temp, features, &countFeatures, quality, min_distance, mask, block_size, use_harris,
861 harris_free_parameter);
862 cvFindCornerSubPix(image, features, countFeatures, cvSize(win_size, win_size), cvSize(-1, -1),
863 cvTermCriteria(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03));
864 cvReleaseImage(&eig);
865 cvReleaseImage(&temp);
871 for (
int boucle = 0; boucle < countFeatures; boucle++) {
872 featuresid[boucle] = globalcountFeatures;
873 globalcountFeatures++;
876 OnNewFeature(_tid, boucle, featuresid[boucle], features[boucle].x, features[boucle].y);
891 if (size > maxFeatures)
895 CvSize Sizeim, SizeI;
896 SizeI = cvGetSize(I);
899 Sizeim = cvGetSize(image);
900 if (SizeI.width != Sizeim.width || SizeI.height != Sizeim.height)
903 if (image == NULL || prev_image == NULL || pyramid == NULL || prev_pyramid == NULL || !b_imOK) {
905 image = cvCreateImage(cvGetSize(I), 8, 1);
906 image->origin = I->origin;
907 prev_image = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
908 pyramid = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
909 prev_pyramid = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
914 countFeatures = size;
915 for (
int i = 0; i < countFeatures; i++) {
916 features[i] = pts[i];
920 globalcountFeatures = size;
928 if (size > maxFeatures)
932 CvSize Sizeim, SizeI;
933 SizeI = cvGetSize(I);
936 Sizeim = cvGetSize(image);
937 if (SizeI.width != Sizeim.width || SizeI.height != Sizeim.height)
940 if (image == NULL || prev_image == NULL || pyramid == NULL || prev_pyramid == NULL || !b_imOK) {
942 image = cvCreateImage(cvGetSize(I), 8, 1);
943 image->origin = I->origin;
944 prev_image = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
945 pyramid = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
946 prev_pyramid = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
951 countFeatures = size;
953 for (
int i = 0; i < countFeatures; i++) {
954 features[i] = pts[i];
955 featuresid[i] = fid[i];
960 globalcountFeatures = max + 1;
977 if (I->depth != IPL_DEPTH_8U || I->nChannels != 1) {
981 CV_SWAP(prev_image, image, swap_temp);
982 CV_SWAP(prev_pyramid, pyramid, swap_temp);
986 if (!initial_guess) {
988 countPrevFeatures = countFeatures;
989 for (
int boucle = 0; boucle < countFeatures; boucle++) {
990 prev_featuresid[boucle] = featuresid[boucle];
993 CvPoint2D32f *swap_features = 0;
994 CV_SWAP(prev_features, features, swap_features);
997 if (countFeatures <= 0)
1000 cvCalcOpticalFlowPyrLK(prev_image, image, prev_pyramid, pyramid, prev_features, features, countFeatures,
1001 cvSize(win_size, win_size), pyramid_level, status, 0,
1002 cvTermCriteria(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03), flags);
1005 flags |= CV_LKFLOW_PYR_A_READY;
1007 flags = CV_LKFLOW_PYR_A_READY;
1008 initial_guess =
false;
1012 for (i = k = 0; i < countFeatures; i++) {
1014 lostDuringTrack[i] = 1;
1016 OnFeatureLost(_tid, i, featuresid[i], features[i].x, features[i].y);
1020 if (IsFeatureValid) {
1021 if (!IsFeatureValid(_tid, features[i].x, features[i].y)) {
1022 lostDuringTrack[i] = 1;
1024 OnFeatureLost(_tid, i, featuresid[i], features[i].x, features[i].y);
1028 features[k] = features[i];
1029 featuresid[k] = featuresid[i];
1031 if (OnMeasureFeature)
1032 OnMeasureFeature(_tid, k, featuresid[k], features[k].x, features[k].y);
1034 lostDuringTrack[i] = 0;
1049 if ((features == 0) || (I.
bitmap == 0) || (!initialized)) {
1072 if (index >= countFeatures) {
1077 x = features[index].x;
1078 y = features[index].y;
1079 id = featuresid[index];
1094 countPrevFeatures = countFeatures;
1095 for (
int boucle = 0; boucle < countFeatures; boucle++) {
1096 prev_featuresid[boucle] = featuresid[boucle];
1099 CvPoint2D32f *swap_features = NULL;
1100 CV_SWAP(prev_features, *guess_pts, swap_features);
1102 CV_SWAP(features, prev_features, swap_features);
1104 flags |= CV_LKFLOW_INITIAL_GUESSES;
1106 initial_guess =
true;
1126 countPrevFeatures = size;
1127 countFeatures = size;
1128 for (
int boucle = 0; boucle < size; boucle++) {
1129 prev_featuresid[boucle] = fid[boucle];
1130 featuresid[boucle] = fid[boucle];
1133 CvPoint2D32f *swap_features = NULL;
1134 CvPoint2D32f *swap_features2 = NULL;
1135 CV_SWAP(prev_features, *init_pts, swap_features);
1140 CV_SWAP(features, *guess_pts, swap_features2);
1142 flags |= CV_LKFLOW_INITIAL_GUESSES;
1144 initial_guess =
true;
1155 void vpKltOpencv::getPrevFeature(
int index,
int &
id,
float &x,
float &y)
const 1157 if (index >= countPrevFeatures) {
1162 x = prev_features[index].x;
1163 y = prev_features[index].y;
1164 id = prev_featuresid[index];
1175 if (maxFeatures == countFeatures) {
1183 features[countFeatures] = f;
1184 featuresid[countFeatures] = id;
1190 if (index >= countFeatures) {
1197 for (
int i = index; i < countFeatures; i++) {
1198 features[i] = features[i + 1];
1199 featuresid[i] = featuresid[i + 1];
1218 vpColor color,
unsigned int thickness)
1221 for (
int i = 0; i < nbFeatures; i++) {
1242 vpColor color,
unsigned int thickness)
1245 for (
int i = 0; i < nbFeatures; i++) {
1269 const long *featuresid_list,
const int &nbFeatures,
vpColor color,
unsigned int thickness)
1272 for (
int i = 0; i < nbFeatures; i++) {
1278 sprintf(
id,
"%ld", featuresid_list[i]);
1301 const int &nbFeatures,
vpColor color,
unsigned int thickness)
1304 for (
int i = 0; i < nbFeatures; i++) {
1310 sprintf(
id,
"%ld", featuresid_list[i]);
1319 class VISP_EXPORT dummy_vpKltOpencv
1322 dummy_vpKltOpencv(){};
1325 #if !defined(VISP_BUILD_SHARED_LIBS) 1328 void dummy_vpKltOpenCV_fct(){};
void addFeature(const float &x, const float &y)
Used to indicate that a value is not in the allowed range.
void setHarrisFreeParameter(double harris_k)
Type * bitmap
points toward the bitmap
vpKltOpencv & operator=(const vpKltOpencv ©)
Class to define colors available for display functionnalities.
void setMaxFeatures(const int maxCount)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
void setMinEigThreshold(double minEigThreshold)
void setMinDistance(double minDistance)
error that can be emited by ViSP classes.
cv::TermCriteria m_termcrit
static int round(const double x)
void display(const vpImage< unsigned char > &I, const vpColor &color=vpColor::red, unsigned int thickness=1)
void setQuality(double qualityLevel)
std::vector< long > m_points_id
Keypoint id.
Error that can be emited by the vpTracker class and its derivates.
void set_u(const double u)
void set_v(const double v)
void initTracking(const cv::Mat &I, const cv::Mat &mask=cv::Mat())
void setInitialGuess(const std::vector< cv::Point2f > &guess_pts)
void setPyramidLevels(const int pyrMaxLevel)
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
void setWindowSize(const int winSize)
Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented in OpenCV. Thus to enable this ...
void suppressFeature(const int &index)
void setBlockSize(const int blockSize)
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.
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
void setUseHarris(const int useHarrisDetector)
void track(const cv::Mat &I)