41 #include <visp3/core/vpImageConvert.h> 42 #include <visp3/sensor/vpRealSense.h> 44 #if defined(VISP_HAVE_REALSENSE) && defined(VISP_HAVE_CPP11_COMPATIBILITY) 46 #include "vpRealSense_impl.h" 52 : m_context(), m_device(NULL), m_num_devices(0), m_serial_no(), m_intrinsics(), m_max_Z(8),
53 m_enableStreams(), m_useStreamPresets(), m_streamPresets(), m_streamParams(), m_invalidDepthValue(0.0f)
106 std::vector<rs::device *> detected_devices;
110 std::string serial_no = device->get_serial();
114 detected_devices.push_back(device);
118 if ((
m_serial_no.empty()) && (m_num_devices > 1)) {
130 std::cout <<
"RealSense Camera - Connecting to camera with Serial No: " << m_device->get_serial() << std::endl;
133 m_enableStreams[rs::stream::infrared2] = m_device->supports(rs::capabilities::infrared2);
136 if (m_device->is_streaming()) {
140 for (
int j = 0; j < 4; j++) {
141 auto s = (rs::stream) j;
142 auto capabilities = (rs::capabilities) j;
143 if (m_device->supports(capabilities) && m_device->is_stream_enabled(s)) {
144 m_device->disable_stream(s);
150 m_device->enable_stream(rs::stream::color,
m_streamPresets[rs::stream::color]);
152 m_device->enable_stream(rs::stream::color,
m_streamParams[rs::stream::color].m_streamWidth,
m_streamParams[rs::stream::color].m_streamHeight,
159 m_device->enable_stream(rs::stream::depth,
m_streamPresets[rs::stream::depth]);
161 m_device->enable_stream(rs::stream::depth,
m_streamParams[rs::stream::depth].m_streamWidth,
m_streamParams[rs::stream::depth].m_streamHeight,
168 m_device->enable_stream(rs::stream::infrared,
m_streamPresets[rs::stream::infrared]);
170 m_device->enable_stream(rs::stream::infrared,
m_streamParams[rs::stream::infrared].m_streamWidth,
m_streamParams[rs::stream::infrared].m_streamHeight,
177 m_device->enable_stream(rs::stream::infrared2,
m_streamPresets[rs::stream::infrared2]);
179 m_device->enable_stream(rs::stream::infrared2,
m_streamParams[rs::stream::infrared2].m_streamWidth,
m_streamParams[rs::stream::infrared2].m_streamHeight,
186 for(
int i = 0; i < 4; ++i) {
187 auto stream = rs::stream(i);
188 if(!m_device->is_stream_enabled(stream))
continue;
189 auto intrin = m_device->get_stream_intrinsics(stream);
196 m_intrinsics[rs::stream::rectified_color] = m_device->get_stream_intrinsics(rs::stream::rectified_color);
199 m_intrinsics[rs::stream::color_aligned_to_depth] = m_device->get_stream_intrinsics(rs::stream::color_aligned_to_depth);
200 m_intrinsics[rs::stream::depth_aligned_to_color] = m_device->get_stream_intrinsics(rs::stream::depth_aligned_to_color);
201 m_intrinsics[rs::stream::depth_aligned_to_rectified_color] = m_device->get_stream_intrinsics(rs::stream::depth_aligned_to_rectified_color);
206 m_intrinsics[rs::stream::depth_aligned_to_infrared2] = m_device->get_stream_intrinsics(rs::stream::depth_aligned_to_infrared2);
207 m_intrinsics[rs::stream::infrared2_aligned_to_depth] = m_device->get_stream_intrinsics(rs::stream::infrared2_aligned_to_depth);
264 double px = intrinsics.ppx;
265 double py = intrinsics.ppy;
266 double u0 = intrinsics.fx;
267 double v0 = intrinsics.fy;
269 double kdu = intrinsics.coeffs[0];
288 if(!
m_device->is_stream_enabled(stream)) {
292 std::map<rs::stream, rs::intrinsics>::const_iterator it_intrin =
m_intrinsics.find(stream);
294 return it_intrin->second;
297 return rs::intrinsics();
311 if(!
m_device->is_stream_enabled(from)) {
314 if(!
m_device->is_stream_enabled(to)) {
317 return m_device->get_extrinsics(from, to);
331 for(
unsigned int i=0; i<3; i++) {
332 t[i] = extrinsics.translation[i];
333 for(
unsigned int j=0; j<3; j++)
334 R[i][j] = extrinsics.rotation[i*3+j];
517 void vpRealSense::acquire(
unsigned char *
const data_image,
unsigned char *
const data_depth, std::vector<vpColVector> *
const data_pointCloud,
518 unsigned char *
const data_infrared,
unsigned char *
const data_infrared2,
const rs::stream &stream_color,
const rs::stream &stream_depth,
519 const rs::stream &stream_infrared,
const rs::stream &stream_infrared2) {
530 if (data_image != NULL) {
531 vp_rs_get_native_frame_data_impl(
m_device,
m_intrinsics, rs::stream::color, data_image, stream_color);
534 if (data_depth != NULL) {
535 vp_rs_get_native_frame_data_impl(
m_device,
m_intrinsics, rs::stream::depth, data_depth, stream_depth);
537 if (data_pointCloud != NULL) {
542 if (data_infrared != NULL) {
543 vp_rs_get_native_frame_data_impl(
m_device,
m_intrinsics, rs::stream::infrared, data_infrared, stream_infrared);
546 if (data_infrared2 != NULL) {
547 vp_rs_get_native_frame_data_impl(
m_device,
m_intrinsics, rs::stream::infrared2, data_infrared2, stream_infrared2);
842 void vpRealSense::acquire(
unsigned char *
const data_image,
unsigned char *
const data_depth, std::vector<vpColVector> *
const data_pointCloud,
843 pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud,
unsigned char *
const data_infrared,
844 unsigned char *
const data_infrared2,
const rs::stream &stream_color,
const rs::stream &stream_depth,
845 const rs::stream &stream_infrared,
const rs::stream &stream_infrared2) {
856 if (data_image != NULL) {
857 vp_rs_get_native_frame_data_impl(
m_device,
m_intrinsics, rs::stream::color, data_image, stream_color);
860 if (data_depth != NULL) {
861 vp_rs_get_native_frame_data_impl(
m_device,
m_intrinsics, rs::stream::depth, data_depth, stream_depth);
864 if (data_pointCloud != NULL) {
868 if (pointcloud != NULL) {
872 if (data_infrared != NULL) {
873 vp_rs_get_native_frame_data_impl(
m_device,
m_intrinsics, rs::stream::infrared, data_infrared, stream_infrared);
876 if (data_infrared2 != NULL) {
877 vp_rs_get_native_frame_data_impl(
m_device,
m_intrinsics, rs::stream::infrared2, data_infrared2, stream_infrared2);
894 void vpRealSense::acquire(
unsigned char *
const data_image,
unsigned char *
const data_depth, std::vector<vpColVector> *
const data_pointCloud,
895 pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pointcloud,
unsigned char *
const data_infrared,
896 unsigned char *
const data_infrared2,
const rs::stream &stream_color,
const rs::stream &stream_depth,
897 const rs::stream &stream_infrared,
const rs::stream &stream_infrared2) {
908 if (data_image != NULL) {
909 vp_rs_get_native_frame_data_impl(
m_device,
m_intrinsics, rs::stream::color, data_image, stream_color);
912 if (data_depth != NULL) {
913 vp_rs_get_native_frame_data_impl(
m_device,
m_intrinsics, rs::stream::depth, data_depth, stream_depth);
916 if (data_pointCloud != NULL) {
920 if (pointcloud != NULL) {
924 if (data_infrared != NULL) {
925 vp_rs_get_native_frame_data_impl(
m_device,
m_intrinsics, rs::stream::infrared, data_infrared, stream_infrared);
928 if (data_infrared2 != NULL) {
929 vp_rs_get_native_frame_data_impl(
m_device,
m_intrinsics, rs::stream::infrared2, data_infrared2, stream_infrared2);
955 os <<
"Device name: " << rs.
getHandler()->get_name() << std::endl;
956 std::streamsize ss = std::cout.precision();
957 for(
int i = 0; i < 4; ++i)
959 auto stream = rs::stream(i);
960 if(!rs.
getHandler()->is_stream_enabled(stream))
continue;
961 auto intrin = rs.
getHandler()->get_stream_intrinsics(stream);
962 std::cout <<
"Capturing " << stream <<
" at " << intrin.width <<
" x " << intrin.height;
963 std::cout << std::setprecision(1) << std::fixed <<
", fov = " << intrin.hfov() <<
" x " << intrin.vfov() <<
", distortion = " << intrin.model() << std::endl;
965 std::cout.precision(ss);
970 #elif !defined(VISP_BUILD_SHARED_LIBS) 972 void dummy_vpRealSense() {};
rs::device * getHandler() const
Get access to device handler.
friend VISP_EXPORT std::ostream & operator<<(std::ostream &os, const vpRealSense &rs)
Implementation of an homogeneous matrix and operations on such kind of matrices.
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
rs::extrinsics getExtrinsics(const rs::stream &from, const rs::stream &to) const
vpCameraParametersProjType
Generic class defining intrinsic camera parameters.
std::map< rs::stream, rs::preset > m_streamPresets
Perspective projection with distortion model.
void setStreamSettings(const rs::stream &stream, const rs::preset &preset)
void setEnableStream(const rs::stream &stream, const bool status)
vpCameraParameters getCameraParameters(const rs::stream &stream, vpCameraParameters::vpCameraParametersProjType type=vpCameraParameters::perspectiveProjWithDistortion) const
rs::intrinsics getIntrinsics(const rs::stream &stream) const
float m_invalidDepthValue
vpHomogeneousMatrix getTransformation(const rs::stream &from, const rs::stream &to) 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.