CameraDepthAI.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2021, 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 #include <rtabmap/core/camera/CameraDepthAI.h>
29 #include <rtabmap/utilite/UTimer.h>
30 #include <rtabmap/utilite/UThread.h>
31 #include <rtabmap/utilite/UEventsManager.h>
32 #include <rtabmap/utilite/UConversion.h>
33 #include <rtabmap/utilite/UFile.h>
34 
35 
36 namespace rtabmap {
37 
38 bool CameraDepthAI::available()
39 {
40 #ifdef RTABMAP_DEPTHAI
41  return true;
42 #else
43  return false;
44 #endif
45 }
46 
47 CameraDepthAI::CameraDepthAI(
48  const std::string & deviceSerial,
49  int resolution,
50  float imageRate,
51  const Transform & localTransform) :
52  Camera(imageRate, localTransform)
53 #ifdef RTABMAP_DEPTHAI
54  ,
55  deviceSerial_(deviceSerial),
56  outputDepth_(false),
57  depthConfidence_(200),
58  resolution_(resolution),
59  imuFirmwareUpdate_(false),
60  imuPublished_(true)
61 #endif
62 {
63 #ifdef RTABMAP_DEPTHAI
64  UASSERT(resolution_>=(int)dai::MonoCameraProperties::SensorResolution::THE_720_P && resolution_<=(int)dai::MonoCameraProperties::SensorResolution::THE_1200_P);
65 #endif
66 }
67 
68 CameraDepthAI::~CameraDepthAI()
69 {
70 #ifdef RTABMAP_DEPTHAI
71  if(device_.get())
72  {
73  device_->close();
74  }
75 #endif
76 }
77 
78 void CameraDepthAI::setOutputDepth(bool enabled, int confidence)
79 {
80 #ifdef RTABMAP_DEPTHAI
81  outputDepth_ = enabled;
82  if(outputDepth_)
83  {
84  depthConfidence_ = confidence;
85  }
86 #else
87  UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
88 #endif
89 }
90 
91 void CameraDepthAI::setIMUFirmwareUpdate(bool enabled)
92 {
93 #ifdef RTABMAP_DEPTHAI
94  imuFirmwareUpdate_ = enabled;
95 #else
96  UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
97 #endif
98 }
99 
100 void CameraDepthAI::setIMUPublished(bool published)
101 {
102 #ifdef RTABMAP_DEPTHAI
103  imuPublished_ = published;
104 #else
105  UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
106 #endif
107 }
108 
109 bool CameraDepthAI::init(const std::string & calibrationFolder, const std::string & cameraName)
110 {
111  UDEBUG("");
112 #ifdef RTABMAP_DEPTHAI
113 
114  std::vector<dai::DeviceInfo> devices = dai::Device::getAllAvailableDevices();
115  if(devices.empty())
116  {
117  return false;
118  }
119 
120  if(device_.get())
121  {
122  device_->close();
123  }
124  accBuffer_.clear();
125  gyroBuffer_.clear();
126 
127  dai::DeviceInfo deviceToUse;
128  if(deviceSerial_.empty())
129  deviceToUse = devices[0];
130  for(size_t i=0; i<devices.size(); ++i)
131  {
132  UINFO("DepthAI device found: %s", devices[i].getMxId().c_str());
133  if(!deviceSerial_.empty() && deviceSerial_.compare(devices[i].getMxId()) == 0)
134  {
135  deviceToUse = devices[i];
136  }
137  }
138 
139  if(deviceToUse.getMxId().empty())
140  {
141  UERROR("Could not find device with serial \"%s\", found devices:", deviceSerial_.c_str());
142  for(size_t i=0; i<devices.size(); ++i)
143  {
144  UERROR("DepthAI device found: %s", devices[i].getMxId().c_str());
145  }
146  return false;
147  }
148  deviceSerial_ = deviceToUse.getMxId();
149 
150  // look for calibration files
151  stereoModel_ = StereoCameraModel();
152  cv::Size targetSize(resolution_<2?1280:resolution_==4?1920:640, resolution_==0?720:resolution_==1?800:resolution_==2?400:resolution_==3?480:1200);
153 
154  dai::Pipeline p;
155  auto monoLeft = p.create<dai::node::MonoCamera>();
156  auto monoRight = p.create<dai::node::MonoCamera>();
157  auto stereo = p.create<dai::node::StereoDepth>();
158  std::shared_ptr<dai::node::IMU> imu;
159  if(imuPublished_)
160  imu = p.create<dai::node::IMU>();
161 
162  auto xoutLeft = p.create<dai::node::XLinkOut>();
163  auto xoutDepthOrRight = p.create<dai::node::XLinkOut>();
164  std::shared_ptr<dai::node::XLinkOut> xoutIMU;
165  if(imuPublished_)
166  xoutIMU = p.create<dai::node::XLinkOut>();
167 
168  // XLinkOut
169  xoutLeft->setStreamName("rectified_left");
170  xoutDepthOrRight->setStreamName(outputDepth_?"depth":"rectified_right");
171  if(imuPublished_)
172  xoutIMU->setStreamName("imu");
173 
174  // MonoCamera
175  monoLeft->setResolution((dai::MonoCameraProperties::SensorResolution)resolution_);
176  monoLeft->setBoardSocket(dai::CameraBoardSocket::LEFT);
177  monoRight->setResolution((dai::MonoCameraProperties::SensorResolution)resolution_);
178  monoRight->setBoardSocket(dai::CameraBoardSocket::RIGHT);
179  if(this->getImageRate()>0)
180  {
181  monoLeft->setFps(this->getImageRate());
182  monoRight->setFps(this->getImageRate());
183  }
184 
185  // StereoDepth
186  stereo->initialConfig.setConfidenceThreshold(depthConfidence_);
187  stereo->initialConfig.setLeftRightCheckThreshold(5);
188  stereo->setRectifyEdgeFillColor(0); // black, to better see the cutout
189  stereo->setLeftRightCheck(true);
190  stereo->setSubpixel(false);
191  stereo->setExtendedDisparity(false);
192 
193  // Link plugins CAM -> STEREO -> XLINK
194  monoLeft->out.link(stereo->left);
195  monoRight->out.link(stereo->right);
196 
197  if(outputDepth_)
198  {
199  // Depth is registered to right image by default, so subscribe to right image when depth is used
200  if(outputDepth_)
201  stereo->rectifiedRight.link(xoutLeft->input);
202  else
203  stereo->rectifiedLeft.link(xoutLeft->input);
204  stereo->depth.link(xoutDepthOrRight->input);
205  }
206  else
207  {
208  stereo->rectifiedLeft.link(xoutLeft->input);
209  stereo->rectifiedRight.link(xoutDepthOrRight->input);
210  }
211 
212  if(imuPublished_)
213  {
214  // enable ACCELEROMETER_RAW and GYROSCOPE_RAW at 200 hz rate
215  imu->enableIMUSensor({dai::IMUSensor::ACCELEROMETER_RAW, dai::IMUSensor::GYROSCOPE_RAW}, 200);
216  // above this threshold packets will be sent in batch of X, if the host is not blocked and USB bandwidth is available
217  imu->setBatchReportThreshold(1);
218  // maximum number of IMU packets in a batch, if it's reached device will block sending until host can receive it
219  // if lower or equal to batchReportThreshold then the sending is always blocking on device
220  // useful to reduce device's CPU load and number of lost packets, if CPU load is high on device side due to multiple nodes
221  imu->setMaxBatchReports(10);
222 
223  // Link plugins IMU -> XLINK
224  imu->out.link(xoutIMU->input);
225 
226  imu->enableFirmwareUpdate(imuFirmwareUpdate_);
227  }
228 
229  device_.reset(new dai::Device(p, deviceToUse));
230 
231  UINFO("Loading eeprom calibration data");
232  dai::CalibrationHandler calibHandler = device_->readCalibration();
233  std::vector<std::vector<float> > matrix = calibHandler.getCameraIntrinsics(dai::CameraBoardSocket::LEFT, dai::Size2f(targetSize.width, targetSize.height));
234  double fx = matrix[0][0];
235  double fy = matrix[1][1];
236  double cx = matrix[0][2];
237  double cy = matrix[1][2];
238  matrix = calibHandler.getCameraExtrinsics(dai::CameraBoardSocket::RIGHT, dai::CameraBoardSocket::LEFT);
239  double baseline = matrix[0][3]/100.0;
240  UINFO("left: fx=%f fy=%f cx=%f cy=%f baseline=%f", fx, fy, cx, cy, baseline);
241  stereoModel_ = StereoCameraModel(device_->getMxId(), fx, fy, cx, cy, baseline, this->getLocalTransform(), targetSize);
242 
243  if(imuPublished_)
244  {
245  // Cannot test the following, I get "IMU calibration data is not available on device yet." with my camera
246  // Update: now (as March 6, 2022) it crashes in "dai::CalibrationHandler::getImuToCameraExtrinsics(dai::CameraBoardSocket, bool)"
247  //matrix = calibHandler.getImuToCameraExtrinsics(dai::CameraBoardSocket::LEFT);
248  //imuLocalTransform_ = Transform(
249  // matrix[0][0], matrix[0][1], matrix[0][2], matrix[0][3],
250  // matrix[1][0], matrix[1][1], matrix[1][2], matrix[1][3],
251  // matrix[2][0], matrix[2][1], matrix[2][2], matrix[2][3]);
252  // Hard-coded: x->down, y->left, z->forward
253  imuLocalTransform_ = Transform(
254  0, 0, 1, 0,
255  0, 1, 0, 0,
256  -1 ,0, 0, 0);
257  UINFO("IMU local transform = %s", imuLocalTransform_.prettyPrint().c_str());
258  }
259  else
260  {
261  UINFO("IMU disabled");
262  }
263 
264  if(imuPublished_)
265  {
266  imuQueue_ = device_->getOutputQueue("imu", 50, false);
267  }
268  leftQueue_ = device_->getOutputQueue("rectified_left", 1, false);
269  rightOrDepthQueue_ = device_->getOutputQueue(outputDepth_?"depth":"rectified_right", 1, false);
270 
271  uSleep(2000); // avoid bad frames on start
272 
273  return true;
274 #else
275  UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
276 #endif
277  return false;
278 }
279 
280 bool CameraDepthAI::isCalibrated() const
281 {
282 #ifdef RTABMAP_DEPTHAI
283  return stereoModel_.isValidForProjection();
284 #else
285  return false;
286 #endif
287 }
288 
289 std::string CameraDepthAI::getSerial() const
290 {
291 #ifdef RTABMAP_DEPTHAI
292  return deviceSerial_;
293 #endif
294  return "";
295 }
296 
297 SensorData CameraDepthAI::captureImage(CameraInfo * info)
298 {
299  SensorData data;
300 #ifdef RTABMAP_DEPTHAI
301 
302  cv::Mat left, depthOrRight;
303  auto rectifL = leftQueue_->get<dai::ImgFrame>();
304  auto rectifRightOrDepth = rightOrDepthQueue_->get<dai::ImgFrame>();
305 
306  if(rectifL.get() && rectifRightOrDepth.get())
307  {
308  auto stampLeft = rectifL->getTimestamp().time_since_epoch().count();
309  auto stampRight = rectifRightOrDepth->getTimestamp().time_since_epoch().count();
310  double stamp = double(stampLeft)/10e8;
311  left = rectifL->getCvFrame();
312  depthOrRight = rectifRightOrDepth->getCvFrame();
313 
314  if(!left.empty() && !depthOrRight.empty())
315  {
316  if(depthOrRight.type() == CV_8UC1)
317  {
318  if(stereoModel_.isValidForRectification())
319  {
320  left = stereoModel_.left().rectifyImage(left);
321  depthOrRight = stereoModel_.right().rectifyImage(depthOrRight);
322  }
323  data = SensorData(left, depthOrRight, stereoModel_, this->getNextSeqID(), stamp);
324  }
325  else
326  {
327  data = SensorData(left, depthOrRight, stereoModel_.left(), this->getNextSeqID(), stamp);
328  }
329 
330  if(fabs(double(stampLeft)/10e8 - double(stampRight)/10e8) >= 0.0001) //0.1 ms
331  {
332  UWARN("Frames are not synchronized! %f vs %f", double(stampLeft)/10e8, double(stampRight)/10e8);
333  }
334 
335  //get imu
336  double stampStart = UTimer::now();
337  while(imuPublished_ && imuQueue_.get())
338  {
339  if(imuQueue_->has())
340  {
341  auto imuData = imuQueue_->get<dai::IMUData>();
342 
343  auto imuPackets = imuData->packets;
344  double accStamp = 0.0;
345  double gyroStamp = 0.0;
346  for(auto& imuPacket : imuPackets) {
347  auto& acceleroValues = imuPacket.acceleroMeter;
348  auto& gyroValues = imuPacket.gyroscope;
349 
350  accStamp = double(acceleroValues.timestamp.get().time_since_epoch().count())/10e8;
351  gyroStamp = double(gyroValues.timestamp.get().time_since_epoch().count())/10e8;
352  accBuffer_.insert(accBuffer_.end(), std::make_pair(accStamp, cv::Vec3f(acceleroValues.x, acceleroValues.y, acceleroValues.z)));
353  gyroBuffer_.insert(gyroBuffer_.end(), std::make_pair(gyroStamp, cv::Vec3f(gyroValues.x, gyroValues.y, gyroValues.z)));
354  if(accBuffer_.size() > 1000)
355  {
356  accBuffer_.erase(accBuffer_.begin());
357  }
358  if(gyroBuffer_.size() > 1000)
359  {
360  gyroBuffer_.erase(gyroBuffer_.begin());
361  }
362  }
363  if(accStamp >= stamp && gyroStamp >= stamp)
364  {
365  break;
366  }
367  }
368  if((UTimer::now() - stampStart) > 0.01)
369  {
370  UWARN("Could not received IMU after 10 ms! Disabling IMU!");
371  imuPublished_ = false;
372  }
373  }
374 
375  cv::Vec3d acc, gyro;
376  bool valid = !accBuffer_.empty() && !gyroBuffer_.empty();
377  //acc
378  if(!accBuffer_.empty())
379  {
380  std::map<double, cv::Vec3f>::const_iterator iterB = accBuffer_.lower_bound(stamp);
381  std::map<double, cv::Vec3f>::const_iterator iterA = iterB;
382  if(iterA != accBuffer_.begin())
383  {
384  iterA = --iterA;
385  }
386  if(iterB == accBuffer_.end())
387  {
388  iterB = --iterB;
389  }
390  if(iterA == iterB && stamp == iterA->first)
391  {
392  acc[0] = iterA->second[0];
393  acc[1] = iterA->second[1];
394  acc[2] = iterA->second[2];
395  }
396  else if(stamp >= iterA->first && stamp <= iterB->first)
397  {
398  float t = (stamp-iterA->first) / (iterB->first-iterA->first);
399  acc[0] = iterA->second[0] + t*(iterB->second[0] - iterA->second[0]);
400  acc[1] = iterA->second[1] + t*(iterB->second[1] - iterA->second[1]);
401  acc[2] = iterA->second[2] + t*(iterB->second[2] - iterA->second[2]);
402  }
403  else
404  {
405  valid = false;
406  if(stamp < iterA->first)
407  {
408  UWARN("Could not find acc data to interpolate at image time %f (earliest is %f). Are sensors synchronized?", stamp, iterA->first);
409  }
410  else if(stamp > iterB->first)
411  {
412  UWARN("Could not find acc data to interpolate at image time %f (latest is %f). Are sensors synchronized?", stamp, iterB->first);
413  }
414  else
415  {
416  UWARN("Could not find acc data to interpolate at image time %f (between %f and %f). Are sensors synchronized?", stamp, iterA->first, iterB->first);
417  }
418  }
419  }
420  //gyro
421  if(!gyroBuffer_.empty())
422  {
423  std::map<double, cv::Vec3f>::const_iterator iterB = gyroBuffer_.lower_bound(stamp);
424  std::map<double, cv::Vec3f>::const_iterator iterA = iterB;
425  if(iterA != gyroBuffer_.begin())
426  {
427  iterA = --iterA;
428  }
429  if(iterB == gyroBuffer_.end())
430  {
431  iterB = --iterB;
432  }
433  if(iterA == iterB && stamp == iterA->first)
434  {
435  gyro[0] = iterA->second[0];
436  gyro[1] = iterA->second[1];
437  gyro[2] = iterA->second[2];
438  }
439  else if(stamp >= iterA->first && stamp <= iterB->first)
440  {
441  float t = (stamp-iterA->first) / (iterB->first-iterA->first);
442  gyro[0] = iterA->second[0] + t*(iterB->second[0] - iterA->second[0]);
443  gyro[1] = iterA->second[1] + t*(iterB->second[1] - iterA->second[1]);
444  gyro[2] = iterA->second[2] + t*(iterB->second[2] - iterA->second[2]);
445  }
446  else
447  {
448  valid = false;
449  if(stamp < iterA->first)
450  {
451  UWARN("Could not find gyro data to interpolate at image time %f (earliest is %f). Are sensors synchronized?", stamp, iterA->first);
452  }
453  else if(stamp > iterB->first)
454  {
455  UWARN("Could not find gyro data to interpolate at image time %f (latest is %f). Are sensors synchronized?", stamp, iterB->first);
456  }
457  else
458  {
459  UWARN("Could not find gyro data to interpolate at image time %f (between %f and %f). Are sensors synchronized?", stamp, iterA->first, iterB->first);
460  }
461  }
462  }
463 
464  if(valid)
465  {
466  data.setIMU(IMU(gyro, cv::Mat::eye(3, 3, CV_64FC1), acc, cv::Mat::eye(3, 3, CV_64FC1), imuLocalTransform_));
467  }
468  }
469  }
470  else
471  {
472  UWARN("Null images received!?");
473  }
474 
475 #else
476  UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
477 #endif
478  return data;
479 }
480 
481 } // namespace rtabmap
rtabmap::CameraDepthAI::setOutputDepth
void setOutputDepth(bool enabled, int confidence=200)
Definition: CameraDepthAI.cpp:78
data
data
UConversion.h
Some conversion functions.
UASSERT
#define UASSERT(condition)
true
#define true
Definition: ConvertUTF.c:57
rtabmap::CameraDepthAI::CameraDepthAI
CameraDepthAI(const std::string &deviceSerial="", int resolution=1, float imageRate=0.0f, const Transform &localTransform=Transform::getIdentity())
Definition: CameraDepthAI.cpp:47
rtabmap::CameraDepthAI::isCalibrated
virtual bool isCalibrated() const
Definition: CameraDepthAI.cpp:280
rtabmap::CameraDepthAI::setIMUFirmwareUpdate
void setIMUFirmwareUpdate(bool enabled)
Definition: CameraDepthAI.cpp:91
rtabmap::CameraDepthAI::getSerial
virtual std::string getSerial() const
Definition: CameraDepthAI.cpp:289
rtabmap::Camera::getLocalTransform
const Transform & getLocalTransform() const
Definition: Camera.h:65
rtabmap::CameraDepthAI::setIMUPublished
void setIMUPublished(bool published)
Definition: CameraDepthAI.cpp:100
rtabmap::Camera::getImageRate
float getImageRate() const
Definition: Camera.h:64
false
#define false
Definition: ConvertUTF.c:56
uSleep
void uSleep(unsigned int ms)
Definition: Posix/UThreadC.h:23
rtabmap::Camera::getNextSeqID
int getNextSeqID()
Definition: Camera.h:86
UDEBUG
#define UDEBUG(...)
UERROR
#define UERROR(...)
UWARN
#define UWARN(...)
UTimer::now
static double now()
Definition: UTimer.cpp:80
rtabmap::CameraDepthAI::captureImage
virtual SensorData captureImage(CameraInfo *info=0)
Definition: CameraDepthAI.cpp:297
rtabmap::SensorData::setIMU
void setIMU(const IMU &imu)
Definition: SensorData.h:289
rtabmap::CameraDepthAI::init
virtual bool init(const std::string &calibrationFolder=".", const std::string &cameraName="")
Definition: CameraDepthAI.cpp:109
UINFO
#define UINFO(...)

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Jan 23 2023 03:37:27