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/vpDisplayX.h>
9#include <visp3/io/vpImageIo.h>
11#include <visp3/mbt/vpMbGenericTracker.h>
14#if defined(VISP_HAVE_PCL)
15#include <pcl/common/common.h>
16#include <pcl/io/pcd_io.h>
32void rs_deproject_pixel_to_point(
float point[3],
const rs_intrinsics &intrin,
const float pixel[2],
float depth)
34 float x = (pixel[0] - intrin.ppx) / intrin.fx;
35 float y = (pixel[1] - intrin.ppy) / intrin.fy;
37 float r2 = x * x + y * y;
38 float f = 1 + intrin.coeffs[0] * r2 + intrin.coeffs[1] * r2 * r2 + intrin.coeffs[4] * r2 * r2 * r2;
39 float ux = x * f + 2 * intrin.coeffs[2] * x * y + intrin.coeffs[3] * (r2 + 2 * x * x);
40 float uy = y * f + 2 * intrin.coeffs[3] * x * y + intrin.coeffs[2] * (r2 + 2 * y * y);
51bool read_data(
unsigned int cpt,
const std::string &input_directory,
vpImage<vpRGBa> &I_color,
55 char buffer[FILENAME_MAX];
58 ss << input_directory <<
"/color_image_%04d.jpg";
59 snprintf(buffer, FILENAME_MAX, ss.str().c_str(), cpt);
60 std::string filename_color = buffer;
63 std::cerr <<
"Cannot read: " << filename_color << std::endl;
70 ss << input_directory <<
"/depth_image_%04d.bin";
71 snprintf(buffer, FILENAME_MAX, ss.str().c_str(), cpt);
72 std::string filename_depth = buffer;
74 std::ifstream file_depth(filename_depth.c_str(), std::ios::in | std::ios::binary);
75 if (!file_depth.is_open()) {
79 unsigned int height = 0, width = 0;
82 I_depth_raw.
resize(height, width);
84 uint16_t depth_value = 0;
85 for (
unsigned int i = 0; i < height; i++) {
86 for (
unsigned int j = 0; j < width; j++) {
88 I_depth_raw[i][j] = depth_value;
93 pointcloud->width = width;
94 pointcloud->height = height;
95 pointcloud->resize((
size_t)width * height);
98 const float depth_scale = 0.00100000005f;
99 rs_intrinsics depth_intrinsic;
100 depth_intrinsic.ppx = 320.503509521484f;
101 depth_intrinsic.ppy = 235.602951049805f;
102 depth_intrinsic.fx = 383.970001220703f;
103 depth_intrinsic.fy = 383.970001220703f;
104 depth_intrinsic.coeffs[0] = 0.0f;
105 depth_intrinsic.coeffs[1] = 0.0f;
106 depth_intrinsic.coeffs[2] = 0.0f;
107 depth_intrinsic.coeffs[3] = 0.0f;
108 depth_intrinsic.coeffs[4] = 0.0f;
110 for (
unsigned int i = 0; i < height; i++) {
111 for (
unsigned int j = 0; j < width; j++) {
112 float scaled_depth = I_depth_raw[i][j] * depth_scale;
114 float pixel[2] = {(float)j, (
float)i};
115 rs_deproject_pixel_to_point(point, depth_intrinsic, pixel, scaled_depth);
116 pointcloud->points[(size_t)(i * width + j)].x = point[0];
117 pointcloud->points[(size_t)(i * width + j)].y = point[1];
118 pointcloud->points[(size_t)(i * width + j)].z = point[2];
126int main(
int argc,
char *argv[])
128 std::string input_directory =
"data";
129 std::string config_color =
"model/cube/cube.xml", config_depth =
"model/cube/cube_depth.xml";
130 std::string model_color =
"model/cube/cube.cao", model_depth =
"model/cube/cube.cao";
131 std::string init_file =
"model/cube/cube.init";
132 unsigned int frame_cpt = 0;
133 bool disable_depth =
false;
135 for (
int i = 1; i < argc; i++) {
136 if (std::string(argv[i]) ==
"--input_directory" && i + 1 < argc) {
137 input_directory = std::string(argv[i + 1]);
138 }
else if (std::string(argv[i]) ==
"--config_color" && i + 1 < argc) {
139 config_color = std::string(argv[i + 1]);
140 }
else if (std::string(argv[i]) ==
"--config_depth" && i + 1 < argc) {
141 config_depth = std::string(argv[i + 1]);
142 }
else if (std::string(argv[i]) ==
"--model_color" && i + 1 < argc) {
143 model_color = std::string(argv[i + 1]);
144 }
else if (std::string(argv[i]) ==
"--model_depth" && i + 1 < argc) {
145 model_depth = std::string(argv[i + 1]);
146 }
else if (std::string(argv[i]) ==
"--init_file" && i + 1 < argc) {
147 init_file = std::string(argv[i + 1]);
148 }
else if (std::string(argv[i]) ==
"--disable_depth") {
149 disable_depth =
true;
150 }
else if (std::string(argv[i]) ==
"--help" || std::string(argv[i]) ==
"-h") {
151 std::cout <<
"Usage: \n"
153 <<
" --input_directory <data directory> --config_color <object.xml> --config_depth <object.xml>"
154 " --model_color <object.cao> --model_depth <object.cao> --init_file <object.init> --disable_depth"
159 <<
" --config_color model/cube/cube.xml --config_depth model/cube/cube.xml"
160 " --model_color model/cube/cube.cao --model_depth model/cube/cube.cao --init_file model/cube/cube.init\n"
166 std::cout <<
"Tracked features: " << std::endl;
167#if defined(VISP_HAVE_OPENCV)
168 std::cout <<
" Use edges : 1" << std::endl;
169#if defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
170 std::cout <<
" Use klt : 1" << std::endl;
172 std::cout <<
" Use klt : 0" << std::endl;
174 std::cout <<
" Use depth : " << !disable_depth << std::endl;
176 std::cout <<
" Use edges : 1" << std::endl;
177 std::cout <<
" Use klt : 0" << std::endl;
178 std::cout <<
" Use depth : 0" << std::endl;
180 std::cout <<
"Config files: " << std::endl;
181 std::cout <<
" Input directory: "
182 <<
"\"" << input_directory <<
"\"" << std::endl;
183 std::cout <<
" Config color: "
184 <<
"\"" << config_color <<
"\"" << std::endl;
185 std::cout <<
" Config depth: "
186 <<
"\"" << config_depth <<
"\"" << std::endl;
187 std::cout <<
" Model color : "
188 <<
"\"" << model_color <<
"\"" << std::endl;
189 std::cout <<
" Model depth : "
190 <<
"\"" << model_depth <<
"\"" << std::endl;
191 std::cout <<
" Init file : "
192 <<
"\"" << init_file <<
"\"" << std::endl;
200 pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud(
new pcl::PointCloud<pcl::PointXYZ>());
204 read_data(frame_cpt, input_directory, I_color, I_depth_raw, pointcloud);
208#if defined(VISP_HAVE_X11)
210#elif defined(VISP_HAVE_GDI)
214#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)
215 unsigned int _posx = 100, _posy = 50, _posdx = 10;
216 d1.
init(I_gray, _posx, _posy,
"Color stream");
217 d2.
init(I_depth, _posx + I_gray.
getWidth() + _posdx, _posy,
"Depth stream");
226 std::vector<int> trackerTypes;
227#if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
236 tracker.loadConfigFile(config_color, config_depth);
239 tracker.loadModel(model_color, model_depth);
242 tracker.getCameraParameters(cam_color, cam_depth);
244 std::cout <<
"Camera parameters for color camera (from XML file): " << cam_color << std::endl;
245 std::cout <<
"Camera parameters for depth camera (from XML file): " << cam_depth << std::endl;
248 tracker.setDisplayFeatures(
true);
254 std::ifstream file((std::string(input_directory +
"/depth_M_color.txt")).c_str());
255 depth_M_color.
load(file);
258 std::map<std::string, vpHomogeneousMatrix> mapOfCameraTransformations;
260 mapOfCameraTransformations[
"Camera2"] = depth_M_color;
261 tracker.setCameraTransformationMatrix(mapOfCameraTransformations);
263 std::cout <<
"depth_M_color: \n" << depth_M_color << std::endl;
266 std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
267 mapOfImages[
"Camera1"] = &I_gray;
268 mapOfImages[
"Camera2"] = &I_depth;
272 std::map<std::string, std::string> mapOfInitFiles;
273 mapOfInitFiles[
"Camera1"] = init_file;
274 tracker.initClick(mapOfImages, mapOfInitFiles,
true);
278 pcl::PointCloud<pcl::PointXYZ>::Ptr empty_pointcloud(
new pcl::PointCloud<pcl::PointXYZ>);
279 std::vector<double> times_vec;
285 quit = !read_data(frame_cpt, input_directory, I_color, I_depth_raw, pointcloud);
292 mapOfImages[
"Camera1"] = &I_gray;
293 std::map<std::string, pcl::PointCloud<pcl::PointXYZ>::ConstPtr> mapOfPointclouds;
295 mapOfPointclouds[
"Camera2"] = empty_pointcloud;
297 mapOfPointclouds[
"Camera2"] = pointcloud;
300 tracker.track(mapOfImages, mapOfPointclouds);
307 std::cout <<
"iter: " << frame_cpt <<
" cMo:\n" << cMo << std::endl;
310 tracker.display(I_gray, I_depth, cMo, depth_M_color * cMo, cam_color, cam_depth,
vpColor::red, 3);
316 times_vec.push_back(t);
318 std::stringstream ss;
319 ss <<
"Computation time: " << t <<
" ms";
322 std::stringstream ss;
323 ss <<
"Nb features: " << tracker.getError().size();
327 std::stringstream ss;
328 ss <<
"Features: edges " << tracker.getNbFeaturesEdge() <<
", klt " << tracker.getNbFeaturesKlt() <<
", depth "
329 << tracker.getNbFeaturesDepthDense();
360 std::cout <<
"To run this tutorial, ViSP should be build with PCL library."
361 " Install libpcl, configure and build again ViSP..."
Generic class defining intrinsic camera parameters.
static const vpColor none
Display for windows using GDI (available on any windows 32 platform).
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)
error that can be emitted by ViSP classes.
const std::string & getStringMessage() const
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 convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
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
static double getMedian(const std::vector< double > &v)
static double getStdev(const std::vector< double > &v, bool useBesselCorrection=false)
static double getMean(const std::vector< double > &v)
Real-time 6D object pose tracking using its CAD model.
VISP_EXPORT double measureTimeMs()