36 #include <visp3/core/vpConfig.h>
38 #if defined(VISP_HAVE_REALSENSE2)
43 #include <visp3/core/vpImageConvert.h>
44 #include <visp3/sensor/vpRealSense2.h>
46 #define MANUAL_POINTCLOUD 1
50 bool operator==(
const rs2_extrinsics &lhs,
const rs2_extrinsics &rhs)
52 for (
int i = 0; i < 3; i++) {
53 for (
int j = 0; j < 3; j++) {
54 if (std::fabs(lhs.rotation[i * 3 + j] - rhs.rotation[i * 3 + j]) > std::numeric_limits<float>::epsilon()) {
59 if (std::fabs(lhs.translation[i] - rhs.translation[i]) > std::numeric_limits<float>::epsilon()) {
72 : m_depthScale(0.0f), m_invalidDepthValue(0.0f), m_max_Z(8.0f), m_pipe(), m_pipelineProfile(), m_pointcloud(),
73 m_points(), m_pos(), m_quat(), m_rot(), m_product_line(), m_init(false)
89 auto data =
m_pipe.wait_for_frames();
90 auto color_frame = data.get_color_frame();
93 *ts = data.get_timestamp();
104 auto data =
m_pipe.wait_for_frames();
105 auto color_frame = data.get_color_frame();
108 *ts = data.get_timestamp();
123 std::vector<vpColVector> *
const data_pointCloud,
unsigned char *
const data_infrared,
124 rs2::align *
const align_to,
double *ts)
126 acquire(data_image, data_depth, data_pointCloud, data_infrared,
nullptr, align_to, ts);
187 std::vector<vpColVector> *
const data_pointCloud,
unsigned char *
const data_infrared1,
188 unsigned char *
const data_infrared2, rs2::align *
const align_to,
double *ts)
190 auto data =
m_pipe.wait_for_frames();
191 if (align_to !=
nullptr) {
194 #if (RS2_API_VERSION > ((2 * 10000) + (9 * 100) + 0))
195 data = align_to->process(data);
197 data = align_to->proccess(data);
201 if (data_image !=
nullptr) {
202 auto color_frame = data.get_color_frame();
206 if (data_depth !=
nullptr || data_pointCloud !=
nullptr) {
207 auto depth_frame = data.get_depth_frame();
208 if (data_depth !=
nullptr) {
212 if (data_pointCloud !=
nullptr) {
217 if (data_infrared1 !=
nullptr) {
218 auto infrared_frame = data.first(RS2_STREAM_INFRARED);
222 if (data_infrared2 !=
nullptr) {
223 auto infrared_frame = data.get_infrared_frame(2);
228 *ts = data.get_timestamp();
232 #if (RS2_API_VERSION > ((2 * 10000) + (31 * 100) + 0))
255 auto data =
m_pipe.wait_for_frames();
257 if (left !=
nullptr) {
258 auto left_fisheye_frame = data.get_fisheye_frame(1);
259 unsigned int width =
static_cast<unsigned int>(left_fisheye_frame.get_width());
260 unsigned int height =
static_cast<unsigned int>(left_fisheye_frame.get_height());
261 left->
resize(height, width);
265 if (right !=
nullptr) {
266 auto right_fisheye_frame = data.get_fisheye_frame(2);
267 unsigned int width =
static_cast<unsigned int>(right_fisheye_frame.get_width());
268 unsigned int height =
static_cast<unsigned int>(right_fisheye_frame.get_height());
269 right->
resize(height, width);
274 *ts = data.get_timestamp();
309 auto data =
m_pipe.wait_for_frames();
311 if (left !=
nullptr) {
312 auto left_fisheye_frame = data.get_fisheye_frame(1);
313 unsigned int width =
static_cast<unsigned int>(left_fisheye_frame.get_width());
314 unsigned int height =
static_cast<unsigned int>(left_fisheye_frame.get_height());
315 left->
resize(height, width);
319 if (right !=
nullptr) {
320 auto right_fisheye_frame = data.get_fisheye_frame(2);
321 unsigned int width =
static_cast<unsigned int>(right_fisheye_frame.get_width());
322 unsigned int height =
static_cast<unsigned int>(right_fisheye_frame.get_height());
323 right->
resize(height, width);
327 auto pose_frame = data.first_or_default(RS2_STREAM_POSE);
328 auto pose_data = pose_frame.as<rs2::pose_frame>().get_pose_data();
331 *ts = data.get_timestamp();
334 if (cMw !=
nullptr) {
335 m_pos[0] =
static_cast<double>(pose_data.translation.x);
336 m_pos[1] =
static_cast<double>(pose_data.translation.y);
337 m_pos[2] =
static_cast<double>(pose_data.translation.z);
339 m_quat[0] =
static_cast<double>(pose_data.rotation.x);
340 m_quat[1] =
static_cast<double>(pose_data.rotation.y);
341 m_quat[2] =
static_cast<double>(pose_data.rotation.z);
342 m_quat[3] =
static_cast<double>(pose_data.rotation.w);
347 if (odo_vel !=
nullptr) {
348 odo_vel->
resize(6,
false);
349 (*odo_vel)[0] =
static_cast<double>(pose_data.velocity.x);
350 (*odo_vel)[1] =
static_cast<double>(pose_data.velocity.y);
351 (*odo_vel)[2] =
static_cast<double>(pose_data.velocity.z);
352 (*odo_vel)[3] =
static_cast<double>(pose_data.angular_velocity.x);
353 (*odo_vel)[4] =
static_cast<double>(pose_data.angular_velocity.y);
354 (*odo_vel)[5] =
static_cast<double>(pose_data.angular_velocity.z);
357 if (odo_acc !=
nullptr) {
358 odo_acc->
resize(6,
false);
359 (*odo_acc)[0] =
static_cast<double>(pose_data.acceleration.x);
360 (*odo_acc)[1] =
static_cast<double>(pose_data.acceleration.y);
361 (*odo_acc)[2] =
static_cast<double>(pose_data.acceleration.z);
362 (*odo_acc)[3] =
static_cast<double>(pose_data.angular_acceleration.x);
363 (*odo_acc)[4] =
static_cast<double>(pose_data.angular_acceleration.y);
364 (*odo_acc)[5] =
static_cast<double>(pose_data.angular_acceleration.z);
367 if (confidence !=
nullptr) {
368 *confidence = pose_data.tracker_confidence;
387 unsigned int *confidence,
double *ts)
389 auto data =
m_pipe.wait_for_frames();
391 if (left !=
nullptr) {
392 auto left_fisheye_frame = data.get_fisheye_frame(1);
393 unsigned int width =
static_cast<unsigned int>(left_fisheye_frame.get_width());
394 unsigned int height =
static_cast<unsigned int>(left_fisheye_frame.get_height());
395 left->
resize(height, width);
399 if (right !=
nullptr) {
400 auto right_fisheye_frame = data.get_fisheye_frame(2);
401 unsigned int width =
static_cast<unsigned int>(right_fisheye_frame.get_width());
402 unsigned int height =
static_cast<unsigned int>(right_fisheye_frame.get_height());
403 right->
resize(height, width);
407 auto pose_frame = data.first_or_default(RS2_STREAM_POSE);
408 auto pose_data = pose_frame.as<rs2::pose_frame>().get_pose_data();
411 *ts = data.get_timestamp();
414 if (cMw !=
nullptr) {
415 m_pos[0] =
static_cast<double>(pose_data.translation.x);
416 m_pos[1] =
static_cast<double>(pose_data.translation.y);
417 m_pos[2] =
static_cast<double>(pose_data.translation.z);
419 m_quat[0] =
static_cast<double>(pose_data.rotation.x);
420 m_quat[1] =
static_cast<double>(pose_data.rotation.y);
421 m_quat[2] =
static_cast<double>(pose_data.rotation.z);
422 m_quat[3] =
static_cast<double>(pose_data.rotation.w);
427 if (odo_vel !=
nullptr) {
428 odo_vel->
resize(6,
false);
429 (*odo_vel)[0] =
static_cast<double>(pose_data.velocity.x);
430 (*odo_vel)[1] =
static_cast<double>(pose_data.velocity.y);
431 (*odo_vel)[2] =
static_cast<double>(pose_data.velocity.z);
432 (*odo_vel)[3] =
static_cast<double>(pose_data.angular_velocity.x);
433 (*odo_vel)[4] =
static_cast<double>(pose_data.angular_velocity.y);
434 (*odo_vel)[5] =
static_cast<double>(pose_data.angular_velocity.z);
437 if (odo_acc !=
nullptr) {
438 odo_acc->
resize(6,
false);
439 (*odo_acc)[0] =
static_cast<double>(pose_data.acceleration.x);
440 (*odo_acc)[1] =
static_cast<double>(pose_data.acceleration.y);
441 (*odo_acc)[2] =
static_cast<double>(pose_data.acceleration.z);
442 (*odo_acc)[3] =
static_cast<double>(pose_data.angular_acceleration.x);
443 (*odo_acc)[4] =
static_cast<double>(pose_data.angular_acceleration.y);
444 (*odo_acc)[5] =
static_cast<double>(pose_data.angular_acceleration.z);
447 auto accel_frame = data.first_or_default(RS2_STREAM_ACCEL);
448 auto accel_data = accel_frame.as<rs2::motion_frame>().get_motion_data();
450 if (imu_acc !=
nullptr) {
451 imu_acc->
resize(3,
false);
452 (*imu_acc)[0] =
static_cast<double>(accel_data.x);
453 (*imu_acc)[1] =
static_cast<double>(accel_data.y);
454 (*imu_acc)[2] =
static_cast<double>(accel_data.z);
457 auto gyro_frame = data.first_or_default(RS2_STREAM_GYRO);
458 auto gyro_data = gyro_frame.as<rs2::motion_frame>().get_motion_data();
460 if (imu_vel !=
nullptr) {
461 imu_vel->
resize(3,
false);
462 (*imu_vel)[0] =
static_cast<double>(gyro_data.x);
463 (*imu_vel)[1] =
static_cast<double>(gyro_data.y);
464 (*imu_vel)[2] =
static_cast<double>(gyro_data.z);
467 if (confidence !=
nullptr) {
468 *confidence = pose_data.tracker_confidence;
487 std::vector<vpColVector> *
const data_pointCloud,
488 pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud,
unsigned char *
const data_infrared,
489 rs2::align *
const align_to,
double *ts)
491 acquire(data_image, data_depth, data_pointCloud, pointcloud, data_infrared,
nullptr, align_to, ts);
509 std::vector<vpColVector> *
const data_pointCloud,
510 pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud,
unsigned char *
const data_infrared1,
511 unsigned char *
const data_infrared2, rs2::align *
const align_to,
double *ts)
513 auto data =
m_pipe.wait_for_frames();
514 if (align_to !=
nullptr) {
517 #if (RS2_API_VERSION > ((2 * 10000) + (9 * 100) + 0))
518 data = align_to->process(data);
520 data = align_to->proccess(data);
524 if (data_image !=
nullptr) {
525 auto color_frame = data.get_color_frame();
529 if (data_depth !=
nullptr || data_pointCloud !=
nullptr || pointcloud !=
nullptr) {
530 auto depth_frame = data.get_depth_frame();
531 if (data_depth !=
nullptr) {
535 if (data_pointCloud !=
nullptr) {
539 if (pointcloud !=
nullptr) {
544 if (data_infrared1 !=
nullptr) {
545 auto infrared_frame = data.first(RS2_STREAM_INFRARED);
549 if (data_infrared2 !=
nullptr) {
550 auto infrared_frame = data.get_infrared_frame(2);
555 *ts = data.get_timestamp();
572 std::vector<vpColVector> *
const data_pointCloud,
573 pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pointcloud,
unsigned char *
const data_infrared,
574 rs2::align *
const align_to,
double *ts)
576 acquire(data_image, data_depth, data_pointCloud, pointcloud, data_infrared,
nullptr, align_to, ts);
594 std::vector<vpColVector> *
const data_pointCloud,
595 pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pointcloud,
unsigned char *
const data_infrared1,
596 unsigned char *
const data_infrared2, rs2::align *
const align_to,
double *ts)
598 auto data =
m_pipe.wait_for_frames();
599 if (align_to !=
nullptr) {
602 #if (RS2_API_VERSION > ((2 * 10000) + (9 * 100) + 0))
603 data = align_to->process(data);
605 data = align_to->proccess(data);
609 auto color_frame = data.get_color_frame();
610 if (data_image !=
nullptr) {
614 if (data_depth !=
nullptr || data_pointCloud !=
nullptr || pointcloud !=
nullptr) {
615 auto depth_frame = data.get_depth_frame();
616 if (data_depth !=
nullptr) {
620 if (data_pointCloud !=
nullptr) {
624 if (pointcloud !=
nullptr) {
629 if (data_infrared1 !=
nullptr) {
630 auto infrared_frame = data.first(RS2_STREAM_INFRARED);
634 if (data_infrared2 !=
nullptr) {
635 auto infrared_frame = data.get_infrared_frame(2);
640 *ts = data.get_timestamp();
677 auto rs_stream =
m_pipelineProfile.get_stream(stream, index).as<rs2::video_stream_profile>();
678 auto intrinsics = rs_stream.get_intrinsics();
681 double u0 = intrinsics.ppx;
682 double v0 = intrinsics.ppy;
683 double px = intrinsics.fx;
684 double py = intrinsics.fy;
688 double kdu = intrinsics.coeffs[0];
693 std::vector<double> tmp_coefs;
694 tmp_coefs.push_back(
static_cast<double>(intrinsics.coeffs[0]));
695 tmp_coefs.push_back(
static_cast<double>(intrinsics.coeffs[1]));
696 tmp_coefs.push_back(
static_cast<double>(intrinsics.coeffs[2]));
697 tmp_coefs.push_back(
static_cast<double>(intrinsics.coeffs[3]));
698 tmp_coefs.push_back(
static_cast<double>(intrinsics.coeffs[4]));
722 auto vsp =
m_pipelineProfile.get_stream(stream, index).as<rs2::video_stream_profile>();
723 return vsp.get_intrinsics();
728 auto vf = frame.as<rs2::video_frame>();
729 unsigned int width = (
unsigned int)vf.get_width();
730 unsigned int height = (
unsigned int)vf.get_height();
731 color.
resize(height, width);
733 if (frame.get_profile().format() == RS2_FORMAT_RGB8) {
735 reinterpret_cast<unsigned char *
>(color.
bitmap), width, height);
737 else if (frame.get_profile().format() == RS2_FORMAT_RGBA8) {
738 memcpy(
reinterpret_cast<unsigned char *
>(color.
bitmap),
739 const_cast<unsigned char *
>(
static_cast<const unsigned char *
>(frame.get_data())),
740 width * height *
sizeof(
vpRGBa));
742 else if (frame.get_profile().format() == RS2_FORMAT_BGR8) {
744 reinterpret_cast<unsigned char *
>(color.
bitmap), width, height);
758 rs2::pipeline *pipe =
new rs2::pipeline;
759 rs2::pipeline_profile *pipelineProfile =
new rs2::pipeline_profile;
760 *pipelineProfile = pipe->start();
762 rs2::device dev = pipelineProfile->get_device();
765 for (rs2::sensor &sensor : dev.query_sensors()) {
767 if (rs2::depth_sensor dpt = sensor.as<rs2::depth_sensor>()) {
774 delete pipelineProfile;
782 auto vf = frame.as<rs2::video_frame>();
783 unsigned int width = (
unsigned int)vf.get_width();
784 unsigned int height = (
unsigned int)vf.get_height();
785 grey.
resize(height, width);
787 if (frame.get_profile().format() == RS2_FORMAT_RGB8) {
789 grey.
bitmap, width, height);
791 else if (frame.get_profile().format() == RS2_FORMAT_RGBA8) {
793 grey.
bitmap, width * height);
795 else if (frame.get_profile().format() == RS2_FORMAT_BGR8) {
797 grey.
bitmap, width, height);
806 auto vf = frame.as<rs2::video_frame>();
807 int size = vf.get_width() * vf.get_height();
809 switch (frame.get_profile().format()) {
810 case RS2_FORMAT_RGB8:
811 case RS2_FORMAT_BGR8:
812 memcpy(data, frame.get_data(), size * 3);
815 case RS2_FORMAT_RGBA8:
816 case RS2_FORMAT_BGRA8:
817 memcpy(data, frame.get_data(), size * 4);
822 memcpy(data, frame.get_data(), size * 2);
826 memcpy(data, frame.get_data(), size);
836 if (
m_depthScale <= std::numeric_limits<float>::epsilon()) {
837 std::stringstream ss;
842 auto vf = depth_frame.as<rs2::video_frame>();
843 const int width = vf.get_width();
844 const int height = vf.get_height();
845 pointcloud.resize((
size_t)(width * height));
847 const uint16_t *p_depth_frame =
reinterpret_cast<const uint16_t *
>(depth_frame.get_data());
848 const rs2_intrinsics depth_intrinsics = depth_frame.get_profile().as<rs2::video_stream_profile>().get_intrinsics();
852 #pragma omp parallel for schedule(dynamic)
853 for (
int i = 0; i < height; i++) {
854 auto depth_pixel_index = i * width;
856 for (
int j = 0; j < width; j++, depth_pixel_index++) {
857 if (p_depth_frame[depth_pixel_index] == 0) {
858 pointcloud[(size_t)depth_pixel_index].resize(4,
false);
862 pointcloud[(size_t)depth_pixel_index][3] = 1.0;
867 auto pixels_distance =
m_depthScale * p_depth_frame[depth_pixel_index];
870 const float pixel[] = { (float)j, (
float)i };
871 rs2_deproject_pixel_to_point(points, &depth_intrinsics, pixel, pixels_distance);
876 pointcloud[(size_t)depth_pixel_index].resize(4,
false);
877 pointcloud[(size_t)depth_pixel_index][0] = points[0];
878 pointcloud[(size_t)depth_pixel_index][1] = points[1];
879 pointcloud[(size_t)depth_pixel_index][2] = points[2];
880 pointcloud[(size_t)depth_pixel_index][3] = 1.0;
888 if (
m_depthScale <= std::numeric_limits<float>::epsilon()) {
889 std::stringstream ss;
894 auto vf = depth_frame.as<rs2::video_frame>();
895 const int width = vf.get_width();
896 const int height = vf.get_height();
897 pointcloud->width = (uint32_t)width;
898 pointcloud->height = (uint32_t)height;
899 pointcloud->resize((
size_t)(width * height));
901 #if MANUAL_POINTCLOUD
902 const uint16_t *p_depth_frame =
reinterpret_cast<const uint16_t *
>(depth_frame.get_data());
903 const rs2_intrinsics depth_intrinsics = depth_frame.get_profile().as<rs2::video_stream_profile>().get_intrinsics();
907 #pragma omp parallel for schedule(dynamic)
908 for (
int i = 0; i < height; i++) {
909 auto depth_pixel_index = i * width;
911 for (
int j = 0; j < width; j++, depth_pixel_index++) {
912 if (p_depth_frame[depth_pixel_index] == 0) {
920 auto pixels_distance =
m_depthScale * p_depth_frame[depth_pixel_index];
923 const float pixel[] = { (float)j, (
float)i };
924 rs2_deproject_pixel_to_point(points, &depth_intrinsics, pixel, pixels_distance);
929 pointcloud->points[(size_t)(depth_pixel_index)].x = points[0];
930 pointcloud->points[(size_t)(depth_pixel_index)].y = points[1];
931 pointcloud->points[(size_t)(depth_pixel_index)].z = points[2];
936 auto vertices =
m_points.get_vertices();
938 for (
size_t i = 0; i <
m_points.size(); i++) {
939 if (vertices[i].z <= 0.0f || vertices[i].z >
m_max_Z) {
945 pointcloud->points[i].x = vertices[i].x;
946 pointcloud->points[i].y = vertices[i].y;
947 pointcloud->points[i].z = vertices[i].z;
954 pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pointcloud)
956 if (
m_depthScale <= std::numeric_limits<float>::epsilon()) {
960 auto vf = depth_frame.as<rs2::video_frame>();
961 const int depth_width = vf.get_width();
962 const int depth_height = vf.get_height();
963 pointcloud->width =
static_cast<uint32_t
>(depth_width);
964 pointcloud->height =
static_cast<uint32_t
>(depth_height);
965 pointcloud->resize(
static_cast<uint32_t
>(depth_width * depth_height));
967 vf = color_frame.as<rs2::video_frame>();
968 const int color_width = vf.get_width();
969 const int color_height = vf.get_height();
971 const uint16_t *p_depth_frame =
reinterpret_cast<const uint16_t *
>(depth_frame.get_data());
972 const rs2_extrinsics depth2ColorExtrinsics =
973 depth_frame.get_profile().as<rs2::video_stream_profile>().get_extrinsics_to(
974 color_frame.get_profile().as<rs2::video_stream_profile>());
975 const rs2_intrinsics depth_intrinsics = depth_frame.get_profile().as<rs2::video_stream_profile>().get_intrinsics();
976 const rs2_intrinsics color_intrinsics = color_frame.get_profile().as<rs2::video_stream_profile>().get_intrinsics();
978 auto color_format = color_frame.as<rs2::video_frame>().get_profile().format();
979 const bool swap_rb = color_format == RS2_FORMAT_BGR8 || color_format == RS2_FORMAT_BGRA8;
980 unsigned int nb_color_pixel = (color_format == RS2_FORMAT_RGB8 || color_format == RS2_FORMAT_BGR8) ? 3 : 4;
981 const unsigned char *p_color_frame =
static_cast<const unsigned char *
>(color_frame.get_data());
982 rs2_extrinsics identity;
983 memset(identity.rotation, 0,
sizeof(identity.rotation));
984 memset(identity.translation, 0,
sizeof(identity.translation));
985 for (
int i = 0; i < 3; i++) {
986 identity.rotation[i * 3 + i] = 1;
988 const bool registered_streams =
989 (depth2ColorExtrinsics == identity) && (color_width == depth_width) && (color_height == depth_height);
993 #pragma omp parallel for schedule(dynamic)
994 for (
int i = 0; i < depth_height; i++) {
995 auto depth_pixel_index = i * depth_width;
997 for (
int j = 0; j < depth_width; j++, depth_pixel_index++) {
998 if (p_depth_frame[depth_pixel_index] == 0) {
1006 #if PCL_VERSION_COMPARE(<, 1, 1, 0)
1007 unsigned int r = 96, g = 157, b = 198;
1008 uint32_t rgb = (
static_cast<uint32_t
>(r) << 16 |
static_cast<uint32_t
>(g) << 8 |
static_cast<uint32_t
>(b));
1010 pointcloud->points[(size_t)depth_pixel_index].rgb = *
reinterpret_cast<float *
>(&rgb);
1012 pointcloud->points[(size_t)depth_pixel_index].r = (uint8_t)96;
1013 pointcloud->points[(size_t)depth_pixel_index].g = (uint8_t)157;
1014 pointcloud->points[(size_t)depth_pixel_index].b = (uint8_t)198;
1020 auto pixels_distance =
m_depthScale * p_depth_frame[depth_pixel_index];
1022 float depth_point[3];
1023 const float pixel[] = { (float)j, (
float)i };
1024 rs2_deproject_pixel_to_point(depth_point, &depth_intrinsics, pixel, pixels_distance);
1026 if (pixels_distance >
m_max_Z) {
1030 pointcloud->points[(size_t)depth_pixel_index].x = depth_point[0];
1031 pointcloud->points[(size_t)depth_pixel_index].y = depth_point[1];
1032 pointcloud->points[(size_t)depth_pixel_index].z = depth_point[2];
1034 if (!registered_streams) {
1035 float color_point[3];
1036 rs2_transform_point_to_point(color_point, &depth2ColorExtrinsics, depth_point);
1037 float color_pixel[2];
1038 rs2_project_point_to_pixel(color_pixel, &color_intrinsics, color_point);
1040 if (color_pixel[1] < 0 || color_pixel[1] >= color_height || color_pixel[0] < 0 ||
1041 color_pixel[0] >= color_width) {
1045 #if PCL_VERSION_COMPARE(<, 1, 1, 0)
1046 unsigned int r = 96, g = 157, b = 198;
1047 uint32_t rgb = (
static_cast<uint32_t
>(r) << 16 |
static_cast<uint32_t
>(g) << 8 |
static_cast<uint32_t
>(b));
1049 pointcloud->points[(size_t)depth_pixel_index].rgb = *
reinterpret_cast<float *
>(&rgb);
1051 pointcloud->points[(size_t)depth_pixel_index].r = (uint8_t)96;
1052 pointcloud->points[(size_t)depth_pixel_index].g = (uint8_t)157;
1053 pointcloud->points[(size_t)depth_pixel_index].b = (uint8_t)198;
1057 unsigned int i_ = (
unsigned int)color_pixel[1];
1058 unsigned int j_ = (
unsigned int)color_pixel[0];
1060 #if PCL_VERSION_COMPARE(<, 1, 1, 0)
1064 (
static_cast<uint32_t
>(p_color_frame[(i_ * (
unsigned int)color_width + j_) * nb_color_pixel]) |
1065 static_cast<uint32_t
>(p_color_frame[(i_ * (
unsigned int)color_width + j_) * nb_color_pixel + 1]) << 8 |
1066 static_cast<uint32_t
>(p_color_frame[(i_ * (
unsigned int)color_width + j_) * nb_color_pixel + 2])
1071 (
static_cast<uint32_t
>(p_color_frame[(i_ * (
unsigned int)color_width + j_) * nb_color_pixel]) << 16 |
1072 static_cast<uint32_t
>(p_color_frame[(i_ * (
unsigned int)color_width + j_) * nb_color_pixel + 1]) << 8 |
1073 static_cast<uint32_t
>(p_color_frame[(i_ * (
unsigned int)color_width + j_) * nb_color_pixel + 2]));
1076 pointcloud->points[(size_t)(i * depth_width + j)].rgb = *
reinterpret_cast<float *
>(&rgb);
1079 pointcloud->points[(size_t)depth_pixel_index].b =
1080 p_color_frame[(i_ * (
unsigned int)color_width + j_) * nb_color_pixel];
1081 pointcloud->points[(size_t)depth_pixel_index].g =
1082 p_color_frame[(i_ * (
unsigned int)color_width + j_) * nb_color_pixel + 1];
1083 pointcloud->points[(size_t)depth_pixel_index].r =
1084 p_color_frame[(i_ * (
unsigned int)color_width + j_) * nb_color_pixel + 2];
1087 pointcloud->points[(size_t)depth_pixel_index].r =
1088 p_color_frame[(i_ * (
unsigned int)color_width + j_) * nb_color_pixel];
1089 pointcloud->points[(size_t)depth_pixel_index].g =
1090 p_color_frame[(i_ * (
unsigned int)color_width + j_) * nb_color_pixel + 1];
1091 pointcloud->points[(size_t)depth_pixel_index].b =
1092 p_color_frame[(i_ * (
unsigned int)color_width + j_) * nb_color_pixel + 2];
1098 #if PCL_VERSION_COMPARE(<, 1, 1, 0)
1101 rgb = (
static_cast<uint32_t
>(p_color_frame[(i * (
unsigned int)color_width + j) * nb_color_pixel]) |
1102 static_cast<uint32_t
>(p_color_frame[(i * (
unsigned int)color_width + j) * nb_color_pixel + 1]) << 8 |
1103 static_cast<uint32_t
>(p_color_frame[(i * (
unsigned int)color_width + j) * nb_color_pixel + 2]) << 16);
1106 rgb = (
static_cast<uint32_t
>(p_color_frame[(i * (
unsigned int)color_width + j) * nb_color_pixel]) << 16 |
1107 static_cast<uint32_t
>(p_color_frame[(i * (
unsigned int)color_width + j) * nb_color_pixel + 1]) << 8 |
1108 static_cast<uint32_t
>(p_color_frame[(i * (
unsigned int)color_width + j) * nb_color_pixel + 2]));
1111 pointcloud->points[(size_t)(i * depth_width + j)].rgb = *
reinterpret_cast<float *
>(&rgb);
1114 pointcloud->points[(size_t)depth_pixel_index].b =
1115 p_color_frame[(i * (
unsigned int)color_width + j) * nb_color_pixel];
1116 pointcloud->points[(size_t)depth_pixel_index].g =
1117 p_color_frame[(i * (
unsigned int)color_width + j) * nb_color_pixel + 1];
1118 pointcloud->points[(size_t)depth_pixel_index].r =
1119 p_color_frame[(i * (
unsigned int)color_width + j) * nb_color_pixel + 2];
1122 pointcloud->points[(size_t)depth_pixel_index].r =
1123 p_color_frame[(i * (
unsigned int)color_width + j) * nb_color_pixel];
1124 pointcloud->points[(size_t)depth_pixel_index].g =
1125 p_color_frame[(i * (
unsigned int)color_width + j) * nb_color_pixel + 1];
1126 pointcloud->points[(size_t)depth_pixel_index].b =
1127 p_color_frame[(i * (
unsigned int)color_width + j) * nb_color_pixel + 2];
1148 if (from_index != -1)
1150 if (from_index == 1)
1152 else if (from_index == 2)
1159 rs2_extrinsics extrinsics = from_stream.get_extrinsics_to(to_stream);
1163 for (
unsigned int i = 0; i < 3; i++) {
1164 t[i] = extrinsics.translation[i];
1165 for (
unsigned int j = 0; j < 3; j++)
1166 R[i][j] = extrinsics.rotation[j * 3 + i];
1173 #if (RS2_API_VERSION > ((2 * 10000) + (31 * 100) + 0))
1186 auto frame =
m_pipe.wait_for_frames();
1187 auto f = frame.first_or_default(RS2_STREAM_POSE);
1188 auto pose_data = f.as<rs2::pose_frame>().get_pose_data();
1191 *ts = frame.get_timestamp();
1193 if (cMw !=
nullptr) {
1194 m_pos[0] =
static_cast<double>(pose_data.translation.x);
1195 m_pos[1] =
static_cast<double>(pose_data.translation.y);
1196 m_pos[2] =
static_cast<double>(pose_data.translation.z);
1198 m_quat[0] =
static_cast<double>(pose_data.rotation.x);
1199 m_quat[1] =
static_cast<double>(pose_data.rotation.y);
1200 m_quat[2] =
static_cast<double>(pose_data.rotation.z);
1201 m_quat[3] =
static_cast<double>(pose_data.rotation.w);
1206 if (odo_vel !=
nullptr) {
1207 odo_vel->
resize(6,
false);
1208 (*odo_vel)[0] =
static_cast<double>(pose_data.velocity.x);
1209 (*odo_vel)[1] =
static_cast<double>(pose_data.velocity.y);
1210 (*odo_vel)[2] =
static_cast<double>(pose_data.velocity.z);
1211 (*odo_vel)[3] =
static_cast<double>(pose_data.angular_velocity.x);
1212 (*odo_vel)[4] =
static_cast<double>(pose_data.angular_velocity.y);
1213 (*odo_vel)[5] =
static_cast<double>(pose_data.angular_velocity.z);
1216 if (odo_acc !=
nullptr) {
1217 odo_acc->
resize(6,
false);
1218 (*odo_acc)[0] =
static_cast<double>(pose_data.acceleration.x);
1219 (*odo_acc)[1] =
static_cast<double>(pose_data.acceleration.y);
1220 (*odo_acc)[2] =
static_cast<double>(pose_data.acceleration.z);
1221 (*odo_acc)[3] =
static_cast<double>(pose_data.angular_acceleration.x);
1222 (*odo_acc)[4] =
static_cast<double>(pose_data.angular_acceleration.y);
1223 (*odo_acc)[5] =
static_cast<double>(pose_data.angular_acceleration.z);
1226 return pose_data.tracker_confidence;
1251 auto frame =
m_pipe.wait_for_frames();
1252 auto f = frame.first_or_default(RS2_STREAM_ACCEL);
1253 auto imu_acc_data = f.as<rs2::motion_frame>().get_motion_data();
1256 *ts = f.get_timestamp();
1258 if (imu_acc !=
nullptr) {
1259 imu_acc->
resize(3,
false);
1260 (*imu_acc)[0] =
static_cast<double>(imu_acc_data.x);
1261 (*imu_acc)[1] =
static_cast<double>(imu_acc_data.y);
1262 (*imu_acc)[2] =
static_cast<double>(imu_acc_data.z);
1288 auto frame =
m_pipe.wait_for_frames();
1289 auto f = frame.first_or_default(RS2_STREAM_GYRO);
1290 auto imu_vel_data = f.as<rs2::motion_frame>().get_motion_data();
1293 *ts = f.get_timestamp();
1295 if (imu_vel !=
nullptr) {
1296 imu_vel->
resize(3,
false);
1297 (*imu_vel)[0] =
static_cast<double>(imu_vel_data.x);
1298 (*imu_vel)[1] =
static_cast<double>(imu_vel_data.x);
1299 (*imu_vel)[2] =
static_cast<double>(imu_vel_data.x);
1325 auto data =
m_pipe.wait_for_frames();
1328 *ts = data.get_timestamp();
1330 if (imu_acc !=
nullptr) {
1331 auto acc_data = data.first_or_default(RS2_STREAM_ACCEL);
1332 auto imu_acc_data = acc_data.as<rs2::motion_frame>().get_motion_data();
1334 imu_acc->
resize(3,
false);
1335 (*imu_acc)[0] =
static_cast<double>(imu_acc_data.x);
1336 (*imu_acc)[1] =
static_cast<double>(imu_acc_data.y);
1337 (*imu_acc)[2] =
static_cast<double>(imu_acc_data.z);
1340 if (imu_vel !=
nullptr) {
1341 auto vel_data = data.first_or_default(RS2_STREAM_GYRO);
1342 auto imu_vel_data = vel_data.as<rs2::motion_frame>().get_motion_data();
1344 imu_vel->
resize(3,
false);
1345 (*imu_vel)[0] =
static_cast<double>(imu_vel_data.x);
1346 (*imu_vel)[1] =
static_cast<double>(imu_vel_data.y);
1347 (*imu_vel)[2] =
static_cast<double>(imu_vel_data.z);
1365 #if (RS2_API_VERSION > ((2 * 10000) + (31 * 100) + 0))
1371 for (rs2::sensor &sensor : dev.query_sensors()) {
1373 if (rs2::depth_sensor dpt = sensor.as<rs2::depth_sensor>()) {
1398 #if (RS2_API_VERSION > ((2 * 10000) + (31 * 100) + 0))
1404 for (rs2::sensor &sensor : dev.query_sensors()) {
1406 if (rs2::depth_sensor dpt = sensor.as<rs2::depth_sensor>()) {
1421 #if (RS2_API_VERSION > ((2 * 10000) + (31 * 100) + 0))
1423 rs2::pipeline *pipe =
new rs2::pipeline;
1424 rs2::pipeline_profile *pipelineProfile =
new rs2::pipeline_profile;
1425 *pipelineProfile = pipe->start();
1427 rs2::device dev = pipelineProfile->get_device();
1429 #if (RS2_API_VERSION > ((2 * 10000) + (31 * 100) + 0))
1436 delete pipelineProfile;
1441 return (std::string(
"unknown"));
1448 void print(
const rs2_extrinsics &extrinsics, std::ostream &os)
1450 std::stringstream ss;
1451 ss <<
"Rotation Matrix:\n";
1453 for (
auto i = 0; i < 3; ++i) {
1454 for (
auto j = 0; j < 3; ++j) {
1455 ss << std::left << std::setw(15) << std::setprecision(5) << extrinsics.rotation[j * 3 + i];
1460 ss <<
"\nTranslation Vector: ";
1461 for (
size_t i = 0; i <
sizeof(extrinsics.translation) /
sizeof(extrinsics.translation[0]); ++i)
1462 ss << std::setprecision(15) << extrinsics.translation[i] <<
" ";
1464 os << ss.str() <<
"\n\n";
1467 void print(
const rs2_intrinsics &intrinsics, std::ostream &os)
1469 std::stringstream ss;
1470 ss << std::left << std::setw(14) <<
"Width: "
1471 <<
"\t" << intrinsics.width <<
"\n"
1472 << std::left << std::setw(14) <<
"Height: "
1473 <<
"\t" << intrinsics.height <<
"\n"
1474 << std::left << std::setw(14) <<
"PPX: "
1475 <<
"\t" << std::setprecision(15) << intrinsics.ppx <<
"\n"
1476 << std::left << std::setw(14) <<
"PPY: "
1477 <<
"\t" << std::setprecision(15) << intrinsics.ppy <<
"\n"
1478 << std::left << std::setw(14) <<
"Fx: "
1479 <<
"\t" << std::setprecision(15) << intrinsics.fx <<
"\n"
1480 << std::left << std::setw(14) <<
"Fy: "
1481 <<
"\t" << std::setprecision(15) << intrinsics.fy <<
"\n"
1482 << std::left << std::setw(14) <<
"Distortion: "
1483 <<
"\t" << rs2_distortion_to_string(intrinsics.model) <<
"\n"
1484 << std::left << std::setw(14) <<
"Coeffs: ";
1486 for (
size_t i = 0; i <
sizeof(intrinsics.coeffs) /
sizeof(intrinsics.coeffs[0]); ++i)
1487 ss <<
"\t" << std::setprecision(15) << intrinsics.coeffs[i] <<
" ";
1489 os << ss.str() <<
"\n\n";
1492 void safe_get_intrinsics(
const rs2::video_stream_profile &profile, rs2_intrinsics &intrinsics)
1495 intrinsics = profile.get_intrinsics();
1501 bool operator==(
const rs2_intrinsics &lhs,
const rs2_intrinsics &rhs)
1503 return lhs.width == rhs.width && lhs.height == rhs.height && lhs.ppx == rhs.ppx && lhs.ppy == rhs.ppy &&
1504 lhs.fx == rhs.fx && lhs.fy == rhs.fy && lhs.model == rhs.model &&
1505 !std::memcmp(lhs.coeffs, rhs.coeffs,
sizeof(rhs.coeffs));
1508 std::string get_str_formats(
const std::set<rs2_format> &formats)
1510 std::stringstream ss;
1511 for (
auto format = formats.begin(); format != formats.end(); ++format) {
1512 ss << *format << ((format != formats.end()) && (next(format) == formats.end()) ?
"" :
"/");
1517 struct stream_and_resolution
1523 std::string stream_name;
1525 bool operator<(
const stream_and_resolution &obj)
const
1527 return (std::make_tuple(stream, stream_index, width, height) <
1528 std::make_tuple(obj.stream, obj.stream_index, obj.width, obj.height));
1532 struct stream_and_index
1537 bool operator<(
const stream_and_index &obj)
const
1539 return (std::make_tuple(stream, stream_index) < std::make_tuple(obj.stream, obj.stream_index));
1567 os << std::left << std::setw(30) << dev.get_info(RS2_CAMERA_INFO_NAME) << std::setw(20)
1568 << dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER) << std::setw(20) << dev.get_info(RS2_CAMERA_INFO_FIRMWARE_VERSION)
1572 os <<
" Device info: \n";
1573 for (
auto j = 0; j < RS2_CAMERA_INFO_COUNT; ++j) {
1574 auto param =
static_cast<rs2_camera_info
>(j);
1575 if (dev.supports(param))
1576 os <<
" " << std::left << std::setw(30) << rs2_camera_info_to_string(rs2_camera_info(param)) <<
": \t"
1577 << dev.get_info(param) <<
"\n";
1582 for (
auto &&sensor : dev.query_sensors()) {
1583 os <<
"Options for " << sensor.get_info(RS2_CAMERA_INFO_NAME) << std::endl;
1585 os << std::setw(55) <<
" Supported options:" << std::setw(10) <<
"min" << std::setw(10) <<
" max" << std::setw(6)
1586 <<
" step" << std::setw(10) <<
" default" << std::endl;
1587 for (
auto j = 0; j < RS2_OPTION_COUNT; ++j) {
1588 auto opt =
static_cast<rs2_option
>(j);
1589 if (sensor.supports(opt)) {
1590 auto range = sensor.get_option_range(opt);
1591 os <<
" " << std::left << std::setw(50) << opt <<
" : " << std::setw(5) << range.min <<
"... "
1592 << std::setw(12) << range.max << std::setw(6) << range.step << std::setw(10) << range.def <<
"\n";
1599 for (
auto &&sensor : dev.query_sensors()) {
1600 os <<
"Stream Profiles supported by " << sensor.get_info(RS2_CAMERA_INFO_NAME) <<
"\n";
1602 os << std::setw(55) <<
" Supported modes:" << std::setw(10) <<
"stream" << std::setw(10) <<
" resolution"
1603 << std::setw(6) <<
" fps" << std::setw(10) <<
" format"
1606 for (
auto &&profile : sensor.get_stream_profiles()) {
1607 if (
auto video = profile.as<rs2::video_stream_profile>()) {
1608 os <<
" " << profile.stream_name() <<
"\t " << video.width() <<
"x" << video.height() <<
"\t@ "
1609 << profile.fps() <<
"Hz\t" << profile.format() <<
"\n";
1612 os <<
" " << profile.stream_name() <<
"\t@ " << profile.fps() <<
"Hz\t" << profile.format() <<
"\n";
1619 std::map<stream_and_index, rs2::stream_profile> streams;
1620 std::map<stream_and_resolution, std::vector<std::pair<std::set<rs2_format>, rs2_intrinsics> > > intrinsics_map;
1621 for (
auto &&sensor : dev.query_sensors()) {
1623 for (
auto &&profile : sensor.get_stream_profiles()) {
1624 if (
auto video = profile.as<rs2::video_stream_profile>()) {
1625 if (streams.find(stream_and_index { profile.stream_type(), profile.stream_index() }) == streams.end()) {
1626 streams[stream_and_index { profile.stream_type(), profile.stream_index() }] = profile;
1629 rs2_intrinsics intrinsics {};
1630 stream_and_resolution stream_res { profile.stream_type(), profile.stream_index(), video.width(), video.height(),
1631 profile.stream_name() };
1632 safe_get_intrinsics(video, intrinsics);
1633 auto it = std::find_if(
1634 (intrinsics_map[stream_res]).begin(), (intrinsics_map[stream_res]).end(),
1635 [&](
const std::pair<std::set<rs2_format>, rs2_intrinsics> &kvp) {
return intrinsics == kvp.second; });
1636 if (it == (intrinsics_map[stream_res]).end()) {
1637 (intrinsics_map[stream_res]).push_back({ {profile.format()}, intrinsics });
1640 it->first.insert(profile.format());
1648 os <<
"Provided Intrinsic:\n";
1649 for (
auto &kvp : intrinsics_map) {
1650 auto stream_res = kvp.first;
1651 for (
auto &intrinsics : kvp.second) {
1652 auto formats = get_str_formats(intrinsics.first);
1653 os <<
"Intrinsic of \"" << stream_res.stream_name <<
"\"\t " << stream_res.width <<
"x" << stream_res.height
1654 <<
"\t " << formats <<
"\n";
1655 if (intrinsics.second == rs2_intrinsics {}) {
1656 os <<
"Intrinsic NOT available!\n\n";
1659 print(intrinsics.second, os);
1665 os <<
"\nProvided Extrinsic:\n";
1666 for (
auto kvp1 = streams.begin(); kvp1 != streams.end(); ++kvp1) {
1667 for (
auto kvp2 = streams.begin(); kvp2 != streams.end(); ++kvp2) {
1668 os <<
"Extrinsic from \"" << kvp1->second.stream_name() <<
"\"\t "
1670 <<
"\t \"" << kvp2->second.stream_name() <<
"\"\n";
1671 auto extrinsics = kvp1->second.get_extrinsics_to(kvp2->second);
1672 print(extrinsics, os);
1679 #elif !defined(VISP_BUILD_SHARED_LIBS)
1682 void dummy_vpRealSense2() { };
bool operator==(const vpArray2D< double > &A) const
friend std::ostream & operator<<(std::ostream &s, const vpArray2D< Type > &A)
Generic class defining intrinsic camera parameters.
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
vpCameraParametersProjType
@ perspectiveProjWithDistortion
Perspective projection with distortion model.
@ ProjWithKannalaBrandtDistortion
Projection with Kannala-Brandt distortion model.
@ perspectiveProjWithoutDistortion
Perspective projection without distortion model.
void initPersProjWithDistortion(double px, double py, double u0, double v0, double kud, double kdu)
void initProjWithKannalaBrandtDistortion(double px, double py, double u0, double v0, const std::vector< double > &distortion_coefficients)
Implementation of column vector and the associated operations.
void resize(unsigned int i, bool flagNullify=true)
error that can be emitted by ViSP classes.
Implementation of an homogeneous matrix and operations on such kind of matrices.
static void RGBToGrey(unsigned char *rgb, unsigned char *grey, unsigned int width, unsigned int height, bool flip=false)
static void RGBaToGrey(unsigned char *rgba, unsigned char *grey, unsigned int width, unsigned int height, unsigned int nThreads=0)
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, unsigned int nThreads=0)
static void BGRToRGBa(unsigned char *bgr, unsigned char *rgba, unsigned int width, unsigned int height, bool flip=false)
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
Type * bitmap
points toward the bitmap
void getNativeFrameData(const rs2::frame &frame, unsigned char *const data)
vpCameraParameters getCameraParameters(const rs2_stream &stream, vpCameraParameters::vpCameraParametersProjType type=vpCameraParameters::perspectiveProjWithDistortion, int index=-1) const
void acquire(vpImage< unsigned char > &grey, double *ts=nullptr)
vpQuaternionVector m_quat
bool open(const rs2::config &cfg=rs2::config())
rs2_intrinsics getIntrinsics(const rs2_stream &stream, int index=-1) const
rs2::pointcloud m_pointcloud
void getIMUAcceleration(vpColVector *imu_acc, double *ts)
void getPointcloud(const rs2::depth_frame &depth_frame, std::vector< vpColVector > &pointcloud)
void getColorFrame(const rs2::frame &frame, vpImage< vpRGBa > &color)
void getIMUVelocity(vpColVector *imu_vel, double *ts)
vpTranslationVector m_pos
std::string m_product_line
std::string getProductLine()
unsigned int getOdometryData(vpHomogeneousMatrix *cMw, vpColVector *odo_vel, vpColVector *odo_acc, double *ts=nullptr)
void getIMUData(vpColVector *imu_vel, vpColVector *imu_acc, double *ts)
vpHomogeneousMatrix getTransformation(const rs2_stream &from, const rs2_stream &to, int from_index=-1) const
void getGreyFrame(const rs2::frame &frame, vpImage< unsigned char > &grey)
float m_invalidDepthValue
rs2::pipeline_profile m_pipelineProfile
Implementation of a rotation matrix and operations on such kind of matrices.
Class that consider the case of a translation vector.