CameraRealSense2.h
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #pragma once
29 
31 #include "rtabmap/core/Camera.h"
32 #include "rtabmap/core/Version.h"
33 
34 #include <pcl/pcl_config.h>
35 
36 #ifdef RTABMAP_REALSENSE2
37 #include <librealsense2/rs.hpp>
38 #include <librealsense2/hpp/rs_frame.hpp>
39 #endif
40 
41 
42 namespace rs2
43 {
44  class context;
45  class device;
46  class syncer;
47 }
48 struct rs2_intrinsics;
49 struct rs2_extrinsics;
50 
51 namespace rtabmap
52 {
53 
54 class RTABMAP_CORE_EXPORT CameraRealSense2 :
55  public Camera
56 {
57 public:
58  static bool available();
59 
60 public:
62  const std::string & deviceId = "",
63  float imageRate = 0,
64  const Transform & localTransform = Transform::getIdentity());
65  virtual ~CameraRealSense2();
66 
67  virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
68  virtual bool isCalibrated() const;
69  virtual std::string getSerial() const;
70  virtual bool odomProvided() const;
71  virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime = 0.06);
72 
73  // parameters are set during initialization
74  // D400 series
75  void setEmitterEnabled(bool enabled);
76  void setIRFormat(bool enabled, bool useDepthInsteadOfRightImage);
77  void setResolution(int width, int height, int fps = 30);
78  void setDepthResolution(int width, int height, int fps = 30);
79  void setGlobalTimeSync(bool enabled);
80 
86  void setDualMode(bool enabled, const Transform & extrinsics);
87  void setJsonConfig(const std::string & json);
88  // T265 related parameters
89  void setImagesRectified(bool enabled);
90  void setOdomProvided(bool enabled, bool imageStreamsDisabled=false, bool onlyLeftStream = false);
91 
92 #ifdef RTABMAP_REALSENSE2
93 private:
94  void close();
95  void imu_callback(rs2::frame frame);
96  void pose_callback(rs2::frame frame);
97  void frame_callback(rs2::frame frame);
98  void multiple_message_callback(rs2::frame frame);
99  void getPoseAndIMU(
100  const double & stamp,
101  Transform & pose,
102  unsigned int & poseConfidence,
103  IMU & imu,
104  int maxWaitTimeMs = 35);
105 #endif
106 
107 protected:
108  virtual SensorData captureImage(SensorCaptureInfo * info = 0);
109 
110 private:
111 #ifdef RTABMAP_REALSENSE2
112  rs2::context ctx_;
113  std::vector<rs2::device> dev_;
114  std::string deviceId_;
115  rs2::syncer syncer_;
116  float depth_scale_meters_;
117  cv::Mat depthBuffer_;
118  cv::Mat rgbBuffer_;
119  CameraModel model_;
120  StereoCameraModel stereoModel_;
121  Transform imuLocalTransform_;
122  std::map<double, cv::Vec3f> accBuffer_;
123  std::map<double, cv::Vec3f> gyroBuffer_;
124  std::map<double, std::pair<Transform, unsigned int> > poseBuffer_; // <stamp, <Pose, confidence: 1=lost, 2=low, 3=high> >
125  UMutex poseMutex_;
126  UMutex imuMutex_;
127  double lastImuStamp_;
128  bool clockSyncWarningShown_;
129  bool imuGlobalSyncWarningShown_;
130 
131  bool emitterEnabled_;
132  bool ir_;
133  bool irDepth_;
134  bool rectifyImages_;
135  bool odometryProvided_;
136  bool odometryImagesDisabled_;
137  bool odometryOnlyLeftStream_;
138  int cameraWidth_;
139  int cameraHeight_;
140  int cameraFps_;
141  int cameraDepthWidth_;
142  int cameraDepthHeight_;
143  int cameraDepthFps_;
144  bool globalTimeSync_;
145  bool dualMode_;
146  Transform dualExtrinsics_;
147  std::string jsonConfig_;
148  bool closing_;
149 
150  static Transform realsense2PoseRotation_;
151  static Transform realsense2PoseRotationInv_;
152 #endif
153 };
154 
155 
156 } // namespace rtabmap
rtabmap::SensorData
Definition: SensorData.h:51
rtabmap::StereoCameraModel
Definition: StereoCameraModel.h:35
rtabmap::Transform::getIdentity
static Transform getIdentity()
Definition: Transform.cpp:411
rtabmap::CameraModel
Definition: CameraModel.h:38
rtabmap::SensorCaptureInfo
Definition: SensorCaptureInfo.h:36
rtabmap::IMU
Definition: IMU.h:19
rtabmap::CameraRealSense2
Definition: CameraRealSense2.h:54
UMutex
Definition: UMutex.h:54
rtabmap_superglue.device
string device
Definition: rtabmap_superglue.py:21
rs2
Definition: CameraRealSense2.h:42
info
else if n * info
rtabmap::Camera
Definition: Camera.h:43
rtabmap_netvlad.init
def init(descriptorDim)
Definition: rtabmap_netvlad.py:30
getPose
Pose3_ getPose(const Expression< PinholeCamera< CALIBRATION > > &cam)
rtabmap::Transform
Definition: Transform.h:41
Camera.h
CameraModel.h
rtabmap
Definition: CameraARCore.cpp:35


rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:07