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 (VISP_HAVE_OPENCV_VERSION >= 0x020403)
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 sprintf(buffer, 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 sprintf(buffer, 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 sprintf(buffer, 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);
84 int 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;
154 tracker.loadConfigFile(config_color, config_depth);
156 tracker.loadConfigFile(config_color);
157 tracker.loadModel(model_color);
160 tracker.getCameraParameters(cam_color, cam_depth);
162 tracker.getCameraParameters(cam_color);
163 tracker.setDisplayFeatures(
true);
164 std::cout <<
"cam_color:\n" << cam_color << std::endl;
165 std::cout <<
"cam_depth:\n" << cam_depth << std::endl;
169 unsigned int depth_width = 0, depth_height = 0;
170 std::vector<vpColVector> pointcloud;
173 unsigned int frame_cpt = first_frame_index;
174 read_data(frame_cpt, input_directory, I, I_depth_raw, depth_width, depth_height, pointcloud, cam_depth,
178 #if defined(VISP_HAVE_X11)
180 #elif defined(VISP_HAVE_GDI)
186 d1.
init(I, 0, 0,
"Color image");
187 d2.
init(I_depth,
static_cast<int>(I.
getWidth()), 0,
"Depth image");
190 if (!disable_depth) {
191 std::ifstream f_extrinsics;
192 f_extrinsics.open(extrinsic_file.c_str());
194 depthMcolor.
load(f_extrinsics);
195 tracker.setCameraTransformationMatrix(
"Camera2", depthMcolor);
196 std::cout <<
"depthMcolor:\n" << depthMcolor << std::endl;
199 if (display_ground_truth) {
200 tracker.initFromPose(I, cMo_ground_truth);
202 tracker.initClick(I, init_file,
true);
206 while (!quit && read_data(frame_cpt, input_directory, I, I_depth_raw, depth_width, depth_height, pointcloud,
207 cam_depth, cMo_ground_truth)) {
212 if (display_ground_truth) {
213 tracker.initFromPose(I, cMo_ground_truth);
215 if (!disable_depth) {
216 std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
217 std::map<std::string, const std::vector<vpColVector> *> mapOfPointClouds;
218 std::map<std::string, unsigned int> mapOfPointCloudWidths;
219 std::map<std::string, unsigned int> mapOfPointCloudHeights;
221 mapOfImages[
"Camera1"] = &I;
222 mapOfPointClouds[
"Camera2"] = &pointcloud;
223 mapOfPointCloudWidths[
"Camera2"] = depth_width;
224 mapOfPointCloudHeights[
"Camera2"] = depth_height;
225 tracker.track(mapOfImages, mapOfPointClouds, mapOfPointCloudWidths, mapOfPointCloudHeights);
232 std::cout <<
"\nFrame: " << frame_cpt << std::endl;
233 if (!display_ground_truth)
234 std::cout <<
"cMo:\n" << cMo << std::endl;
235 std::cout <<
"cMo ground truth:\n" << cMo_ground_truth << std::endl;
236 if (!disable_depth) {
237 tracker.display(I, I_depth, cMo, depthMcolor * cMo, cam_color, cam_depth,
vpColor::red, 2);
244 std::ostringstream oss;
245 oss <<
"Frame: " << frame_cpt;
248 if (!display_ground_truth) {
250 std::stringstream ss;
251 ss <<
"Nb features: " << tracker.getError().size();
255 std::stringstream ss;
256 ss <<
"Features: edges " << tracker.getNbFeaturesEdge() <<
", klt " << tracker.getNbFeaturesKlt()
257 <<
", depth " << tracker.getNbFeaturesDepthDense();
286 }
catch (std::exception &e) {
287 std::cerr <<
"Catch exception: " << e.what() << std::endl;
295 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 flush(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))
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)