CameraMyntEye.cpp
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 #include "rtabmap/utilite/UThread.h"
29 #include "rtabmap/utilite/UTimer.h"
30 #include "rtabmap/utilite/UEventsManager.h"
31 #include <rtabmap/core/camera/CameraMyntEye.h>
32 #ifdef RTABMAP_MYNTEYE
33 #include <mynteye/api.h>
34 #include <mynteye/device.h>
35 #include <mynteye/context.h>
36 
37 #ifndef M_PI
38 #define M_PI 3.14159265358979323846
39 #endif
40 #endif
41 
42 namespace rtabmap
43 {
44 
45 CameraMyntEye::CameraMyntEye(const std::string & device, bool apiRectification, bool apiDepth, float imageRate, const Transform & localTransform) :
46  Camera(imageRate, localTransform)
47 #ifdef RTABMAP_MYNTEYE
48  ,
49  deviceName_(device),
50  apiRectification_(apiRectification),
51  apiDepth_(apiDepth),
52  autoExposure_(true),
53  gain_(24),
54  brightness_(120),
55  contrast_(116),
56  dataReady_(0),
57  lastFramesStamp_(0.0),
58  stamp_(0),
59  publishInterIMU_(false),
60  softTimeBegin_(0.0),
61  hardTimeBegin_(0),
62  unitHardTime_(std::numeric_limits<std::uint32_t>::max()*10)
63 #endif
64 {
65 #ifdef RTABMAP_MYNTEYE
66  lastHardTimes_ = std::vector<std::uint64_t>((size_t)mynteye::Stream::LAST+1, 0);
67  acc_ = std::vector<std::uint64_t>((size_t)mynteye::Stream::LAST+1, 0);
68 #endif
69 }
70 CameraMyntEye::~CameraMyntEye()
71 {
72 #ifdef RTABMAP_MYNTEYE
73  if (api_) {
74  api_->Stop(mynteye::Source::ALL);
75  }
76  dataReady_.release();
77 #endif
78 }
79 
80 #ifdef RTABMAP_MYNTEYE
81 std::shared_ptr<mynteye::IntrinsicsBase> getDefaultIntrinsics() {
82  auto res = std::make_shared<mynteye::IntrinsicsPinhole>();
83  res->width = 640;
84  res->height = 400;
85  res->model = 0;
86  res->fx = 3.6220059643202876e+02;
87  res->fy = 3.6350065250745848e+02;
88  res->cx = 4.0658699068023441e+02;
89  res->cy = 2.3435161110061483e+02;
90  double codffs[5] = {
91  -2.5034765682756088e-01,
92  5.0579399202897619e-02,
93  -7.0536676161976066e-04,
94  -8.5255451307033846e-03,
95  0.
96  };
97  for (unsigned int i = 0; i < 5; i++) {
98  res->coeffs[i] = codffs[i];
99  }
100  return res;
101 }
102 
103 std::shared_ptr<mynteye::Extrinsics> getDefaultExtrinsics() {
104  auto res = std::make_shared<mynteye::Extrinsics>();
105  double rotation[9] = {
106  9.9867908939669447e-01, -6.3445566137485428e-03, 5.0988459509619687e-02,
107  5.9890316389333252e-03, 9.9995670037792639e-01, 7.1224201868366971e-03,
108  -5.1031440326695092e-02, -6.8076406092671274e-03, 9.9867384471984544e-01
109  };
110  double translation[3] = {-1.2002489764113250e+02, -1.1782637409050747e+00,
111  -5.2058205159996538e+00};
112  for (unsigned int i = 0; i < 3; i++) {
113  for (unsigned int j = 0; j < 3; j++) {
114  res->rotation[i][j] = rotation[i*3 + j];
115  }
116  }
117  for (unsigned int i = 0; i < 3; i++) {
118  res->translation[i] = translation[i];
119  }
120  return res;
121 }
122 
123 double CameraMyntEye::hardTimeToSoftTime(std::uint64_t hardTime) {
124  if (hardTimeBegin_==0) {
125  softTimeBegin_ = UTimer::now();
126  hardTimeBegin_ = hardTime;
127  }
128 
129  std::uint64_t time_ns_detal = (hardTime - hardTimeBegin_);
130  std::uint64_t time_ns_detal_s = time_ns_detal / 1000000;
131  std::uint64_t time_ns_detal_ns = time_ns_detal % 1000000;
132  double time_sec_double = static_cast<double>(time_ns_detal_s) + 1e-9*static_cast<double>(time_ns_detal_ns * 1000);
133 
134  return softTimeBegin_ + time_sec_double;
135 }
136 
137 inline bool is_overflow(std::uint64_t now, std::uint64_t pre, std::uint64_t unit_hard_time) {
138 
139  return (now < pre) && ((pre - now) > (unit_hard_time / 2));
140 }
141 
142 double CameraMyntEye::checkUpTimeStamp(std::uint64_t _hard_time, std::uint8_t stream) {
143  UASSERT(stream < (std::uint8_t)mynteye::Stream::LAST+1);
144  if (is_overflow(_hard_time, lastHardTimes_[stream], unitHardTime_)) {
145  acc_[stream]++;
146  }
147 
148  lastHardTimes_[stream] = _hard_time;
149 
150  return hardTimeToSoftTime(acc_[stream] * unitHardTime_ + _hard_time);
151 }
152 #endif
153 
154 void CameraMyntEye::publishInterIMU(bool enabled)
155 {
156 #ifdef RTABMAP_MYNTEYE
157  publishInterIMU_ = enabled;
158 #endif
159 }
160 
161 void CameraMyntEye::setAutoExposure()
162 {
163 #ifdef RTABMAP_MYNTEYE
164  autoExposure_ = true;
165 #endif
166 }
167 
168 void CameraMyntEye::setManualExposure(int gain, int brightness, int constrast)
169 {
170 #ifdef RTABMAP_MYNTEYE
171  UASSERT(gain>=0 && gain<=48);
172  UASSERT(brightness>=0 && brightness<=240);
173  UASSERT(constrast>=0 && constrast<=254);
174  autoExposure_ = false;
175  gain_ = gain;
176  brightness_ = brightness;
177  contrast_ = constrast;
178 #endif
179 }
180 
181 void CameraMyntEye::setIrControl(int value)
182 {
183 #ifdef RTABMAP_MYNTEYE
184  UASSERT(value>=0 && value<=160);
185  irControl_ = value;
186 #endif
187 }
188 
189 bool CameraMyntEye::init(const std::string & calibrationFolder, const std::string & cameraName)
190 {
191 #ifdef RTABMAP_MYNTEYE
192  mynteye::Context context;
193  auto &&devices = context.devices();
194 
195  softTimeBegin_ = 0.0;
196  hardTimeBegin_ = 0;
197  imuBuffer_.clear();
198  lastHardTimes_ = std::vector<std::uint64_t>((size_t)mynteye::Stream::LAST+1, 0);
199  acc_ = std::vector<std::uint64_t>((size_t)mynteye::Stream::LAST+1, 0);
200 
201  size_t n = devices.size();
202  if(n==0)
203  {
204  UERROR("No Mynt Eye devices detected!");
205  return false;
206  }
207  UINFO("MYNT EYE devices:");
208 
209  for (size_t i = 0; i < n; i++) {
210  auto &&device = devices[i];
211  auto &&name = device->GetInfo(mynteye::Info::DEVICE_NAME);
212  auto &&serial_number = device->GetInfo(mynteye::Info::SERIAL_NUMBER);
213  if(!deviceName_.empty() && serial_number.compare(deviceName_) == 0)
214  {
215  device_ = devices[i];
216  }
217  UINFO(" index: %d, name: %s, serial number: %s", (int)i, name.c_str(), serial_number.c_str());
218  }
219 
220  if(device_.get() == 0)
221  {
222  //take first one by default
223  device_ = devices[0];
224  }
225 
226  UINFO("");
227  api_ = mynteye::API::Create(device_);
228 
229  UINFO("");
230  auto in_left_base = api_->GetIntrinsicsBase(mynteye::Stream::LEFT);
231  auto in_right_base = api_->GetIntrinsicsBase(mynteye::Stream::RIGHT);
232  UINFO("");
233  auto ex = api_->GetExtrinsics(mynteye::Stream::RIGHT, mynteye::Stream::LEFT);
234 
235  UINFO("");
236  if(!in_left_base || !in_right_base)
237  {
238  UERROR("Unknown calibration model! Using default ones");
239  in_left_base = getDefaultIntrinsics();
240  in_right_base = getDefaultIntrinsics();
241  ex = *(getDefaultExtrinsics());
242  }
243  cv::Size size{in_left_base->width, in_left_base->height};
244 
245  cv::Mat K1, K2, D1, D2;
246 
247  if(in_left_base->calib_model() == mynteye::CalibrationModel::PINHOLE &&
248  in_right_base->calib_model() == mynteye::CalibrationModel::PINHOLE)
249  {
250  auto in_left = *std::dynamic_pointer_cast<mynteye::IntrinsicsPinhole>(in_left_base);
251  auto in_right = *std::dynamic_pointer_cast<mynteye::IntrinsicsPinhole>(in_right_base);
252 
253  K1 = (cv::Mat_<double>(3, 3) << in_left.fx, 0, in_left.cx, 0, in_left.fy, in_left.cy, 0, 0, 1);
254  K2 = (cv::Mat_<double>(3, 3) << in_right.fx, 0, in_right.cx, 0, in_right.fy, in_right.cy, 0, 0, 1);
255  D1 = cv::Mat(1, 5, CV_64F, in_left.coeffs).clone();
256  D2 = cv::Mat(1, 5, CV_64F, in_right.coeffs).clone();
257  }
258  else if(in_left_base->calib_model() == mynteye::CalibrationModel::KANNALA_BRANDT &&
259  in_right_base->calib_model() == mynteye::CalibrationModel::KANNALA_BRANDT)
260  {
261  //equidistant
262  auto in_left = *std::dynamic_pointer_cast<mynteye::IntrinsicsEquidistant>(in_left_base);
263  auto in_right = *std::dynamic_pointer_cast<mynteye::IntrinsicsEquidistant>(in_right_base);
264 
266  UINFO("left coeff = %f %f %f %f %f %f %f %f",
267  in_left.coeffs[0], in_left.coeffs[1], in_left.coeffs[2], in_left.coeffs[3],
268  in_left.coeffs[4], in_left.coeffs[5], in_left.coeffs[6], in_left.coeffs[7]);
269  UINFO("right coeff = %f %f %f %f %f %f %f %f",
270  in_left.coeffs[0], in_left.coeffs[1], in_left.coeffs[2], in_left.coeffs[3],
271  in_left.coeffs[4], in_left.coeffs[5], in_left.coeffs[6], in_left.coeffs[7]);
272 
273  K1 = (cv::Mat_<double>(3, 3) << in_left.coeffs[4], 0, in_left.coeffs[6], 0, in_left.coeffs[5], in_left.coeffs[7], 0, 0, 1);
274  K2 = (cv::Mat_<double>(3, 3) << in_right.coeffs[4], 0, in_right.coeffs[6], 0, in_right.coeffs[5], in_right.coeffs[7], 0, 0, 1);
275  // convert to (k1,k2,p1,p2,k3,k4)
276  D1 = (cv::Mat_<double>(1, 6) << in_left.coeffs[0], in_left.coeffs[1], 0, 0, in_left.coeffs[2], in_left.coeffs[3]);
277  D2 = (cv::Mat_<double>(1, 6) << in_right.coeffs[0], in_right.coeffs[1], 0, 0, in_right.coeffs[2], in_right.coeffs[3]);
278  }
279 
280  bool is_data_use_mm_instead_of_m =
281  abs(ex.translation[0]) > 1.0 ||
282  abs(ex.translation[1]) > 1.0 ||
283  abs(ex.translation[2]) > 1.0;
284  if(is_data_use_mm_instead_of_m)
285  {
286  ex.translation[0] *= 0.001;
287  ex.translation[1] *= 0.001;
288  ex.translation[2] *= 0.001;
289  }
290 
291  Transform extrinsics(
292  ex.rotation[0][0], ex.rotation[0][1], ex.rotation[0][2], ex.translation[0],
293  ex.rotation[1][0], ex.rotation[1][1], ex.rotation[1][2], ex.translation[1],
294  ex.rotation[2][0], ex.rotation[2][1], ex.rotation[2][2], ex.translation[2]);
295 
296  cv::Mat P1 = cv::Mat::eye(3, 4, CV_64FC1);
297  P1.at<double>(0,0) = K1.at<double>(0,0);
298  P1.at<double>(1,1) = K1.at<double>(1,1);
299  P1.at<double>(0,2) = K1.at<double>(0,2);
300  P1.at<double>(1,2) = K1.at<double>(1,2);
301 
302  cv::Mat P2 = cv::Mat::eye(3, 4, CV_64FC1);
303  P2.at<double>(0,0) = K2.at<double>(0,0);
304  P2.at<double>(1,1) = K2.at<double>(1,1);
305  P2.at<double>(0,2) = K2.at<double>(0,2);
306  P2.at<double>(1,2) = K2.at<double>(1,2);
307 
308  CameraModel leftModel(this->getSerial(), size, K1, D1, cv::Mat::eye(3, 3, CV_64F), P1, this->getLocalTransform());
309  CameraModel rightModel(this->getSerial(), size, K2, D2, cv::Mat::eye(3, 3, CV_64F), P2, this->getLocalTransform());
310  UINFO("raw: fx=%f fy=%f cx=%f cy=%f",
311  leftModel.fx(),
312  leftModel.fy(),
313  leftModel.cx(),
314  leftModel.cy());
315 
316  UINFO("stereo extrinsics = %s", extrinsics.prettyPrint().c_str());
317  stereoModel_ = StereoCameraModel(this->getSerial(), leftModel, rightModel, extrinsics);
318 
319  if(!stereoModel_.isValidForRectification())
320  {
321  UERROR("Could not initialize stereo rectification.");
322  return false;
323  }
324 
325  stereoModel_.initRectificationMap();
326  UINFO("baseline = %f rectified: fx=%f fy=%f cx=%f cy=%f",
327  stereoModel_.baseline(),
328  stereoModel_.left().fx(),
329  stereoModel_.left().fy(),
330  stereoModel_.left().cx(),
331  stereoModel_.left().cy());
332 
333  // get left to imu camera transform
334  auto &&exImu = api_->GetMotionExtrinsics(mynteye::Stream::LEFT);
335  is_data_use_mm_instead_of_m =
336  abs(exImu.translation[0]) > 1.0 ||
337  abs(exImu.translation[1]) > 1.0 ||
338  abs(exImu.translation[2]) > 1.0;
339 
340  if (is_data_use_mm_instead_of_m) {
341  exImu.translation[0] *= 0.001;
342  exImu.translation[1] *= 0.001;
343  exImu.translation[2] *= 0.001;
344  }
345 
346  if (exImu.rotation[0][0] == 0 && exImu.rotation[2][2] == 0)
347  {
348  imuLocalTransform_ = Transform(
349  0, 0, 1, exImu.translation[0],
350  0,-1, 0, exImu.translation[1],
351  1, 0, 0, exImu.translation[2]);
352  }
353  else
354  {
355  imuLocalTransform_ = Transform(
356  exImu.rotation[0][0], exImu.rotation[0][1], exImu.rotation[0][2], exImu.translation[0],
357  exImu.rotation[1][0], exImu.rotation[1][1], exImu.rotation[1][2], exImu.translation[1],
358  exImu.rotation[2][0], exImu.rotation[2][1], exImu.rotation[2][2], exImu.translation[2]);
359  }
360  UINFO("imu extrinsics = %s", imuLocalTransform_.prettyPrint().c_str());
361 
362  for(int i=0;i<(int)mynteye::Stream::LAST; ++i)
363  {
364  UINFO("Support stream %d = %s", i, api_->Supports((mynteye::Stream)i)?"true":"false");
365  }
366 
367  if (api_->Supports(apiRectification_?mynteye::Stream::LEFT_RECTIFIED:mynteye::Stream::LEFT) &&
368  api_->Supports(apiRectification_?mynteye::Stream::RIGHT_RECTIFIED:mynteye::Stream::RIGHT))
369  {
370  api_->EnableStreamData(apiRectification_?mynteye::Stream::LEFT_RECTIFIED:mynteye::Stream::LEFT);
371  api_->SetStreamCallback(apiRectification_?mynteye::Stream::LEFT_RECTIFIED:mynteye::Stream::LEFT,
372  [&](const mynteye::api::StreamData &data)
373  {
374  double stamp = checkUpTimeStamp(data.img->timestamp, (std::uint8_t)(apiRectification_?mynteye::Stream::LEFT_RECTIFIED:mynteye::Stream::LEFT));
375 
376  UScopeMutex s(dataMutex_);
377  bool notify = false;
378  leftFrameBuffer_ = data.frame;
379  if(stamp_>0 && stamp_ == data.img->timestamp && !rightFrameBuffer_.empty())
380  {
381  notify = lastFrames_.first.empty();
382  lastFrames_.first = leftFrameBuffer_;
383  lastFrames_.second = rightFrameBuffer_;
384  lastFramesStamp_ = stamp;
385  leftFrameBuffer_ = cv::Mat();
386  rightFrameBuffer_ = cv::Mat();
387  stamp_ = 0;
388  }
389  else
390  {
391  stamp_ = data.img->timestamp;
392  }
393  if(notify)
394  {
395  dataReady_.release();
396  }
397  });
398 
399  if(apiRectification_ && apiDepth_ && api_->Supports(mynteye::Stream::DEPTH))
400  {
401  api_->EnableStreamData(mynteye::Stream::DEPTH);
402  }
403  else
404  {
405  api_->EnableStreamData(apiRectification_?mynteye::Stream::RIGHT_RECTIFIED:mynteye::Stream::RIGHT);
406  apiDepth_ = false;
407  }
408  api_->SetStreamCallback(
409  apiDepth_?mynteye::Stream::DEPTH:apiRectification_?mynteye::Stream::RIGHT_RECTIFIED:mynteye::Stream::RIGHT,
410  [&](const mynteye::api::StreamData &data)
411  {
412  double stamp = checkUpTimeStamp(data.img->timestamp, (std::uint8_t)(apiDepth_?mynteye::Stream::DEPTH:apiRectification_?mynteye::Stream::RIGHT_RECTIFIED:mynteye::Stream::RIGHT));
413 
414  UScopeMutex s(dataMutex_);
415  bool notify = false;
416  rightFrameBuffer_ = data.frame;
417  if(stamp_>0 && stamp_ == data.img->timestamp && !leftFrameBuffer_.empty())
418  {
419  notify = lastFrames_.first.empty();
420  lastFrames_.first = leftFrameBuffer_;
421  lastFrames_.second = rightFrameBuffer_;
422  lastFramesStamp_ = stamp;
423  leftFrameBuffer_ = cv::Mat();
424  rightFrameBuffer_ = cv::Mat();
425  stamp_ = 0;
426  }
427  else
428  {
429  stamp_ = data.img->timestamp;
430  }
431  if(notify)
432  {
433  dataReady_.release();
434  }
435  });
436 
437  api_->SetMotionCallback([this](const mynteye::api::MotionData &data) {
438 
439  double stamp = checkUpTimeStamp(data.imu->timestamp, (std::uint8_t)mynteye::Stream::LAST);
440  if(data.imu->flag == 0)
441  {
442  //UWARN("%f %f %f %f %f %f",
443  // data.imu->gyro[0] * M_PI / 180, data.imu->gyro[1] * M_PI / 180, data.imu->gyro[2] * M_PI / 180,
444  // data.imu->accel[0] * 9.8, data.imu->accel[1] * 9.8, data.imu->accel[2] * 9.8);
445  cv::Vec3d gyro(data.imu->gyro[0] * M_PI / 180, data.imu->gyro[1] * M_PI / 180, data.imu->gyro[2] * M_PI / 180);
446  cv::Vec3d acc(data.imu->accel[0] * 9.8, data.imu->accel[1] * 9.8, data.imu->accel[2] * 9.8);
447  if(publishInterIMU_)
448  {
449  IMU imu(gyro, cv::Mat::eye(3,3,CV_64FC1),
450  acc, cv::Mat::eye(3,3,CV_64FC1),
451  imuLocalTransform_);
452  UEventsManager::post(new IMUEvent(imu, stamp));
453  }
454  else
455  {
456  UScopeMutex lock(imuMutex_);
457  imuBuffer_.insert(imuBuffer_.end(), std::make_pair(stamp, std::make_pair(gyro, acc)));
458  if(imuBuffer_.size() > 1000)
459  {
460  imuBuffer_.erase(imuBuffer_.begin());
461  }
462  }
463  }
464 
465  uSleep(1);
466 
467  });
468 
469  api_->SetOptionValue(mynteye::Option::EXPOSURE_MODE, autoExposure_?0:1);
470  if(!autoExposure_)
471  {
472  api_->SetOptionValue(mynteye::Option::GAIN, gain_);
473  api_->SetOptionValue(mynteye::Option::BRIGHTNESS, brightness_);
474  api_->SetOptionValue(mynteye::Option::CONTRAST, contrast_);
475  }
476  api_->SetOptionValue(mynteye::Option::IR_CONTROL, irControl_);
477 
478  api_->Start(mynteye::Source::ALL);
479  uSleep(500); // To buffer some imus before sending images
480  return true;
481  }
482  UERROR("Streams missing.");
483  return false;
484 #else
485  UERROR("Not built with Mynt Eye support!");
486  return false;
487 #endif
488 }
489 
490 bool CameraMyntEye::isCalibrated() const
491 {
492  return true;
493 }
494 
495 std::string CameraMyntEye::getSerial() const
496 {
497 #ifdef RTABMAP_MYNTEYE
498  if(device_.get())
499  {
500  return device_->GetInfo(mynteye::Info::SERIAL_NUMBER);
501  }
502 #endif
503  return "";
504 }
505 
506 bool CameraMyntEye::available()
507 {
508 #ifdef RTABMAP_MYNTEYE
509  return true;
510 #else
511  return false;
512 #endif
513 }
514 
515 #ifdef RTABMAP_MYNTEYE
516 void CameraMyntEye::getPoseAndIMU(
517  const double & stamp,
518  IMU & imu,
519  int maxWaitTimeMs) const
520 {
521  imu = IMU();
522  if(imuBuffer_.empty())
523  {
524  return;
525  }
526 
527  // Interpolate gyro,acc
528  cv::Vec3d gyro;
529  cv::Vec3d acc;
530  {
531  imuMutex_.lock();
532  int waitTry = 0;
533  while(maxWaitTimeMs > 0 && imuBuffer_.rbegin()->first < stamp && waitTry < maxWaitTimeMs)
534  {
535  imuMutex_.unlock();
536  ++waitTry;
537  uSleep(1);
538  imuMutex_.lock();
539  }
540  if(imuBuffer_.rbegin()->first < stamp)
541  {
542  if(maxWaitTimeMs>0)
543  {
544  UWARN("Could not find gyro/acc data to interpolate at time %f after waiting %d ms (last is %f)...", stamp, maxWaitTimeMs, imuBuffer_.rbegin()->first);
545  }
546  imuMutex_.unlock();
547  return;
548  }
549  else
550  {
551  std::map<double, std::pair<cv::Vec3f, cv::Vec3f> >::const_iterator iterB = imuBuffer_.lower_bound(stamp);
552  std::map<double, std::pair<cv::Vec3f, cv::Vec3f> >::const_iterator iterA = iterB;
553  if(iterA != imuBuffer_.begin())
554  {
555  iterA = --iterA;
556  }
557  if(iterB == imuBuffer_.end())
558  {
559  iterB = --iterB;
560  }
561  if(iterA == iterB && stamp == iterA->first)
562  {
563  gyro[0] = iterA->second.first[0];
564  gyro[1] = iterA->second.first[1];
565  gyro[2] = iterA->second.first[2];
566  acc[0] = iterA->second.second[0];
567  acc[1] = iterA->second.second[1];
568  acc[2] = iterA->second.second[2];
569  }
570  else if(stamp >= iterA->first && stamp <= iterB->first)
571  {
572  float t = (stamp-iterA->first) / (iterB->first-iterA->first);
573  gyro[0] = iterA->second.first[0] + t*(iterB->second.first[0] - iterA->second.first[0]);
574  gyro[1] = iterA->second.first[1] + t*(iterB->second.first[1] - iterA->second.first[1]);
575  gyro[2] = iterA->second.first[2] + t*(iterB->second.first[2] - iterA->second.first[2]);
576  acc[0] = iterA->second.second[0] + t*(iterB->second.second[0] - iterA->second.second[0]);
577  acc[1] = iterA->second.second[1] + t*(iterB->second.second[1] - iterA->second.second[1]);
578  acc[2] = iterA->second.second[2] + t*(iterB->second.second[2] - iterA->second.second[2]);
579  }
580  else
581  {
582  if(stamp < iterA->first)
583  {
584  UWARN("Could not find acc data to interpolate at image time %f (earliest is %f). Are sensors synchronized?", stamp, iterA->first);
585  }
586  else
587  {
588  UWARN("Could not find acc data to interpolate at image time %f (between %f and %f). Are sensors synchronized?", stamp, iterA->first, iterB->first);
589  }
590  imuMutex_.unlock();
591  return;
592  }
593  }
594  imuMutex_.unlock();
595  }
596 
597  imu = IMU(gyro, cv::Mat::eye(3, 3, CV_64FC1), acc, cv::Mat::eye(3, 3, CV_64FC1), imuLocalTransform_);
598 }
599 #endif
600 
601 SensorData CameraMyntEye::captureImage(CameraInfo * info)
602 {
603  SensorData data;
604 #ifdef RTABMAP_MYNTEYE
605  if(!dataReady_.acquire(1, 3000))
606  {
607  UERROR("Did not receive frame since 3 seconds...");
608  return data;
609  }
610 
611  cv::Mat left;
612  cv::Mat right;
613  double stamp = 0.0;
614 
615  dataMutex_.lock();
616  if(!lastFrames_.first.empty())
617  {
618  left = lastFrames_.first;
619  right = lastFrames_.second;
620  stamp = lastFramesStamp_;
621  lastFrames_ = std::pair<cv::Mat, cv::Mat>();
622  }
623  dataMutex_.unlock();
624 
625  if(!left.empty() && !right.empty())
626  {
627  if(right.type() == CV_16UC1)
628  {
629  data = SensorData(left, right, stereoModel_.left(), 0, stamp);
630  }
631  else
632  {
633  if(!apiRectification_)
634  {
635  left = stereoModel_.left().rectifyImage(left);
636  right = stereoModel_.right().rectifyImage(right);
637  }
638  data = SensorData(left, right, stereoModel_, 0, stamp);
639  }
640  if(!publishInterIMU_ && imuBuffer_.size())
641  {
642  IMU imu;
643  getPoseAndIMU(stamp, imu, 60);
644  if(!imu.empty())
645  {
646  data.setIMU(imu);
647  }
648  }
649  }
650 #else
651  UERROR("Not built with Mynt Eye support!");
652 #endif
653  return data;
654 }
655 
656 } // namespace rtabmap
rtabmap::CameraMyntEye::setManualExposure
void setManualExposure(int gain=24, int brightness=120, int constrast=116)
Definition: CameraMyntEye.cpp:168
rtabmap::CameraMyntEye::setIrControl
void setIrControl(int value)
Definition: CameraMyntEye.cpp:181
rtabmap::Transform::prettyPrint
std::string prettyPrint() const
Definition: Transform.cpp:295
UEventsManager::post
static void post(UEvent *event, bool async=true, const UEventsSender *sender=0)
Definition: UEventsManager.cpp:54
rtabmap::CameraMyntEye::CameraMyntEye
CameraMyntEye(const std::string &device="", bool apiRectification=false, bool apiDepth=false, float imageRate=0, const Transform &localTransform=CameraModel::opticalRotation())
Definition: CameraMyntEye.cpp:45
rtabmap::IMU::empty
bool empty() const
Definition: IMU.h:63
rtabmap::Camera::getLocalTransform
const Transform & getLocalTransform() const
Definition: Camera.h:64
rtabmap::CameraMyntEye::isCalibrated
virtual bool isCalibrated() const
Definition: CameraMyntEye.cpp:490
rtabmap::CameraMyntEye::getSerial
virtual std::string getSerial() const
Definition: CameraMyntEye.cpp:495
glm::e
GLM_FUNC_DECL genType e()
rtabmap::CameraMyntEye::captureImage
virtual SensorData captureImage(CameraInfo *info=0)
Definition: CameraMyntEye.cpp:601
glm::uint8_t
detail::uint8 uint8_t
Definition: fwd.hpp:908
UASSERT
#define UASSERT(condition)
rtabmap::CameraModel::cx
double cx() const
Definition: CameraModel.h:104
true
#define true
Definition: ConvertUTF.c:57
rtabmap::CameraModel::fy
double fy() const
Definition: CameraModel.h:103
glm::uint64_t
detail::uint64 uint64_t
Definition: fwd.hpp:920
glm::abs
GLM_FUNC_DECL genType abs(genType const &x)
rtabmap_superglue.device
string device
Definition: rtabmap_superglue.py:21
rtabmap::CameraModel::cy
double cy() const
Definition: CameraModel.h:105
glm::rotation
GLM_FUNC_DECL detail::tquat< T, P > rotation(detail::tvec3< T, P > const &orig, detail::tvec3< T, P > const &dest)
glm::uint32_t
detail::uint32 uint32_t
Definition: fwd.hpp:916
false
#define false
Definition: ConvertUTF.c:56
uSleep
void uSleep(unsigned int ms)
Definition: Posix/UThreadC.h:23
rtabmap::CameraMyntEye::publishInterIMU
void publishInterIMU(bool enabled)
Definition: CameraMyntEye.cpp:154
rtabmap::CameraModel::fx
double fx() const
Definition: CameraModel.h:102
glm::max
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
rtabmap::CameraMyntEye::init
virtual bool init(const std::string &calibrationFolder=".", const std::string &cameraName="")
Definition: CameraMyntEye.cpp:189
UERROR
#define UERROR(...)
UWARN
#define UWARN(...)
UTimer::now
static double now()
Definition: UTimer.cpp:80
rtabmap::SensorData::setIMU
void setIMU(const IMU &imu)
Definition: SensorData.h:268
UINFO
#define UINFO(...)

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:34:58