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) && defined(VISP_HAVE_PCL_COMMON)
14 #include <pcl/common/common.h>
15 #include <pcl/point_cloud.h>
16 #include <pcl/point_types.h>
20 struct vpRealsenseIntrinsics_t
33 void rs_deproject_pixel_to_point(
float point[3],
const vpRealsenseIntrinsics_t &intrin,
const float pixel[2],
float depth)
35 float x = (pixel[0] - intrin.ppx) / intrin.fx;
36 float y = (pixel[1] - intrin.ppy) / intrin.fy;
38 float r2 = x * x + y * y;
39 float f = 1 + intrin.coeffs[0] * r2 + intrin.coeffs[1] * r2 * r2 + intrin.coeffs[4] * r2 * r2 * r2;
40 float ux = x * f + 2 * intrin.coeffs[2] * x * y + intrin.coeffs[3] * (r2 + 2 * x * x);
41 float uy = y * f + 2 * intrin.coeffs[3] * x * y + intrin.coeffs[2] * (r2 + 2 * y * y);
52 bool read_data(
unsigned int cpt,
const std::string &input_directory,
vpImage<vpRGBa> &I_color,
56 char buffer[FILENAME_MAX];
59 ss << input_directory <<
"/color_image_%04d.jpg";
60 snprintf(buffer, FILENAME_MAX, ss.str().c_str(), cpt);
61 std::string filename_color = buffer;
64 std::cerr <<
"Cannot read: " << filename_color << std::endl;
71 ss << input_directory <<
"/depth_image_%04d.bin";
72 snprintf(buffer, FILENAME_MAX, ss.str().c_str(), cpt);
73 std::string filename_depth = buffer;
75 std::ifstream file_depth(filename_depth.c_str(), std::ios::in | std::ios::binary);
76 if (!file_depth.is_open()) {
80 unsigned int height = 0, width = 0;
83 I_depth_raw.
resize(height, width);
85 uint16_t depth_value = 0;
86 for (
unsigned int i = 0; i < height; i++) {
87 for (
unsigned int j = 0; j < width; j++) {
89 I_depth_raw[i][j] = depth_value;
94 pointcloud->width = width;
95 pointcloud->height = height;
96 pointcloud->resize((
size_t)width * height);
99 const float depth_scale = 0.00100000005f;
100 vpRealsenseIntrinsics_t depth_intrinsic;
101 depth_intrinsic.ppx = 320.503509521484f;
102 depth_intrinsic.ppy = 235.602951049805f;
103 depth_intrinsic.fx = 383.970001220703f;
104 depth_intrinsic.fy = 383.970001220703f;
105 depth_intrinsic.coeffs[0] = 0.0f;
106 depth_intrinsic.coeffs[1] = 0.0f;
107 depth_intrinsic.coeffs[2] = 0.0f;
108 depth_intrinsic.coeffs[3] = 0.0f;
109 depth_intrinsic.coeffs[4] = 0.0f;
111 for (
unsigned int i = 0; i < height; i++) {
112 for (
unsigned int j = 0; j < width; j++) {
113 float scaled_depth = I_depth_raw[i][j] * depth_scale;
115 float pixel[2] = { (float)j, (
float)i };
116 rs_deproject_pixel_to_point(point, depth_intrinsic, pixel, scaled_depth);
117 pointcloud->points[(size_t)(i * width + j)].x = point[0];
118 pointcloud->points[(size_t)(i * width + j)].y = point[1];
119 pointcloud->points[(size_t)(i * width + j)].z = point[2];
127 int main(
int argc,
char *argv[])
129 std::string input_directory =
"data";
130 std::string config_color =
"model/cube/cube.xml", config_depth =
"model/cube/cube_depth.xml";
131 std::string model_color =
"model/cube/cube.cao", model_depth =
"model/cube/cube.cao";
132 std::string init_file =
"model/cube/cube.init";
133 unsigned int frame_cpt = 0;
134 bool disable_depth =
false;
136 for (
int i = 1; i < argc; i++) {
137 if (std::string(argv[i]) ==
"--input_directory" && i + 1 < argc) {
138 input_directory = std::string(argv[i + 1]);
140 else if (std::string(argv[i]) ==
"--config_color" && i + 1 < argc) {
141 config_color = std::string(argv[i + 1]);
143 else if (std::string(argv[i]) ==
"--config_depth" && i + 1 < argc) {
144 config_depth = std::string(argv[i + 1]);
146 else if (std::string(argv[i]) ==
"--model_color" && i + 1 < argc) {
147 model_color = std::string(argv[i + 1]);
149 else if (std::string(argv[i]) ==
"--model_depth" && i + 1 < argc) {
150 model_depth = std::string(argv[i + 1]);
152 else if (std::string(argv[i]) ==
"--init_file" && i + 1 < argc) {
153 init_file = std::string(argv[i + 1]);
155 else if (std::string(argv[i]) ==
"--disable_depth") {
156 disable_depth =
true;
158 else if (std::string(argv[i]) ==
"--help" || std::string(argv[i]) ==
"-h") {
159 std::cout <<
"Usage: \n"
161 <<
" --input_directory <data directory> --config_color <object.xml> --config_depth <object.xml>"
162 " --model_color <object.cao> --model_depth <object.cao> --init_file <object.init> --disable_depth"
167 <<
" --config_color model/cube/cube.xml --config_depth model/cube/cube.xml"
168 " --model_color model/cube/cube.cao --model_depth model/cube/cube.cao --init_file model/cube/cube.init\n"
174 std::cout <<
"Tracked features: " << std::endl;
175 #if defined(VISP_HAVE_OPENCV)
176 std::cout <<
" Use edges : 1" << std::endl;
177 #if defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
178 std::cout <<
" Use klt : 1" << std::endl;
180 std::cout <<
" Use klt : 0" << std::endl;
182 std::cout <<
" Use depth : " << !disable_depth << std::endl;
184 std::cout <<
" Use edges : 1" << std::endl;
185 std::cout <<
" Use klt : 0" << std::endl;
186 std::cout <<
" Use depth : 0" << std::endl;
188 std::cout <<
"Config files: " << std::endl;
189 std::cout <<
" Input directory: "
190 <<
"\"" << input_directory <<
"\"" << std::endl;
191 std::cout <<
" Config color: "
192 <<
"\"" << config_color <<
"\"" << std::endl;
193 std::cout <<
" Config depth: "
194 <<
"\"" << config_depth <<
"\"" << std::endl;
195 std::cout <<
" Model color : "
196 <<
"\"" << model_color <<
"\"" << std::endl;
197 std::cout <<
" Model depth : "
198 <<
"\"" << model_depth <<
"\"" << std::endl;
199 std::cout <<
" Init file : "
200 <<
"\"" << init_file <<
"\"" << std::endl;
208 pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud(
new pcl::PointCloud<pcl::PointXYZ>());
212 read_data(frame_cpt, input_directory, I_color, I_depth_raw, pointcloud);
216 #if defined(VISP_HAVE_X11)
218 #elif defined(VISP_HAVE_GDI)
222 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)
223 unsigned int _posx = 100, _posy = 50, _posdx = 10;
224 d1.
init(I_gray, _posx, _posy,
"Color stream");
225 d2.
init(I_depth, _posx + I_gray.
getWidth() + _posdx, _posy,
"Depth stream");
234 std::vector<int> trackerTypes;
235 #if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
243 #if defined(VISP_HAVE_PUGIXML)
245 tracker.loadConfigFile(config_color, config_depth);
252 tracker.setCameraParameters(cam_color, cam_depth);
265 tracker.setMovingEdge(me);
268 #if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
270 tracker.setKltMaskBorder(5);
279 tracker.setKltOpencv(klt);
284 tracker.setDepthNormalPclPlaneEstimationMethod(2);
285 tracker.setDepthNormalPclPlaneEstimationRansacMaxIter(200);
286 tracker.setDepthNormalPclPlaneEstimationRansacThreshold(0.001);
287 tracker.setDepthNormalSamplingStep(2, 2);
289 tracker.setDepthDenseSamplingStep(4, 4);
293 tracker.setNearClippingDistance(0.001);
294 tracker.setFarClippingDistance(5.0);
298 tracker.loadModel(model_color, model_depth);
301 tracker.getCameraParameters(cam_color, cam_depth);
303 std::cout <<
"Camera parameters for color camera (from XML file): " << cam_color << std::endl;
304 std::cout <<
"Camera parameters for depth camera (from XML file): " << cam_depth << std::endl;
307 tracker.setDisplayFeatures(
true);
313 std::ifstream file((std::string(input_directory +
"/depth_M_color.txt")).c_str());
314 depth_M_color.
load(file);
317 std::map<std::string, vpHomogeneousMatrix> mapOfCameraTransformations;
319 mapOfCameraTransformations[
"Camera2"] = depth_M_color;
320 tracker.setCameraTransformationMatrix(mapOfCameraTransformations);
322 std::cout <<
"depth_M_color: \n" << depth_M_color << std::endl;
325 std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
326 mapOfImages[
"Camera1"] = &I_gray;
327 mapOfImages[
"Camera2"] = &I_depth;
331 std::map<std::string, std::string> mapOfInitFiles;
332 mapOfInitFiles[
"Camera1"] = init_file;
333 tracker.initClick(mapOfImages, mapOfInitFiles,
true);
337 pcl::PointCloud<pcl::PointXYZ>::Ptr empty_pointcloud(
new pcl::PointCloud<pcl::PointXYZ>);
338 std::vector<double> times_vec;
344 quit = !read_data(frame_cpt, input_directory, I_color, I_depth_raw, pointcloud);
351 mapOfImages[
"Camera1"] = &I_gray;
352 std::map<std::string, pcl::PointCloud<pcl::PointXYZ>::ConstPtr> mapOfPointclouds;
354 mapOfPointclouds[
"Camera2"] = empty_pointcloud;
356 mapOfPointclouds[
"Camera2"] = pointcloud;
359 tracker.track(mapOfImages, mapOfPointclouds);
366 std::cout <<
"iter: " << frame_cpt <<
" cMo:\n" << cMo << std::endl;
369 tracker.display(I_gray, I_depth, cMo, depth_M_color * cMo, cam_color, cam_depth,
vpColor::red, 3);
375 times_vec.push_back(t);
377 std::stringstream ss;
378 ss <<
"Computation time: " << t <<
" ms";
381 std::stringstream ss;
382 ss <<
"Nb features: " << tracker.getError().size();
386 std::stringstream ss;
387 ss <<
"Features: edges " << tracker.getNbFeaturesEdge() <<
", klt " << tracker.getNbFeaturesKlt() <<
", depth "
388 << tracker.getNbFeaturesDepthDense();
420 std::cout <<
"To run this tutorial, ViSP should be build with PCL library."
421 " 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()