41#include <visp3/core/vpConfig.h>
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))
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>
54void createDepthHist(std::vector<uint32_t>& histogram2,
const pcl::PointCloud<pcl::PointXYZ>::Ptr& pointcloud,
double depth_scale)
56 std::fill(histogram2.begin(), histogram2.end(), 0);
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)];
65 for (
int i = 2; i < 0x10000; i++)
66 histogram2[i] += histogram2[i - 1];
70void createDepthHist(std::vector<uint32_t>& histogram2,
const std::vector<vpColVector>& pointcloud,
double depth_scale)
72 std::fill(histogram2.begin(), histogram2.end(), 0);
74 for (
size_t i = 0; i < pointcloud.size(); i++) {
76 ++histogram2[
static_cast<uint32_t
>(pt[2] * depth_scale)];
79 for (
int i = 2; i < 0x10000; i++)
80 histogram2[i] += histogram2[i - 1];
84unsigned char getDepthColor(
const std::vector<uint32_t>& histogram2,
double z,
double depth_scale)
87 return static_cast<unsigned char>(histogram2[
static_cast<uint32_t
>(z*depth_scale)] * 255 / histogram2[0xFFFF]);
91int main(
int argc,
char *argv[])
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") {
104 }
else if (std::string(argv[i]) ==
"--no_align") {
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;
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);
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);
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");
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);
141 rs2::align align_to(align_to_depth ? RS2_STREAM_DEPTH : RS2_STREAM_COLOR);
145 if (color_pointcloud) {
146 rs.
acquire(
reinterpret_cast<unsigned char *
>(I_color.bitmap),
147 reinterpret_cast<unsigned char *
>(I_depth_raw.bitmap),
151 no_align ? NULL : &align_to);
153 rs.
acquire(
reinterpret_cast<unsigned char *
>(I_color.bitmap),
154 reinterpret_cast<unsigned char *
>(I_depth_raw.bitmap),
158 no_align ? NULL : &align_to);
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);
170 double x = pcl_pt.x / Z;
171 double y = pcl_pt.y / Z;
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);
184 createDepthHist(histogram, pointcloud, depth_scale);
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);
191 double x = pcl_pt.x / Z;
192 double y = pcl_pt.y / Z;
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);
208 createDepthHist(histogram2, vp_pointcloud, depth_scale);
209 for (
size_t i = 0; i < vp_pointcloud.size(); i++) {
213 double x = pt[0] / Z;
214 double y = pt[1] / Z;
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);
228 I_display.insert(I_depth,
vpImagePoint(I_color.getHeight(),0));
231 I_display2.insert(I_pcl,
vpImagePoint(I_color.getHeight(),0));
234 I_display3.insert(I_pcl2,
vpImagePoint(I_color.getHeight(),0));
240 const int nb_lines = 5;
241 for (
int i = 1; i < nb_lines; i++) {
242 const int col_idx = i*(width/nb_lines);
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
static const vpColor green
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 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 ...
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())