1 #include <visp3/core/vpIoTools.h>
3 #include <visp3/gui/vpDisplayGDI.h>
4 #include <visp3/gui/vpDisplayOpenCV.h>
5 #include <visp3/gui/vpDisplayX.h>
6 #include <visp3/gui/vpPlot.h>
8 #include <visp3/mbt/vpMbGenericTracker.h>
10 #include <visp3/io/vpVideoReader.h>
11 #include <visp3/io/vpVideoWriter.h>
13 #if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_VIDEOIO) && defined(HAVE_OPENCV_HIGHGUI)
20 std::vector<double> vec { t[0], t[1], t[2], tu[0], tu[1], tu[2] };
27 int main(
int argc,
char **argv)
29 #if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_VIDEOIO) && defined(HAVE_OPENCV_HIGHGUI)
30 std::string opt_videoname =
"model/teabox/teabox.mp4";
31 std::string opt_modelname =
"model/teabox/teabox.cao";
33 int opt_video_first_frame = -1;
34 int opt_downscale_img = 1;
35 bool opt_verbose =
false;
37 bool opt_display_scale_auto =
false;
40 std::string opt_save_results;
41 unsigned int thickness = 2;
44 std::shared_ptr<vpDisplay>
display;
45 std::shared_ptr<vpPlot> plot;
46 std::shared_ptr<vpVideoWriter> writer;
49 for (
int i = 0; i < argc; i++) {
50 if (std::string(argv[i]) ==
"--video") {
51 opt_videoname = std::string(argv[++i]);
53 else if (std::string(argv[i]) ==
"--video-first-frame") {
54 opt_video_first_frame = std::atoi(argv[++i]);
56 else if (std::string(argv[i]) ==
"--model") {
57 opt_modelname = std::string(argv[++i]);
59 else if (std::string(argv[i]) ==
"--tracker") {
60 opt_tracker = atoi(argv[++i]);
62 else if (std::string(argv[i]) ==
"--downscale-img") {
63 opt_downscale_img = std::atoi(argv[++i]);
65 else if (std::string(argv[i]) ==
"--save") {
66 opt_save = std::string(argv[++i]);
68 else if (std::string(argv[i]) ==
"--save-results") {
69 opt_save_results = std::string(argv[++i]);
71 else if (std::string(argv[i]) ==
"--plot") {
74 else if (std::string(argv[i]) ==
"--dof") {
75 for (
int j = 0; j < 6; j++) {
76 int val = std::atoi(argv[++i]);
77 if (val == 0 || val == 1) {
78 opt_dof_to_estimate[j] = val;
81 std::cout <<
"Error: wrong value after --dof option. Authorized values are 0 or 1 for each 6 dof to estimate." << std::endl;
86 else if (std::string(argv[i]) ==
"--display-scale-auto") {
87 opt_display_scale_auto =
true;
89 else if (std::string(argv[i]) ==
"--verbose" || std::string(argv[i]) ==
"-v") {
92 else if (std::string(argv[i]) ==
"--help" || std::string(argv[i]) ==
"-h") {
93 std::cout <<
"\nSYNOPSIS " << std::endl
95 <<
" [--video <video name>]"
96 <<
" [--video-first-frame <image index>"
97 <<
" [--model <model name>"
98 <<
" [--tracker <0=egde|1=keypoint|2=hybrid>]"
99 <<
" [--downscale-img <scale factor>]"
100 <<
" [--dof <0/1 0/1 0/1 0/1 0/1 0/1>]"
101 <<
" [--save <e.g. results-%04d.png>]"
102 <<
" [--save-results <e.g. tracking_poses.npz>]"
103 <<
" [--display-scale-auto]"
108 std::cout <<
"\nOPTIONS " << std::endl
109 <<
" --video <video name>" << std::endl
110 <<
" Input video name." << std::endl
111 <<
" Default: model/teabox/teabox.mp4" << std::endl
113 <<
" --video-first-frame <image index>" << std::endl
114 <<
" Index of the first image to process." << std::endl
115 <<
" Set to -1 to process the first image of the video." << std::endl
116 <<
" Default: -1" << std::endl
118 <<
" --model <model name>" << std::endl
119 <<
" CAD model filename. Supported formats are .cao and .wrl." << std::endl
120 <<
" To use wrl format, ViSP need to be built with Coin3D third-party." << std::endl
121 <<
" Default: model/teabox/teabox.cao" << std::endl
123 <<
" --tracker <0=egde|1=keypoint|2=hybrid>" << std::endl
124 <<
" Tracker type:" << std::endl
125 <<
" - when 0: use only moving-edges" << std::endl
126 <<
" - when 1: use only KLT keypoints" << std::endl
127 <<
" - when 2: use hybrid scheme, moving-edges and KLT keypoints." << std::endl
128 <<
" Default: 0" << std::endl
130 <<
" --downscale-img <scale factor>" << std::endl
131 <<
" Downscale input image width and height by this factor." << std::endl
132 <<
" When set to 1, image not down scaled. When set to 2, image width" << std::endl
133 <<
" and height is divided by 2." << std::endl
134 <<
" Default: 1" << std::endl
136 <<
" --dof <0/1 0/1 0/1 0/1 0/1 0/1>" << std::endl
137 <<
" 6-dim vector of 0 and 1 to indicate which dof [tx ty tz rx ry rz]" << std::endl
138 <<
" has to be estimated." << std::endl
139 <<
" When set to 1 the dof is estimated. When rather set to 0 the dof" << std::endl
140 <<
" is not estimated. It's value is the one from the initialisation." << std::endl
141 <<
" Default: 1 1 1 1 1 1 (to estimate all 6 dof)" << std::endl
143 <<
" --save <e.g. results-%04d.png>" << std::endl
144 <<
" Name of the saved image sequence that contains tracking results in overlay." << std::endl
145 <<
" When the name contains a folder like in the next example, the folder" << std::endl
146 <<
" is created if it doesn't exist."
147 <<
" Example: \"result/image-%04d.png\"." << std::endl
149 <<
" --save-results <e.g. tracking_results.npz>" << std::endl
150 <<
" Name of the npz file containing cMo data estimated from MBT." << std::endl
151 <<
" When the name contains a folder like in the next example, the folder" << std::endl
152 <<
" is created if it doesn't exist."
153 <<
" Example: \"result/tracking_results.npz\"." << std::endl
155 <<
" --display-scale-auto" << std::endl
156 <<
" Enable display window auto scaling to ensure that the image is fully" << std::endl
157 <<
" visible on the screen. Useful for large images." << std::endl
158 <<
" Note that this option doesn't affect the size of the processed images." << std::endl
160 <<
" --plot" << std::endl
161 <<
" Open a window that plots the estimated pose evolution." << std::endl
163 <<
" --verbose, -v" << std::endl
164 <<
" Enable verbose mode." << std::endl
166 <<
" --help, -h" << std::endl
167 <<
" Display this helper message." << std::endl
175 if (!parentname.empty())
176 objectname = parentname +
"/" + objectname;
178 std::cout <<
" *********** Tracker config ************ " << std::endl;
179 std::cout <<
"Video name : " << opt_videoname << std::endl;
180 std::cout <<
"Tracker cad model file : " << objectname <<
".[cao or wrl]" << std::endl;
181 std::cout <<
"Tracker init file : " << objectname <<
".init" << std::endl;
182 std::cout <<
"Tracker optional init image: " << objectname <<
".[png,ppm,jpg]" << std::endl;
183 if (opt_downscale_img > 1) {
184 std::cout <<
"Downscale image factor : " << opt_downscale_img << std::endl;
186 std::cout <<
"Dof to estimate : " << opt_dof_to_estimate.t() << std::endl;
189 if (!opt_save.empty()) {
191 if (!parent.empty()) {
192 std::cout <<
"Create output directory: " << parent << std::endl;
196 if (!opt_save_results.empty()) {
198 if (!parent.empty()) {
199 std::cout <<
"Create output directory for the npz file: " << parent << std::endl;
213 if (opt_video_first_frame > 0) {
216 if (opt_downscale_img > 1) {
218 Ivideo.
subsample(opt_downscale_img, opt_downscale_img, I);
225 if (!opt_save.empty()) {
226 writer = std::make_shared<vpVideoWriter>();
227 writer->setFileName(opt_save);
231 #if defined(VISP_HAVE_X11)
232 display = std::make_shared<vpDisplayX>();
233 #elif defined(VISP_HAVE_GDI)
234 display = std::make_shared<vpDisplayGDI>();
235 #elif defined(HAVE_OPENCV_HIGHGUI)
236 display = std::make_shared<vpDisplayOpenCV>();
238 if (opt_display_scale_auto) {
241 display->init(I, 100, 100,
"Model-based tracker");
244 plot = std::make_shared<vpPlot>(2, 700, 700,
display->getWindowXPosition() + I.
getWidth() /
display->getDownScalingFactor() + 30,
245 display->getWindowYPosition(),
"Estimated pose");
246 plot->initGraph(0, 3);
247 plot->setTitle(0,
"Translation [m]");
251 plot->initGraph(1, 3);
252 plot->setTitle(1,
"Attitude thetaU [deg]");
260 if (opt_tracker == 0)
262 #if defined(VISP_HAVE_MODULE_KLT) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
263 else if (opt_tracker == 1)
269 std::cout <<
"klt and hybrid model-based tracker are not available since visp_klt module is not available. "
270 "In CMakeGUI turn visp_klt module ON, configure and build ViSP again."
279 #if defined(VISP_HAVE_PUGIXML)
281 std::cout <<
"Tracker config file : " << objectname +
".xml" << std::endl;
290 if (opt_tracker == 0 || opt_tracker == 2) {
305 #if defined(VISP_HAVE_MODULE_KLT) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
306 if (opt_tracker == 1 || opt_tracker == 2) {
366 if (opt_dof_to_estimate != 1.) {
373 std::cout <<
"Camera parameters: \n" << cam << std::endl;
376 std::cout <<
"Initialize tracker on image size: " << I.
getWidth() <<
" x " << I.
getHeight() << std::endl;
378 std::vector<double> vec_poses;
379 if (!opt_save_results.empty()) {
392 tracker.
initClick(I, objectname +
".init",
true);
396 if (opt_downscale_img > 1) {
398 Ivideo.
subsample(opt_downscale_img, opt_downscale_img, I);
403 std::stringstream ss;
406 std::cout <<
"-- " << ss.str() << std::endl;
422 std::stringstream ss;
427 #if defined(VISP_HAVE_MODULE_KLT) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
434 std::cout << ss.str() << std::endl;
435 std::cout <<
"cMo:\n" << cMo << std::endl;
440 std::stringstream ss;
441 ss <<
"Projection error: " << std::setprecision(2) << proj_error <<
" deg";
444 std::cout << ss.str() << std::endl;
457 if (!opt_save.empty()) {
459 writer->saveFrame(O);
462 if (!opt_save_results.empty()) {
463 std::vector<double> vec_pose = poseToVec(cMo);
464 vec_poses.insert(vec_poses.end(), vec_pose.begin(), vec_pose.end());
472 if (!opt_save_results.empty()) {
473 visp::cnpy::npz_save(opt_save_results,
"vec_poses", vec_poses.data(), { static_cast<size_t>(vec_poses.size()/6), 6 },
"a");
477 std::cout <<
"Catch a ViSP exception: " << e << std::endl;
480 #ifdef VISP_HAVE_OGRE
481 catch (Ogre::Exception &e) {
482 std::cout <<
"Catch an Ogre exception: " << e.getDescription() << std::endl;
489 std::cout <<
"Install OpenCV and rebuild ViSP to use this example." << std::endl;
Generic class defining intrinsic camera parameters.
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
Implementation of column vector and the associated operations.
static const vpColor none
static const vpColor blue
static const vpColor green
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void display(const vpImage< unsigned char > &I)
static void displayFrame(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color=vpColor::none, unsigned int thickness=1, const vpImagePoint &offset=vpImagePoint(0, 0), const std::string &frameName="", const vpColor &textColor=vpColor::black, const vpImagePoint &textOffset=vpImagePoint(15, 15))
static void getImage(const vpImage< unsigned char > &Is, vpImage< vpRGBa > &Id)
static void flush(const vpImage< unsigned char > &I)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
error that can be emitted by ViSP classes.
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpThetaUVector getThetaUVector() const
vpRotationMatrix getRotationMatrix() const
vpTranslationVector getTranslationVector() const
void subsample(unsigned int v_scale, unsigned int h_scale, vpImage< Type > &sampled) const
unsigned int getWidth() const
unsigned int getHeight() const
Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented in OpenCV. Thus to enable this ...
void setBlockSize(int blockSize)
void setQuality(double qualityLevel)
void setHarrisFreeParameter(double harris_k)
void setMaxFeatures(int maxCount)
void setMinDistance(double minDistance)
void setWindowSize(int winSize)
void setPyramidLevels(int pyrMaxLevel)
static double rad(double deg)
static double deg(double rad)
Real-time 6D object pose tracking using its CAD model.
virtual int getTrackerType() const
virtual void setKltMaskBorder(const unsigned int &e)
virtual void loadConfigFile(const std::string &configFile, bool verbose=true) vp_override
virtual unsigned int getNbFeaturesEdge() const
virtual void setOgreShowConfigDialog(bool showConfigDialog) vp_override
virtual void setAngleDisappear(const double &a) vp_override
virtual void getPose(vpHomogeneousMatrix &cMo) const vp_override
virtual void setGoodMovingEdgesRatioThreshold(double threshold)
virtual void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false) vp_override
virtual void getCameraParameters(vpCameraParameters &camera) const vp_override
virtual unsigned int getNbFeaturesKlt() const
virtual void setOgreVisibilityTest(const bool &v) vp_override
virtual void setMovingEdge(const vpMe &me)
virtual void setAngleAppear(const double &a) vp_override
virtual void loadModel(const std::string &modelFile, bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix()) vp_override
virtual void setKltOpencv(const vpKltOpencv &t)
virtual void setFarClippingDistance(const double &dist) vp_override
virtual void setNearClippingDistance(const double &dist) vp_override
virtual void setScanLineVisibilityTest(const bool &v) vp_override
virtual void getClipping(unsigned int &clippingFlag1, unsigned int &clippingFlag2) const
virtual void setCameraParameters(const vpCameraParameters &camera) vp_override
virtual void setTrackerType(int type)
virtual void setClipping(const unsigned int &flags) vp_override
virtual void initClick(const vpImage< unsigned char > &I1, const vpImage< unsigned char > &I2, const std::string &initFile1, const std::string &initFile2, bool displayHelp=false, const vpHomogeneousMatrix &T1=vpHomogeneousMatrix(), const vpHomogeneousMatrix &T2=vpHomogeneousMatrix())
virtual double computeCurrentProjectionError(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo, const vpCameraParameters &_cam) vp_override
virtual void setDisplayFeatures(bool displayF) vp_override
virtual void track(const vpImage< unsigned char > &I) vp_override
virtual void setEstimatedDoF(const vpColVector &v)
void setMu1(const double &mu_1)
void setRange(const unsigned int &range)
void setLikelihoodThresholdType(const vpLikelihoodThresholdType likelihood_threshold_type)
void setMaskNumber(const unsigned int &mask_number)
void setThreshold(const double &threshold)
void setSampleStep(const double &sample_step)
void setMaskSize(const unsigned int &mask_size)
void setMu2(const double &mu_2)
Implementation of a rotation vector as axis-angle minimal representation.
Class that consider the case of a translation vector.
Class that enables to manipulate easily a video file or a sequence of images. As it inherits from the...
void acquire(vpImage< vpRGBa > &I)
void open(vpImage< vpRGBa > &I)
void setFileName(const std::string &filename)
void setFirstFrameIndex(const long first_frame)
long getFrameIndex() const
void display(vpImage< unsigned char > &I, const std::string &title)
Display a gray-scale image.
void npz_save(std::string zipname, std::string fname, const T *data, const std::vector< size_t > &shape, std::string mode="w")