4 #include <visp3/core/vpDisplay.h>
5 #include <visp3/core/vpIoTools.h>
6 #include <visp3/io/vpImageIo.h>
7 #include <visp3/gui/vpDisplayX.h>
8 #include <visp3/gui/vpDisplayGDI.h>
9 #include <visp3/gui/vpDisplayOpenCV.h>
10 #include <visp3/mbt/vpMbGenericTracker.h>
12 #if (VISP_HAVE_OPENCV_VERSION >= 0x020403)
15 vpImage<uint16_t> &I_depth_raw,
unsigned int &depth_width,
unsigned int &depth_height,
19 char buffer[FILENAME_MAX];
22 ss << input_directory <<
"/images/%04d.jpg";
23 sprintf(buffer, ss.str().c_str(), cpt);
24 std::string filename_img = buffer;
27 std::cerr <<
"Cannot read: " << filename_img << std::endl;
34 ss << input_directory <<
"/depth/Image%04d.exr";
35 sprintf(buffer, ss.str().c_str(), cpt);
36 std::string filename_depth = buffer;
38 cv::Mat depth_raw = cv::imread(filename_depth, cv::IMREAD_ANYDEPTH | cv::IMREAD_ANYCOLOR);
39 if (depth_raw.empty()) {
40 std::cerr <<
"Cannot read: " << filename_depth << std::endl;
44 depth_width =
static_cast<unsigned int>(depth_raw.cols);
45 depth_height =
static_cast<unsigned int>(depth_raw.rows);
46 I_depth_raw.
resize(depth_height, depth_width);
47 pointcloud.resize(depth_width*depth_height);
49 for (
int i = 0; i < depth_raw.rows; i++) {
50 for (
int j = 0; j < depth_raw.cols; j++) {
51 I_depth_raw[i][j] =
static_cast<uint16_t
>(32767.5f * depth_raw.at<cv::Vec3f>(i, j)[0]);
52 double x = 0.0, y = 0.0;
54 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]);
56 size_t idx =
static_cast<size_t>(i*depth_raw.cols + j);
57 pointcloud[idx].resize(3);
58 pointcloud[idx][0] = x*Z;
59 pointcloud[idx][1] = y*Z;
60 pointcloud[idx][2] = Z;
66 ss << input_directory <<
"/camera_poses/Camera_%03d.txt";
67 sprintf(buffer, ss.str().c_str(), cpt);
68 std::string filename_pose = buffer;
71 f_pose.open(filename_pose.c_str());
72 if (!f_pose.is_open()) {
73 std::cerr <<
"Cannot read: " << filename_pose << std::endl;
77 cMo_ground_truth.
load(f_pose);
83 int main(
int argc,
char *argv[])
85 std::string input_directory =
".";
86 std::string config_color =
"teabox.xml", config_depth =
"teabox_depth.xml";
87 std::string model_color =
"teabox.cao", model_depth =
"teabox.cao";
88 std::string init_file =
"teabox.init";
89 std::string extrinsic_file =
"depth_M_color.txt";
90 unsigned int first_frame_index = 1;
91 bool disable_depth =
false;
92 bool display_ground_truth =
false;
95 for (
int i = 1; i < argc; i++) {
96 if (std::string(argv[i]) ==
"--input_directory" && i + 1 < argc) {
97 input_directory = std::string(argv[i + 1]);
98 }
else if (std::string(argv[i]) ==
"--config_color" && i + 1 < argc) {
99 config_color = std::string(argv[i + 1]);
100 }
else if (std::string(argv[i]) ==
"--config_depth" && i + 1 < argc) {
101 config_depth = std::string(argv[i + 1]);
102 }
else if (std::string(argv[i]) ==
"--model_color" && i + 1 < argc) {
103 model_color = std::string(argv[i + 1]);
104 }
else if (std::string(argv[i]) ==
"--model_depth" && i + 1 < argc) {
105 model_depth = std::string(argv[i + 1]);
106 }
else if (std::string(argv[i]) ==
"--init_file" && i + 1 < argc) {
107 init_file = std::string(argv[i + 1]);
108 }
else if (std::string(argv[i]) ==
"--extrinsics" && i + 1 < argc) {
109 extrinsic_file = std::string(argv[i + 1]);
110 }
else if (std::string(argv[i]) ==
"--disable_depth") {
111 disable_depth =
true;
112 }
else if (std::string(argv[i]) ==
"--display_ground_truth") {
113 display_ground_truth =
true;
114 }
else if (std::string(argv[i]) ==
"--click") {
116 }
else if (std::string(argv[i]) ==
"--first_frame_index" && i+1 < argc) {
117 first_frame_index =
static_cast<unsigned int>(atoi(argv[i+1]));
119 else if (std::string(argv[i]) ==
"--help" || std::string(argv[i]) ==
"-h") {
120 std::cout <<
"Usage: \n" << argv[0] <<
" [--input_directory <data directory> (default: .)]"
121 " [--config_color <object.xml> (default: teabox.xml)] [--config_depth <object.xml> (default: teabox_depth.xml)]"
122 " [--model_color <object.cao> (default: teabox.cao)] [--model_depth <object.cao> (default: teabox.cao)]"
123 " [--init_file <object.init> (default: teabox.init)]"
124 " [--extrinsics <depth to color transformation> (default: depth_M_color.txt)] [--disable_depth]"
125 " [--display_ground_truth] [--click] [--first_frame_index <index> (default: 1)]" << std::endl;
130 std::cout <<
"input_directory: " << input_directory << std::endl;
131 std::cout <<
"config_color: " << config_color << std::endl;
132 std::cout <<
"config_depth: " << config_depth << std::endl;
133 std::cout <<
"model_color: " << model_color << std::endl;
134 std::cout <<
"model_depth: " << model_depth << std::endl;
135 std::cout <<
"init_file: " << model_depth << std::endl;
136 std::cout <<
"extrinsic_file: " << extrinsic_file << std::endl;
137 std::cout <<
"first_frame_index: " << first_frame_index << std::endl;
138 std::cout <<
"disable_depth: " << disable_depth << std::endl;
139 std::cout <<
"display_ground_truth: " << display_ground_truth << std::endl;
140 std::cout <<
"click: " << click << std::endl;
142 std::vector<int> tracker_types;
149 tracker.loadConfigFile(config_color, config_depth);
151 tracker.loadConfigFile(config_color);
152 tracker.loadModel(model_color);
155 tracker.getCameraParameters(cam_color, cam_depth);
157 tracker.getCameraParameters(cam_color);
158 tracker.setDisplayFeatures(
true);
159 std::cout <<
"cam_color:\n" << cam_color << std::endl;
160 std::cout <<
"cam_depth:\n" << cam_depth << std::endl;
164 unsigned int depth_width = 0, depth_height = 0;
165 std::vector<vpColVector> pointcloud;
168 unsigned int frame_cpt = first_frame_index;
169 read_data(frame_cpt, input_directory, I, I_depth_raw, depth_width, depth_height, pointcloud, cam_depth, cMo_ground_truth);
172 #if defined(VISP_HAVE_X11)
174 #elif defined(VISP_HAVE_GDI)
180 d1.
init(I, 0, 0,
"Color image");
181 d2.
init(I_depth,
static_cast<int>(I.
getWidth()), 0,
"Depth image");
184 if (!disable_depth) {
185 std::ifstream f_extrinsics;
186 f_extrinsics.open(extrinsic_file.c_str());
188 depthMcolor.
load(f_extrinsics);
189 tracker.setCameraTransformationMatrix(
"Camera2", depthMcolor);
190 std::cout <<
"depthMcolor:\n" << depthMcolor << std::endl;
193 if (display_ground_truth) {
194 tracker.initFromPose(I, cMo_ground_truth);
196 tracker.initClick(I, init_file,
true);
200 while (!quit && read_data(frame_cpt, input_directory, I, I_depth_raw, depth_width, depth_height, pointcloud, cam_depth, cMo_ground_truth)) {
205 if (display_ground_truth) {
206 tracker.initFromPose(I, cMo_ground_truth);
208 if (!disable_depth) {
209 std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
210 std::map<std::string, const std::vector<vpColVector> *> mapOfPointClouds;
211 std::map<std::string, unsigned int> mapOfPointCloudWidths;
212 std::map<std::string, unsigned int> mapOfPointCloudHeights;
214 mapOfImages[
"Camera1"] = &I;
215 mapOfPointClouds[
"Camera2"] = &pointcloud;
216 mapOfPointCloudWidths[
"Camera2"] = depth_width;
217 mapOfPointCloudHeights[
"Camera2"] = depth_height;
218 tracker.track(mapOfImages, mapOfPointClouds, mapOfPointCloudWidths, mapOfPointCloudHeights);
225 std::cout <<
"\nFrame: " << frame_cpt << std::endl;
226 if (!display_ground_truth)
227 std::cout <<
"cMo:\n" << cMo << std::endl;
228 std::cout <<
"cMo ground truth:\n" << cMo_ground_truth << std::endl;
229 if (!disable_depth) {
230 tracker.display(I, I_depth, cMo, depthMcolor*cMo, cam_color, cam_depth,
vpColor::red, 2);
238 std::ostringstream oss;
239 oss <<
"Frame: " << frame_cpt;
242 if (!display_ground_truth) {
244 std::stringstream ss;
245 ss <<
"Nb features: " << tracker.getError().size();
249 std::stringstream ss;
250 ss <<
"Features: edges " << tracker.getNbFeaturesEdge()
251 <<
", klt " << tracker.getNbFeaturesKlt()
252 <<
", depth " << tracker.getNbFeaturesDepthDense();
281 }
catch (std::exception& e) {
282 std::cerr <<
"Catch exception: " << e.what() << std::endl;
290 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 winx=-1, int winy=-1, const std::string &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)
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)