2#include <visp3/core/vpConfig.h>
3#include <visp3/core/vpIoTools.h>
4#include <visp3/gui/vpDisplayFactory.h>
5#include <visp3/gui/vpPlot.h>
7#include <visp3/mbt/vpMbGenericTracker.h>
9#include <visp3/io/vpVideoReader.h>
10#include <visp3/io/vpVideoWriter.h>
12#if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_VIDEOIO) && defined(HAVE_OPENCV_HIGHGUI)
13#ifdef ENABLE_VISP_NAMESPACE
23 std::vector<double> vec {
t[0],
t[1],
t[2],
tu[0],
tu[1],
tu[2] };
30int main(
int argc,
char **argv)
32#if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_VIDEOIO) && defined(HAVE_OPENCV_HIGHGUI) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) && defined(VISP_HAVE_DISPLAY)
33 std::string opt_videoname =
"model/teabox/teabox.mp4";
34 std::string opt_modelname =
"model/teabox/teabox.cao";
36 int opt_video_first_frame = -1;
37 int opt_downscale_img = 1;
38 bool opt_verbose =
false;
40 bool opt_display_scale_auto =
false;
41 bool opt_step_by_step =
false;
44#if defined(VISP_HAVE_MINIZ) && defined(VISP_HAVE_WORKING_REGEX)
45 std::string opt_save_results;
50 std::shared_ptr<vpDisplay> display;
51 std::shared_ptr<vpPlot>
plot;
52 std::shared_ptr<vpVideoWriter> writer;
54 unsigned int right_display_offset = 170;
57 for (
int i = 1;
i < argc;
i++) {
58 if (std::string(argv[i]) ==
"--video" && i + 1 < argc) {
59 opt_videoname = std::string(argv[++i]);
61 else if (std::string(argv[i]) ==
"--video-first-frame" && i + 1 < argc) {
62 opt_video_first_frame = std::atoi(argv[++i]);
64 else if (std::string(argv[i]) ==
"--model" && i + 1 < argc) {
65 opt_modelname = std::string(argv[++i]);
67 else if (std::string(argv[i]) ==
"--tracker" && i + 1 < argc) {
68 opt_tracker = atoi(argv[++i]);
70 else if (std::string(argv[i]) ==
"--downscale-img" && i + 1 < argc) {
71 opt_downscale_img = std::atoi(argv[++i]);
73 else if (std::string(argv[i]) ==
"--save" && i + 1 < argc) {
74 opt_save = std::string(argv[++i]);
76#if defined(VISP_HAVE_MINIZ) && defined(VISP_HAVE_WORKING_REGEX)
77 else if (std::string(argv[i]) ==
"--save-results" && i + 1 < argc) {
78 opt_save_results = std::string(argv[++i]);
81 else if (std::string(argv[i]) ==
"--plot") {
84 else if (std::string(argv[i]) ==
"--dof" && i + 6 < argc) {
85 for (
int j = 0;
j < 6;
j++) {
86 int val = std::atoi(argv[++i]);
87 if (val == 0 || val == 1) {
88 opt_dof_to_estimate[
j] = val;
91 std::cout <<
"Error: wrong value after --dof option. Authorized values are 0 or 1 for each 6 dof to estimate." << std::endl;
96 else if (std::string(argv[i]) ==
"--display-scale-auto") {
97 opt_display_scale_auto =
true;
99 else if (std::string(argv[i]) ==
"--step-by-step") {
100 opt_step_by_step =
true;
102 else if (std::string(argv[i]) ==
"--verbose" || std::string(argv[i]) ==
"-v") {
105 else if (std::string(argv[i]) ==
"--help" || std::string(argv[i]) ==
"-h") {
106 std::cout <<
"\nSYNOPSIS " << std::endl
108 <<
" [--video <video name>]"
109 <<
" [--video-first-frame <image index>"
110 <<
" [--model <model name>"
111 <<
" [--tracker <0=egde|1=keypoint|2=hybrid>]"
112 <<
" [--downscale-img <scale factor>]"
113 <<
" [--dof <0/1 0/1 0/1 0/1 0/1 0/1>]"
114 <<
" [--save <e.g. results-%04d.png>]"
115#if defined(VISP_HAVE_MINIZ) && defined(VISP_HAVE_WORKING_REGEX)
116 <<
" [--save-results <e.g. tracking_poses.npz>]"
118 <<
" [--display-scale-auto]"
119 <<
" [--step-by-step]"
124 std::cout <<
"\nOPTIONS " << std::endl
125 <<
" --video <video name>" << std::endl
126 <<
" Input video name." << std::endl
127 <<
" Default: model/teabox/teabox.mp4" << std::endl
129 <<
" --video-first-frame <image index>" << std::endl
130 <<
" Index of the first image to process." << std::endl
131 <<
" Set to -1 to process the first image of the video." << std::endl
132 <<
" Default: -1" << std::endl
134 <<
" --model <model name>" << std::endl
135 <<
" CAD model filename. Supported formats are .cao and .wrl." << std::endl
136 <<
" To use wrl format, ViSP need to be built with Coin3D third-party." << std::endl
137 <<
" Default: model/teabox/teabox.cao" << std::endl
139 <<
" --tracker <0=egde|1=keypoint|2=hybrid>" << std::endl
140 <<
" Tracker type:" << std::endl
141 <<
" - when 0: use only moving-edges" << std::endl
142 <<
" - when 1: use only KLT keypoints" << std::endl
143 <<
" - when 2: use hybrid scheme, moving-edges and KLT keypoints." << std::endl
144 <<
" Default: 0" << std::endl
146 <<
" --downscale-img <scale factor>" << std::endl
147 <<
" Downscale input image width and height by this factor." << std::endl
148 <<
" When set to 1, image not down scaled. When set to 2, image width" << std::endl
149 <<
" and height is divided by 2." << std::endl
150 <<
" Default: 1" << std::endl
152 <<
" --dof <0/1 0/1 0/1 0/1 0/1 0/1>" << std::endl
153 <<
" 6-dim vector of 0 and 1 to indicate which dof [tx ty tz rx ry rz]" << std::endl
154 <<
" has to be estimated." << std::endl
155 <<
" When set to 1 the dof is estimated. When rather set to 0 the dof" << std::endl
156 <<
" is not estimated. It's value is the one from the initialisation." << std::endl
157 <<
" Default: 1 1 1 1 1 1 (to estimate all 6 dof)" << std::endl
159 <<
" --save <e.g. results-%04d.png>" << std::endl
160 <<
" Name of the saved image sequence that contains tracking results in overlay." << std::endl
161 <<
" When the name contains a folder like in the next example, the folder" << std::endl
162 <<
" is created if it doesn't exist."
163 <<
" Example: \"result/image-%04d.png\"." << std::endl
165#if defined(VISP_HAVE_MINIZ) && defined(VISP_HAVE_WORKING_REGEX)
166 <<
" --save-results <e.g. tracking_results.npz>" << std::endl
167 <<
" Name of the npz file containing cMo data estimated from MBT." << std::endl
168 <<
" When the name contains a folder like in the next example, the folder" << std::endl
169 <<
" is created if it doesn't exist."
170 <<
" Example: \"result/tracking_results.npz\"." << std::endl
173 <<
" --display-scale-auto" << std::endl
174 <<
" Enable display window auto scaling to ensure that the image is fully" << std::endl
175 <<
" visible on the screen. Useful for large images." << std::endl
176 <<
" Note that this option doesn't affect the size of the processed images." << std::endl
178 <<
" --step-by-step" << std::endl
179 <<
" Enable step by step mode wainting for a user click to process next image." << std::endl
181 <<
" --plot" << std::endl
182 <<
" Open a window that plots the estimated pose evolution." << std::endl
184 <<
" --verbose, -v" << std::endl
185 <<
" Enable verbose mode." << std::endl
187 <<
" --help, -h" << std::endl
188 <<
" Display this helper message." << std::endl
193 std::cout <<
"Error: unknown option: " << argv[
i] << std::endl;
194 std::cout <<
"To get the helper run: " << argv[0] <<
" --help" << std::endl;
201 if (!parentname.empty())
202 objectname = parentname +
"/" + objectname;
204 std::cout <<
" *********** Tracker config ************ " << std::endl;
205 std::cout <<
"Video name : " << opt_videoname << std::endl;
206 std::cout <<
"Tracker cad model file : " << objectname <<
".[cao or wrl]" << std::endl;
207 std::cout <<
"Tracker init file : " << objectname <<
".init" << std::endl;
208 std::cout <<
"Tracker optional init image: " << objectname <<
".[png,ppm,jpg]" << std::endl;
209 if (opt_downscale_img > 1) {
210 std::cout <<
"Downscale image factor : " << opt_downscale_img << std::endl;
212 std::cout <<
"Dof to estimate : " << opt_dof_to_estimate.t() << std::endl;
215 if (!opt_save.empty()) {
217 if (!parent.empty()) {
218 std::cout <<
"Create output directory: " << parent << std::endl;
222#if defined(VISP_HAVE_MINIZ) && defined(VISP_HAVE_WORKING_REGEX)
223 if (!opt_save_results.empty()) {
225 if (!parent.empty()) {
226 std::cout <<
"Create output directory for the npz file: " << parent << std::endl;
241 if (opt_video_first_frame > 0) {
244 if (opt_downscale_img > 1) {
246 Ivideo.
subsample(opt_downscale_img, opt_downscale_img, I);
253 if (!opt_save.empty()) {
254 writer = std::make_shared<vpVideoWriter>();
255 writer->setFileName(opt_save);
260 if (opt_display_scale_auto) {
263 display->init(I, 100, 100,
"Model-based tracker");
266 plot = std::make_shared<vpPlot>(2, 700, 700, display->getWindowXPosition() + I.getWidth() / display->getDownScalingFactor() + 30,
267 display->getWindowYPosition(),
"Estimated pose");
268 plot->initGraph(0, 3);
269 plot->setTitle(0,
"Translation [m]");
273 plot->initGraph(1, 3);
274 plot->setTitle(1,
"Attitude thetaU [deg]");
282 if (opt_tracker == 0)
284#if defined(VISP_HAVE_MODULE_KLT) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
285 else if (opt_tracker == 1)
291 std::cout <<
"klt and hybrid model-based tracker are not available since visp_klt module is not available. "
292 "In CMakeGUI turn visp_klt module ON, configure and build ViSP again."
301#if defined(VISP_HAVE_PUGIXML)
303 std::cout <<
"Tracker config file : " << objectname +
".xml" << std::endl;
304 tracker.loadConfigFile(objectname +
".xml");
312 if (opt_tracker == 0 || opt_tracker == 2) {
330#if defined(VISP_HAVE_MODULE_KLT) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
331 if (opt_tracker == 1 || opt_tracker == 2) {
341 tracker.setKltOpencv(klt_settings);
352 tracker.setNearClippingDistance(0.1);
353 tracker.setFarClippingDistance(100.0);
361 cam.initPersProjWithoutDistortion(839.21470, 839.44555, 325.66776, 243.69727);
362 tracker.setCameraParameters(cam);
369 tracker.setOgreVisibilityTest(
false);
370 tracker.setOgreShowConfigDialog(
false);
373 tracker.setScanLineVisibilityTest(
true);
379 tracker.loadModel(objectname +
".cao");
383 tracker.loadModel(objectname +
".wrl");
386 tracker.setDisplayFeatures(
true);
389 tracker.setGoodMovingEdgesRatioThreshold(0.2);
391 if (opt_dof_to_estimate != 1.) {
392 tracker.setEstimatedDoF(opt_dof_to_estimate);
397 tracker.getCameraParameters(cam);
398 std::cout <<
"Camera parameters: \n" <<
cam << std::endl;
401 std::cout <<
"Initialize tracker on image size: " << I.getWidth() <<
" x " << I.getHeight() << std::endl;
403 std::vector<double> vec_poses;
404#if defined(VISP_HAVE_MINIZ) && defined(VISP_HAVE_WORKING_REGEX)
405 if (!opt_save_results.empty()) {
406 const unsigned int height = I.getHeight(),
width = I.getWidth();
410 const double cam_px =
cam.get_px(), cam_py =
cam.get_py(), cam_u0 =
cam.get_u0(), cam_v0 =
cam.get_v0();
419 tracker.initClick(I, objectname +
".init",
true);
422#if defined(VISP_HAVE_NLOHMANN_JSON)
423 std::string json_config_file =
"mbt-config.json";
424 std::cout <<
"Save tracker configuration in: " << json_config_file << std::endl;
425 tracker.saveConfigFile(json_config_file);
428 while (!quit && !g.
end()) {
430 if (opt_downscale_img > 1) {
432 Ivideo.
subsample(opt_downscale_img, opt_downscale_img, I);
437 std::stringstream ss;
440 std::cout <<
"-- " << ss.str() << std::endl;
455 vpDisplay::displayText(I, 20 * display->getDownScalingFactor(), (I.getWidth() - right_display_offset) * display->getDownScalingFactor(), std::string(
"Mode: ") + (opt_step_by_step ? std::string(
"step-by-step") : std::string(
"continuous")),
vpColor::red);
457 if (opt_step_by_step) {
461 std::stringstream ss;
464 ss <<
" edge: " <<
tracker.getNbFeaturesEdge();
466#if defined(VISP_HAVE_MODULE_KLT) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
468 ss <<
" klt: " <<
tracker.getNbFeaturesKlt();
473 std::cout << ss.str() << std::endl;
474 std::cout <<
"cMo:\n" <<
cMo << std::endl;
478 double proj_error =
tracker.computeCurrentProjectionError(I, cMo, cam);
479 std::stringstream ss;
480 ss <<
"Projection error: " << std::setprecision(2) << proj_error <<
" deg";
483 std::cout << ss.str() << std::endl;
495#if defined(VISP_HAVE_MINIZ) && defined(VISP_HAVE_WORKING_REGEX)
496 if (!opt_save_results.empty()) {
497 std::vector<double> vec_pose = poseToVec(cMo);
498 vec_poses.insert(vec_poses.end(), vec_pose.begin(), vec_pose.end());
508 opt_step_by_step = !opt_step_by_step;
513 std::stringstream ss;
514 ss <<
"Loop time: " << std::fixed << std::setprecision(0) <<
vpTime::measureTimeMs() - time_start <<
" ms";
515 vpDisplay::displayText(I, (I.getHeight() - 20) * display->getDownScalingFactor(), (I.getWidth() - right_display_offset) * display->getDownScalingFactor(), ss.str(),
vpColor::red);
519 if (!opt_save.empty()) {
521 writer->saveFrame(O);
526#if defined(VISP_HAVE_MINIZ) && defined(VISP_HAVE_WORKING_REGEX)
527 if (!opt_save_results.empty()) {
528 visp::cnpy::npz_save(opt_save_results,
"vec_poses", vec_poses.data(), { static_cast<size_t>(vec_poses.size()/6), 6 },
"a");
533 std::cout <<
"Catch a ViSP exception: " <<
e << std::endl;
537 catch (Ogre::Exception &e) {
538 std::cout <<
"Catch an Ogre exception: " <<
e.getDescription() << std::endl;
545 std::cout <<
"Install OpenCV and rebuild ViSP to use this example." << std::endl;
Generic class defining intrinsic camera parameters.
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.
Definition of the vpImage class member functions.
void subsample(unsigned int v_scale, unsigned int h_scale, vpImage< Type > &sampled) 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.
void setMu1(const double &mu_1)
void setMinThreshold(const double &minThreshold)
void setInitRange(const int &initRange)
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 setThresholdMarginRatio(const double &thresholdMarginRatio)
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 open(vpImage< vpRGBa > &I) VP_OVERRIDE
void setFileName(const std::string &filename)
void setFirstFrameIndex(const long first_frame)
long getFrameIndex() const
void acquire(vpImage< vpRGBa > &I) VP_OVERRIDE
VISP_EXPORT void npz_save(const std::string &zipname, std::string fname, const std::vector< std::string > &data_vec, const std::vector< size_t > &shape, const std::string &mode="w")
std::shared_ptr< vpDisplay > createDisplay()
Return a smart pointer vpDisplay specialization if a GUI library is available or nullptr otherwise.
VISP_EXPORT double measureTimeMs()