4#include <visp3/core/vpDisplay.h>
5#include <visp3/core/vpIoTools.h>
6#include <visp3/gui/vpDisplayGDI.h>
7#include <visp3/gui/vpDisplayOpenCV.h>
8#include <visp3/gui/vpDisplayX.h>
9#include <visp3/io/vpImageIo.h>
10#include <visp3/mbt/vpMbGenericTracker.h>
12#if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGCODECS)
16 vpImage<uint16_t> &I_depth_raw,
unsigned int &depth_width,
unsigned int &depth_height,
20 char buffer[FILENAME_MAX];
23 ss << input_directory <<
"/images/%04d.jpg";
24 snprintf(buffer, FILENAME_MAX, ss.str().c_str(), cpt);
25 std::string filename_img = buffer;
28 std::cerr <<
"Cannot read: " << filename_img << std::endl;
35 ss << input_directory <<
"/depth/Image%04d.exr";
36 snprintf(buffer, FILENAME_MAX, ss.str().c_str(), cpt);
37 std::string filename_depth = buffer;
39 cv::Mat depth_raw = cv::imread(filename_depth, cv::IMREAD_ANYDEPTH | cv::IMREAD_ANYCOLOR);
40 if (depth_raw.empty()) {
41 std::cerr <<
"Cannot read: " << filename_depth << std::endl;
45 depth_width =
static_cast<unsigned int>(depth_raw.cols);
46 depth_height =
static_cast<unsigned int>(depth_raw.rows);
47 I_depth_raw.
resize(depth_height, depth_width);
48 pointcloud.resize(depth_width * depth_height);
50 for (
int i = 0; i < depth_raw.rows; i++) {
51 for (
int j = 0; j < depth_raw.cols; j++) {
52 I_depth_raw[i][j] =
static_cast<uint16_t
>(32767.5f * depth_raw.at<cv::Vec3f>(i, j)[0]);
53 double x = 0.0, y = 0.0;
55 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]);
57 size_t idx =
static_cast<size_t>(i * depth_raw.cols + j);
58 pointcloud[idx].resize(3);
59 pointcloud[idx][0] = x * Z;
60 pointcloud[idx][1] = y * Z;
61 pointcloud[idx][2] = Z;
67 ss << input_directory <<
"/camera_poses/Camera_%03d.txt";
68 snprintf(buffer, FILENAME_MAX, ss.str().c_str(), cpt);
69 std::string filename_pose = buffer;
72 f_pose.open(filename_pose.c_str());
73 if (!f_pose.is_open()) {
74 std::cerr <<
"Cannot read: " << filename_pose << std::endl;
78 cMo_ground_truth.
load(f_pose);
84int main(
int argc,
char *argv[])
86 std::string input_directory =
".";
87 std::string config_color =
"teabox.xml", config_depth =
"teabox_depth.xml";
88 std::string model_color =
"teabox.cao", model_depth =
"teabox.cao";
89 std::string init_file =
"teabox.init";
90 std::string extrinsic_file =
"depth_M_color.txt";
91 unsigned int first_frame_index = 1;
92 bool disable_depth =
false;
93 bool display_ground_truth =
false;
96 for (
int i = 1; i < argc; i++) {
97 if (std::string(argv[i]) ==
"--input_directory" && i + 1 < argc) {
98 input_directory = std::string(argv[i + 1]);
99 }
else if (std::string(argv[i]) ==
"--config_color" && i + 1 < argc) {
100 config_color = std::string(argv[i + 1]);
101 }
else if (std::string(argv[i]) ==
"--config_depth" && i + 1 < argc) {
102 config_depth = std::string(argv[i + 1]);
103 }
else if (std::string(argv[i]) ==
"--model_color" && i + 1 < argc) {
104 model_color = std::string(argv[i + 1]);
105 }
else if (std::string(argv[i]) ==
"--model_depth" && i + 1 < argc) {
106 model_depth = std::string(argv[i + 1]);
107 }
else if (std::string(argv[i]) ==
"--init_file" && i + 1 < argc) {
108 init_file = std::string(argv[i + 1]);
109 }
else if (std::string(argv[i]) ==
"--extrinsics" && i + 1 < argc) {
110 extrinsic_file = std::string(argv[i + 1]);
111 }
else if (std::string(argv[i]) ==
"--disable_depth") {
112 disable_depth =
true;
113 }
else if (std::string(argv[i]) ==
"--display_ground_truth") {
114 display_ground_truth =
true;
115 }
else if (std::string(argv[i]) ==
"--click") {
117 }
else if (std::string(argv[i]) ==
"--first_frame_index" && i + 1 < argc) {
118 first_frame_index =
static_cast<unsigned int>(atoi(argv[i + 1]));
119 }
else if (std::string(argv[i]) ==
"--help" || std::string(argv[i]) ==
"-h") {
123 <<
" [--input_directory <data directory> (default: .)]"
124 " [--config_color <object.xml> (default: teabox.xml)] [--config_depth <object.xml> (default: "
126 " [--model_color <object.cao> (default: teabox.cao)] [--model_depth <object.cao> (default: teabox.cao)]"
127 " [--init_file <object.init> (default: teabox.init)]"
128 " [--extrinsics <depth to color transformation> (default: depth_M_color.txt)] [--disable_depth]"
129 " [--display_ground_truth] [--click] [--first_frame_index <index> (default: 1)]"
135 std::cout <<
"input_directory: " << input_directory << std::endl;
136 std::cout <<
"config_color: " << config_color << std::endl;
137 std::cout <<
"config_depth: " << config_depth << std::endl;
138 std::cout <<
"model_color: " << model_color << std::endl;
139 std::cout <<
"model_depth: " << model_depth << std::endl;
140 std::cout <<
"init_file: " << model_depth << std::endl;
141 std::cout <<
"extrinsic_file: " << extrinsic_file << std::endl;
142 std::cout <<
"first_frame_index: " << first_frame_index << std::endl;
143 std::cout <<
"disable_depth: " << disable_depth << std::endl;
144 std::cout <<
"display_ground_truth: " << display_ground_truth << std::endl;
145 std::cout <<
"click: " << click << std::endl;
147 std::vector<int> tracker_types;
148#if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
158 tracker.loadConfigFile(config_color, config_depth);
160 tracker.loadConfigFile(config_color);
161 tracker.loadModel(model_color);
164 tracker.getCameraParameters(cam_color, cam_depth);
166 tracker.getCameraParameters(cam_color);
167 tracker.setDisplayFeatures(
true);
168 std::cout <<
"cam_color:\n" << cam_color << std::endl;
169 std::cout <<
"cam_depth:\n" << cam_depth << std::endl;
173 unsigned int depth_width = 0, depth_height = 0;
174 std::vector<vpColVector> pointcloud;
177 unsigned int frame_cpt = first_frame_index;
178 read_data(frame_cpt, input_directory, I, I_depth_raw, depth_width, depth_height, pointcloud, cam_depth,
182#if defined(VISP_HAVE_X11)
184#elif defined(VISP_HAVE_GDI)
186#elif defined (HAVE_OPENCV_HIGHGUI)
190 d1.
init(I, 0, 0,
"Color image");
191 d2.
init(I_depth,
static_cast<int>(I.
getWidth()), 0,
"Depth image");
194 if (!disable_depth) {
195 std::ifstream f_extrinsics;
196 f_extrinsics.open(extrinsic_file.c_str());
198 depthMcolor.
load(f_extrinsics);
199 tracker.setCameraTransformationMatrix(
"Camera2", depthMcolor);
200 std::cout <<
"depthMcolor:\n" << depthMcolor << std::endl;
203 if (display_ground_truth) {
204 tracker.initFromPose(I, cMo_ground_truth);
206 tracker.initClick(I, init_file,
true);
210 while (!quit && read_data(frame_cpt, input_directory, I, I_depth_raw, depth_width, depth_height, pointcloud,
211 cam_depth, cMo_ground_truth)) {
216 if (display_ground_truth) {
217 tracker.initFromPose(I, cMo_ground_truth);
219 if (!disable_depth) {
220 std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
221 std::map<std::string, const std::vector<vpColVector> *> mapOfPointClouds;
222 std::map<std::string, unsigned int> mapOfPointCloudWidths;
223 std::map<std::string, unsigned int> mapOfPointCloudHeights;
225 mapOfImages[
"Camera1"] = &I;
226 mapOfPointClouds[
"Camera2"] = &pointcloud;
227 mapOfPointCloudWidths[
"Camera2"] = depth_width;
228 mapOfPointCloudHeights[
"Camera2"] = depth_height;
229 tracker.track(mapOfImages, mapOfPointClouds, mapOfPointCloudWidths, mapOfPointCloudHeights);
236 std::cout <<
"\nFrame: " << frame_cpt << std::endl;
237 if (!display_ground_truth)
238 std::cout <<
"cMo:\n" << cMo << std::endl;
239 std::cout <<
"cMo ground truth:\n" << cMo_ground_truth << std::endl;
240 if (!disable_depth) {
241 tracker.display(I, I_depth, cMo, depthMcolor * cMo, cam_color, cam_depth,
vpColor::red, 2);
248 std::ostringstream oss;
249 oss <<
"Frame: " << frame_cpt;
252 if (!display_ground_truth) {
254 std::stringstream ss;
255 ss <<
"Nb features: " << tracker.getError().size();
259 std::stringstream ss;
260 ss <<
"Features: edges " << tracker.getNbFeaturesEdge() <<
", klt " << tracker.getNbFeaturesKlt()
261 <<
", depth " << tracker.getNbFeaturesDepthDense();
290 }
catch (std::exception &e) {
291 std::cerr <<
"Catch exception: " << e.what() << std::endl;
299 std::cout <<
"To run this tutorial, ViSP should be built with OpenCV and pugixml libraries." << std::endl;
Generic class defining intrinsic camera parameters.
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="")
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 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)
Definition of the vpImage class member functions.
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)