42 #include <visp3/core/vpImageConvert.h>
43 #include <visp3/sensor/vpRealSense.h>
45 #if defined(VISP_HAVE_REALSENSE) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
47 #include "vpRealSense_impl.h"
53 : m_context(), m_device(NULL), m_num_devices(0), m_serial_no(), m_intrinsics(), m_max_Z(8), m_enableStreams(),
54 m_useStreamPresets(), m_streamPresets(), m_streamParams(), m_invalidDepthValue(0.0f)
105 std::vector<rs::device *> detected_devices;
109 std::string serial_no = device->get_serial();
113 detected_devices.push_back(device);
120 "RealSense Camera - Multiple cameras detected (%d) but "
121 "no input serial number specified. Exiting!",
128 "RealSense Camera - No camera detected with input "
129 "serial_no \"%s\" Exiting!",
139 std::cout <<
"RealSense Camera - Connecting to camera with Serial No: " <<
m_serial_no << std::endl;
143 ?
m_device->supports(rs::capabilities::infrared2)
150 for (
int j = 0; j < 4; j++) {
151 auto s = (rs::stream)j;
152 auto capabilities = (rs::capabilities)j;
204 for (
int i = 0; i < 4; ++i) {
205 auto stream = rs::stream(i);
206 if (!
m_device->is_stream_enabled(stream))
208 auto intrin =
m_device->get_stream_intrinsics(stream);
215 m_intrinsics[rs::stream::rectified_color] =
m_device->get_stream_intrinsics(rs::stream::rectified_color);
219 m_device->get_stream_intrinsics(rs::stream::color_aligned_to_depth);
221 m_device->get_stream_intrinsics(rs::stream::depth_aligned_to_color);
222 m_intrinsics[rs::stream::depth_aligned_to_rectified_color] =
223 m_device->get_stream_intrinsics(rs::stream::depth_aligned_to_rectified_color);
229 m_device->get_stream_intrinsics(rs::stream::depth_aligned_to_infrared2);
231 m_device->get_stream_intrinsics(rs::stream::infrared2_aligned_to_depth);
270 "RealSense Camera - Multiple cameras detected (%d) but "
271 "no input serial number specified. Exiting!",
293 double u0 = intrinsics.ppx;
294 double v0 = intrinsics.ppy;
295 double px = intrinsics.fx;
296 double py = intrinsics.fy;
298 double kdu = intrinsics.coeffs[0];
316 "retrieve intrinsics. Exiting!");
319 return m_device->get_stream_intrinsics(stream);
333 "retrieve extrinsics. Exiting!");
335 if (!
m_device->is_stream_enabled(from)) {
337 "RealSense Camera - stream (%d) is not enabled to "
338 "retrieve extrinsics. Exiting!",
341 if (!
m_device->is_stream_enabled(to)) {
343 "RealSense Camera - stream (%d) is not enabled to "
344 "retrieve extrinsics. Exiting!",
347 return m_device->get_extrinsics(from, to);
362 for (
unsigned int i = 0; i < 3; i++) {
363 t[i] = extrinsics.translation[i];
364 for (
unsigned int j = 0; j < 3; j++)
365 R[i][j] = extrinsics.rotation[j * 3 + i];
465 std::vector<vpColVector> &pointcloud)
499 std::vector<vpColVector> &pointcloud)
566 std::vector<vpColVector> *
const data_pointCloud,
unsigned char *
const data_infrared,
567 unsigned char *
const data_infrared2,
const rs::stream &stream_color,
568 const rs::stream &stream_depth,
const rs::stream &stream_infrared,
569 const rs::stream &stream_infrared2)
581 if (data_image != NULL) {
582 vp_rs_get_native_frame_data_impl(
m_device,
m_intrinsics, rs::stream::color, data_image, stream_color);
585 if (data_depth != NULL) {
586 vp_rs_get_native_frame_data_impl(
m_device,
m_intrinsics, rs::stream::depth, data_depth, stream_depth);
588 if (data_pointCloud != NULL) {
593 if (data_infrared != NULL) {
594 vp_rs_get_native_frame_data_impl(
m_device,
m_intrinsics, rs::stream::infrared, data_infrared, stream_infrared);
597 if (data_infrared2 != NULL) {
598 vp_rs_get_native_frame_data_impl(
m_device,
m_intrinsics, rs::stream::infrared2, data_infrared2, stream_infrared2);
764 pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud)
796 pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud)
828 pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pointcloud)
860 pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pointcloud)
904 std::vector<vpColVector> *
const data_pointCloud,
905 pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud,
unsigned char *
const data_infrared,
906 unsigned char *
const data_infrared2,
const rs::stream &stream_color,
907 const rs::stream &stream_depth,
const rs::stream &stream_infrared,
908 const rs::stream &stream_infrared2)
920 if (data_image != NULL) {
921 vp_rs_get_native_frame_data_impl(
m_device,
m_intrinsics, rs::stream::color, data_image, stream_color);
924 if (data_depth != NULL) {
925 vp_rs_get_native_frame_data_impl(
m_device,
m_intrinsics, rs::stream::depth, data_depth, stream_depth);
928 if (data_pointCloud != NULL) {
932 if (pointcloud != NULL) {
936 if (data_infrared != NULL) {
937 vp_rs_get_native_frame_data_impl(
m_device,
m_intrinsics, rs::stream::infrared, data_infrared, stream_infrared);
940 if (data_infrared2 != NULL) {
941 vp_rs_get_native_frame_data_impl(
m_device,
m_intrinsics, rs::stream::infrared2, data_infrared2, stream_infrared2);
964 std::vector<vpColVector> *
const data_pointCloud,
965 pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pointcloud,
unsigned char *
const data_infrared,
966 unsigned char *
const data_infrared2,
const rs::stream &stream_color,
967 const rs::stream &stream_depth,
const rs::stream &stream_infrared,
968 const rs::stream &stream_infrared2)
980 if (data_image != NULL) {
981 vp_rs_get_native_frame_data_impl(
m_device,
m_intrinsics, rs::stream::color, data_image, stream_color);
984 if (data_depth != NULL) {
985 vp_rs_get_native_frame_data_impl(
m_device,
m_intrinsics, rs::stream::depth, data_depth, stream_depth);
988 if (data_pointCloud != NULL) {
992 if (pointcloud != NULL) {
997 if (data_infrared != NULL) {
998 vp_rs_get_native_frame_data_impl(
m_device,
m_intrinsics, rs::stream::infrared, data_infrared, stream_infrared);
1001 if (data_infrared2 != NULL) {
1002 vp_rs_get_native_frame_data_impl(
m_device,
m_intrinsics, rs::stream::infrared2, data_infrared2, stream_infrared2);
1029 os <<
"Device name: " << rs.
getHandler()->get_name() << std::endl;
1030 std::streamsize ss = std::cout.precision();
1031 for (
int i = 0; i < 4; ++i) {
1032 auto stream = rs::stream(i);
1033 if (!rs.
getHandler()->is_stream_enabled(stream))
1035 auto intrin = rs.
getHandler()->get_stream_intrinsics(stream);
1036 std::cout <<
"Capturing " << stream <<
" at " << intrin.width <<
" x " << intrin.height;
1037 std::cout << std::setprecision(1) << std::fixed <<
", fov = " << intrin.hfov() <<
" x " << intrin.vfov()
1038 <<
", distortion = " << intrin.model() << std::endl;
1040 std::cout.precision(ss);
1045 #elif !defined(VISP_BUILD_SHARED_LIBS)
1048 void dummy_vpRealSense(){};
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
void initPersProjWithDistortion(double px, double py, double u0, double v0, double kud, double kdu)
error that can be emited by ViSP classes.
Implementation of an homogeneous matrix and operations on such kind of matrices.
void setDeviceBySerialNumber(const std::string &serial_no)
void setStreamSettings(const rs::stream &stream, const rs::preset &preset)
std::map< rs::stream, bool > m_enableStreams
rs::device * getHandler() const
Get access to device handler.
rs::extrinsics getExtrinsics(const rs::stream &from, const rs::stream &to) const
vpCameraParameters getCameraParameters(const rs::stream &stream, vpCameraParameters::vpCameraParametersProjType type=vpCameraParameters::perspectiveProjWithDistortion) const
void acquire(std::vector< vpColVector > &pointcloud)
std::map< rs::stream, bool > m_useStreamPresets
vpHomogeneousMatrix getTransformation(const rs::stream &from, const rs::stream &to) const
void setEnableStream(const rs::stream &stream, bool status)
std::map< rs::stream, rs::intrinsics > m_intrinsics
std::map< rs::stream, rs::preset > m_streamPresets
float m_invalidDepthValue
rs::intrinsics getIntrinsics(const rs::stream &stream) const
std::map< rs::stream, vpRsStreamParams > m_streamParams
float m_max_Z
Maximal Z depth in meter.
Implementation of a rotation matrix and operations on such kind of matrices.
Class that consider the case of a translation vector.