4 #include <visp3/core/vpDisplay.h>
5 #include <visp3/core/vpIoTools.h>
6 #include <visp3/core/vpXmlParserCamera.h>
7 #include <visp3/gui/vpDisplayGDI.h>
8 #include <visp3/gui/vpDisplayOpenCV.h>
9 #include <visp3/gui/vpDisplayX.h>
10 #include <visp3/io/vpImageIo.h>
11 #include <visp3/mbt/vpMbGenericTracker.h>
13 #if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGCODECS) && defined(VISP_HAVE_PUGIXML)
16 bool read_data(
unsigned int cpt,
const std::string &video_color_images,
const std::string &video_depth_images,
17 bool disable_depth,
const std::string &video_ground_truth,
19 unsigned int &depth_width,
unsigned int &depth_height,
23 char buffer[FILENAME_MAX];
25 snprintf(buffer, FILENAME_MAX, video_color_images.c_str(), cpt);
26 std::string filename_color = buffer;
29 std::cerr <<
"Cannot read: " << filename_color << std::endl;
36 snprintf(buffer, FILENAME_MAX, video_depth_images.c_str(), cpt);
37 std::string filename_depth = buffer;
40 std::cerr <<
"Cannot read: " << filename_depth << std::endl;
43 cv::Mat depth_raw = cv::imread(filename_depth, cv::IMREAD_ANYDEPTH | cv::IMREAD_ANYCOLOR);
44 if (depth_raw.empty()) {
45 std::cerr <<
"Cannot read: " << filename_depth << std::endl;
49 depth_width =
static_cast<unsigned int>(depth_raw.cols);
50 depth_height =
static_cast<unsigned int>(depth_raw.rows);
51 I_depth_raw.
resize(depth_height, depth_width);
52 pointcloud.resize(depth_width * depth_height);
54 for (
int i = 0; i < depth_raw.rows; i++) {
55 for (
int j = 0; j < depth_raw.cols; j++) {
56 I_depth_raw[i][j] =
static_cast<uint16_t
>(32767.5f * depth_raw.at<cv::Vec3f>(i, j)[0]);
57 double x = 0.0, y = 0.0;
59 double Z = depth_raw.at<cv::Vec3f>(i, j)[0] > 2.0f ? 0.0 :
static_cast<double>(depth_raw.at<cv::Vec3f>(i, j)[0]);
61 size_t idx =
static_cast<size_t>(i * depth_raw.cols + j);
62 pointcloud[idx].resize(3);
63 pointcloud[idx][0] = x * Z;
64 pointcloud[idx][1] = y * Z;
65 pointcloud[idx][2] = Z;
71 snprintf(buffer, FILENAME_MAX, video_ground_truth.c_str(), cpt);
72 std::string filename_pose = buffer;
74 cMo_ground_truth.
load(filename_pose);
80 void usage(
const char **argv,
int error,
const std::string &data_path,
const std::string &model_path,
int first_frame)
82 std::cout <<
"Synopsis" << std::endl
84 <<
" [--data-path <path>] [--model-path <path>] [--first-frame <index>] [--disable-depth] "
85 <<
" [--disable-klt] [--step-by-step] [--display-ground-truth] [--help, -h]" << std::endl
87 std::cout <<
"Description" << std::endl
88 <<
" --data-path <path> Path to the data generated by Blender get_camera_pose_teabox.py" << std::endl
90 <<
" Default: " << data_path << std::endl
92 <<
" --model-path <path> Path to the cad model and tracker settings." << std::endl
93 <<
" Default: " << model_path << std::endl
95 <<
" --first-frame <index> First frame number to process." << std::endl
96 <<
" Default: " << first_frame << std::endl
98 <<
" --disable-depth Flag to turn off tracker depth features." << std::endl
100 <<
" --disable-klt Flag to turn off tracker keypoints features." << std::endl
102 <<
" --step-by-step Flag to enable step by step mode." << std::endl
104 <<
" --display-ground-truth Flag to enable displaying ground truth." << std::endl
105 <<
" When this flag is enabled, there is no tracking. This flag is useful" << std::endl
106 <<
" to validate the ground truth over the rendered images." << std::endl
108 <<
" --help, -h Print this helper message." << std::endl
111 std::cout <<
"Error" << std::endl
113 <<
"Unsupported parameter " << argv[error] << std::endl;
117 int main(
int argc,
const char **argv)
119 std::string opt_data_path =
"data/teabox";
120 std::string opt_model_path =
"model/teabox";
121 unsigned int opt_first_frame = 1;
122 bool opt_disable_depth =
false;
123 bool opt_disable_klt =
false;
124 bool opt_display_ground_truth =
false;
125 bool opt_step_by_step =
false;
127 for (
int i = 1; i < argc; i++) {
128 if (std::string(argv[i]) ==
"--data-path" && i + 1 < argc) {
129 opt_data_path = std::string(argv[i + 1]);
132 else if (std::string(argv[i]) ==
"--model-path" && i + 1 < argc) {
133 opt_model_path = std::string(argv[i + 1]);
136 else if (std::string(argv[i]) ==
"--disable-depth") {
137 opt_disable_depth =
true;
139 else if (std::string(argv[i]) ==
"--disable-klt") {
140 opt_disable_klt =
true;
142 else if (std::string(argv[i]) ==
"--display-ground-truth") {
143 opt_display_ground_truth =
true;
145 else if (std::string(argv[i]) ==
"--step-by-step") {
146 opt_step_by_step =
true;
148 else if (std::string(argv[i]) ==
"--first-frame" && i + 1 < argc) {
149 opt_first_frame =
static_cast<unsigned int>(atoi(argv[i + 1]));
152 else if (std::string(argv[i]) ==
"--help" || std::string(argv[i]) ==
"-h") {
153 usage(argv, 0, opt_data_path, opt_model_path, opt_first_frame);
157 usage(argv, i, opt_data_path, opt_model_path, opt_first_frame);
166 std::string color_camera_name =
"Camera_L";
167 std::string depth_camera_name =
"Camera_R";
175 std::cout <<
"Input data" << std::endl;
176 std::cout <<
" Color images : " << video_color_images << std::endl;
177 std::cout <<
" Depth images : " << (opt_disable_depth ?
"Disabled" : video_depth_images) << std::endl;
178 std::cout <<
" Extrinsics : " << (opt_disable_depth ?
"Disabled" : extrinsic_file) << std::endl;
179 std::cout <<
" Color intrinsics: " << color_intrinsic_file << std::endl;
180 std::cout <<
" Depth intrinsics: " << (opt_disable_depth ?
"Disabled" : depth_intrinsic_file) << std::endl;
181 std::cout <<
" Ground truth : " << ground_truth << std::endl;
182 std::cout <<
"Tracker settings" << std::endl;
183 std::cout <<
" Color config : " << mbt_config_color << std::endl;
184 std::cout <<
" Depth config : " << mbt_config_depth << std::endl;
185 std::cout <<
" CAD model : " << mbt_cad_model << std::endl;
186 std::cout <<
" First frame : " << opt_first_frame << std::endl;
187 std::cout <<
" Step by step : " << opt_step_by_step << std::endl;
188 if (opt_display_ground_truth) {
189 std::cout <<
" Ground truth is used to project the cad model (no tracking)" << std::endl;
192 std::cout <<
" Init file : " << mbt_init_file << std::endl;
193 std::cout <<
" Features : moving-edges " << (opt_disable_klt ?
"" :
"+ keypoints") << (opt_disable_depth ?
"" :
" + depth") << std::endl;
196 std::vector<int> tracker_types;
197 if (opt_disable_klt) {
201 #if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
204 std::cout <<
"Warning: keypoints cannot be used as features since ViSP is not build with OpenCV 3rd party" << std::endl;
207 if (!opt_disable_depth)
211 if (!opt_disable_depth)
212 tracker.loadConfigFile(mbt_config_color, mbt_config_depth);
214 tracker.loadConfigFile(mbt_config_color);
215 tracker.loadModel(mbt_cad_model);
222 std::cout <<
"Cannot found intrinsics for camera " << color_camera_name << std::endl;
226 std::cout <<
"Cannot found intrinsics for camera " << depth_camera_name << std::endl;
229 if (!opt_disable_depth)
230 tracker.setCameraParameters(cam_color, cam_depth);
232 tracker.setCameraParameters(cam_color);
235 if (!opt_disable_depth)
238 tracker.getCameraParameters(cam_color);
239 tracker.setDisplayFeatures(
true);
240 std::cout <<
"cam_color:\n" << cam_color << std::endl;
242 if (!opt_disable_depth)
243 std::cout <<
"cam_depth:\n" << cam_depth << std::endl;
247 unsigned int depth_width = 0, depth_height = 0;
248 std::vector<vpColVector> pointcloud;
251 unsigned int frame_cpt = opt_first_frame;
252 read_data(frame_cpt, video_color_images, video_depth_images, opt_disable_depth, ground_truth,
253 I, I_depth_raw, depth_width, depth_height, pointcloud, cam_depth, cMo_ground_truth);
256 #if defined(VISP_HAVE_X11)
258 #elif defined(VISP_HAVE_GDI)
260 #elif defined (HAVE_OPENCV_HIGHGUI)
264 d1.
init(I, 0, 0,
"Color image");
265 if (!opt_disable_depth) {
266 d2.
init(I_depth,
static_cast<int>(I.
getWidth()), 0,
"Depth image");
270 if (!opt_disable_depth) {
271 depth_M_color.
load(extrinsic_file);
272 tracker.setCameraTransformationMatrix(
"Camera2", depth_M_color);
273 std::cout <<
"depth_M_color:\n" << depth_M_color << std::endl;
276 if (opt_display_ground_truth) {
277 tracker.initFromPose(I, cMo_ground_truth);
280 tracker.initClick(I, mbt_init_file,
true);
285 while (!quit && read_data(frame_cpt, video_color_images, video_depth_images, opt_disable_depth,
286 ground_truth, I, I_depth_raw, depth_width, depth_height, pointcloud, cam_depth,
292 if (opt_display_ground_truth) {
293 tracker.initFromPose(I, cMo_ground_truth);
296 if (!opt_disable_depth) {
297 std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
298 std::map<std::string, const std::vector<vpColVector> *> mapOfPointClouds;
299 std::map<std::string, unsigned int> mapOfPointCloudWidths;
300 std::map<std::string, unsigned int> mapOfPointCloudHeights;
302 mapOfImages[
"Camera1"] = &I;
303 mapOfPointClouds[
"Camera2"] = &pointcloud;
304 mapOfPointCloudWidths[
"Camera2"] = depth_width;
305 mapOfPointCloudHeights[
"Camera2"] = depth_height;
306 tracker.track(mapOfImages, mapOfPointClouds, mapOfPointCloudWidths, mapOfPointCloudHeights);
314 std::cout <<
"\nFrame: " << frame_cpt << std::endl;
315 if (!opt_display_ground_truth)
316 std::cout <<
"cMo:\n" << cMo << std::endl;
317 std::cout <<
"cMo ground truth:\n" << cMo_ground_truth << std::endl;
318 if (!opt_disable_depth) {
319 tracker.display(I, I_depth, cMo, depth_M_color * cMo, cam_color, cam_depth,
vpColor::red, 2);
327 std::ostringstream oss;
328 oss <<
"Frame: " << frame_cpt;
330 if (opt_step_by_step) {
338 if (!opt_display_ground_truth) {
340 std::stringstream ss;
341 ss <<
"Nb features: " << tracker.getError().size();
345 std::stringstream ss;
346 ss <<
"Features: edges " << tracker.getNbFeaturesEdge() <<
", klt " << tracker.getNbFeaturesKlt()
347 <<
", depth " << tracker.getNbFeaturesDepthDense();
362 opt_step_by_step =
true;
365 opt_step_by_step =
false;
371 opt_step_by_step =
true;
381 catch (std::exception &e) {
382 std::cerr <<
"Catch exception: " << e.what() << std::endl;
390 std::cout <<
"To run this tutorial, ViSP should be built with OpenCV and pugixml libraries." << std::endl;
Generic class defining intrinsic camera parameters.
@ perspectiveProjWithoutDistortion
Perspective projection without distortion model.
static const vpColor none
Display for windows using GDI (available on any windows 32 platform).
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
void init(vpImage< unsigned char > &I, int win_x=-1, int win_y=-1, const std::string &win_title="") vp_override
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 setTitle(const vpImage< unsigned char > &I, const std::string &windowtitle)
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)
Implementation of an homogeneous matrix and operations on such kind of matrices.
void load(std::ifstream &f)
static void createDepthHistogram(const vpImage< uint16_t > &src_depth, vpImage< vpRGBa > &dest_rgba)
static void read(vpImage< unsigned char > &I, const std::string &filename, int backend=IO_DEFAULT_BACKEND)
unsigned int getWidth() const
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
unsigned int getHeight() const
Real-time 6D object pose tracking using its CAD model.
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
XML parser to load and save intrinsic camera parameters.
vpCameraParameters getCameraParameters() const
int parse(vpCameraParameters &cam, const std::string &filename, const std::string &camera_name, const vpCameraParameters::vpCameraParametersProjType &projModel, unsigned int image_width=0, unsigned int image_height=0, bool verbose=true)