36 #include <visp3/core/vpConfig.h> 38 #if defined(VISP_HAVE_REALSENSE2) && defined(VISP_HAVE_CPP11_COMPATIBILITY) 43 #include <visp3/core/vpImageConvert.h> 44 #include <visp3/sensor/vpRealSense2.h> 46 #define MANUAL_POINTCLOUD 1 52 : m_colorIntrinsics(), m_depth2ColorExtrinsics(), m_depthIntrinsics(), m_depthScale(0.0f), m_invalidDepthValue(0.0f),
53 m_max_Z(8.0f), m_pipe(), m_pipelineProfile(), m_pointcloud(), m_points()
69 auto data =
m_pipe.wait_for_frames();
70 auto color_frame = data.get_color_frame();
80 auto data =
m_pipe.wait_for_frames();
81 auto color_frame = data.get_color_frame();
94 std::vector<vpColVector> *
const data_pointCloud,
unsigned char *
const data_infrared,
95 rs2::align *
const align_to)
97 auto data =
m_pipe.wait_for_frames();
99 #if (RS2_API_VERSION > ((2 * 10000) + (9 * 100) + 0)) 100 data = align_to->process(data);
102 data = align_to->proccess(data);
105 if (data_image != NULL) {
106 auto color_frame = data.get_color_frame();
110 if (data_depth != NULL || data_pointCloud != NULL) {
111 auto depth_frame = data.get_depth_frame();
112 if (data_depth != NULL)
115 if (data_pointCloud != NULL)
119 if (data_infrared != NULL) {
120 auto infrared_frame = data.first(RS2_STREAM_INFRARED);
137 std::vector<vpColVector> *
const data_pointCloud,
138 pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud,
unsigned char *
const data_infrared,
139 rs2::align *
const align_to)
141 auto data =
m_pipe.wait_for_frames();
142 if (align_to != NULL)
143 #if (RS2_API_VERSION > ((2 * 10000) + (9 * 100) + 0)) 144 data = align_to->process(data);
146 data = align_to->proccess(data);
149 if (data_image != NULL) {
150 auto color_frame = data.get_color_frame();
154 if (data_depth != NULL || data_pointCloud != NULL || pointcloud != NULL) {
155 auto depth_frame = data.get_depth_frame();
156 if (data_depth != NULL)
159 if (data_pointCloud != NULL)
162 if (pointcloud != NULL)
166 if (data_infrared != NULL) {
167 auto infrared_frame = data.first(RS2_STREAM_INFRARED);
183 std::vector<vpColVector> *
const data_pointCloud,
184 pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pointcloud,
unsigned char *
const data_infrared,
185 rs2::align *
const align_to)
187 auto data =
m_pipe.wait_for_frames();
188 if (align_to != NULL)
189 #if (RS2_API_VERSION > ((2 * 10000) + (9 * 100) + 0)) 190 data = align_to->process(data);
192 data = align_to->proccess(data);
195 auto color_frame = data.get_color_frame();
196 if (data_image != NULL) {
200 if (data_depth != NULL || data_pointCloud != NULL || pointcloud != NULL) {
201 auto depth_frame = data.get_depth_frame();
202 if (data_depth != NULL)
205 if (data_pointCloud != NULL)
208 if (pointcloud != NULL)
212 if (data_infrared != NULL) {
213 auto infrared_frame = data.first(RS2_STREAM_INFRARED);
243 auto rs_stream =
m_pipelineProfile.get_stream(stream).as<rs2::video_stream_profile>();
244 auto intrinsics = rs_stream.get_intrinsics();
247 double u0 = intrinsics.ppx;
248 double v0 = intrinsics.ppy;
249 double px = intrinsics.fx;
250 double py = intrinsics.fy;
253 double kdu = intrinsics.coeffs[0];
272 return vsp.get_intrinsics();
277 auto vf = frame.as<rs2::video_frame>();
278 const unsigned int width = (
unsigned int)vf.get_width();
279 const unsigned int height = (
unsigned int)vf.get_height();
280 color.
resize(height, width);
282 if (frame.get_profile().format() == RS2_FORMAT_RGB8) {
284 }
else if (frame.get_profile().format() == RS2_FORMAT_RGBA8) {
285 memcpy((
unsigned char *)color.
bitmap, (
unsigned char *)frame.get_data(), width * height *
sizeof(
vpRGBa));
286 }
else if (frame.get_profile().format() == RS2_FORMAT_BGR8) {
304 auto vf = frame.as<rs2::video_frame>();
305 const unsigned int width = (
unsigned int)vf.get_width();
306 const unsigned int height = (
unsigned int)vf.get_height();
307 grey.
resize(height, width);
309 if (frame.get_profile().format() == RS2_FORMAT_RGB8) {
311 }
else if (frame.get_profile().format() == RS2_FORMAT_RGBA8) {
313 }
else if (frame.get_profile().format() == RS2_FORMAT_BGR8) {
322 auto vf = frame.as<rs2::video_frame>();
323 int size = vf.get_width() * vf.get_height();
325 switch (frame.get_profile().format()) {
326 case RS2_FORMAT_RGB8:
327 case RS2_FORMAT_BGR8:
328 memcpy(data, frame.get_data(), size * 3);
331 case RS2_FORMAT_RGBA8:
332 case RS2_FORMAT_BGRA8:
333 memcpy(data, frame.get_data(), size * 4);
338 memcpy(data, frame.get_data(), size * 2);
342 memcpy(data, frame.get_data(), size);
352 if (
m_depthScale <= std::numeric_limits<float>::epsilon()) {
353 std::stringstream ss;
358 auto vf = depth_frame.as<rs2::video_frame>();
359 const int width = vf.get_width();
360 const int height = vf.get_height();
361 pointcloud.resize((
size_t)(width * height));
363 const uint16_t *p_depth_frame =
reinterpret_cast<const uint16_t *
>(depth_frame.get_data());
367 #pragma omp parallel for schedule(dynamic) 368 for (
int i = 0; i < height; i++) {
369 auto depth_pixel_index = i * width;
371 for (
int j = 0; j < width; j++, depth_pixel_index++) {
372 if (p_depth_frame[depth_pixel_index] == 0) {
373 pointcloud[(size_t)depth_pixel_index].resize(4,
false);
377 pointcloud[(size_t)depth_pixel_index][3] = 1.0;
382 auto pixels_distance =
m_depthScale * p_depth_frame[depth_pixel_index];
385 const float pixel[] = {(float)j, (
float)i};
386 rs2_deproject_pixel_to_point(points, &
m_depthIntrinsics, pixel, pixels_distance);
391 pointcloud[(size_t)depth_pixel_index].resize(4,
false);
392 pointcloud[(size_t)depth_pixel_index][0] = points[0];
393 pointcloud[(size_t)depth_pixel_index][1] = points[1];
394 pointcloud[(size_t)depth_pixel_index][2] = points[2];
395 pointcloud[(size_t)depth_pixel_index][3] = 1.0;
403 if (
m_depthScale <= std::numeric_limits<float>::epsilon()) {
404 std::stringstream ss;
409 auto vf = depth_frame.as<rs2::video_frame>();
410 const int width = vf.get_width();
411 const int height = vf.get_height();
412 pointcloud->width = (uint32_t)width;
413 pointcloud->height = (uint32_t)height;
414 pointcloud->resize((
size_t)(width * height));
416 #if MANUAL_POINTCLOUD // faster to compute manually when tested 417 const uint16_t *p_depth_frame =
reinterpret_cast<const uint16_t *
>(depth_frame.get_data());
421 #pragma omp parallel for schedule(dynamic) 422 for (
int i = 0; i < height; i++) {
423 auto depth_pixel_index = i * width;
425 for (
int j = 0; j < width; j++, depth_pixel_index++) {
426 if (p_depth_frame[depth_pixel_index] == 0) {
434 auto pixels_distance =
m_depthScale * p_depth_frame[depth_pixel_index];
437 const float pixel[] = {(float)j, (
float)i};
438 rs2_deproject_pixel_to_point(points, &
m_depthIntrinsics, pixel, pixels_distance);
443 pointcloud->points[(size_t)(depth_pixel_index)].x = points[0];
444 pointcloud->points[(size_t)(depth_pixel_index)].y = points[1];
445 pointcloud->points[(size_t)(depth_pixel_index)].z = points[2];
450 auto vertices =
m_points.get_vertices();
452 for (
size_t i = 0; i <
m_points.size(); i++) {
453 if (vertices[i].z <= 0.0f || vertices[i].z >
m_max_Z) {
458 pointcloud->points[i].x = vertices[i].x;
459 pointcloud->points[i].y = vertices[i].y;
460 pointcloud->points[i].z = vertices[i].z;
467 pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pointcloud)
469 if (
m_depthScale <= std::numeric_limits<float>::epsilon()) {
470 std::stringstream ss;
475 auto vf = depth_frame.as<rs2::video_frame>();
476 const int width = vf.get_width();
477 const int height = vf.get_height();
478 pointcloud->width = (uint32_t)width;
479 pointcloud->height = (uint32_t)height;
480 pointcloud->resize((
size_t)(width * height));
482 const uint16_t *p_depth_frame =
reinterpret_cast<const uint16_t *
>(depth_frame.get_data());
485 auto color_format = color_frame.as<rs2::video_frame>().get_profile().format();
486 bool swap_rgb = color_format == RS2_FORMAT_BGR8 || color_format == RS2_FORMAT_BGRA8;
487 unsigned int nb_color_pixel = (color_format == RS2_FORMAT_RGB8 || color_format == RS2_FORMAT_BGR8) ? 3 : 4;
488 const unsigned char *p_color_frame =
reinterpret_cast<const unsigned char *
>(color_frame.get_data());
492 #pragma omp parallel for schedule(dynamic) 493 for (
int i = 0; i < height; i++) {
494 auto depth_pixel_index = i * width;
496 for (
int j = 0; j < width; j++, depth_pixel_index++) {
497 if (p_depth_frame[depth_pixel_index] == 0) {
505 #if PCL_VERSION_COMPARE(<, 1, 1, 0) 506 unsigned int r = 96, g = 157, b = 198;
507 uint32_t rgb = (
static_cast<uint32_t
>(r) << 16 | static_cast<uint32_t>(g) << 8 |
static_cast<uint32_t
>(b));
509 pointcloud->points[(size_t)depth_pixel_index].rgb = *reinterpret_cast<float *>(&rgb);
511 pointcloud->points[(size_t)depth_pixel_index].r = (uint8_t)96;
512 pointcloud->points[(size_t)depth_pixel_index].g = (uint8_t)157;
513 pointcloud->points[(size_t)depth_pixel_index].b = (uint8_t)198;
519 auto pixels_distance =
m_depthScale * p_depth_frame[depth_pixel_index];
521 float depth_point[3];
522 const float pixel[] = {(float)j, (
float)i};
523 rs2_deproject_pixel_to_point(depth_point, &
m_depthIntrinsics, pixel, pixels_distance);
528 pointcloud->points[(size_t)depth_pixel_index].x = depth_point[0];
529 pointcloud->points[(size_t)depth_pixel_index].y = depth_point[1];
530 pointcloud->points[(size_t)depth_pixel_index].z = depth_point[2];
532 float color_point[3];
534 float color_pixel[2];
537 if (color_pixel[1] < 0 || color_pixel[1] >= color_height || color_pixel[0] < 0 || color_pixel[0] >= color_width) {
541 #if PCL_VERSION_COMPARE(<, 1, 1, 0) 542 unsigned int r = 96, g = 157, b = 198;
543 uint32_t rgb = (
static_cast<uint32_t
>(r) << 16 | static_cast<uint32_t>(g) << 8 |
static_cast<uint32_t
>(b));
545 pointcloud->points[(size_t)depth_pixel_index].rgb = *reinterpret_cast<float *>(&rgb);
547 pointcloud->points[(size_t)depth_pixel_index].r = (uint8_t)96;
548 pointcloud->points[(size_t)depth_pixel_index].g = (uint8_t)157;
549 pointcloud->points[(size_t)depth_pixel_index].b = (uint8_t)198;
552 unsigned int i_ = (
unsigned int)color_pixel[1];
553 unsigned int j_ = (
unsigned int)color_pixel[0];
555 #if PCL_VERSION_COMPARE(<, 1, 1, 0) 559 (
static_cast<uint32_t
>(p_color_frame[(i_ * (
unsigned int)color_width + j_) * nb_color_pixel]) |
560 static_cast<uint32_t>(p_color_frame[(i_ * (
unsigned int)color_width + j_) * nb_color_pixel + 1]) << 8 |
561 static_cast<uint32_t
>(p_color_frame[(i_ * (
unsigned int)color_width + j_) * nb_color_pixel + 2]) << 16);
563 rgb = (
static_cast<uint32_t
>(p_color_frame[(i_ * (
unsigned int)color_width + j_) * nb_color_pixel]) << 16 |
564 static_cast<uint32_t>(p_color_frame[(i_ * (
unsigned int)color_width + j_) * nb_color_pixel + 1]) << 8 |
565 static_cast<uint32_t
>(p_color_frame[(i_ * (
unsigned int)color_width + j_) * nb_color_pixel + 2]));
568 pointcloud->points[(size_t)(i * depth_width + j)].rgb = *
reinterpret_cast<float *
>(&rgb);
571 pointcloud->points[(size_t)depth_pixel_index].b =
572 (uint32_t)p_color_frame[(i_ * (
unsigned int)color_width + j_) * nb_color_pixel];
573 pointcloud->points[(size_t)depth_pixel_index].g =
574 (uint32_t)p_color_frame[(i_ * (
unsigned int)color_width + j_) * nb_color_pixel + 1];
575 pointcloud->points[(size_t)depth_pixel_index].r =
576 (uint32_t)p_color_frame[(i_ * (
unsigned int)color_width + j_) * nb_color_pixel + 2];
578 pointcloud->points[(size_t)depth_pixel_index].r =
579 (uint32_t)p_color_frame[(i_ * (
unsigned int)color_width + j_) * nb_color_pixel];
580 pointcloud->points[(size_t)depth_pixel_index].g =
581 (uint32_t)p_color_frame[(i_ * (
unsigned int)color_width + j_) * nb_color_pixel + 1];
582 pointcloud->points[(size_t)depth_pixel_index].b =
583 (uint32_t)p_color_frame[(i_ * (
unsigned int)color_width + j_) * nb_color_pixel + 2];
602 rs2_extrinsics extrinsics = from_stream.get_extrinsics_to(to_stream);
606 for (
unsigned int i = 0; i < 3; i++) {
607 t[i] = extrinsics.translation[i];
608 for (
unsigned int j = 0; j < 3; j++)
609 R[i][j] = extrinsics.rotation[j * 3 + i];
626 for (rs2::sensor &sensor : dev.query_sensors()) {
628 if (rs2::depth_sensor dpt = sensor.as<rs2::depth_sensor>()) {
634 auto depth_stream =
m_pipelineProfile.get_stream(RS2_STREAM_DEPTH).as<rs2::video_stream_profile>();
640 }
catch (
const std::runtime_error &e) {
641 std::cout <<
"Error getting depth to color extrinsics: " << e.what() << std::endl;
643 }
catch (
const std::runtime_error &e) {
644 std::cout <<
"Error getting depth intrinsics: " << e.what() << std::endl;
649 }
catch (
const std::runtime_error &e) {
650 std::cout <<
"Error getting color intrinsics: " << e.what() << std::endl;
657 void print(
const rs2_extrinsics &extrinsics, std::ostream &os)
659 std::stringstream ss;
660 ss <<
"Rotation Matrix:\n";
662 for (
auto i = 0; i < 3; ++i) {
663 for (
auto j = 0; j < 3; ++j) {
664 ss << std::left << std::setw(15) << std::setprecision(5) << extrinsics.rotation[j * 3 + i];
669 ss <<
"\nTranslation Vector: ";
670 for (
size_t i = 0; i <
sizeof(extrinsics.translation) /
sizeof(extrinsics.translation[0]); ++i)
671 ss << std::setprecision(15) << extrinsics.translation[i] <<
" ";
673 os << ss.str() <<
"\n\n";
676 void print(
const rs2_intrinsics &intrinsics, std::ostream &os)
678 std::stringstream ss;
679 ss << std::left << std::setw(14) <<
"Width: " 680 <<
"\t" << intrinsics.width <<
"\n" 681 << std::left << std::setw(14) <<
"Height: " 682 <<
"\t" << intrinsics.height <<
"\n" 683 << std::left << std::setw(14) <<
"PPX: " 684 <<
"\t" << std::setprecision(15) << intrinsics.ppx <<
"\n" 685 << std::left << std::setw(14) <<
"PPY: " 686 <<
"\t" << std::setprecision(15) << intrinsics.ppy <<
"\n" 687 << std::left << std::setw(14) <<
"Fx: " 688 <<
"\t" << std::setprecision(15) << intrinsics.fx <<
"\n" 689 << std::left << std::setw(14) <<
"Fy: " 690 <<
"\t" << std::setprecision(15) << intrinsics.fy <<
"\n" 691 << std::left << std::setw(14) <<
"Distortion: " 692 <<
"\t" << rs2_distortion_to_string(intrinsics.model) <<
"\n" 693 << std::left << std::setw(14) <<
"Coeffs: ";
695 for (
size_t i = 0; i <
sizeof(intrinsics.coeffs) /
sizeof(intrinsics.coeffs[0]); ++i)
696 ss <<
"\t" << std::setprecision(15) << intrinsics.coeffs[i] <<
" ";
698 os << ss.str() <<
"\n\n";
701 void safe_get_intrinsics(
const rs2::video_stream_profile &profile, rs2_intrinsics &intrinsics)
704 intrinsics = profile.get_intrinsics();
709 bool operator==(
const rs2_intrinsics &lhs,
const rs2_intrinsics &rhs)
711 return lhs.width == rhs.width && lhs.height == rhs.height && lhs.ppx == rhs.ppx && lhs.ppy == rhs.ppy &&
712 lhs.fx == rhs.fx && lhs.fy == rhs.fy && lhs.model == rhs.model &&
713 !std::memcmp(lhs.coeffs, rhs.coeffs,
sizeof(rhs.coeffs));
716 std::string get_str_formats(
const std::set<rs2_format> &formats)
718 std::stringstream ss;
719 for (
auto format = formats.begin(); format != formats.end(); ++format) {
720 ss << *format << ((format != formats.end()) && (next(format) == formats.end()) ?
"" :
"/");
725 struct stream_and_resolution {
730 std::string stream_name;
732 bool operator<(
const stream_and_resolution &obj)
const 734 return (std::make_tuple(stream, stream_index, width, height) <
735 std::make_tuple(obj.stream, obj.stream_index, obj.width, obj.height));
739 struct stream_and_index {
743 bool operator<(
const stream_and_index &obj)
const 745 return (std::make_tuple(stream, stream_index) < std::make_tuple(obj.stream, obj.stream_index));
772 os << std::left << std::setw(30) <<
"Device Name" << std::setw(20) <<
"Serial Number" << std::setw(20)
773 <<
"Firmware Version" << std::endl;
776 os << std::left << std::setw(30) << dev.get_info(RS2_CAMERA_INFO_NAME) << std::setw(20)
777 << dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER) << std::setw(20) << dev.get_info(RS2_CAMERA_INFO_FIRMWARE_VERSION)
781 os <<
" Device info: \n";
782 for (
auto j = 0; j < RS2_CAMERA_INFO_COUNT; ++j) {
783 auto param =
static_cast<rs2_camera_info
>(j);
784 if (dev.supports(param))
785 os <<
" " << std::left << std::setw(30) << rs2_camera_info_to_string(rs2_camera_info(param)) <<
": \t" 786 << dev.get_info(param) <<
"\n";
791 for (
auto &&sensor : dev.query_sensors()) {
792 os <<
"Options for " << sensor.get_info(RS2_CAMERA_INFO_NAME) << std::endl;
794 os << std::setw(55) <<
" Supported options:" << std::setw(10) <<
"min" << std::setw(10) <<
" max" << std::setw(6)
795 <<
" step" << std::setw(10) <<
" default" << std::endl;
796 for (
auto j = 0; j < RS2_OPTION_COUNT; ++j) {
797 auto opt =
static_cast<rs2_option
>(j);
798 if (sensor.supports(opt)) {
799 auto range = sensor.get_option_range(opt);
800 os <<
" " << std::left << std::setw(50) << opt <<
" : " << std::setw(5) << range.min <<
"... " 801 << std::setw(12) << range.max << std::setw(6) << range.step << std::setw(10) << range.def <<
"\n";
808 for (
auto &&sensor : dev.query_sensors()) {
809 os <<
"Stream Profiles supported by " << sensor.get_info(RS2_CAMERA_INFO_NAME) <<
"\n";
811 os << std::setw(55) <<
" Supported modes:" << std::setw(10) <<
"stream" << std::setw(10) <<
" resolution" 812 << std::setw(6) <<
" fps" << std::setw(10) <<
" format" 815 for (
auto &&profile : sensor.get_stream_profiles()) {
816 if (
auto video = profile.as<rs2::video_stream_profile>()) {
817 os <<
" " << profile.stream_name() <<
"\t " << video.width() <<
"x" << video.height() <<
"\t@ " 818 << profile.fps() <<
"Hz\t" << profile.format() <<
"\n";
820 os <<
" " << profile.stream_name() <<
"\t@ " << profile.fps() <<
"Hz\t" << profile.format() <<
"\n";
827 std::map<stream_and_index, rs2::stream_profile> streams;
828 std::map<stream_and_resolution, std::vector<std::pair<std::set<rs2_format>, rs2_intrinsics> > > intrinsics_map;
829 for (
auto &&sensor : dev.query_sensors()) {
831 for (
auto &&profile : sensor.get_stream_profiles()) {
832 if (
auto video = profile.as<rs2::video_stream_profile>()) {
833 if (streams.find(stream_and_index{profile.stream_type(), profile.stream_index()}) == streams.end()) {
834 streams[stream_and_index{profile.stream_type(), profile.stream_index()}] = profile;
837 rs2_intrinsics intrinsics{};
838 stream_and_resolution stream_res{profile.stream_type(), profile.stream_index(), video.width(), video.height(),
839 profile.stream_name()};
840 safe_get_intrinsics(video, intrinsics);
841 auto it = std::find_if(
842 (intrinsics_map[stream_res]).begin(), (intrinsics_map[stream_res]).end(),
843 [&](
const std::pair<std::set<rs2_format>, rs2_intrinsics> &kvp) {
return intrinsics == kvp.second; });
844 if (it == (intrinsics_map[stream_res]).end()) {
845 (intrinsics_map[stream_res]).push_back({{profile.format()}, intrinsics});
847 it->first.insert(profile.format());
855 os <<
"Provided Intrinsic:\n";
856 for (
auto &kvp : intrinsics_map) {
857 auto stream_res = kvp.first;
858 for (
auto &intrinsics : kvp.second) {
859 auto formats = get_str_formats(intrinsics.first);
860 os <<
"Intrinsic of \"" << stream_res.stream_name <<
"\"\t " << stream_res.width <<
"x" << stream_res.height
861 <<
"\t " << formats <<
"\n";
862 if (intrinsics.second == rs2_intrinsics{}) {
863 os <<
"Intrinsic NOT available!\n\n";
865 print(intrinsics.second, os);
871 os <<
"\nProvided Extrinsic:\n";
872 for (
auto kvp1 = streams.begin(); kvp1 != streams.end(); ++kvp1) {
873 for (
auto kvp2 = streams.begin(); kvp2 != streams.end(); ++kvp2) {
874 os <<
"Extrinsic from \"" << kvp1->second.stream_name() <<
"\"\t " 876 <<
"\t \"" << kvp2->second.stream_name() <<
"\"\n";
877 auto extrinsics = kvp1->second.get_extrinsics_to(kvp2->second);
878 print(extrinsics, os);
885 #elif !defined(VISP_BUILD_SHARED_LIBS) 888 void dummy_vpRealSense2(){};
static void BGRToRGBa(unsigned char *bgr, unsigned char *rgba, unsigned int width, unsigned int height, bool flip=false)
static void RGBToGrey(unsigned char *rgb, unsigned char *grey, unsigned int size)
Implementation of an homogeneous matrix and operations on such kind of matrices.
static void RGBToRGBa(unsigned char *rgb, unsigned char *rgba, unsigned int size)
static void BGRToGrey(unsigned char *bgr, unsigned char *grey, unsigned int width, unsigned int height, bool flip=false)
Type * bitmap
points toward the bitmap
error that can be emited by ViSP classes.
void open(const rs2::config &cfg=rs2::config())
rs2_intrinsics m_depthIntrinsics
vpCameraParameters getCameraParameters(const rs2_stream &stream, vpCameraParameters::vpCameraParametersProjType type=vpCameraParameters::perspectiveProjWithDistortion) const
Implementation of a rotation matrix and operations on such kind of matrices.
void initPersProjWithoutDistortion(const double px, const double py, const double u0, const double v0)
rs2_intrinsics m_colorIntrinsics
rs2::pipeline_profile m_pipelineProfile
vpCameraParametersProjType
void getGreyFrame(const rs2::frame &frame, vpImage< unsigned char > &grey)
Generic class defining intrinsic camera parameters.
rs2_extrinsics m_depth2ColorExtrinsics
void resize(const unsigned int h, const unsigned int w)
resize the image : Image initialization
void acquire(vpImage< unsigned char > &grey)
rs2::pointcloud m_pointcloud
void getPointcloud(const rs2::depth_frame &depth_frame, std::vector< vpColVector > &pointcloud)
vpHomogeneousMatrix getTransformation(const rs2_stream &from, const rs2_stream &to) const
rs2_intrinsics getIntrinsics(const rs2_stream &stream) const
float m_invalidDepthValue
void getNativeFrameData(const rs2::frame &frame, unsigned char *const data)
friend VISP_EXPORT std::ostream & operator<<(std::ostream &os, const vpRealSense2 &rs)
static void RGBaToGrey(unsigned char *rgba, unsigned char *grey, unsigned int size)
void getColorFrame(const rs2::frame &frame, vpImage< vpRGBa > &color)
Class that consider the case of a translation vector.
void initPersProjWithDistortion(const double px, const double py, const double u0, const double v0, const double kud, const double kdu)