Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
testRealSense2_D435_align.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2023 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 https://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) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) && \
44 (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
54{
55void createDepthHist(std::vector<uint32_t> &histogram2, const pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud,
56 double depth_scale)
57{
58 std::fill(histogram2.begin(), histogram2.end(), 0);
59
60 for (uint32_t i = 0; i < pointcloud->height; i++) {
61 for (uint32_t j = 0; j < pointcloud->width; j++) {
62 const pcl::PointXYZ &pcl_pt = pointcloud->at(j, i);
63 ++histogram2[static_cast<uint32_t>(pcl_pt.z * depth_scale)];
64 }
65 }
66
67 for (int i = 2; i < 0x10000; i++)
68 histogram2[i] += histogram2[i - 1]; // Build a cumulative histogram for
69 // the indices in [1,0xFFFF]
70}
71
72void createDepthHist(std::vector<uint32_t> &histogram2, const std::vector<vpColVector> &pointcloud, double depth_scale)
73{
74 std::fill(histogram2.begin(), histogram2.end(), 0);
75
76 for (size_t i = 0; i < pointcloud.size(); i++) {
77 const vpColVector &pt = pointcloud[i];
78 ++histogram2[static_cast<uint32_t>(pt[2] * depth_scale)];
79 }
80
81 for (int i = 2; i < 0x10000; i++)
82 histogram2[i] += histogram2[i - 1]; // Build a cumulative histogram for
83 // the indices in [1,0xFFFF]
84}
85
86unsigned char getDepthColor(const std::vector<uint32_t> &histogram2, double z, double depth_scale)
87{
88 // 0-255 based on histogram location
89 return static_cast<unsigned char>(histogram2[static_cast<uint32_t>(z * depth_scale)] * 255 / histogram2[0xFFFF]);
90}
91} // namespace
92
93int main(int argc, char *argv[])
94{
95 bool align_to_depth = false;
96 bool color_pointcloud = false;
97 bool col_vector = false;
98 bool no_align = false;
99 for (int i = 1; i < argc; i++) {
100 if (std::string(argv[i]) == "--align_to_depth") {
101 align_to_depth = true;
102 } else if (std::string(argv[i]) == "--color") {
103 color_pointcloud = true;
104 } else if (std::string(argv[i]) == "--col_vector") {
105 col_vector = true;
106 } else if (std::string(argv[i]) == "--no_align") {
107 no_align = true;
108 }
109 }
110
111 std::cout << "align_to_depth: " << align_to_depth << std::endl;
112 std::cout << "color_pointcloud: " << color_pointcloud << std::endl;
113 std::cout << "col_vector: " << col_vector << std::endl;
114 std::cout << "no_align: " << no_align << std::endl;
115
116 vpRealSense2 rs;
117 rs2::config config;
118 const int width = 640, height = 480, fps = 30;
119 config.enable_stream(RS2_STREAM_COLOR, width, height, RS2_FORMAT_RGBA8, fps);
120 config.enable_stream(RS2_STREAM_DEPTH, width, height, RS2_FORMAT_Z16, fps);
121 config.enable_stream(RS2_STREAM_INFRARED, width, height, RS2_FORMAT_Y8, fps);
122 rs.open(config);
123 const double depth_scale = 1.0 / rs.getDepthScale();
124
125 vpImage<vpRGBa> I_color(height, width), I_depth(height, width), I_pcl(height, width), I_pcl2(height, width);
126 vpImage<vpRGBa> I_display(height * 2, width), I_display2(height * 2, width), I_display3(height * 2, width);
127 vpImage<uint16_t> I_depth_raw(height, width);
128
129#ifdef VISP_HAVE_X11
130 vpDisplayX d1, d2, d3;
131#else
132 vpDisplayGDI d1, d2, d3;
133#endif
134 d1.init(I_display, 0, 0, "Color + depth");
135 d2.init(I_display2, width, 0, "Color + ROS pointcloud");
136 d3.init(I_display3, 2 * width, 0, "Color + pointcloud");
137
138 pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud(new pcl::PointCloud<pcl::PointXYZ>());
139 pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointcloud_color(new pcl::PointCloud<pcl::PointXYZRGB>());
140 std::vector<vpColVector> vp_pointcloud;
141 std::vector<uint32_t> histogram(0x10000), histogram2(0x10000);
142
143 rs2::align align_to(align_to_depth ? RS2_STREAM_DEPTH : RS2_STREAM_COLOR);
144 vpCameraParameters cam_projection =
145 align_to_depth ? rs.getCameraParameters(RS2_STREAM_DEPTH) : rs.getCameraParameters(RS2_STREAM_COLOR);
146
147 while (true) {
148 if (color_pointcloud) {
149 rs.acquire(reinterpret_cast<unsigned char *>(I_color.bitmap),
150 reinterpret_cast<unsigned char *>(I_depth_raw.bitmap), &vp_pointcloud, pointcloud_color, 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), &vp_pointcloud, pointcloud, NULL,
155 no_align ? NULL : &align_to);
156 }
157
158 vpImageConvert::createDepthHistogram(I_depth_raw, I_depth);
159
160 I_pcl = vpRGBa(0, 0, 0);
161 if (color_pointcloud) {
162 for (uint32_t i = 0; i < pointcloud_color->height; i++) {
163 for (uint32_t j = 0; j < pointcloud_color->width; j++) {
164 const pcl::PointXYZRGB &pcl_pt = pointcloud_color->at(j, i);
165 double Z = pcl_pt.z;
166 if (Z > 1e-2) {
167 double x = pcl_pt.x / Z;
168 double y = pcl_pt.y / Z;
169
170 vpImagePoint imPt;
171 vpMeterPixelConversion::convertPoint(cam_projection, x, y, imPt);
172 unsigned int u =
173 std::min(static_cast<unsigned int>(width - 1), static_cast<unsigned int>(std::max(0.0, imPt.get_u())));
174 unsigned int v =
175 std::min(static_cast<unsigned int>(height - 1), static_cast<unsigned int>(std::max(0.0, imPt.get_v())));
176 I_pcl[v][u] = vpRGBa(pcl_pt.r, pcl_pt.g, pcl_pt.b);
177 }
178 }
179 }
180 } else {
181 createDepthHist(histogram, pointcloud, depth_scale);
182
183 for (uint32_t i = 0; i < pointcloud->height; i++) {
184 for (uint32_t j = 0; j < pointcloud->width; j++) {
185 const pcl::PointXYZ &pcl_pt = pointcloud->at(j, i);
186 double Z = pcl_pt.z;
187 if (Z > 1e-2) {
188 double x = pcl_pt.x / Z;
189 double y = pcl_pt.y / Z;
190
191 vpImagePoint imPt;
192 vpMeterPixelConversion::convertPoint(cam_projection, x, y, imPt);
193 unsigned int u =
194 std::min(static_cast<unsigned int>(width - 1), static_cast<unsigned int>(std::max(0.0, imPt.get_u())));
195 unsigned int v =
196 std::min(static_cast<unsigned int>(height - 1), static_cast<unsigned int>(std::max(0.0, imPt.get_v())));
197 unsigned char depth_viz = getDepthColor(histogram, pcl_pt.z, depth_scale);
198 I_pcl[v][u] = vpRGBa(depth_viz, depth_viz, depth_viz);
199 }
200 }
201 }
202 }
203
204 I_pcl2 = vpRGBa(0, 0, 0);
205 createDepthHist(histogram2, vp_pointcloud, depth_scale);
206 for (size_t i = 0; i < vp_pointcloud.size(); i++) {
207 const vpColVector &pt = vp_pointcloud[i];
208 double Z = pt[2];
209 if (Z > 1e-2) {
210 double x = pt[0] / Z;
211 double y = pt[1] / Z;
212
213 vpImagePoint imPt;
214 vpMeterPixelConversion::convertPoint(cam_projection, x, y, imPt);
215 unsigned int u =
216 std::min(static_cast<unsigned int>(width - 1), static_cast<unsigned int>(std::max(0.0, imPt.get_u())));
217 unsigned int v =
218 std::min(static_cast<unsigned int>(height - 1), static_cast<unsigned int>(std::max(0.0, imPt.get_v())));
219 unsigned char depth_viz = getDepthColor(histogram2, Z, depth_scale);
220 I_pcl2[v][u] = vpRGBa(depth_viz, depth_viz, depth_viz);
221 }
222 }
223
224 I_display.insert(I_color, vpImagePoint(0, 0));
225 I_display.insert(I_depth, vpImagePoint(I_color.getHeight(), 0));
226
227 I_display2.insert(I_color, vpImagePoint(0, 0));
228 I_display2.insert(I_pcl, vpImagePoint(I_color.getHeight(), 0));
229
230 I_display3.insert(I_color, vpImagePoint(0, 0));
231 I_display3.insert(I_pcl2, vpImagePoint(I_color.getHeight(), 0));
232
233 vpDisplay::display(I_display);
234 vpDisplay::display(I_display2);
235 vpDisplay::display(I_display3);
236
237 const int nb_lines = 5;
238 for (int i = 1; i < nb_lines; i++) {
239 const int col_idx = i * (width / nb_lines);
240 vpDisplay::displayLine(I_display, 0, col_idx, I_display.getRows() - 1, col_idx, vpColor::green, 2);
241 vpDisplay::displayLine(I_display2, 0, col_idx, I_display.getRows() - 1, col_idx, vpColor::green, 2);
242 vpDisplay::displayLine(I_display3, 0, col_idx, I_display.getRows() - 1, col_idx, vpColor::green, 2);
243 }
244
245 vpDisplay::flush(I_display);
246 vpDisplay::flush(I_display2);
247 vpDisplay::flush(I_display3);
248
249 if (vpDisplay::getClick(I_display, false) || vpDisplay::getClick(I_display2, false) ||
250 vpDisplay::getClick(I_display3, false)) {
251 break;
252 }
253 }
254
255 return EXIT_SUCCESS;
256}
257
258#else
259int main() { return EXIT_SUCCESS; }
260#endif
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
static const vpColor green
Definition vpColor.h:214
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...
Definition vpDisplayX.h:132
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 ...
double get_u() const
double get_v() const
Definition of the vpImage class member functions.
Definition vpImage.h:135
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
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()