32 #ifdef RTABMAP_MYNTEYE
33 #include <mynteye/api.h>
34 #include <mynteye/device.h>
35 #include <mynteye/context.h>
38 #define M_PI 3.14159265358979323846
46 Camera(imageRate, localTransform)
47 #ifdef RTABMAP_MYNTEYE
50 apiRectification_(apiRectification),
57 lastFramesStamp_(0.0),
59 publishInterIMU_(
false),
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);
72 #ifdef RTABMAP_MYNTEYE
74 api_->Stop(mynteye::Source::ALL);
80 #ifdef RTABMAP_MYNTEYE
81 std::shared_ptr<mynteye::IntrinsicsBase> getDefaultIntrinsics() {
82 auto res = std::make_shared<mynteye::IntrinsicsPinhole>();
86 res->fx = 3.6220059643202876e+02;
87 res->fy = 3.6350065250745848e+02;
88 res->cx = 4.0658699068023441e+02;
89 res->cy = 2.3435161110061483e+02;
91 -2.5034765682756088e-01,
92 5.0579399202897619e-02,
93 -7.0536676161976066e-04,
94 -8.5255451307033846e-03,
97 for (
unsigned int i = 0;
i < 5;
i++) {
98 res->coeffs[
i] = codffs[
i];
103 std::shared_ptr<mynteye::Extrinsics> getDefaultExtrinsics() {
104 auto res = std::make_shared<mynteye::Extrinsics>();
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
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++) {
117 for (
unsigned int i = 0;
i < 3;
i++) {
123 double CameraMyntEye::hardTimeToSoftTime(
std::uint64_t hardTime) {
124 if (hardTimeBegin_==0) {
126 hardTimeBegin_ = hardTime;
132 double time_sec_double =
static_cast<double>(time_ns_detal_s) + 1
e-9*
static_cast<double>(time_ns_detal_ns * 1000);
134 return softTimeBegin_ + time_sec_double;
139 return (now < pre) && ((pre - now) > (unit_hard_time / 2));
144 if (is_overflow(_hard_time, lastHardTimes_[stream], unitHardTime_)) {
148 lastHardTimes_[
stream] = _hard_time;
150 return hardTimeToSoftTime(acc_[stream] * unitHardTime_ + _hard_time);
156 #ifdef RTABMAP_MYNTEYE
163 #ifdef RTABMAP_MYNTEYE
164 autoExposure_ =
true;
170 #ifdef RTABMAP_MYNTEYE
172 UASSERT(brightness>=0 && brightness<=240);
173 UASSERT(constrast>=0 && constrast<=254);
174 autoExposure_ =
false;
176 brightness_ = brightness;
177 contrast_ = constrast;
183 #ifdef RTABMAP_MYNTEYE
191 #ifdef RTABMAP_MYNTEYE
192 mynteye::Context context;
193 auto &&devices = context.devices();
195 softTimeBegin_ = 0.0;
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);
201 size_t n = devices.size();
204 UERROR(
"No Mynt Eye devices detected!");
207 UINFO(
"MYNT EYE devices:");
209 for (
size_t i = 0;
i <
n;
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)
215 device_ = devices[
i];
217 UINFO(
" index: %d, name: %s, serial number: %s", (
int)
i,
name.c_str(), serial_number.c_str());
220 if(device_.get() == 0)
223 device_ = devices[0];
227 api_ = mynteye::API::Create(device_);
230 auto in_left_base = api_->GetIntrinsicsBase(mynteye::Stream::LEFT);
231 auto in_right_base = api_->GetIntrinsicsBase(mynteye::Stream::RIGHT);
233 auto ex = api_->GetExtrinsics(mynteye::Stream::RIGHT, mynteye::Stream::LEFT);
236 if(!in_left_base || !in_right_base)
238 UERROR(
"Unknown calibration model! Using default ones");
239 in_left_base = getDefaultIntrinsics();
240 in_right_base = getDefaultIntrinsics();
241 ex = *(getDefaultExtrinsics());
243 cv::Size
size{in_left_base->width, in_left_base->height};
245 cv::Mat K1, K2, D1, D2;
247 if(in_left_base->calib_model() == mynteye::CalibrationModel::PINHOLE &&
248 in_right_base->calib_model() == mynteye::CalibrationModel::PINHOLE)
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);
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();
258 else if(in_left_base->calib_model() == mynteye::CalibrationModel::KANNALA_BRANDT &&
259 in_right_base->calib_model() == mynteye::CalibrationModel::KANNALA_BRANDT)
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);
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]);
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);
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]);
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)
286 ex.translation[0] *= 0.001;
287 ex.translation[1] *= 0.001;
288 ex.translation[2] *= 0.001;
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]);
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);
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);
310 UINFO(
"raw: fx=%f fy=%f cx=%f cy=%f",
319 if(!stereoModel_.isValidForRectification())
321 UERROR(
"Could not initialize stereo rectification.");
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());
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;
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;
346 if (exImu.rotation[0][0] == 0 && exImu.rotation[2][2] == 0)
349 0, 0, 1, exImu.translation[0],
350 0,-1, 0, exImu.translation[1],
351 1, 0, 0, exImu.translation[2]);
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]);
360 UINFO(
"imu extrinsics = %s", imuLocalTransform_.prettyPrint().c_str());
362 for(
int i=0;
i<(
int)mynteye::Stream::LAST; ++
i)
364 UINFO(
"Support stream %d = %s",
i, api_->Supports((mynteye::Stream)
i)?
"true":
"false");
367 if (api_->Supports(apiRectification_?mynteye::Stream::LEFT_RECTIFIED:mynteye::Stream::LEFT) &&
368 api_->Supports(apiRectification_?mynteye::Stream::RIGHT_RECTIFIED:mynteye::Stream::RIGHT))
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)
374 double stamp = checkUpTimeStamp(
data.img->timestamp, (
std::uint8_t)(apiRectification_?mynteye::Stream::LEFT_RECTIFIED:mynteye::Stream::LEFT));
378 leftFrameBuffer_ =
data.frame;
379 if(stamp_>0 && stamp_ ==
data.img->timestamp && !rightFrameBuffer_.empty())
381 notify = lastFrames_.first.empty();
382 lastFrames_.first = leftFrameBuffer_;
383 lastFrames_.second = rightFrameBuffer_;
384 lastFramesStamp_ = stamp;
385 leftFrameBuffer_ = cv::Mat();
386 rightFrameBuffer_ = cv::Mat();
391 stamp_ =
data.img->timestamp;
395 dataReady_.release();
399 if(apiRectification_ && apiDepth_ && api_->Supports(mynteye::Stream::DEPTH))
401 api_->EnableStreamData(mynteye::Stream::DEPTH);
405 api_->EnableStreamData(apiRectification_?mynteye::Stream::RIGHT_RECTIFIED:mynteye::Stream::RIGHT);
408 api_->SetStreamCallback(
409 apiDepth_?mynteye::Stream::DEPTH:apiRectification_?mynteye::Stream::RIGHT_RECTIFIED:mynteye::Stream::RIGHT,
410 [&](
const mynteye::api::StreamData &
data)
412 double stamp = checkUpTimeStamp(
data.img->timestamp, (
std::uint8_t)(apiDepth_?mynteye::Stream::DEPTH:apiRectification_?mynteye::Stream::RIGHT_RECTIFIED:mynteye::Stream::RIGHT));
416 rightFrameBuffer_ =
data.frame;
417 if(stamp_>0 && stamp_ ==
data.img->timestamp && !leftFrameBuffer_.empty())
419 notify = lastFrames_.first.empty();
420 lastFrames_.first = leftFrameBuffer_;
421 lastFrames_.second = rightFrameBuffer_;
422 lastFramesStamp_ = stamp;
423 leftFrameBuffer_ = cv::Mat();
424 rightFrameBuffer_ = cv::Mat();
429 stamp_ =
data.img->timestamp;
433 dataReady_.release();
437 api_->SetMotionCallback([
this](
const mynteye::api::MotionData &
data) {
439 double stamp = checkUpTimeStamp(
data.imu->timestamp, (
std::uint8_t)mynteye::Stream::LAST);
440 if(
data.imu->flag == 0)
446 cv::Vec3d acc(
data.imu->accel[0] * 9.8,
data.imu->accel[1] * 9.8,
data.imu->accel[2] * 9.8);
449 IMU imu(gyro, cv::Mat::eye(3,3,CV_64FC1),
450 acc, cv::Mat::eye(3,3,CV_64FC1),
457 imuBuffer_.insert(imuBuffer_.end(), std::make_pair(stamp, std::make_pair(gyro, acc)));
458 if(imuBuffer_.size() > 1000)
460 imuBuffer_.erase(imuBuffer_.begin());
469 api_->SetOptionValue(mynteye::Option::EXPOSURE_MODE, autoExposure_?0:1);
472 api_->SetOptionValue(mynteye::Option::GAIN, gain_);
473 api_->SetOptionValue(mynteye::Option::BRIGHTNESS, brightness_);
474 api_->SetOptionValue(mynteye::Option::CONTRAST, contrast_);
476 api_->SetOptionValue(mynteye::Option::IR_CONTROL, irControl_);
478 api_->Start(mynteye::Source::ALL);
482 UERROR(
"Streams missing.");
485 UERROR(
"Not built with Mynt Eye support!");
497 #ifdef RTABMAP_MYNTEYE
500 return device_->GetInfo(mynteye::Info::SERIAL_NUMBER);
508 #ifdef RTABMAP_MYNTEYE
515 #ifdef RTABMAP_MYNTEYE
516 void CameraMyntEye::getPoseAndIMU(
517 const double & stamp,
519 int maxWaitTimeMs)
const
522 if(imuBuffer_.empty())
533 while(maxWaitTimeMs > 0 && imuBuffer_.rbegin()->first < stamp && waitTry < maxWaitTimeMs)
540 if(imuBuffer_.rbegin()->first < stamp)
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);
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())
557 if(iterB == imuBuffer_.end())
561 if(iterA == iterB && stamp == iterA->first)
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];
570 else if(stamp >= iterA->first && stamp <= iterB->first)
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]);
582 if(stamp < iterA->first)
584 UWARN(
"Could not find acc data to interpolate at image time %f (earliest is %f). Are sensors synchronized?", stamp, iterA->first);
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);
597 imu = IMU(gyro, cv::Mat::eye(3, 3, CV_64FC1), acc, cv::Mat::eye(3, 3, CV_64FC1), imuLocalTransform_);
604 #ifdef RTABMAP_MYNTEYE
605 if(!dataReady_.acquire(1, 3000))
607 UERROR(
"Did not receive frame since 3 seconds...");
616 if(!lastFrames_.first.empty())
618 left = lastFrames_.first;
619 right = lastFrames_.second;
620 stamp = lastFramesStamp_;
621 lastFrames_ = std::pair<cv::Mat, cv::Mat>();
625 if(!left.empty() && !right.empty())
627 if(right.type() == CV_16UC1)
633 if(!apiRectification_)
635 left = stereoModel_.left().rectifyImage(left);
636 right = stereoModel_.right().rectifyImage(right);
643 getPoseAndIMU(stamp, imu, 60);
651 UERROR(
"Not built with Mynt Eye support!");