mirror of
https://github.com/introlab/rtabmap.git
synced 2026-10-07 02:27:47 +08:00
matlabbe
700704bec9
* Working rtabmap_lidar-mapping example (live and pcap) * finalizing merge, added some deprecated * fixed build * Working deskewing for Lidar + Camera/IMU (no camera pose correction yet) and Lidar + Odom Sensor in main UI. * backward compatibility * fixed some not used variable warnings, fixed qt build for lidar mapping example * Refactored CameraMobile, added AREngine background support, fixed LidarVPL16 build error with PCL 1.8 * ARCoreJava: buffer last depth image in case its stamp i higher than pose stamp. CameraMobile: added pose buffer. SensorCaptureThread: to get pose, odomSensor should be explicitly set, but can be same as lidar or camera inputs. * Working external lidar on iOS * util3d::commonFiltering()/adjustNormalsToViewPoint() added organized cloud support. MainWindow: updated odomSensor setup * fixed winsock include order * reverted camera tool * disable imu filtering when odom sensor is used * Updated package version * fixed windows build * fixing more windows build erros
657 lines
21 KiB
C++
657 lines
21 KiB
C++
/*
|
|
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
|
All rights reserved.
|
|
|
|
Redistribution and use in source and binary forms, with or without
|
|
modification, are permitted provided that the following conditions are met:
|
|
* Redistributions of source code must retain the above copyright
|
|
notice, this list of conditions and the following disclaimer.
|
|
* Redistributions in binary form must reproduce the above copyright
|
|
notice, this list of conditions and the following disclaimer in the
|
|
documentation and/or other materials provided with the distribution.
|
|
* Neither the name of the Universite de Sherbrooke nor the
|
|
names of its contributors may be used to endorse or promote products
|
|
derived from this software without specific prior written permission.
|
|
|
|
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
|
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
|
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
|
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
|
|
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
|
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
|
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
|
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
|
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
|
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
|
*/
|
|
|
|
#include "rtabmap/utilite/UThread.h"
|
|
#include "rtabmap/utilite/UTimer.h"
|
|
#include "rtabmap/utilite/UEventsManager.h"
|
|
#include <rtabmap/core/camera/CameraMyntEye.h>
|
|
#ifdef RTABMAP_MYNTEYE
|
|
#include <mynteye/api.h>
|
|
#include <mynteye/device.h>
|
|
#include <mynteye/context.h>
|
|
|
|
#ifndef M_PI
|
|
#define M_PI 3.14159265358979323846
|
|
#endif
|
|
#endif
|
|
|
|
namespace rtabmap
|
|
{
|
|
|
|
CameraMyntEye::CameraMyntEye(const std::string & device, bool apiRectification, bool apiDepth, float imageRate, const Transform & localTransform) :
|
|
Camera(imageRate, localTransform)
|
|
#ifdef RTABMAP_MYNTEYE
|
|
,
|
|
deviceName_(device),
|
|
apiRectification_(apiRectification),
|
|
apiDepth_(apiDepth),
|
|
autoExposure_(true),
|
|
gain_(24),
|
|
brightness_(120),
|
|
contrast_(116),
|
|
dataReady_(0),
|
|
lastFramesStamp_(0.0),
|
|
stamp_(0),
|
|
publishInterIMU_(false),
|
|
softTimeBegin_(0.0),
|
|
hardTimeBegin_(0),
|
|
unitHardTime_(std::numeric_limits<std::uint32_t>::max()*10)
|
|
#endif
|
|
{
|
|
#ifdef RTABMAP_MYNTEYE
|
|
lastHardTimes_ = std::vector<std::uint64_t>((size_t)mynteye::Stream::LAST+1, 0);
|
|
acc_ = std::vector<std::uint64_t>((size_t)mynteye::Stream::LAST+1, 0);
|
|
#endif
|
|
}
|
|
CameraMyntEye::~CameraMyntEye()
|
|
{
|
|
#ifdef RTABMAP_MYNTEYE
|
|
if (api_) {
|
|
api_->Stop(mynteye::Source::ALL);
|
|
}
|
|
dataReady_.release();
|
|
#endif
|
|
}
|
|
|
|
#ifdef RTABMAP_MYNTEYE
|
|
std::shared_ptr<mynteye::IntrinsicsBase> getDefaultIntrinsics() {
|
|
auto res = std::make_shared<mynteye::IntrinsicsPinhole>();
|
|
res->width = 640;
|
|
res->height = 400;
|
|
res->model = 0;
|
|
res->fx = 3.6220059643202876e+02;
|
|
res->fy = 3.6350065250745848e+02;
|
|
res->cx = 4.0658699068023441e+02;
|
|
res->cy = 2.3435161110061483e+02;
|
|
double codffs[5] = {
|
|
-2.5034765682756088e-01,
|
|
5.0579399202897619e-02,
|
|
-7.0536676161976066e-04,
|
|
-8.5255451307033846e-03,
|
|
0.
|
|
};
|
|
for (unsigned int i = 0; i < 5; i++) {
|
|
res->coeffs[i] = codffs[i];
|
|
}
|
|
return res;
|
|
}
|
|
|
|
std::shared_ptr<mynteye::Extrinsics> getDefaultExtrinsics() {
|
|
auto res = std::make_shared<mynteye::Extrinsics>();
|
|
double rotation[9] = {
|
|
9.9867908939669447e-01, -6.3445566137485428e-03, 5.0988459509619687e-02,
|
|
5.9890316389333252e-03, 9.9995670037792639e-01, 7.1224201868366971e-03,
|
|
-5.1031440326695092e-02, -6.8076406092671274e-03, 9.9867384471984544e-01
|
|
};
|
|
double translation[3] = {-1.2002489764113250e+02, -1.1782637409050747e+00,
|
|
-5.2058205159996538e+00};
|
|
for (unsigned int i = 0; i < 3; i++) {
|
|
for (unsigned int j = 0; j < 3; j++) {
|
|
res->rotation[i][j] = rotation[i*3 + j];
|
|
}
|
|
}
|
|
for (unsigned int i = 0; i < 3; i++) {
|
|
res->translation[i] = translation[i];
|
|
}
|
|
return res;
|
|
}
|
|
|
|
double CameraMyntEye::hardTimeToSoftTime(std::uint64_t hardTime) {
|
|
if (hardTimeBegin_==0) {
|
|
softTimeBegin_ = UTimer::now();
|
|
hardTimeBegin_ = hardTime;
|
|
}
|
|
|
|
std::uint64_t time_ns_detal = (hardTime - hardTimeBegin_);
|
|
std::uint64_t time_ns_detal_s = time_ns_detal / 1000000;
|
|
std::uint64_t time_ns_detal_ns = time_ns_detal % 1000000;
|
|
double time_sec_double = static_cast<double>(time_ns_detal_s) + 1e-9*static_cast<double>(time_ns_detal_ns * 1000);
|
|
|
|
return softTimeBegin_ + time_sec_double;
|
|
}
|
|
|
|
inline bool is_overflow(std::uint64_t now, std::uint64_t pre, std::uint64_t unit_hard_time) {
|
|
|
|
return (now < pre) && ((pre - now) > (unit_hard_time / 2));
|
|
}
|
|
|
|
double CameraMyntEye::checkUpTimeStamp(std::uint64_t _hard_time, std::uint8_t stream) {
|
|
UASSERT(stream < (std::uint8_t)mynteye::Stream::LAST+1);
|
|
if (is_overflow(_hard_time, lastHardTimes_[stream], unitHardTime_)) {
|
|
acc_[stream]++;
|
|
}
|
|
|
|
lastHardTimes_[stream] = _hard_time;
|
|
|
|
return hardTimeToSoftTime(acc_[stream] * unitHardTime_ + _hard_time);
|
|
}
|
|
#endif
|
|
|
|
void CameraMyntEye::publishInterIMU(bool enabled)
|
|
{
|
|
#ifdef RTABMAP_MYNTEYE
|
|
publishInterIMU_ = enabled;
|
|
#endif
|
|
}
|
|
|
|
void CameraMyntEye::setAutoExposure()
|
|
{
|
|
#ifdef RTABMAP_MYNTEYE
|
|
autoExposure_ = true;
|
|
#endif
|
|
}
|
|
|
|
void CameraMyntEye::setManualExposure(int gain, int brightness, int constrast)
|
|
{
|
|
#ifdef RTABMAP_MYNTEYE
|
|
UASSERT(gain>=0 && gain<=48);
|
|
UASSERT(brightness>=0 && brightness<=240);
|
|
UASSERT(constrast>=0 && constrast<=254);
|
|
autoExposure_ = false;
|
|
gain_ = gain;
|
|
brightness_ = brightness;
|
|
contrast_ = constrast;
|
|
#endif
|
|
}
|
|
|
|
void CameraMyntEye::setIrControl(int value)
|
|
{
|
|
#ifdef RTABMAP_MYNTEYE
|
|
UASSERT(value>=0 && value<=160);
|
|
irControl_ = value;
|
|
#endif
|
|
}
|
|
|
|
bool CameraMyntEye::init(const std::string & calibrationFolder, const std::string & cameraName)
|
|
{
|
|
#ifdef RTABMAP_MYNTEYE
|
|
mynteye::Context context;
|
|
auto &&devices = context.devices();
|
|
|
|
softTimeBegin_ = 0.0;
|
|
hardTimeBegin_ = 0;
|
|
imuBuffer_.clear();
|
|
lastHardTimes_ = std::vector<std::uint64_t>((size_t)mynteye::Stream::LAST+1, 0);
|
|
acc_ = std::vector<std::uint64_t>((size_t)mynteye::Stream::LAST+1, 0);
|
|
|
|
size_t n = devices.size();
|
|
if(n==0)
|
|
{
|
|
UERROR("No Mynt Eye devices detected!");
|
|
return false;
|
|
}
|
|
UINFO("MYNT EYE devices:");
|
|
|
|
for (size_t i = 0; i < n; i++) {
|
|
auto &&device = devices[i];
|
|
auto &&name = device->GetInfo(mynteye::Info::DEVICE_NAME);
|
|
auto &&serial_number = device->GetInfo(mynteye::Info::SERIAL_NUMBER);
|
|
if(!deviceName_.empty() && serial_number.compare(deviceName_) == 0)
|
|
{
|
|
device_ = devices[i];
|
|
}
|
|
UINFO(" index: %d, name: %s, serial number: %s", (int)i, name.c_str(), serial_number.c_str());
|
|
}
|
|
|
|
if(device_.get() == 0)
|
|
{
|
|
//take first one by default
|
|
device_ = devices[0];
|
|
}
|
|
|
|
UINFO("");
|
|
api_ = mynteye::API::Create(device_);
|
|
|
|
UINFO("");
|
|
auto in_left_base = api_->GetIntrinsicsBase(mynteye::Stream::LEFT);
|
|
auto in_right_base = api_->GetIntrinsicsBase(mynteye::Stream::RIGHT);
|
|
UINFO("");
|
|
auto ex = api_->GetExtrinsics(mynteye::Stream::RIGHT, mynteye::Stream::LEFT);
|
|
|
|
UINFO("");
|
|
if(!in_left_base || !in_right_base)
|
|
{
|
|
UERROR("Unknown calibration model! Using default ones");
|
|
in_left_base = getDefaultIntrinsics();
|
|
in_right_base = getDefaultIntrinsics();
|
|
ex = *(getDefaultExtrinsics());
|
|
}
|
|
cv::Size size{in_left_base->width, in_left_base->height};
|
|
|
|
cv::Mat K1, K2, D1, D2;
|
|
|
|
if(in_left_base->calib_model() == mynteye::CalibrationModel::PINHOLE &&
|
|
in_right_base->calib_model() == mynteye::CalibrationModel::PINHOLE)
|
|
{
|
|
auto in_left = *std::dynamic_pointer_cast<mynteye::IntrinsicsPinhole>(in_left_base);
|
|
auto in_right = *std::dynamic_pointer_cast<mynteye::IntrinsicsPinhole>(in_right_base);
|
|
|
|
K1 = (cv::Mat_<double>(3, 3) << in_left.fx, 0, in_left.cx, 0, in_left.fy, in_left.cy, 0, 0, 1);
|
|
K2 = (cv::Mat_<double>(3, 3) << in_right.fx, 0, in_right.cx, 0, in_right.fy, in_right.cy, 0, 0, 1);
|
|
D1 = cv::Mat(1, 5, CV_64F, in_left.coeffs).clone();
|
|
D2 = cv::Mat(1, 5, CV_64F, in_right.coeffs).clone();
|
|
}
|
|
else if(in_left_base->calib_model() == mynteye::CalibrationModel::KANNALA_BRANDT &&
|
|
in_right_base->calib_model() == mynteye::CalibrationModel::KANNALA_BRANDT)
|
|
{
|
|
//equidistant
|
|
auto in_left = *std::dynamic_pointer_cast<mynteye::IntrinsicsEquidistant>(in_left_base);
|
|
auto in_right = *std::dynamic_pointer_cast<mynteye::IntrinsicsEquidistant>(in_right_base);
|
|
|
|
/** The distortion coefficients: k2,k3,k4,k5,mu,mv,u0,v0 */
|
|
UINFO("left coeff = %f %f %f %f %f %f %f %f",
|
|
in_left.coeffs[0], in_left.coeffs[1], in_left.coeffs[2], in_left.coeffs[3],
|
|
in_left.coeffs[4], in_left.coeffs[5], in_left.coeffs[6], in_left.coeffs[7]);
|
|
UINFO("right coeff = %f %f %f %f %f %f %f %f",
|
|
in_left.coeffs[0], in_left.coeffs[1], in_left.coeffs[2], in_left.coeffs[3],
|
|
in_left.coeffs[4], in_left.coeffs[5], in_left.coeffs[6], in_left.coeffs[7]);
|
|
|
|
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);
|
|
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);
|
|
// convert to (k1,k2,p1,p2,k3,k4)
|
|
D1 = (cv::Mat_<double>(1, 6) << in_left.coeffs[0], in_left.coeffs[1], 0, 0, in_left.coeffs[2], in_left.coeffs[3]);
|
|
D2 = (cv::Mat_<double>(1, 6) << in_right.coeffs[0], in_right.coeffs[1], 0, 0, in_right.coeffs[2], in_right.coeffs[3]);
|
|
}
|
|
|
|
bool is_data_use_mm_instead_of_m =
|
|
abs(ex.translation[0]) > 1.0 ||
|
|
abs(ex.translation[1]) > 1.0 ||
|
|
abs(ex.translation[2]) > 1.0;
|
|
if(is_data_use_mm_instead_of_m)
|
|
{
|
|
ex.translation[0] *= 0.001;
|
|
ex.translation[1] *= 0.001;
|
|
ex.translation[2] *= 0.001;
|
|
}
|
|
|
|
Transform extrinsics(
|
|
ex.rotation[0][0], ex.rotation[0][1], ex.rotation[0][2], ex.translation[0],
|
|
ex.rotation[1][0], ex.rotation[1][1], ex.rotation[1][2], ex.translation[1],
|
|
ex.rotation[2][0], ex.rotation[2][1], ex.rotation[2][2], ex.translation[2]);
|
|
|
|
cv::Mat P1 = cv::Mat::eye(3, 4, CV_64FC1);
|
|
P1.at<double>(0,0) = K1.at<double>(0,0);
|
|
P1.at<double>(1,1) = K1.at<double>(1,1);
|
|
P1.at<double>(0,2) = K1.at<double>(0,2);
|
|
P1.at<double>(1,2) = K1.at<double>(1,2);
|
|
|
|
cv::Mat P2 = cv::Mat::eye(3, 4, CV_64FC1);
|
|
P2.at<double>(0,0) = K2.at<double>(0,0);
|
|
P2.at<double>(1,1) = K2.at<double>(1,1);
|
|
P2.at<double>(0,2) = K2.at<double>(0,2);
|
|
P2.at<double>(1,2) = K2.at<double>(1,2);
|
|
|
|
CameraModel leftModel(this->getSerial(), size, K1, D1, cv::Mat::eye(3, 3, CV_64F), P1, this->getLocalTransform());
|
|
CameraModel rightModel(this->getSerial(), size, K2, D2, cv::Mat::eye(3, 3, CV_64F), P2, this->getLocalTransform());
|
|
UINFO("raw: fx=%f fy=%f cx=%f cy=%f",
|
|
leftModel.fx(),
|
|
leftModel.fy(),
|
|
leftModel.cx(),
|
|
leftModel.cy());
|
|
|
|
UINFO("stereo extrinsics = %s", extrinsics.prettyPrint().c_str());
|
|
stereoModel_ = StereoCameraModel(this->getSerial(), leftModel, rightModel, extrinsics);
|
|
|
|
if(!stereoModel_.isValidForRectification())
|
|
{
|
|
UERROR("Could not initialize stereo rectification.");
|
|
return false;
|
|
}
|
|
|
|
stereoModel_.initRectificationMap();
|
|
UINFO("baseline = %f rectified: fx=%f fy=%f cx=%f cy=%f",
|
|
stereoModel_.baseline(),
|
|
stereoModel_.left().fx(),
|
|
stereoModel_.left().fy(),
|
|
stereoModel_.left().cx(),
|
|
stereoModel_.left().cy());
|
|
|
|
// get left to imu camera transform
|
|
auto &&exImu = api_->GetMotionExtrinsics(mynteye::Stream::LEFT);
|
|
is_data_use_mm_instead_of_m =
|
|
abs(exImu.translation[0]) > 1.0 ||
|
|
abs(exImu.translation[1]) > 1.0 ||
|
|
abs(exImu.translation[2]) > 1.0;
|
|
|
|
if (is_data_use_mm_instead_of_m) {
|
|
exImu.translation[0] *= 0.001;
|
|
exImu.translation[1] *= 0.001;
|
|
exImu.translation[2] *= 0.001;
|
|
}
|
|
|
|
if (exImu.rotation[0][0] == 0 && exImu.rotation[2][2] == 0)
|
|
{
|
|
imuLocalTransform_ = Transform(
|
|
0, 0, 1, exImu.translation[0],
|
|
0,-1, 0, exImu.translation[1],
|
|
1, 0, 0, exImu.translation[2]);
|
|
}
|
|
else
|
|
{
|
|
imuLocalTransform_ = Transform(
|
|
exImu.rotation[0][0], exImu.rotation[0][1], exImu.rotation[0][2], exImu.translation[0],
|
|
exImu.rotation[1][0], exImu.rotation[1][1], exImu.rotation[1][2], exImu.translation[1],
|
|
exImu.rotation[2][0], exImu.rotation[2][1], exImu.rotation[2][2], exImu.translation[2]);
|
|
}
|
|
UINFO("imu extrinsics = %s", imuLocalTransform_.prettyPrint().c_str());
|
|
|
|
for(int i=0;i<(int)mynteye::Stream::LAST; ++i)
|
|
{
|
|
UINFO("Support stream %d = %s", i, api_->Supports((mynteye::Stream)i)?"true":"false");
|
|
}
|
|
|
|
if (api_->Supports(apiRectification_?mynteye::Stream::LEFT_RECTIFIED:mynteye::Stream::LEFT) &&
|
|
api_->Supports(apiRectification_?mynteye::Stream::RIGHT_RECTIFIED:mynteye::Stream::RIGHT))
|
|
{
|
|
api_->EnableStreamData(apiRectification_?mynteye::Stream::LEFT_RECTIFIED:mynteye::Stream::LEFT);
|
|
api_->SetStreamCallback(apiRectification_?mynteye::Stream::LEFT_RECTIFIED:mynteye::Stream::LEFT,
|
|
[&](const mynteye::api::StreamData &data)
|
|
{
|
|
double stamp = checkUpTimeStamp(data.img->timestamp, (std::uint8_t)(apiRectification_?mynteye::Stream::LEFT_RECTIFIED:mynteye::Stream::LEFT));
|
|
|
|
UScopeMutex s(dataMutex_);
|
|
bool notify = false;
|
|
leftFrameBuffer_ = data.frame;
|
|
if(stamp_>0 && stamp_ == data.img->timestamp && !rightFrameBuffer_.empty())
|
|
{
|
|
notify = lastFrames_.first.empty();
|
|
lastFrames_.first = leftFrameBuffer_;
|
|
lastFrames_.second = rightFrameBuffer_;
|
|
lastFramesStamp_ = stamp;
|
|
leftFrameBuffer_ = cv::Mat();
|
|
rightFrameBuffer_ = cv::Mat();
|
|
stamp_ = 0;
|
|
}
|
|
else
|
|
{
|
|
stamp_ = data.img->timestamp;
|
|
}
|
|
if(notify)
|
|
{
|
|
dataReady_.release();
|
|
}
|
|
});
|
|
|
|
if(apiRectification_ && apiDepth_ && api_->Supports(mynteye::Stream::DEPTH))
|
|
{
|
|
api_->EnableStreamData(mynteye::Stream::DEPTH);
|
|
}
|
|
else
|
|
{
|
|
api_->EnableStreamData(apiRectification_?mynteye::Stream::RIGHT_RECTIFIED:mynteye::Stream::RIGHT);
|
|
apiDepth_ = false;
|
|
}
|
|
api_->SetStreamCallback(
|
|
apiDepth_?mynteye::Stream::DEPTH:apiRectification_?mynteye::Stream::RIGHT_RECTIFIED:mynteye::Stream::RIGHT,
|
|
[&](const mynteye::api::StreamData &data)
|
|
{
|
|
double stamp = checkUpTimeStamp(data.img->timestamp, (std::uint8_t)(apiDepth_?mynteye::Stream::DEPTH:apiRectification_?mynteye::Stream::RIGHT_RECTIFIED:mynteye::Stream::RIGHT));
|
|
|
|
UScopeMutex s(dataMutex_);
|
|
bool notify = false;
|
|
rightFrameBuffer_ = data.frame;
|
|
if(stamp_>0 && stamp_ == data.img->timestamp && !leftFrameBuffer_.empty())
|
|
{
|
|
notify = lastFrames_.first.empty();
|
|
lastFrames_.first = leftFrameBuffer_;
|
|
lastFrames_.second = rightFrameBuffer_;
|
|
lastFramesStamp_ = stamp;
|
|
leftFrameBuffer_ = cv::Mat();
|
|
rightFrameBuffer_ = cv::Mat();
|
|
stamp_ = 0;
|
|
}
|
|
else
|
|
{
|
|
stamp_ = data.img->timestamp;
|
|
}
|
|
if(notify)
|
|
{
|
|
dataReady_.release();
|
|
}
|
|
});
|
|
|
|
api_->SetMotionCallback([this](const mynteye::api::MotionData &data) {
|
|
|
|
double stamp = checkUpTimeStamp(data.imu->timestamp, (std::uint8_t)mynteye::Stream::LAST);
|
|
if(data.imu->flag == 0)
|
|
{
|
|
//UWARN("%f %f %f %f %f %f",
|
|
// data.imu->gyro[0] * M_PI / 180, data.imu->gyro[1] * M_PI / 180, data.imu->gyro[2] * M_PI / 180,
|
|
// data.imu->accel[0] * 9.8, data.imu->accel[1] * 9.8, data.imu->accel[2] * 9.8);
|
|
cv::Vec3d gyro(data.imu->gyro[0] * M_PI / 180, data.imu->gyro[1] * M_PI / 180, data.imu->gyro[2] * M_PI / 180);
|
|
cv::Vec3d acc(data.imu->accel[0] * 9.8, data.imu->accel[1] * 9.8, data.imu->accel[2] * 9.8);
|
|
if(publishInterIMU_)
|
|
{
|
|
IMU imu(gyro, cv::Mat::eye(3,3,CV_64FC1),
|
|
acc, cv::Mat::eye(3,3,CV_64FC1),
|
|
imuLocalTransform_);
|
|
UEventsManager::post(new IMUEvent(imu, stamp));
|
|
}
|
|
else
|
|
{
|
|
UScopeMutex lock(imuMutex_);
|
|
imuBuffer_.insert(imuBuffer_.end(), std::make_pair(stamp, std::make_pair(gyro, acc)));
|
|
if(imuBuffer_.size() > 1000)
|
|
{
|
|
imuBuffer_.erase(imuBuffer_.begin());
|
|
}
|
|
}
|
|
}
|
|
|
|
uSleep(1);
|
|
|
|
});
|
|
|
|
api_->SetOptionValue(mynteye::Option::EXPOSURE_MODE, autoExposure_?0:1);
|
|
if(!autoExposure_)
|
|
{
|
|
api_->SetOptionValue(mynteye::Option::GAIN, gain_);
|
|
api_->SetOptionValue(mynteye::Option::BRIGHTNESS, brightness_);
|
|
api_->SetOptionValue(mynteye::Option::CONTRAST, contrast_);
|
|
}
|
|
api_->SetOptionValue(mynteye::Option::IR_CONTROL, irControl_);
|
|
|
|
api_->Start(mynteye::Source::ALL);
|
|
uSleep(500); // To buffer some imus before sending images
|
|
return true;
|
|
}
|
|
UERROR("Streams missing.");
|
|
return false;
|
|
#else
|
|
UERROR("Not built with Mynt Eye support!");
|
|
return false;
|
|
#endif
|
|
}
|
|
|
|
bool CameraMyntEye::isCalibrated() const
|
|
{
|
|
return true;
|
|
}
|
|
|
|
std::string CameraMyntEye::getSerial() const
|
|
{
|
|
#ifdef RTABMAP_MYNTEYE
|
|
if(device_.get())
|
|
{
|
|
return device_->GetInfo(mynteye::Info::SERIAL_NUMBER);
|
|
}
|
|
#endif
|
|
return "";
|
|
}
|
|
|
|
bool CameraMyntEye::available()
|
|
{
|
|
#ifdef RTABMAP_MYNTEYE
|
|
return true;
|
|
#else
|
|
return false;
|
|
#endif
|
|
}
|
|
|
|
#ifdef RTABMAP_MYNTEYE
|
|
void CameraMyntEye::getPoseAndIMU(
|
|
const double & stamp,
|
|
IMU & imu,
|
|
int maxWaitTimeMs) const
|
|
{
|
|
imu = IMU();
|
|
if(imuBuffer_.empty())
|
|
{
|
|
return;
|
|
}
|
|
|
|
// Interpolate gyro,acc
|
|
cv::Vec3d gyro;
|
|
cv::Vec3d acc;
|
|
{
|
|
imuMutex_.lock();
|
|
int waitTry = 0;
|
|
while(maxWaitTimeMs > 0 && imuBuffer_.rbegin()->first < stamp && waitTry < maxWaitTimeMs)
|
|
{
|
|
imuMutex_.unlock();
|
|
++waitTry;
|
|
uSleep(1);
|
|
imuMutex_.lock();
|
|
}
|
|
if(imuBuffer_.rbegin()->first < stamp)
|
|
{
|
|
if(maxWaitTimeMs>0)
|
|
{
|
|
UWARN("Could not find gyro/acc data to interpolate at time %f after waiting %d ms (last is %f)...", stamp, maxWaitTimeMs, imuBuffer_.rbegin()->first);
|
|
}
|
|
imuMutex_.unlock();
|
|
return;
|
|
}
|
|
else
|
|
{
|
|
std::map<double, std::pair<cv::Vec3f, cv::Vec3f> >::const_iterator iterB = imuBuffer_.lower_bound(stamp);
|
|
std::map<double, std::pair<cv::Vec3f, cv::Vec3f> >::const_iterator iterA = iterB;
|
|
if(iterA != imuBuffer_.begin())
|
|
{
|
|
iterA = --iterA;
|
|
}
|
|
if(iterB == imuBuffer_.end())
|
|
{
|
|
iterB = --iterB;
|
|
}
|
|
if(iterA == iterB && stamp == iterA->first)
|
|
{
|
|
gyro[0] = iterA->second.first[0];
|
|
gyro[1] = iterA->second.first[1];
|
|
gyro[2] = iterA->second.first[2];
|
|
acc[0] = iterA->second.second[0];
|
|
acc[1] = iterA->second.second[1];
|
|
acc[2] = iterA->second.second[2];
|
|
}
|
|
else if(stamp >= iterA->first && stamp <= iterB->first)
|
|
{
|
|
float t = (stamp-iterA->first) / (iterB->first-iterA->first);
|
|
gyro[0] = iterA->second.first[0] + t*(iterB->second.first[0] - iterA->second.first[0]);
|
|
gyro[1] = iterA->second.first[1] + t*(iterB->second.first[1] - iterA->second.first[1]);
|
|
gyro[2] = iterA->second.first[2] + t*(iterB->second.first[2] - iterA->second.first[2]);
|
|
acc[0] = iterA->second.second[0] + t*(iterB->second.second[0] - iterA->second.second[0]);
|
|
acc[1] = iterA->second.second[1] + t*(iterB->second.second[1] - iterA->second.second[1]);
|
|
acc[2] = iterA->second.second[2] + t*(iterB->second.second[2] - iterA->second.second[2]);
|
|
}
|
|
else
|
|
{
|
|
if(stamp < iterA->first)
|
|
{
|
|
UWARN("Could not find acc data to interpolate at image time %f (earliest is %f). Are sensors synchronized?", stamp, iterA->first);
|
|
}
|
|
else
|
|
{
|
|
UWARN("Could not find acc data to interpolate at image time %f (between %f and %f). Are sensors synchronized?", stamp, iterA->first, iterB->first);
|
|
}
|
|
imuMutex_.unlock();
|
|
return;
|
|
}
|
|
}
|
|
imuMutex_.unlock();
|
|
}
|
|
|
|
imu = IMU(gyro, cv::Mat::eye(3, 3, CV_64FC1), acc, cv::Mat::eye(3, 3, CV_64FC1), imuLocalTransform_);
|
|
}
|
|
#endif
|
|
|
|
SensorData CameraMyntEye::captureImage(SensorCaptureInfo * info)
|
|
{
|
|
SensorData data;
|
|
#ifdef RTABMAP_MYNTEYE
|
|
if(!dataReady_.acquire(1, 3000))
|
|
{
|
|
UERROR("Did not receive frame since 3 seconds...");
|
|
return data;
|
|
}
|
|
|
|
cv::Mat left;
|
|
cv::Mat right;
|
|
double stamp = 0.0;
|
|
|
|
dataMutex_.lock();
|
|
if(!lastFrames_.first.empty())
|
|
{
|
|
left = lastFrames_.first;
|
|
right = lastFrames_.second;
|
|
stamp = lastFramesStamp_;
|
|
lastFrames_ = std::pair<cv::Mat, cv::Mat>();
|
|
}
|
|
dataMutex_.unlock();
|
|
|
|
if(!left.empty() && !right.empty())
|
|
{
|
|
if(right.type() == CV_16UC1)
|
|
{
|
|
data = SensorData(left, right, stereoModel_.left(), 0, stamp);
|
|
}
|
|
else
|
|
{
|
|
if(!apiRectification_)
|
|
{
|
|
left = stereoModel_.left().rectifyImage(left);
|
|
right = stereoModel_.right().rectifyImage(right);
|
|
}
|
|
data = SensorData(left, right, stereoModel_, 0, stamp);
|
|
}
|
|
if(!publishInterIMU_ && imuBuffer_.size())
|
|
{
|
|
IMU imu;
|
|
getPoseAndIMU(stamp, imu, 60);
|
|
if(!imu.empty())
|
|
{
|
|
data.setIMU(imu);
|
|
}
|
|
}
|
|
}
|
|
#else
|
|
UERROR("Not built with Mynt Eye support!");
|
|
#endif
|
|
return data;
|
|
}
|
|
|
|
} // namespace rtabmap
|