Visual Servoing Platform version 3.5.0
vpKltOpencv.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5 *
6 * This software is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 * See the file LICENSE.txt at the root directory of this source
11 * distribution for additional information about the GNU GPL.
12 *
13 * For using ViSP with software that can not be combined with the GNU
14 * GPL, please contact Inria about acquiring a ViSP Professional
15 * Edition License.
16 *
17 * See http://visp.inria.fr for more information.
18 *
19 * This software was developed at:
20 * Inria Rennes - Bretagne Atlantique
21 * Campus Universitaire de Beaulieu
22 * 35042 Rennes Cedex
23 * France
24 *
25 * If you have questions regarding the use of this file, please contact
26 * Inria at visp@inria.fr
27 *
28 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30 *
31 * Description:
32 * Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented
33 * with opencv.
34 *
35 * Authors:
36 * Fabien Servant
37 * Fabien Spindler
38 *
39 *****************************************************************************/
40
48#include <visp3/core/vpConfig.h>
49
50#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020408)
51
52#include <string>
53
54#include <visp3/core/vpDisplay.h>
55#include <visp3/core/vpTrackingException.h>
56#include <visp3/klt/vpKltOpencv.h>
57
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)
65{
66 m_termcrit = cv::TermCriteria(cv::TermCriteria::COUNT | cv::TermCriteria::EPS, 20, 0.03);
67}
68
73 : m_gray(), m_prevGray(), m_points_id(), m_maxCount(500), m_termcrit(), m_winSize(10), m_qualityLevel(0.01),
74 m_minDistance(15), m_minEigThreshold(1e-4), m_harris_k(0.04), m_blockSize(3), m_useHarrisDetector(1),
75 m_pyrMaxLevel(3), m_next_points_id(0), m_initial_guess(false)
76{
77 *this = copy;
78}
79
84{
85 m_gray = copy.m_gray;
87 m_points[0] = copy.m_points[0];
88 m_points[1] = copy.m_points[1];
92 m_winSize = copy.m_winSize;
102
103 return *this;
104}
105
107
118void vpKltOpencv::initTracking(const cv::Mat &I, const cv::Mat &mask)
119{
121
122 // cvtColor(I, m_gray, cv::COLOR_BGR2GRAY);
123 I.copyTo(m_gray);
124
125 for (size_t i = 0; i < 2; i++) {
126 m_points[i].clear();
127 }
128
129 m_points_id.clear();
130
131 cv::goodFeaturesToTrack(m_gray, m_points[1], m_maxCount, m_qualityLevel, m_minDistance, mask, m_blockSize, false,
132 m_harris_k);
133
134 if (m_points[1].size() > 0) {
135 cv::cornerSubPix(m_gray, m_points[1], cv::Size(m_winSize, m_winSize), cv::Size(-1, -1), m_termcrit);
136
137 for (size_t i = 0; i < m_points[1].size(); i++)
138 m_points_id.push_back(m_next_points_id++);
139 }
140}
141
147void vpKltOpencv::track(const cv::Mat &I)
148{
149 if (m_points[1].size() == 0)
150 throw vpTrackingException(vpTrackingException::fatalError, "Not enough key points to track.");
151
152 std::vector<float> err;
153 int flags = 0;
154
155 cv::swap(m_prevGray, m_gray);
156
157 if (m_initial_guess) {
158 flags |= cv::OPTFLOW_USE_INITIAL_FLOW;
159 m_initial_guess = false;
160 } else {
161 std::swap(m_points[1], m_points[0]);
162 }
163
164 // cvtColor(I, m_gray, cv::COLOR_BGR2GRAY);
165 I.copyTo(m_gray);
166
167 if (m_prevGray.empty()) {
168 m_gray.copyTo(m_prevGray);
169 }
170
171 std::vector<uchar> status;
172
173 cv::calcOpticalFlowPyrLK(m_prevGray, m_gray, m_points[0], m_points[1], status, err, cv::Size(m_winSize, m_winSize),
175
176 // Remove points that are lost
177 for (int i = (int)status.size() - 1; i >= 0; i--) {
178 if (status[(size_t)i] == 0) { // point is lost
179 m_points[0].erase(m_points[0].begin() + i);
180 m_points[1].erase(m_points[1].begin() + i);
181 m_points_id.erase(m_points_id.begin() + i);
182 }
183 }
184}
185
199void vpKltOpencv::getFeature(const int &index, long &id, float &x, float &y) const
200{
201 if ((size_t)index >= m_points[1].size()) {
202 throw(vpException(vpException::badValue, "Feature [%d] doesn't exist", index));
203 }
204
205 x = m_points[1][(size_t)index].x;
206 y = m_points[1][(size_t)index].y;
207 id = m_points_id[(size_t)index];
208}
209
217void vpKltOpencv::display(const vpImage<unsigned char> &I, const vpColor &color, unsigned int thickness)
218{
219 vpKltOpencv::display(I, m_points[1], m_points_id, color, thickness);
220}
221
234void vpKltOpencv::display(const vpImage<unsigned char> &I, const std::vector<cv::Point2f> &features,
235 const vpColor &color, unsigned int thickness)
236{
237 vpImagePoint ip;
238 for (size_t i = 0; i < features.size(); i++) {
239 ip.set_u(vpMath::round(features[i].x));
240 ip.set_v(vpMath::round(features[i].y));
241 vpDisplay::displayCross(I, ip, 10 + thickness, color, thickness);
242 }
243}
244
257void vpKltOpencv::display(const vpImage<vpRGBa> &I, const std::vector<cv::Point2f> &features, const vpColor &color,
258 unsigned int thickness)
259{
260 vpImagePoint ip;
261 for (size_t i = 0; i < features.size(); i++) {
262 ip.set_u(vpMath::round(features[i].x));
263 ip.set_v(vpMath::round(features[i].y));
264 vpDisplay::displayCross(I, ip, 10 + thickness, color, thickness);
265 }
266}
267
282void vpKltOpencv::display(const vpImage<unsigned char> &I, const std::vector<cv::Point2f> &features,
283 const std::vector<long> &featuresid, const vpColor &color, unsigned int thickness)
284{
285 vpImagePoint ip;
286 for (size_t i = 0; i < features.size(); i++) {
287 ip.set_u(vpMath::round(features[i].x));
288 ip.set_v(vpMath::round(features[i].y));
289 vpDisplay::displayCross(I, ip, 10, color, thickness);
290
291 std::ostringstream id;
292 id << featuresid[i];
293 ip.set_u(vpMath::round(features[i].x + 5));
294 vpDisplay::displayText(I, ip, id.str(), color);
295 }
296}
297
312void vpKltOpencv::display(const vpImage<vpRGBa> &I, const std::vector<cv::Point2f> &features,
313 const std::vector<long> &featuresid, const vpColor &color, unsigned int thickness)
314{
315 vpImagePoint ip;
316 for (size_t i = 0; i < features.size(); i++) {
317 ip.set_u(vpMath::round(features[i].x));
318 ip.set_v(vpMath::round(features[i].y));
319 vpDisplay::displayCross(I, ip, 10, color, thickness);
320
321 std::ostringstream id;
322 id << featuresid[i];
323 ip.set_u(vpMath::round(features[i].x + 5));
324 vpDisplay::displayText(I, ip, id.str(), color);
325 }
326}
327
334void vpKltOpencv::setMaxFeatures(int maxCount) { m_maxCount = maxCount; }
335
343void vpKltOpencv::setWindowSize(int winSize) { m_winSize = winSize; }
344
356void vpKltOpencv::setQuality(double qualityLevel) { m_qualityLevel = qualityLevel; }
357
364void vpKltOpencv::setHarrisFreeParameter(double harris_k) { m_harris_k = harris_k; }
365
372void vpKltOpencv::setUseHarris(int useHarrisDetector) { m_useHarrisDetector = useHarrisDetector; }
373
381void vpKltOpencv::setMinDistance(double minDistance) { m_minDistance = minDistance; }
382
388void vpKltOpencv::setMinEigThreshold(double minEigThreshold) { m_minEigThreshold = minEigThreshold; }
389
399void vpKltOpencv::setBlockSize(int blockSize) { m_blockSize = blockSize; }
400
409void vpKltOpencv::setPyramidLevels(int pyrMaxLevel) { m_pyrMaxLevel = pyrMaxLevel; }
410
423void vpKltOpencv::setInitialGuess(const std::vector<cv::Point2f> &guess_pts)
424{
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()));
430 }
431
432 m_points[1] = guess_pts;
433 m_initial_guess = true;
434}
435
450void vpKltOpencv::setInitialGuess(const std::vector<cv::Point2f> &init_pts, const std::vector<cv::Point2f> &guess_pts,
451 const std::vector<long> &fid)
452{
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()));
458 }
459
460 m_points[0] = init_pts;
461 m_points[1] = guess_pts;
462 m_points_id = fid;
463 m_initial_guess = true;
464}
465
474void vpKltOpencv::initTracking(const cv::Mat &I, const std::vector<cv::Point2f> &pts)
475{
476 m_initial_guess = false;
477 m_points[1] = pts;
479 m_points_id.clear();
480 for (size_t i = 0; i < m_points[1].size(); i++) {
481 m_points_id.push_back(m_next_points_id++);
482 }
483
484 I.copyTo(m_gray);
485}
486
487void vpKltOpencv::initTracking(const cv::Mat &I, const std::vector<cv::Point2f> &pts, const std::vector<long> &ids)
488{
489 m_initial_guess = false;
490 m_points[1] = pts;
491 m_points_id.clear();
492
493 if (ids.size() != pts.size()) {
495 for (size_t i = 0; i < m_points[1].size(); i++)
496 m_points_id.push_back(m_next_points_id++);
497 } else {
498 long max = 0;
499 for (size_t i = 0; i < m_points[1].size(); i++) {
500 m_points_id.push_back(ids[i]);
501 if (ids[i] > max)
502 max = ids[i];
503 }
504 m_next_points_id = max + 1;
505 }
506
507 I.copyTo(m_gray);
508}
509
517void vpKltOpencv::addFeature(const float &x, const float &y)
518{
519 cv::Point2f f(x, y);
520 m_points[1].push_back(f);
521 m_points_id.push_back(m_next_points_id++);
522}
523
536void vpKltOpencv::addFeature(const long &id, const float &x, const float &y)
537{
538 cv::Point2f f(x, y);
539 m_points[1].push_back(f);
540 m_points_id.push_back(id);
541 if (id >= m_next_points_id)
542 m_next_points_id = id + 1;
543}
544
552void vpKltOpencv::addFeature(const cv::Point2f &f)
553{
554 m_points[1].push_back(f);
555 m_points_id.push_back(m_next_points_id++);
556}
557
562void vpKltOpencv::suppressFeature(const int &index)
563{
564 if ((size_t)index >= m_points[1].size()) {
565 throw(vpException(vpException::badValue, "Feature [%d] doesn't exist", index));
566 }
567
568 m_points[1].erase(m_points[1].begin() + index);
569 m_points_id.erase(m_points_id.begin() + index);
570}
571
572#elif defined(VISP_HAVE_OPENCV)
573
574#include <string>
575
576#include <visp3/klt/vpKltOpencv.h>
577
578void vpKltOpencv::clean()
579{
580 if (image)
581 cvReleaseImage(&image);
582 if (prev_image)
583 cvReleaseImage(&prev_image);
584 if (pyramid)
585 cvReleaseImage(&pyramid);
586 if (prev_pyramid)
587 cvReleaseImage(&prev_pyramid);
588
589 image = NULL;
590 prev_image = NULL;
591 pyramid = NULL;
592 prev_pyramid = NULL;
593
594 swap_temp = NULL;
595 countFeatures = 0;
596 countPrevFeatures = 0;
597 flags = 0;
598 initialized = 0;
599 globalcountFeatures = 0;
600}
601
602void vpKltOpencv::cleanAll()
603{
604 clean();
605 if (features)
606 cvFree(&features);
607 if (prev_features)
608 cvFree(&prev_features);
609 if (status)
610 cvFree(&status);
611 if (lostDuringTrack)
612 cvFree(&lostDuringTrack);
613 if (featuresid)
614 cvFree(&featuresid);
615 if (prev_featuresid)
616 cvFree(&prev_featuresid);
617 features = NULL;
618 prev_features = NULL;
619 status = NULL;
620 lostDuringTrack = 0;
621 featuresid = NULL;
622 prev_featuresid = NULL;
623}
624
625void vpKltOpencv::reset() { clean(); }
626
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),
636 IsFeatureValid(0)
637{
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));
644}
645
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),
655 IsFeatureValid(0)
656{
657 *this = copy;
658}
659
664{
665 // Shallow copy of primitives
666 initialized = copy.initialized;
667 maxFeatures = copy.maxFeatures;
668 countFeatures = copy.countFeatures;
669 countPrevFeatures = copy.countPrevFeatures;
670 globalcountFeatures = copy.globalcountFeatures;
671 flags = copy.flags;
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;
679 _tid = copy._tid;
680
681 OnInitialize = copy.OnInitialize;
682 OnFeatureLost = copy.OnFeatureLost;
683 OnNewFeature = copy.OnNewFeature;
684 OnMeasureFeature = copy.OnMeasureFeature;
685 IsFeatureValid = copy.IsFeatureValid;
686
687 initial_guess = copy.initial_guess;
688 lostDuringTrack = copy.lostDuringTrack;
689
690 if (!initialized) {
691 status = 0;
692 lostDuringTrack = 0;
693 countFeatures = 0;
694 countPrevFeatures = 0;
695 flags = 0;
696 initialized = 0;
697 globalcountFeatures = 0;
698 }
699
700 if (copy.image) {
701 image = cvCreateImage(cvGetSize(copy.image), 8, 1);
702 // /*IplImage **/cvCopyImage(copy.image,image);
703 cvCopy(copy.image, image, 0);
704 }
705
706 if (copy.prev_image) {
707 prev_image = cvCreateImage(cvGetSize(copy.prev_image), IPL_DEPTH_8U, 1);
708 // /*IplImage **/ cvCopyImage(copy.prev_image,prev_image);
709 cvCopy(copy.prev_image, prev_image, 0);
710 }
711
712 if (copy.pyramid) {
713 pyramid = cvCreateImage(cvGetSize(copy.pyramid), IPL_DEPTH_8U, 1);
714 // /*IplImage **/cvCopyImage(copy.pyramid,pyramid);
715 cvCopy(copy.pyramid, pyramid, 0);
716 }
717
718 if (copy.prev_pyramid) {
719 prev_pyramid = cvCreateImage(cvGetSize(copy.prev_pyramid), IPL_DEPTH_8U, 1);
720 // /*IplImage **/cvCopyImage(copy.prev_pyramid,prev_pyramid);
721 cvCopy(copy.prev_pyramid, prev_pyramid, 0);
722 }
723
724 // Deep copy of arrays
725 if (copy.features) {
726 /*CvPoint2D32f **/ features = (CvPoint2D32f *)cvAlloc((unsigned int)copy.maxFeatures * sizeof(CvPoint2D32f));
727 for (int i = 0; i < copy.maxFeatures; i++)
728 features[i] = copy.features[i];
729 }
730
731 if (copy.prev_features) {
732 /*CvPoint2D32f **/ 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];
735 }
736
737 if (copy.featuresid) {
738 /*long **/ featuresid = (long *)cvAlloc((unsigned int)copy.maxFeatures * sizeof(long));
739 for (int i = 0; i < copy.maxFeatures; i++)
740 featuresid[i] = copy.featuresid[i];
741 }
742
743 if (copy.prev_featuresid) {
744 /*long **/ 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];
747 }
748
749 if (copy.status) {
750 /*char **/ status = (char *)cvAlloc((unsigned int)copy.maxFeatures * sizeof(char));
751 for (int i = 0; i < copy.maxFeatures; i++)
752 status[i] = copy.status[i];
753 }
754
755 if (copy.lostDuringTrack) {
756 /*bool **/ lostDuringTrack = (bool *)cvAlloc((unsigned int)copy.maxFeatures * sizeof(bool));
757 for (int i = 0; i < copy.maxFeatures; i++)
758 lostDuringTrack[i] = copy.lostDuringTrack[i];
759 }
760
761 return *this;
762}
763
764vpKltOpencv::~vpKltOpencv() { cleanAll(); }
765
773void vpKltOpencv::setMaxFeatures(int input)
774{
775 initialized = 0;
776 maxFeatures = input;
777
778 if (features)
779 cvFree(&features);
780 if (prev_features)
781 cvFree(&prev_features);
782 if (status)
783 cvFree(&status);
784 if (lostDuringTrack)
785 cvFree(&lostDuringTrack);
786 if (featuresid)
787 cvFree(&featuresid);
788 if (prev_featuresid)
789 cvFree(&prev_featuresid);
790
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));
797}
798
809void vpKltOpencv::initTracking(const IplImage *I, const IplImage *mask)
810{
811 if (!I) {
812 throw(vpException(vpTrackingException::initializationError, "Image Not initialized"));
813 }
814
815 if (I->depth != IPL_DEPTH_8U || I->nChannels != 1) {
816 throw(vpException(vpTrackingException::initializationError, "Bad Image format"));
817 }
818
819 if (mask) {
820 if (mask->depth != IPL_DEPTH_8U || I->nChannels != 1) {
821 throw(vpException(vpTrackingException::initializationError, "Bad Image format"));
822 }
823 }
824
825 // Creation des buffers
826 CvSize Sizeim, SizeI;
827 SizeI = cvGetSize(I);
828 bool b_imOK = true;
829 if (image != NULL) {
830 Sizeim = cvGetSize(image);
831 if (SizeI.width != Sizeim.width || SizeI.height != Sizeim.height)
832 b_imOK = false;
833 }
834 if (image == NULL || prev_image == NULL || pyramid == NULL || prev_pyramid == NULL || !b_imOK) {
835 reset();
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);
841 } else {
842 swap_temp = 0;
843 countFeatures = 0;
844 countPrevFeatures = 0;
845 flags = 0;
846 initialized = 0;
847 globalcountFeatures = 0;
848 }
849
850 initialized = 1;
851
852 // Import
853 cvCopy(I, image, 0);
854
855 // Recherche de points d'interets
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);
866
867 if (OnInitialize)
868 OnInitialize(_tid);
869
870 // printf("Number of features at init: %d\n", countFeatures);
871 for (int boucle = 0; boucle < countFeatures; boucle++) {
872 featuresid[boucle] = globalcountFeatures;
873 globalcountFeatures++;
874
875 if (OnNewFeature) {
876 OnNewFeature(_tid, boucle, featuresid[boucle], features[boucle].x, features[boucle].y);
877 }
878 }
879}
880
889void vpKltOpencv::initTracking(const IplImage *I, CvPoint2D32f *pts, int size)
890{
891 if (size > maxFeatures)
892 throw(vpException(vpTrackingException::initializationError, "Cannot initialize tracker from points"));
893
894 // Creation des buffers
895 CvSize Sizeim, SizeI;
896 SizeI = cvGetSize(I);
897 bool b_imOK = true;
898 if (image != NULL) {
899 Sizeim = cvGetSize(image);
900 if (SizeI.width != Sizeim.width || SizeI.height != Sizeim.height)
901 b_imOK = false;
902 }
903 if (image == NULL || prev_image == NULL || pyramid == NULL || prev_pyramid == NULL || !b_imOK) {
904 reset();
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);
910 } else {
911 flags = 0;
912 }
913 // Save current features as previous features
914 countFeatures = size;
915 for (int i = 0; i < countFeatures; i++) {
916 features[i] = pts[i];
917 featuresid[i] = i;
918 }
919
920 globalcountFeatures = size;
921 initialized = 1;
922
923 cvCopy(I, image, 0);
924}
925
926void vpKltOpencv::initTracking(const IplImage *I, CvPoint2D32f *pts, long *fid, int size)
927{
928 if (size > maxFeatures)
929 throw(vpException(vpTrackingException::initializationError, "Cannot initialize tracker from points"));
930
931 // Creation des buffers
932 CvSize Sizeim, SizeI;
933 SizeI = cvGetSize(I);
934 bool b_imOK = true;
935 if (image != NULL) {
936 Sizeim = cvGetSize(image);
937 if (SizeI.width != Sizeim.width || SizeI.height != Sizeim.height)
938 b_imOK = false;
939 }
940 if (image == NULL || prev_image == NULL || pyramid == NULL || prev_pyramid == NULL || !b_imOK) {
941 reset();
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);
947 } else {
948 flags = 0;
949 }
950 // Save current features as previous features
951 countFeatures = size;
952 long max = 0;
953 for (int i = 0; i < countFeatures; i++) {
954 features[i] = pts[i];
955 featuresid[i] = fid[i];
956 if (fid[i] > max)
957 max = fid[i];
958 }
959
960 globalcountFeatures = max + 1;
961 initialized = 1;
962
963 cvCopy(I, image, 0);
964}
965
966void vpKltOpencv::track(const IplImage *I)
967{
968 if (!initialized) {
969 vpERROR_TRACE("KLT Not initialized");
970 throw(vpException(vpTrackingException::initializationError, "KLT Not initialized"));
971 }
972
973 if (!I) {
974 throw(vpException(vpTrackingException::initializationError, "Image Not initialized"));
975 }
976
977 if (I->depth != IPL_DEPTH_8U || I->nChannels != 1) {
978 throw(vpException(vpTrackingException::initializationError, "Bad Image format"));
979 }
980
981 CV_SWAP(prev_image, image, swap_temp);
982 CV_SWAP(prev_pyramid, pyramid, swap_temp);
983
984 cvCopy(I, image, 0);
985
986 if (!initial_guess) {
987 // Save current features as previous features
988 countPrevFeatures = countFeatures;
989 for (int boucle = 0; boucle < countFeatures; boucle++) {
990 prev_featuresid[boucle] = featuresid[boucle];
991 }
992
993 CvPoint2D32f *swap_features = 0;
994 CV_SWAP(prev_features, features, swap_features);
995 }
996
997 if (countFeatures <= 0)
998 return;
999
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);
1003
1004 if (!initial_guess)
1005 flags |= CV_LKFLOW_PYR_A_READY;
1006 else {
1007 flags = CV_LKFLOW_PYR_A_READY;
1008 initial_guess = false;
1009 }
1010
1011 int i, k;
1012 for (i = k = 0; i < countFeatures; i++) {
1013 if (!status[i]) {
1014 lostDuringTrack[i] = 1;
1015 if (OnFeatureLost)
1016 OnFeatureLost(_tid, i, featuresid[i], features[i].x, features[i].y);
1017 continue;
1018 }
1019
1020 if (IsFeatureValid) {
1021 if (!IsFeatureValid(_tid, features[i].x, features[i].y)) {
1022 lostDuringTrack[i] = 1;
1023 if (OnFeatureLost)
1024 OnFeatureLost(_tid, i, featuresid[i], features[i].x, features[i].y);
1025 continue;
1026 }
1027 }
1028 features[k] = features[i];
1029 featuresid[k] = featuresid[i];
1030
1031 if (OnMeasureFeature)
1032 OnMeasureFeature(_tid, k, featuresid[k], features[k].x, features[k].y);
1033
1034 lostDuringTrack[i] = 0;
1035 k++;
1036 }
1037 countFeatures = k;
1038}
1039
1047void vpKltOpencv::display(const vpImage<unsigned char> &I, vpColor color, unsigned int thickness)
1048{
1049 if ((features == 0) || (I.bitmap == 0) || (!initialized)) {
1050 vpERROR_TRACE(" Memory problem ");
1051 throw(vpException(vpException::memoryAllocationError, " Memory problem"));
1052 }
1053
1054 vpKltOpencv::display(I, features, featuresid, countFeatures, color, thickness);
1055}
1056
1070void vpKltOpencv::getFeature(int index, long &id, float &x, float &y) const
1071{
1072 if (index >= countFeatures) {
1073 vpERROR_TRACE(" Memory problem ");
1074 throw(vpException(vpException::memoryAllocationError, " Memory problem"));
1075 }
1076
1077 x = features[index].x;
1078 y = features[index].y;
1079 id = featuresid[index];
1080}
1081
1091void vpKltOpencv::setInitialGuess(CvPoint2D32f **guess_pts)
1092{
1093 // Save current features as previous features
1094 countPrevFeatures = countFeatures;
1095 for (int boucle = 0; boucle < countFeatures; boucle++) {
1096 prev_featuresid[boucle] = featuresid[boucle];
1097 }
1098
1099 CvPoint2D32f *swap_features = NULL;
1100 CV_SWAP(prev_features, *guess_pts, swap_features);
1101
1102 CV_SWAP(features, prev_features, swap_features);
1103
1104 flags |= CV_LKFLOW_INITIAL_GUESSES;
1105
1106 initial_guess = true;
1107}
1108
1124void vpKltOpencv::setInitialGuess(CvPoint2D32f **init_pts, CvPoint2D32f **guess_pts, long *fid, int size)
1125{
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];
1131 }
1132
1133 CvPoint2D32f *swap_features = NULL;
1134 CvPoint2D32f *swap_features2 = NULL;
1135 CV_SWAP(prev_features, *init_pts, swap_features);
1136
1137 // if(swap_features) cvFree(&swap_features);
1138 // swap_features = NULL;
1139
1140 CV_SWAP(features, *guess_pts, swap_features2);
1141
1142 flags |= CV_LKFLOW_INITIAL_GUESSES;
1143
1144 initial_guess = true;
1145}
1146
1155void vpKltOpencv::getPrevFeature(int index, int &id, float &x, float &y) const
1156{
1157 if (index >= countPrevFeatures) {
1158 vpERROR_TRACE(" Memory problem ");
1159 throw(vpException(vpException::memoryAllocationError, " Memory problem"));
1160 }
1161
1162 x = prev_features[index].x;
1163 y = prev_features[index].y;
1164 id = prev_featuresid[index];
1165}
1166
1173void vpKltOpencv::addFeature(const int &id, const float &x, const float &y)
1174{
1175 if (maxFeatures == countFeatures) {
1176 vpERROR_TRACE(" Cannot add the feature ");
1177 return;
1178 }
1179
1180 CvPoint2D32f f;
1181 f.x = x;
1182 f.y = y;
1183 features[countFeatures] = f;
1184 featuresid[countFeatures] = id;
1185 countFeatures++;
1186}
1187
1188void vpKltOpencv::suppressFeature(int index)
1189{
1190 if (index >= countFeatures) {
1191 vpERROR_TRACE(" Memory problem ");
1192 throw(vpException(vpException::memoryAllocationError, " Memory problem"));
1193 }
1194
1195 countFeatures--;
1196
1197 for (int i = index; i < countFeatures; i++) {
1198 features[i] = features[i + 1];
1199 featuresid[i] = featuresid[i + 1];
1200 }
1201}
1202
1217void vpKltOpencv::display(const vpImage<unsigned char> &I, const CvPoint2D32f *features_list, const int &nbFeatures,
1218 vpColor color, unsigned int thickness)
1219{
1220 vpImagePoint ip;
1221 for (int i = 0; i < nbFeatures; i++) {
1222 ip.set_u(vpMath::round(features_list[i].x));
1223 ip.set_v(vpMath::round(features_list[i].y));
1224 vpDisplay::displayCross(I, ip, 10 + thickness, color, thickness);
1225 }
1226}
1241void vpKltOpencv::display(const vpImage<vpRGBa> &I, const CvPoint2D32f *features_list, const int &nbFeatures,
1242 vpColor color, unsigned int thickness)
1243{
1244 vpImagePoint ip;
1245 for (int i = 0; i < nbFeatures; i++) {
1246 ip.set_u(vpMath::round(features_list[i].x));
1247 ip.set_v(vpMath::round(features_list[i].y));
1248 vpDisplay::displayCross(I, ip, 10 + thickness, color, thickness);
1249 }
1250}
1251
1268void vpKltOpencv::display(const vpImage<unsigned char> &I, const CvPoint2D32f *features_list,
1269 const long *featuresid_list, const int &nbFeatures, vpColor color, unsigned int thickness)
1270{
1271 vpImagePoint ip;
1272 for (int i = 0; i < nbFeatures; i++) {
1273 ip.set_u(vpMath::round(features_list[i].x));
1274 ip.set_v(vpMath::round(features_list[i].y));
1275 vpDisplay::displayCross(I, ip, 10 + thickness, color, thickness);
1276
1277 char id[10];
1278 sprintf(id, "%ld", featuresid_list[i]);
1279 ip.set_u(vpMath::round(features_list[i].x + 5));
1280 vpDisplay::displayText(I, ip, id, color);
1281 }
1282}
1283
1300void vpKltOpencv::display(const vpImage<vpRGBa> &I, const CvPoint2D32f *features_list, const long *featuresid_list,
1301 const int &nbFeatures, vpColor color, unsigned int thickness)
1302{
1303 vpImagePoint ip;
1304 for (int i = 0; i < nbFeatures; i++) {
1305 ip.set_u(vpMath::round(features_list[i].x));
1306 ip.set_v(vpMath::round(features_list[i].y));
1307 vpDisplay::displayCross(I, ip, 10, color, thickness);
1308
1309 char id[10];
1310 sprintf(id, "%ld", featuresid_list[i]);
1311 ip.set_u(vpMath::round(features_list[i].x + 5));
1312 vpDisplay::displayText(I, ip, id, color);
1313 }
1314}
1315#else
1316
1317// Work arround to avoid visp_klt library empty when OpenCV is not installed
1318// or used
1319class VISP_EXPORT dummy_vpKltOpencv
1320{
1321public:
1322 dummy_vpKltOpencv(){};
1323};
1324
1325#if !defined(VISP_BUILD_SHARED_LIBS)
1326// Work arround to avoid warning: libvisp_klt.a(vpKltOpenCV.cpp.o) has no
1327// symbols
1328void dummy_vpKltOpenCV_fct(){};
1329#endif
1330
1331#endif
Class to define RGB colors available for display functionnalities.
Definition: vpColor.h:158
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.
Definition: vpException.h:72
@ badValue
Used to indicate that a value is not in the allowed range.
Definition: vpException.h:97
@ memoryAllocationError
Memory allocation error.
Definition: vpException.h:88
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
void set_u(double u)
Definition: vpImagePoint.h:225
void set_v(double v)
Definition: vpImagePoint.h:236
Type * bitmap
points toward the bitmap
Definition: vpImage.h:143
Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented in OpenCV. Thus to enable this ...
Definition: vpKltOpencv.h:79
virtual ~vpKltOpencv()
std::vector< long > m_points_id
Keypoint id.
Definition: vpKltOpencv.h:163
int m_useHarrisDetector
Definition: vpKltOpencv.h:172
void setBlockSize(int blockSize)
void setQuality(double qualityLevel)
void track(const cv::Mat &I)
double m_minDistance
Definition: vpKltOpencv.h:168
vpKltOpencv & operator=(const vpKltOpencv &copy)
Definition: vpKltOpencv.cpp:83
cv::TermCriteria m_termcrit
Definition: vpKltOpencv.h:165
double m_minEigThreshold
Definition: vpKltOpencv.h:169
void setHarrisFreeParameter(double harris_k)
void getFeature(const int &index, long &id, float &x, float &y) const
int m_pyrMaxLevel
Definition: vpKltOpencv.h:173
std::vector< cv::Point2f > m_points[2]
Previous [0] and current [1] keypoint location.
Definition: vpKltOpencv.h:162
double m_qualityLevel
Definition: vpKltOpencv.h:167
void suppressFeature(const int &index)
bool m_initial_guess
Definition: vpKltOpencv.h:175
cv::Mat m_gray
Definition: vpKltOpencv.h:161
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)
double m_harris_k
Definition: vpKltOpencv.h:170
void initTracking(const cv::Mat &I, const cv::Mat &mask=cv::Mat())
long m_next_points_id
Definition: vpKltOpencv.h:174
cv::Mat m_prevGray
Definition: vpKltOpencv.h:161
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)
Definition: vpMath.h:247
Error that can be emited by the vpTracker class and its derivates.
#define vpERROR_TRACE
Definition: vpDebug.h:393