39 #include <visp3/core/vpImageConvert.h>
40 #include <visp3/sensor/vpRealSense.h>
42 #if defined(VISP_HAVE_REALSENSE) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
44 #include "vpRealSense_impl.h"
50 : m_context(), m_device(nullptr), m_num_devices(0), m_serial_no(), m_intrinsics(), m_max_Z(8), m_enableStreams(),
51 m_useStreamPresets(), m_streamPresets(), m_streamParams(), m_invalidDepthValue(0.0f)
102 std::vector<rs::device *> detected_devices;
106 std::string serial_no = device->get_serial();
110 detected_devices.push_back(device);
117 "RealSense Camera - Multiple cameras detected (%d) but "
118 "no input serial number specified. Exiting!",
125 "RealSense Camera - No camera detected with input "
126 "serial_no \"%s\" Exiting!",
136 std::cout <<
"RealSense Camera - Connecting to camera with Serial No: " <<
m_serial_no << std::endl;
140 ?
m_device->supports(rs::capabilities::infrared2)
147 for (
int j = 0; j < 4; j++) {
148 auto s = (rs::stream)j;
149 auto capabilities = (rs::capabilities)j;
205 for (
int i = 0; i < 4; ++i) {
206 auto stream = rs::stream(i);
207 if (!
m_device->is_stream_enabled(stream))
209 auto intrin =
m_device->get_stream_intrinsics(stream);
216 m_intrinsics[rs::stream::rectified_color] =
m_device->get_stream_intrinsics(rs::stream::rectified_color);
220 m_device->get_stream_intrinsics(rs::stream::color_aligned_to_depth);
222 m_device->get_stream_intrinsics(rs::stream::depth_aligned_to_color);
223 m_intrinsics[rs::stream::depth_aligned_to_rectified_color] =
224 m_device->get_stream_intrinsics(rs::stream::depth_aligned_to_rectified_color);
230 m_device->get_stream_intrinsics(rs::stream::depth_aligned_to_infrared2);
232 m_device->get_stream_intrinsics(rs::stream::infrared2_aligned_to_depth);
271 "RealSense Camera - Multiple cameras detected (%d) but "
272 "no input serial number specified. Exiting!",
294 double u0 = intrinsics.ppx;
295 double v0 = intrinsics.ppy;
296 double px = intrinsics.fx;
297 double py = intrinsics.fy;
299 double kdu = intrinsics.coeffs[0];
318 "retrieve intrinsics. Exiting!");
321 return m_device->get_stream_intrinsics(stream);
335 "retrieve extrinsics. Exiting!");
337 if (!
m_device->is_stream_enabled(from)) {
339 "RealSense Camera - stream (%d) is not enabled to "
340 "retrieve extrinsics. Exiting!",
343 if (!
m_device->is_stream_enabled(to)) {
345 "RealSense Camera - stream (%d) is not enabled to "
346 "retrieve extrinsics. Exiting!",
349 return m_device->get_extrinsics(from, to);
364 for (
unsigned int i = 0; i < 3; i++) {
365 t[i] = extrinsics.translation[i];
366 for (
unsigned int j = 0; j < 3; j++)
367 R[i][j] = extrinsics.rotation[j * 3 + i];
467 std::vector<vpColVector> &pointcloud)
501 std::vector<vpColVector> &pointcloud)
568 std::vector<vpColVector> *
const data_pointCloud,
unsigned char *
const data_infrared,
569 unsigned char *
const data_infrared2,
const rs::stream &stream_color,
570 const rs::stream &stream_depth,
const rs::stream &stream_infrared,
571 const rs::stream &stream_infrared2)
583 if (data_image !=
nullptr) {
584 vp_rs_get_native_frame_data_impl(
m_device,
m_intrinsics, rs::stream::color, data_image, stream_color);
587 if (data_depth !=
nullptr) {
588 vp_rs_get_native_frame_data_impl(
m_device,
m_intrinsics, rs::stream::depth, data_depth, stream_depth);
590 if (data_pointCloud !=
nullptr) {
595 if (data_infrared !=
nullptr) {
596 vp_rs_get_native_frame_data_impl(
m_device,
m_intrinsics, rs::stream::infrared, data_infrared, stream_infrared);
599 if (data_infrared2 !=
nullptr) {
600 vp_rs_get_native_frame_data_impl(
m_device,
m_intrinsics, rs::stream::infrared2, data_infrared2, stream_infrared2);
673 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
766 pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud)
798 pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud)
830 pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pointcloud)
862 pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pointcloud)
906 std::vector<vpColVector> *
const data_pointCloud,
907 pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud,
unsigned char *
const data_infrared,
908 unsigned char *
const data_infrared2,
const rs::stream &stream_color,
909 const rs::stream &stream_depth,
const rs::stream &stream_infrared,
910 const rs::stream &stream_infrared2)
922 if (data_image !=
nullptr) {
923 vp_rs_get_native_frame_data_impl(
m_device,
m_intrinsics, rs::stream::color, data_image, stream_color);
926 if (data_depth !=
nullptr) {
927 vp_rs_get_native_frame_data_impl(
m_device,
m_intrinsics, rs::stream::depth, data_depth, stream_depth);
930 if (data_pointCloud !=
nullptr) {
934 if (pointcloud !=
nullptr) {
938 if (data_infrared !=
nullptr) {
939 vp_rs_get_native_frame_data_impl(
m_device,
m_intrinsics, rs::stream::infrared, data_infrared, stream_infrared);
942 if (data_infrared2 !=
nullptr) {
943 vp_rs_get_native_frame_data_impl(
m_device,
m_intrinsics, rs::stream::infrared2, data_infrared2, stream_infrared2);
966 std::vector<vpColVector> *
const data_pointCloud,
967 pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pointcloud,
unsigned char *
const data_infrared,
968 unsigned char *
const data_infrared2,
const rs::stream &stream_color,
969 const rs::stream &stream_depth,
const rs::stream &stream_infrared,
970 const rs::stream &stream_infrared2)
982 if (data_image !=
nullptr) {
983 vp_rs_get_native_frame_data_impl(
m_device,
m_intrinsics, rs::stream::color, data_image, stream_color);
986 if (data_depth !=
nullptr) {
987 vp_rs_get_native_frame_data_impl(
m_device,
m_intrinsics, rs::stream::depth, data_depth, stream_depth);
990 if (data_pointCloud !=
nullptr) {
994 if (pointcloud !=
nullptr) {
999 if (data_infrared !=
nullptr) {
1000 vp_rs_get_native_frame_data_impl(
m_device,
m_intrinsics, rs::stream::infrared, data_infrared, stream_infrared);
1003 if (data_infrared2 !=
nullptr) {
1004 vp_rs_get_native_frame_data_impl(
m_device,
m_intrinsics, rs::stream::infrared2, data_infrared2, stream_infrared2);
1031 os <<
"Device name: " << rs.
getHandler()->get_name() << std::endl;
1032 std::streamsize ss = std::cout.precision();
1033 for (
int i = 0; i < 4; ++i) {
1034 auto stream = rs::stream(i);
1035 if (!rs.
getHandler()->is_stream_enabled(stream))
1037 auto intrin = rs.
getHandler()->get_stream_intrinsics(stream);
1038 std::cout <<
"Capturing " << stream <<
" at " << intrin.width <<
" x " << intrin.height;
1039 std::cout << std::setprecision(1) << std::fixed <<
", fov = " << intrin.hfov() <<
" x " << intrin.vfov()
1040 <<
", distortion = " << intrin.model() << std::endl;
1042 std::cout.precision(ss);
1047 #elif !defined(VISP_BUILD_SHARED_LIBS)
1049 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
Perspective projection with distortion model.
void initPersProjWithDistortion(double px, double py, double u0, double v0, double kud, double kdu)
error that can be emitted 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.