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(NULL), 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;
201 for (
int i = 0; i < 4; ++i) {
202 auto stream = rs::stream(i);
203 if (!
m_device->is_stream_enabled(stream))
205 auto intrin =
m_device->get_stream_intrinsics(stream);
212 m_intrinsics[rs::stream::rectified_color] =
m_device->get_stream_intrinsics(rs::stream::rectified_color);
216 m_device->get_stream_intrinsics(rs::stream::color_aligned_to_depth);
218 m_device->get_stream_intrinsics(rs::stream::depth_aligned_to_color);
219 m_intrinsics[rs::stream::depth_aligned_to_rectified_color] =
220 m_device->get_stream_intrinsics(rs::stream::depth_aligned_to_rectified_color);
226 m_device->get_stream_intrinsics(rs::stream::depth_aligned_to_infrared2);
228 m_device->get_stream_intrinsics(rs::stream::infrared2_aligned_to_depth);
267 "RealSense Camera - Multiple cameras detected (%d) but "
268 "no input serial number specified. Exiting!",
290 double u0 = intrinsics.ppx;
291 double v0 = intrinsics.ppy;
292 double px = intrinsics.fx;
293 double py = intrinsics.fy;
295 double kdu = intrinsics.coeffs[0];
313 "retrieve intrinsics. Exiting!");
316 return m_device->get_stream_intrinsics(stream);
330 "retrieve extrinsics. Exiting!");
332 if (!
m_device->is_stream_enabled(from)) {
334 "RealSense Camera - stream (%d) is not enabled to "
335 "retrieve extrinsics. Exiting!",
338 if (!
m_device->is_stream_enabled(to)) {
340 "RealSense Camera - stream (%d) is not enabled to "
341 "retrieve extrinsics. Exiting!",
344 return m_device->get_extrinsics(from, to);
359 for (
unsigned int i = 0; i < 3; i++) {
360 t[i] = extrinsics.translation[i];
361 for (
unsigned int j = 0; j < 3; j++)
362 R[i][j] = extrinsics.rotation[j * 3 + i];
462 std::vector<vpColVector> &pointcloud)
496 std::vector<vpColVector> &pointcloud)
563 std::vector<vpColVector> *
const data_pointCloud,
unsigned char *
const data_infrared,
564 unsigned char *
const data_infrared2,
const rs::stream &stream_color,
565 const rs::stream &stream_depth,
const rs::stream &stream_infrared,
566 const rs::stream &stream_infrared2)
578 if (data_image != NULL) {
579 vp_rs_get_native_frame_data_impl(
m_device,
m_intrinsics, rs::stream::color, data_image, stream_color);
582 if (data_depth != NULL) {
583 vp_rs_get_native_frame_data_impl(
m_device,
m_intrinsics, rs::stream::depth, data_depth, stream_depth);
585 if (data_pointCloud != NULL) {
590 if (data_infrared != NULL) {
591 vp_rs_get_native_frame_data_impl(
m_device,
m_intrinsics, rs::stream::infrared, data_infrared, stream_infrared);
594 if (data_infrared2 != NULL) {
595 vp_rs_get_native_frame_data_impl(
m_device,
m_intrinsics, rs::stream::infrared2, data_infrared2, stream_infrared2);
761 pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud)
793 pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud)
825 pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pointcloud)
857 pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pointcloud)
901 std::vector<vpColVector> *
const data_pointCloud,
902 pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud,
unsigned char *
const data_infrared,
903 unsigned char *
const data_infrared2,
const rs::stream &stream_color,
904 const rs::stream &stream_depth,
const rs::stream &stream_infrared,
905 const rs::stream &stream_infrared2)
917 if (data_image != NULL) {
918 vp_rs_get_native_frame_data_impl(
m_device,
m_intrinsics, rs::stream::color, data_image, stream_color);
921 if (data_depth != NULL) {
922 vp_rs_get_native_frame_data_impl(
m_device,
m_intrinsics, rs::stream::depth, data_depth, stream_depth);
925 if (data_pointCloud != NULL) {
929 if (pointcloud != NULL) {
933 if (data_infrared != NULL) {
934 vp_rs_get_native_frame_data_impl(
m_device,
m_intrinsics, rs::stream::infrared, data_infrared, stream_infrared);
937 if (data_infrared2 != NULL) {
938 vp_rs_get_native_frame_data_impl(
m_device,
m_intrinsics, rs::stream::infrared2, data_infrared2, stream_infrared2);
961 std::vector<vpColVector> *
const data_pointCloud,
962 pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pointcloud,
unsigned char *
const data_infrared,
963 unsigned char *
const data_infrared2,
const rs::stream &stream_color,
964 const rs::stream &stream_depth,
const rs::stream &stream_infrared,
965 const rs::stream &stream_infrared2)
977 if (data_image != NULL) {
978 vp_rs_get_native_frame_data_impl(
m_device,
m_intrinsics, rs::stream::color, data_image, stream_color);
981 if (data_depth != NULL) {
982 vp_rs_get_native_frame_data_impl(
m_device,
m_intrinsics, rs::stream::depth, data_depth, stream_depth);
985 if (data_pointCloud != NULL) {
989 if (pointcloud != NULL) {
994 if (data_infrared != NULL) {
995 vp_rs_get_native_frame_data_impl(
m_device,
m_intrinsics, rs::stream::infrared, data_infrared, stream_infrared);
998 if (data_infrared2 != NULL) {
999 vp_rs_get_native_frame_data_impl(
m_device,
m_intrinsics, rs::stream::infrared2, data_infrared2, stream_infrared2);
1026 os <<
"Device name: " << rs.
getHandler()->get_name() << std::endl;
1027 std::streamsize ss = std::cout.precision();
1028 for (
int i = 0; i < 4; ++i) {
1029 auto stream = rs::stream(i);
1030 if (!rs.
getHandler()->is_stream_enabled(stream))
1032 auto intrin = rs.
getHandler()->get_stream_intrinsics(stream);
1033 std::cout <<
"Capturing " << stream <<
" at " << intrin.width <<
" x " << intrin.height;
1034 std::cout << std::setprecision(1) << std::fixed <<
", fov = " << intrin.hfov() <<
" x " << intrin.vfov()
1035 <<
", distortion = " << intrin.model() << std::endl;
1037 std::cout.precision(ss);
1042 #elif !defined(VISP_BUILD_SHARED_LIBS)
1045 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.