Visual Servoing Platform version 3.5.0
testRealSense2_D435_align.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5 *
6 * This software is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 * See the file LICENSE.txt at the root directory of this source
11 * distribution for additional information about the GNU GPL.
12 *
13 * For using ViSP with software that can not be combined with the GNU
14 * GPL, please contact Inria about acquiring a ViSP Professional
15 * Edition License.
16 *
17 * See http://visp.inria.fr for more information.
18 *
19 * This software was developed at:
20 * Inria Rennes - Bretagne Atlantique
21 * Campus Universitaire de Beaulieu
22 * 35042 Rennes Cedex
23 * France
24 *
25 * If you have questions regarding the use of this file, please contact
26 * Inria at visp@inria.fr
27 *
28 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30 *
31 * Description:
32 * Test Intel RealSense D435 acquisition with librealsense2.
33 *
34 *****************************************************************************/
40#include <iostream>
41#include <visp3/core/vpConfig.h>
42
43#if defined(VISP_HAVE_REALSENSE2) && defined(VISP_HAVE_PCL) && \
44 (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) && (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI))
45
46#include <visp3/core/vpImage.h>
47#include <visp3/core/vpImageConvert.h>
48#include <visp3/core/vpMeterPixelConversion.h>
49#include <visp3/gui/vpDisplayGDI.h>
50#include <visp3/gui/vpDisplayX.h>
51#include <visp3/sensor/vpRealSense2.h>
52
53namespace {
54void createDepthHist(std::vector<uint32_t>& histogram2, const pcl::PointCloud<pcl::PointXYZ>::Ptr& pointcloud, double depth_scale)
55{
56 std::fill(histogram2.begin(), histogram2.end(), 0);
57
58 for (uint32_t i = 0; i < pointcloud->height; i++) {
59 for (uint32_t j = 0; j < pointcloud->width; j++) {
60 const pcl::PointXYZ& pcl_pt = pointcloud->at(j, i);
61 ++histogram2[static_cast<uint32_t>(pcl_pt.z * depth_scale)];
62 }
63 }
64
65 for (int i = 2; i < 0x10000; i++)
66 histogram2[i] += histogram2[i - 1]; // Build a cumulative histogram for
67 // the indices in [1,0xFFFF]
68}
69
70void createDepthHist(std::vector<uint32_t>& histogram2, const std::vector<vpColVector>& pointcloud, double depth_scale)
71{
72 std::fill(histogram2.begin(), histogram2.end(), 0);
73
74 for (size_t i = 0; i < pointcloud.size(); i++) {
75 const vpColVector& pt = pointcloud[i];
76 ++histogram2[static_cast<uint32_t>(pt[2] * depth_scale)];
77 }
78
79 for (int i = 2; i < 0x10000; i++)
80 histogram2[i] += histogram2[i - 1]; // Build a cumulative histogram for
81 // the indices in [1,0xFFFF]
82}
83
84unsigned char getDepthColor(const std::vector<uint32_t>& histogram2, double z, double depth_scale)
85{
86 // 0-255 based on histogram location
87 return static_cast<unsigned char>(histogram2[static_cast<uint32_t>(z*depth_scale)] * 255 / histogram2[0xFFFF]);
88}
89}
90
91int main(int argc, char *argv[])
92{
93 bool align_to_depth = false;
94 bool color_pointcloud = false;
95 bool col_vector = false;
96 bool no_align = false;
97 for (int i = 1; i < argc; i++) {
98 if (std::string(argv[i]) == "--align_to_depth") {
99 align_to_depth = true;
100 } else if (std::string(argv[i]) == "--color") {
101 color_pointcloud = true;
102 } else if (std::string(argv[i]) == "--col_vector") {
103 col_vector = true;
104 } else if (std::string(argv[i]) == "--no_align") {
105 no_align = true;
106 }
107 }
108
109 std::cout << "align_to_depth: " << align_to_depth << std::endl;
110 std::cout << "color_pointcloud: " << color_pointcloud << std::endl;
111 std::cout << "col_vector: " << col_vector << std::endl;
112 std::cout << "no_align: " << no_align << std::endl;
113
114 vpRealSense2 rs;
115 rs2::config config;
116 const int width = 640, height = 480, fps = 30;
117 config.enable_stream(RS2_STREAM_COLOR, width, height, RS2_FORMAT_RGBA8, fps);
118 config.enable_stream(RS2_STREAM_DEPTH, width, height, RS2_FORMAT_Z16, fps);
119 config.enable_stream(RS2_STREAM_INFRARED, width, height, RS2_FORMAT_Y8, fps);
120 rs.open(config);
121 const double depth_scale = 1.0 / rs.getDepthScale();
122
123 vpImage<vpRGBa> I_color(height, width), I_depth(height, width), I_pcl(height, width), I_pcl2(height, width);
124 vpImage<vpRGBa> I_display(height*2, width), I_display2(height*2, width), I_display3(height*2, width);
125 vpImage<uint16_t> I_depth_raw(height, width);
126
127#ifdef VISP_HAVE_X11
128 vpDisplayX d1, d2, d3;
129#else
130 vpDisplayGDI d1, d2, d3;
131#endif
132 d1.init(I_display, 0, 0, "Color + depth");
133 d2.init(I_display2, width, 0, "Color + ROS pointcloud");
134 d3.init(I_display3, 2*width, 0, "Color + pointcloud");
135
136 pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud(new pcl::PointCloud<pcl::PointXYZ>());
137 pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointcloud_color(new pcl::PointCloud<pcl::PointXYZRGB>());
138 std::vector<vpColVector> vp_pointcloud;
139 std::vector<uint32_t> histogram(0x10000), histogram2(0x10000);
140
141 rs2::align align_to(align_to_depth ? RS2_STREAM_DEPTH : RS2_STREAM_COLOR);
142 vpCameraParameters cam_projection = align_to_depth ? rs.getCameraParameters(RS2_STREAM_DEPTH) : rs.getCameraParameters(RS2_STREAM_COLOR);
143
144 while(true) {
145 if (color_pointcloud) {
146 rs.acquire(reinterpret_cast<unsigned char *>(I_color.bitmap),
147 reinterpret_cast<unsigned char *>(I_depth_raw.bitmap),
148 &vp_pointcloud,
149 pointcloud_color,
150 NULL,
151 no_align ? NULL : &align_to);
152 } else {
153 rs.acquire(reinterpret_cast<unsigned char *>(I_color.bitmap),
154 reinterpret_cast<unsigned char *>(I_depth_raw.bitmap),
155 &vp_pointcloud,
156 pointcloud,
157 NULL,
158 no_align ? NULL : &align_to);
159 }
160
161 vpImageConvert::createDepthHistogram(I_depth_raw, I_depth);
162
163 I_pcl = vpRGBa(0,0,0);
164 if (color_pointcloud) {
165 for (uint32_t i = 0; i < pointcloud_color->height; i++) {
166 for (uint32_t j = 0; j < pointcloud_color->width; j++) {
167 const pcl::PointXYZRGB& pcl_pt = pointcloud_color->at(j, i);
168 double Z = pcl_pt.z;
169 if (Z > 1e-2) {
170 double x = pcl_pt.x / Z;
171 double y = pcl_pt.y / Z;
172
173 vpImagePoint imPt;
174 vpMeterPixelConversion::convertPoint(cam_projection, x, y, imPt);
175 unsigned int u = std::min(static_cast<unsigned int>(width-1),
176 static_cast<unsigned int>(std::max(0.0, imPt.get_u())));
177 unsigned int v = std::min(static_cast<unsigned int>(height-1),
178 static_cast<unsigned int>(std::max(0.0, imPt.get_v())));
179 I_pcl[v][u] = vpRGBa(pcl_pt.r, pcl_pt.g, pcl_pt.b);
180 }
181 }
182 }
183 } else {
184 createDepthHist(histogram, pointcloud, depth_scale);
185
186 for (uint32_t i = 0; i < pointcloud->height; i++) {
187 for (uint32_t j = 0; j < pointcloud->width; j++) {
188 const pcl::PointXYZ& pcl_pt = pointcloud->at(j, i);
189 double Z = pcl_pt.z;
190 if (Z > 1e-2) {
191 double x = pcl_pt.x / Z;
192 double y = pcl_pt.y / Z;
193
194 vpImagePoint imPt;
195 vpMeterPixelConversion::convertPoint(cam_projection, x, y, imPt);
196 unsigned int u = std::min(static_cast<unsigned int>(width-1),
197 static_cast<unsigned int>(std::max(0.0, imPt.get_u())));
198 unsigned int v = std::min(static_cast<unsigned int>(height-1),
199 static_cast<unsigned int>(std::max(0.0, imPt.get_v())));
200 unsigned char depth_viz = getDepthColor(histogram, pcl_pt.z, depth_scale);
201 I_pcl[v][u] = vpRGBa(depth_viz, depth_viz, depth_viz);
202 }
203 }
204 }
205 }
206
207 I_pcl2 = vpRGBa(0,0,0);
208 createDepthHist(histogram2, vp_pointcloud, depth_scale);
209 for (size_t i = 0; i < vp_pointcloud.size(); i++) {
210 const vpColVector& pt = vp_pointcloud[i];
211 double Z = pt[2];
212 if (Z > 1e-2) {
213 double x = pt[0] / Z;
214 double y = pt[1] / Z;
215
216 vpImagePoint imPt;
217 vpMeterPixelConversion::convertPoint(cam_projection, x, y, imPt);
218 unsigned int u = std::min(static_cast<unsigned int>(width-1),
219 static_cast<unsigned int>(std::max(0.0, imPt.get_u())));
220 unsigned int v = std::min(static_cast<unsigned int>(height-1),
221 static_cast<unsigned int>(std::max(0.0, imPt.get_v())));
222 unsigned char depth_viz = getDepthColor(histogram2, Z, depth_scale);
223 I_pcl2[v][u] = vpRGBa(depth_viz, depth_viz, depth_viz);
224 }
225 }
226
227 I_display.insert(I_color, vpImagePoint(0,0));
228 I_display.insert(I_depth, vpImagePoint(I_color.getHeight(),0));
229
230 I_display2.insert(I_color, vpImagePoint(0,0));
231 I_display2.insert(I_pcl, vpImagePoint(I_color.getHeight(),0));
232
233 I_display3.insert(I_color, vpImagePoint(0,0));
234 I_display3.insert(I_pcl2, vpImagePoint(I_color.getHeight(),0));
235
236 vpDisplay::display(I_display);
237 vpDisplay::display(I_display2);
238 vpDisplay::display(I_display3);
239
240 const int nb_lines = 5;
241 for (int i = 1; i < nb_lines; i++) {
242 const int col_idx = i*(width/nb_lines);
243 vpDisplay::displayLine(I_display, 0, col_idx, I_display.getRows()-1, col_idx, vpColor::green, 2);
244 vpDisplay::displayLine(I_display2, 0, col_idx, I_display.getRows()-1, col_idx, vpColor::green, 2);
245 vpDisplay::displayLine(I_display3, 0, col_idx, I_display.getRows()-1, col_idx, vpColor::green, 2);
246 }
247
248 vpDisplay::flush(I_display);
249 vpDisplay::flush(I_display2);
250 vpDisplay::flush(I_display3);
251
252 if (vpDisplay::getClick(I_display, false) ||
253 vpDisplay::getClick(I_display2, false) ||
254 vpDisplay::getClick(I_display3, false)) {
255 break;
256 }
257 }
258
259 return EXIT_SUCCESS;
260}
261
262#else
263int main()
264{
265 return 0;
266}
267#endif
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
static const vpColor green
Definition: vpColor.h:220
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:129
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:135
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 displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
static void flush(const vpImage< unsigned char > &I)
static void createDepthHistogram(const vpImage< uint16_t > &src_depth, vpImage< vpRGBa > &dest_rgba)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:88
double get_u() const
Definition: vpImagePoint.h:262
double get_v() const
Definition: vpImagePoint.h:273
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
Definition: vpRGBa.h:67
void acquire(vpImage< unsigned char > &grey, double *ts=NULL)
vpCameraParameters getCameraParameters(const rs2_stream &stream, vpCameraParameters::vpCameraParametersProjType type=vpCameraParameters::perspectiveProjWithDistortion, int index=-1) const
bool open(const rs2::config &cfg=rs2::config())
float getDepthScale()