4 #include <visp3/core/vpDisplay.h>
5 #include <visp3/core/vpIoTools.h>
6 #include <visp3/gui/vpDisplayGDI.h>
7 #include <visp3/gui/vpDisplayX.h>
8 #include <visp3/io/vpImageIo.h>
10 #include <visp3/mbt/vpMbGenericTracker.h>
13 #if defined(VISP_HAVE_PCL)
14 #include <pcl/common/common.h>
15 #include <pcl/io/pcd_io.h>
19 struct vpRealsenseIntrinsics_t
32 void rs_deproject_pixel_to_point(
float point[3],
const vpRealsenseIntrinsics_t &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);
51 bool 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 vpRealsenseIntrinsics_t 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];
126 int 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]);
139 else if (std::string(argv[i]) ==
"--config_color" && i + 1 < argc) {
140 config_color = std::string(argv[i + 1]);
142 else if (std::string(argv[i]) ==
"--config_depth" && i + 1 < argc) {
143 config_depth = std::string(argv[i + 1]);
145 else if (std::string(argv[i]) ==
"--model_color" && i + 1 < argc) {
146 model_color = std::string(argv[i + 1]);
148 else if (std::string(argv[i]) ==
"--model_depth" && i + 1 < argc) {
149 model_depth = std::string(argv[i + 1]);
151 else if (std::string(argv[i]) ==
"--init_file" && i + 1 < argc) {
152 init_file = std::string(argv[i + 1]);
154 else if (std::string(argv[i]) ==
"--disable_depth") {
155 disable_depth =
true;
157 else if (std::string(argv[i]) ==
"--help" || std::string(argv[i]) ==
"-h") {
158 std::cout <<
"Usage: \n"
160 <<
" --input_directory <data directory> --config_color <object.xml> --config_depth <object.xml>"
161 " --model_color <object.cao> --model_depth <object.cao> --init_file <object.init> --disable_depth"
166 <<
" --config_color model/cube/cube.xml --config_depth model/cube/cube.xml"
167 " --model_color model/cube/cube.cao --model_depth model/cube/cube.cao --init_file model/cube/cube.init\n"
173 std::cout <<
"Tracked features: " << std::endl;
174 #if defined(VISP_HAVE_OPENCV)
175 std::cout <<
" Use edges : 1" << std::endl;
176 #if defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
177 std::cout <<
" Use klt : 1" << std::endl;
179 std::cout <<
" Use klt : 0" << std::endl;
181 std::cout <<
" Use depth : " << !disable_depth << std::endl;
183 std::cout <<
" Use edges : 1" << std::endl;
184 std::cout <<
" Use klt : 0" << std::endl;
185 std::cout <<
" Use depth : 0" << std::endl;
187 std::cout <<
"Config files: " << std::endl;
188 std::cout <<
" Input directory: "
189 <<
"\"" << input_directory <<
"\"" << std::endl;
190 std::cout <<
" Config color: "
191 <<
"\"" << config_color <<
"\"" << std::endl;
192 std::cout <<
" Config depth: "
193 <<
"\"" << config_depth <<
"\"" << std::endl;
194 std::cout <<
" Model color : "
195 <<
"\"" << model_color <<
"\"" << std::endl;
196 std::cout <<
" Model depth : "
197 <<
"\"" << model_depth <<
"\"" << std::endl;
198 std::cout <<
" Init file : "
199 <<
"\"" << init_file <<
"\"" << std::endl;
207 pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud(
new pcl::PointCloud<pcl::PointXYZ>());
211 read_data(frame_cpt, input_directory, I_color, I_depth_raw, pointcloud);
215 #if defined(VISP_HAVE_X11)
217 #elif defined(VISP_HAVE_GDI)
221 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)
222 unsigned int _posx = 100, _posy = 50, _posdx = 10;
223 d1.
init(I_gray, _posx, _posy,
"Color stream");
224 d2.
init(I_depth, _posx + I_gray.
getWidth() + _posdx, _posy,
"Depth stream");
233 std::vector<int> trackerTypes;
234 #if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
242 #if defined(VISP_HAVE_PUGIXML)
244 tracker.loadConfigFile(config_color, config_depth);
251 tracker.setCameraParameters(cam_color, cam_depth);
264 tracker.setMovingEdge(me);
267 #if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
269 tracker.setKltMaskBorder(5);
278 tracker.setKltOpencv(klt);
283 tracker.setDepthNormalPclPlaneEstimationMethod(2);
284 tracker.setDepthNormalPclPlaneEstimationRansacMaxIter(200);
285 tracker.setDepthNormalPclPlaneEstimationRansacThreshold(0.001);
286 tracker.setDepthNormalSamplingStep(2, 2);
288 tracker.setDepthDenseSamplingStep(4, 4);
292 tracker.setNearClippingDistance(0.001);
293 tracker.setFarClippingDistance(5.0);
297 tracker.loadModel(model_color, model_depth);
300 tracker.getCameraParameters(cam_color, cam_depth);
302 std::cout <<
"Camera parameters for color camera (from XML file): " << cam_color << std::endl;
303 std::cout <<
"Camera parameters for depth camera (from XML file): " << cam_depth << std::endl;
306 tracker.setDisplayFeatures(
true);
312 std::ifstream file((std::string(input_directory +
"/depth_M_color.txt")).c_str());
313 depth_M_color.
load(file);
316 std::map<std::string, vpHomogeneousMatrix> mapOfCameraTransformations;
318 mapOfCameraTransformations[
"Camera2"] = depth_M_color;
319 tracker.setCameraTransformationMatrix(mapOfCameraTransformations);
321 std::cout <<
"depth_M_color: \n" << depth_M_color << std::endl;
324 std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
325 mapOfImages[
"Camera1"] = &I_gray;
326 mapOfImages[
"Camera2"] = &I_depth;
330 std::map<std::string, std::string> mapOfInitFiles;
331 mapOfInitFiles[
"Camera1"] = init_file;
332 tracker.initClick(mapOfImages, mapOfInitFiles,
true);
336 pcl::PointCloud<pcl::PointXYZ>::Ptr empty_pointcloud(
new pcl::PointCloud<pcl::PointXYZ>);
337 std::vector<double> times_vec;
343 quit = !read_data(frame_cpt, input_directory, I_color, I_depth_raw, pointcloud);
350 mapOfImages[
"Camera1"] = &I_gray;
351 std::map<std::string, pcl::PointCloud<pcl::PointXYZ>::ConstPtr> mapOfPointclouds;
353 mapOfPointclouds[
"Camera2"] = empty_pointcloud;
355 mapOfPointclouds[
"Camera2"] = pointcloud;
358 tracker.track(mapOfImages, mapOfPointclouds);
365 std::cout <<
"iter: " << frame_cpt <<
" cMo:\n" << cMo << std::endl;
368 tracker.display(I_gray, I_depth, cMo, depth_M_color * cMo, cam_color, cam_depth,
vpColor::red, 3);
374 times_vec.push_back(t);
376 std::stringstream ss;
377 ss <<
"Computation time: " << t <<
" ms";
380 std::stringstream ss;
381 ss <<
"Nb features: " << tracker.getError().size();
385 std::stringstream ss;
386 ss <<
"Features: edges " << tracker.getNbFeaturesEdge() <<
", klt " << tracker.getNbFeaturesKlt() <<
", depth "
387 << tracker.getNbFeaturesDepthDense();
419 std::cout <<
"To run this tutorial, ViSP should be build with PCL library."
420 " Install libpcl, configure and build again ViSP..."
Generic class defining intrinsic camera parameters.
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
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="") 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 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)
unsigned int getWidth() const
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
unsigned int getHeight() const
Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented in OpenCV. Thus to enable this ...
void setBlockSize(int blockSize)
void setQuality(double qualityLevel)
void setHarrisFreeParameter(double harris_k)
void setMaxFeatures(int maxCount)
void setMinDistance(double minDistance)
void setWindowSize(int winSize)
void setPyramidLevels(int pyrMaxLevel)
static double rad(double deg)
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.
@ ROBUST_FEATURE_ESTIMATION
void setMu1(const double &mu_1)
void setRange(const unsigned int &range)
void setLikelihoodThresholdType(const vpLikelihoodThresholdType likelihood_threshold_type)
void setMaskNumber(const unsigned int &mask_number)
void setThreshold(const double &threshold)
void setSampleStep(const double &sample_step)
void setMaskSize(const unsigned int &mask_size)
void setMu2(const double &mu_2)
VISP_EXPORT double measureTimeMs()