42 #include <visp3/core/vpImageConvert.h> 43 #include <visp3/sensor/vpRealSense.h> 45 #if defined(VISP_HAVE_REALSENSE) && defined(VISP_HAVE_CPP11_COMPATIBILITY) 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);
118 if ((
m_serial_no.empty()) && (m_num_devices > 1)) {
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)
146 if (m_device->is_streaming()) {
150 for (
int j = 0; j < 4; j++) {
151 auto s = (rs::stream)j;
152 auto capabilities = (rs::capabilities)j;
153 if (m_device->supports(capabilities) && m_device->is_stream_enabled(s)) {
154 m_device->disable_stream(s);
160 m_device->enable_stream(rs::stream::color,
m_streamPresets[rs::stream::color]);
162 m_device->enable_stream(rs::stream::color,
m_streamParams[rs::stream::color].m_streamWidth,
171 m_device->enable_stream(rs::stream::depth,
m_streamPresets[rs::stream::depth]);
173 m_device->enable_stream(rs::stream::depth,
m_streamParams[rs::stream::depth].m_streamWidth,
182 m_device->enable_stream(rs::stream::infrared,
m_streamPresets[rs::stream::infrared]);
184 m_device->enable_stream(rs::stream::infrared,
m_streamParams[rs::stream::infrared].m_streamWidth,
193 m_device->enable_stream(rs::stream::infrared2,
m_streamPresets[rs::stream::infrared2]);
195 m_device->enable_stream(rs::stream::infrared2,
m_streamParams[rs::stream::infrared2].m_streamWidth,
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(){};
vpHomogeneousMatrix getTransformation(const rs::stream &from, const rs::stream &to) const
friend VISP_EXPORT std::ostream & operator<<(std::ostream &os, const vpRealSense &rs)
rs::extrinsics getExtrinsics(const rs::stream &from, const rs::stream &to) const
Implementation of an homogeneous matrix and operations on such kind of matrices.
rs::device * getHandler() const
Get access to device handler.
error that can be emited by ViSP classes.
void acquire(std::vector< vpColVector > &pointcloud)
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)
void setDeviceBySerialNumber(const std::string &serial_no)
std::map< rs::stream, vpRsStreamParams > m_streamParams
std::map< rs::stream, bool > m_useStreamPresets
vpCameraParametersProjType
Generic class defining intrinsic camera parameters.
std::map< rs::stream, rs::preset > m_streamPresets
rs::intrinsics getIntrinsics(const rs::stream &stream) const
void setStreamSettings(const rs::stream &stream, const rs::preset &preset)
void setEnableStream(const rs::stream &stream, const bool status)
float m_invalidDepthValue
vpCameraParameters getCameraParameters(const rs::stream &stream, vpCameraParameters::vpCameraParametersProjType type=vpCameraParameters::perspectiveProjWithDistortion) const
std::map< rs::stream, rs::intrinsics > m_intrinsics
std::map< rs::stream, bool > m_enableStreams
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)
float m_max_Z
Maximal Z depth in meter.