Visual Servoing Platform  version 3.6.1 under development (2024-12-04)
testRealSense2_D435_align.cpp

Test Intel RealSense D435 acquisition with librealsense2.

/*
* ViSP, open source Visual Servoing Platform software.
* Copyright (C) 2005 - 2024 by Inria. All rights reserved.
*
* This software is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
* For using ViSP with software that can not be combined with the GNU
* GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
* See https://visp.inria.fr for more information.
*
* This software was developed at:
* Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
*
* If you have questions regarding the use of this file, please contact
* Inria at visp@inria.fr
*
* This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
* WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
*
* Description:
* Test Intel RealSense D435 acquisition with librealsense2.
*/
#include <iostream>
#include <visp3/core/vpConfig.h>
#if defined(VISP_HAVE_REALSENSE2) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) \
&& defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) && (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI))
#include <visp3/core/vpImage.h>
#include <visp3/core/vpImageConvert.h>
#include <visp3/core/vpMeterPixelConversion.h>
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayX.h>
#include <visp3/sensor/vpRealSense2.h>
#ifdef ENABLE_VISP_NAMESPACE
using namespace VISP_NAMESPACE_NAME;
#endif
namespace
{
void createDepthHist(std::vector<uint32_t> &histogram2, const pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud,
double depth_scale)
{
std::fill(histogram2.begin(), histogram2.end(), 0);
for (uint32_t i = 0; i < pointcloud->height; i++) {
for (uint32_t j = 0; j < pointcloud->width; j++) {
const pcl::PointXYZ &pcl_pt = pointcloud->at(j, i);
++histogram2[static_cast<uint32_t>(pcl_pt.z * depth_scale)];
}
}
for (int i = 2; i < 0x10000; i++)
histogram2[i] += histogram2[i - 1]; // Build a cumulative histogram for
// the indices in [1,0xFFFF]
}
void createDepthHist(std::vector<uint32_t> &histogram2, const std::vector<vpColVector> &pointcloud, double depth_scale)
{
std::fill(histogram2.begin(), histogram2.end(), 0);
for (size_t i = 0; i < pointcloud.size(); i++) {
const vpColVector &pt = pointcloud[i];
++histogram2[static_cast<uint32_t>(pt[2] * depth_scale)];
}
for (int i = 2; i < 0x10000; i++)
histogram2[i] += histogram2[i - 1]; // Build a cumulative histogram for
// the indices in [1,0xFFFF]
}
unsigned char getDepthColor(const std::vector<uint32_t> &histogram2, double z, double depth_scale)
{
// 0-255 based on histogram location
return static_cast<unsigned char>(histogram2[static_cast<uint32_t>(z * depth_scale)] * 255 / histogram2[0xFFFF]);
}
} // namespace
int main(int argc, char *argv[])
{
bool align_to_depth = false;
bool color_pointcloud = false;
bool col_vector = false;
bool no_align = false;
for (int i = 1; i < argc; i++) {
if (std::string(argv[i]) == "--align_to_depth") {
align_to_depth = true;
}
else if (std::string(argv[i]) == "--color") {
color_pointcloud = true;
}
else if (std::string(argv[i]) == "--col_vector") {
col_vector = true;
}
else if (std::string(argv[i]) == "--no_align") {
no_align = true;
}
}
std::cout << "align_to_depth: " << align_to_depth << std::endl;
std::cout << "color_pointcloud: " << color_pointcloud << std::endl;
std::cout << "col_vector: " << col_vector << std::endl;
std::cout << "no_align: " << no_align << std::endl;
rs2::config config;
const int width = 640, height = 480, fps = 30;
config.enable_stream(RS2_STREAM_COLOR, width, height, RS2_FORMAT_RGBA8, fps);
config.enable_stream(RS2_STREAM_DEPTH, width, height, RS2_FORMAT_Z16, fps);
config.enable_stream(RS2_STREAM_INFRARED, width, height, RS2_FORMAT_Y8, fps);
rs.open(config);
const double depth_scale = 1.0 / rs.getDepthScale();
vpImage<vpRGBa> I_color(height, width), I_depth(height, width), I_pcl(height, width), I_pcl2(height, width);
vpImage<vpRGBa> I_display(height * 2, width), I_display2(height * 2, width), I_display3(height * 2, width);
vpImage<uint16_t> I_depth_raw(height, width);
#ifdef VISP_HAVE_X11
vpDisplayX d1, d2, d3;
#else
vpDisplayGDI d1, d2, d3;
#endif
d1.init(I_display, 0, 0, "Color + depth");
d2.init(I_display2, width, 0, "Color + ROS pointcloud");
d3.init(I_display3, 2 * width, 0, "Color + pointcloud");
pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud(new pcl::PointCloud<pcl::PointXYZ>());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointcloud_color(new pcl::PointCloud<pcl::PointXYZRGB>());
std::vector<vpColVector> vp_pointcloud;
std::vector<uint32_t> histogram(0x10000), histogram2(0x10000);
rs2::align align_to(align_to_depth ? RS2_STREAM_DEPTH : RS2_STREAM_COLOR);
vpCameraParameters cam_projection =
align_to_depth ? rs.getCameraParameters(RS2_STREAM_DEPTH) : rs.getCameraParameters(RS2_STREAM_COLOR);
while (true) {
if (color_pointcloud) {
rs.acquire(reinterpret_cast<unsigned char *>(I_color.bitmap),
reinterpret_cast<unsigned char *>(I_depth_raw.bitmap), &vp_pointcloud, pointcloud_color, nullptr,
no_align ? nullptr : &align_to);
}
else {
rs.acquire(reinterpret_cast<unsigned char *>(I_color.bitmap),
reinterpret_cast<unsigned char *>(I_depth_raw.bitmap), &vp_pointcloud, pointcloud, nullptr,
no_align ? nullptr : &align_to);
}
vpImageConvert::createDepthHistogram(I_depth_raw, I_depth);
I_pcl = vpRGBa(0, 0, 0);
if (color_pointcloud) {
for (uint32_t i = 0; i < pointcloud_color->height; i++) {
for (uint32_t j = 0; j < pointcloud_color->width; j++) {
const pcl::PointXYZRGB &pcl_pt = pointcloud_color->at(j, i);
double Z = pcl_pt.z;
if (Z > 1e-2) {
double x = pcl_pt.x / Z;
double y = pcl_pt.y / Z;
vpMeterPixelConversion::convertPoint(cam_projection, x, y, imPt);
unsigned int u =
std::min<unsigned int>(static_cast<unsigned int>(width - 1), static_cast<unsigned int>(std::max<double>(0.0, imPt.get_u())));
unsigned int v =
std::min<unsigned int>(static_cast<unsigned int>(height - 1), static_cast<unsigned int>(std::max<double>(0.0, imPt.get_v())));
I_pcl[v][u] = vpRGBa(pcl_pt.r, pcl_pt.g, pcl_pt.b);
}
}
}
}
else {
createDepthHist(histogram, pointcloud, depth_scale);
for (uint32_t i = 0; i < pointcloud->height; i++) {
for (uint32_t j = 0; j < pointcloud->width; j++) {
const pcl::PointXYZ &pcl_pt = pointcloud->at(j, i);
double Z = pcl_pt.z;
if (Z > 1e-2) {
double x = pcl_pt.x / Z;
double y = pcl_pt.y / Z;
vpMeterPixelConversion::convertPoint(cam_projection, x, y, imPt);
unsigned int u =
std::min<unsigned int>(static_cast<unsigned int>(width - 1), static_cast<unsigned int>(std::max<double>(0.0, imPt.get_u())));
unsigned int v =
std::min<unsigned int>(static_cast<unsigned int>(height - 1), static_cast<unsigned int>(std::max<double>(0.0, imPt.get_v())));
unsigned char depth_viz = getDepthColor(histogram, pcl_pt.z, depth_scale);
I_pcl[v][u] = vpRGBa(depth_viz, depth_viz, depth_viz);
}
}
}
}
I_pcl2 = vpRGBa(0, 0, 0);
createDepthHist(histogram2, vp_pointcloud, depth_scale);
for (size_t i = 0; i < vp_pointcloud.size(); i++) {
const vpColVector &pt = vp_pointcloud[i];
double Z = pt[2];
if (Z > 1e-2) {
double x = pt[0] / Z;
double y = pt[1] / Z;
vpMeterPixelConversion::convertPoint(cam_projection, x, y, imPt);
unsigned int u =
std::min<unsigned int>(static_cast<unsigned int>(width - 1), static_cast<unsigned int>(std::max<double>(0.0, imPt.get_u())));
unsigned int v =
std::min<unsigned int>(static_cast<unsigned int>(height - 1), static_cast<unsigned int>(std::max<double>(0.0, imPt.get_v())));
unsigned char depth_viz = getDepthColor(histogram2, Z, depth_scale);
I_pcl2[v][u] = vpRGBa(depth_viz, depth_viz, depth_viz);
}
}
I_display.insert(I_color, vpImagePoint(0, 0));
I_display.insert(I_depth, vpImagePoint(I_color.getHeight(), 0));
I_display2.insert(I_color, vpImagePoint(0, 0));
I_display2.insert(I_pcl, vpImagePoint(I_color.getHeight(), 0));
I_display3.insert(I_color, vpImagePoint(0, 0));
I_display3.insert(I_pcl2, vpImagePoint(I_color.getHeight(), 0));
vpDisplay::display(I_display);
vpDisplay::display(I_display2);
vpDisplay::display(I_display3);
const int nb_lines = 5;
for (int i = 1; i < nb_lines; i++) {
const int col_idx = i * (width / nb_lines);
vpDisplay::displayLine(I_display, 0, col_idx, I_display.getRows() - 1, col_idx, vpColor::green, 2);
vpDisplay::displayLine(I_display2, 0, col_idx, I_display.getRows() - 1, col_idx, vpColor::green, 2);
vpDisplay::displayLine(I_display3, 0, col_idx, I_display.getRows() - 1, col_idx, vpColor::green, 2);
}
vpDisplay::flush(I_display);
vpDisplay::flush(I_display2);
vpDisplay::flush(I_display3);
if (vpDisplay::getClick(I_display, false) || vpDisplay::getClick(I_display2, false) ||
vpDisplay::getClick(I_display3, false)) {
break;
}
}
return EXIT_SUCCESS;
}
#else
int main() { return EXIT_SUCCESS; }
#endif
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
static const vpColor green
Definition: vpColor.h:220
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:130
void init(vpImage< unsigned char > &I, int winx=-1, int winy=-1, const std::string &title="") VP_OVERRIDE
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:82
double get_u() const
Definition: vpImagePoint.h:136
double get_v() const
Definition: vpImagePoint.h:147
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
Definition: vpRGBa.h:65
vpCameraParameters getCameraParameters(const rs2_stream &stream, vpCameraParameters::vpCameraParametersProjType type=vpCameraParameters::perspectiveProjWithDistortion, int index=-1) const
void acquire(vpImage< unsigned char > &grey, double *ts=nullptr)
bool open(const rs2::config &cfg=rs2::config())
float getDepthScale()