41#include <visp3/core/vpConfig.h>
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))
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>
55void createDepthHist(std::vector<uint32_t> &histogram2,
const pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud,
58 std::fill(histogram2.begin(), histogram2.end(), 0);
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)];
67 for (
int i = 2; i < 0x10000; i++)
68 histogram2[i] += histogram2[i - 1];
72void createDepthHist(std::vector<uint32_t> &histogram2,
const std::vector<vpColVector> &pointcloud,
double depth_scale)
74 std::fill(histogram2.begin(), histogram2.end(), 0);
76 for (
size_t i = 0; i < pointcloud.size(); i++) {
78 ++histogram2[
static_cast<uint32_t
>(pt[2] * depth_scale)];
81 for (
int i = 2; i < 0x10000; i++)
82 histogram2[i] += histogram2[i - 1];
86unsigned char getDepthColor(
const std::vector<uint32_t> &histogram2,
double z,
double depth_scale)
89 return static_cast<unsigned char>(histogram2[
static_cast<uint32_t
>(z * depth_scale)] * 255 / histogram2[0xFFFF]);
93int main(
int argc,
char *argv[])
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") {
106 }
else if (std::string(argv[i]) ==
"--no_align") {
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;
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);
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);
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");
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);
143 rs2::align align_to(align_to_depth ? RS2_STREAM_DEPTH : RS2_STREAM_COLOR);
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);
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);
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);
167 double x = pcl_pt.x / Z;
168 double y = pcl_pt.y / Z;
173 std::min(
static_cast<unsigned int>(width - 1),
static_cast<unsigned int>(std::max(0.0, imPt.
get_u())));
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);
181 createDepthHist(histogram, pointcloud, depth_scale);
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);
188 double x = pcl_pt.x / Z;
189 double y = pcl_pt.y / Z;
194 std::min(
static_cast<unsigned int>(width - 1),
static_cast<unsigned int>(std::max(0.0, imPt.
get_u())));
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);
205 createDepthHist(histogram2, vp_pointcloud, depth_scale);
206 for (
size_t i = 0; i < vp_pointcloud.size(); i++) {
210 double x = pt[0] / Z;
211 double y = pt[1] / Z;
216 std::min(
static_cast<unsigned int>(width - 1),
static_cast<unsigned int>(std::max(0.0, imPt.
get_u())));
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);
225 I_display.insert(I_depth,
vpImagePoint(I_color.getHeight(), 0));
228 I_display2.insert(I_pcl,
vpImagePoint(I_color.getHeight(), 0));
231 I_display3.insert(I_pcl2,
vpImagePoint(I_color.getHeight(), 0));
237 const int nb_lines = 5;
238 for (
int i = 1; i < nb_lines; i++) {
239 const int col_idx = i * (width / nb_lines);
259int main() {
return EXIT_SUCCESS; }
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 ...
Definition of the vpImage class member functions.
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())