00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #include "rtabmap/core/OdometryOkvis.h"
00029 #include "rtabmap/core/OdometryInfo.h"
00030 #include "rtabmap/core/util3d_transforms.h"
00031 #include "rtabmap/utilite/ULogger.h"
00032 #include "rtabmap/utilite/UTimer.h"
00033 #include "rtabmap/utilite/UStl.h"
00034 #include "rtabmap/utilite/UThread.h"
00035
00036 #ifdef RTABMAP_OKVIS
00037 #include <iostream>
00038 #include <fstream>
00039 #include <stdlib.h>
00040 #include <memory>
00041 #include <functional>
00042 #include <atomic>
00043
00044 #include <Eigen/Core>
00045 #pragma GCC diagnostic push
00046 #pragma GCC diagnostic ignored "-Wnon-virtual-dtor"
00047 #pragma GCC diagnostic ignored "-Woverloaded-virtual"
00048 #include <opencv2/opencv.hpp>
00049 #pragma GCC diagnostic pop
00050 #include <okvis/VioParametersReader.hpp>
00051 #include <okvis/ThreadedKFVio.hpp>
00052 #include <okvis/cameras/PinholeCamera.hpp>
00053 #include <okvis/cameras/NoDistortion.hpp>
00054 #include <okvis/cameras/RadialTangentialDistortion.hpp>
00055 #include <okvis/cameras/EquidistantDistortion.hpp>
00056 #include <okvis/cameras/RadialTangentialDistortion8.hpp>
00057 #include <boost/filesystem.hpp>
00058 #endif
00059
00060 namespace rtabmap {
00061
00062 #ifdef RTABMAP_OKVIS
00063 class OkvisCallbackHandler
00064 {
00065 public:
00066 OkvisCallbackHandler()
00067 {
00068 }
00069
00070 Transform getLastTransform()
00071 {
00072 UDEBUG("");
00073 Transform tf;
00074 mutex_.lock();
00075 tf = transform_;
00076 mutex_.unlock();
00077
00078 return tf;
00079 }
00080
00081 std::map<int, cv::Point3f> getLastLandmarks()
00082 {
00083 UDEBUG("");
00084 std::map<int, cv::Point3f> landmarks;
00085 mutexLandmarks_.lock();
00086 landmarks = landmarks_;
00087 mutexLandmarks_.unlock();
00088 return landmarks;
00089 }
00090
00091 public:
00092 void fullStateCallback(
00093 const okvis::Time & t, const okvis::kinematics::Transformation & T_WS,
00094 const Eigen::Matrix<double, 9, 1> & ,
00095 const Eigen::Matrix<double, 3, 1> & )
00096 {
00097 UDEBUG("");
00098 Transform tf = Transform::fromEigen4d(T_WS.T());
00099
00100 mutex_.lock();
00101 transform_ = tf;
00102 mutex_.unlock();
00103 }
00104
00105 void landmarksCallback(const okvis::Time & t,
00106 const okvis::MapPointVector & landmarksVector,
00107 const okvis::MapPointVector & )
00108 {
00109 UDEBUG("");
00110 mutexLandmarks_.lock();
00111 landmarks_.clear();
00112 for(unsigned int i=0; i<landmarksVector.size(); ++i)
00113 {
00114 landmarks_.insert(std::make_pair((int)landmarksVector[i].id, cv::Point3f(landmarksVector[i].point[0], landmarksVector[i].point[1], landmarksVector[i].point[2])));
00115 }
00116 mutexLandmarks_.unlock();
00117 }
00118
00119
00120
00121 private:
00122 Transform transform_;
00123 std::map<int, cv::Point3f> landmarks_;
00124 UMutex mutex_;
00125 UMutex mutexLandmarks_;
00126 };
00127 #endif
00128
00129 OdometryOkvis::OdometryOkvis(const ParametersMap & parameters) :
00130 Odometry(parameters),
00131 #ifdef RTABMAP_OKVIS
00132 okvisCallbackHandler_(new OkvisCallbackHandler),
00133 okvisEstimator_(0),
00134 imagesProcessed_(0),
00135 initGravity_(false),
00136 #endif
00137 okvisParameters_(parameters),
00138 previousPose_(Transform::getIdentity())
00139 {
00140 #ifdef RTABMAP_OKVIS
00141 Parameters::parse(parameters, Parameters::kOdomOKVISConfigPath(), configFilename_);
00142 if(configFilename_.empty())
00143 {
00144 UERROR("OKVIS config file is empty (%s)!", Parameters::kOdomOKVISConfigPath().c_str());
00145 }
00146 #endif
00147 }
00148
00149 OdometryOkvis::~OdometryOkvis()
00150 {
00151 UDEBUG("");
00152 #ifdef RTABMAP_OKVIS
00153 delete okvisEstimator_;
00154 delete okvisCallbackHandler_;
00155 #endif
00156 }
00157
00158 void OdometryOkvis::reset(const Transform & initialPose)
00159 {
00160 Odometry::reset(initialPose);
00161 #ifdef RTABMAP_OKVIS
00162 if(!initGravity_)
00163 {
00164 if(okvisEstimator_)
00165 {
00166 delete okvisEstimator_;
00167 okvisEstimator_ = 0;
00168 }
00169 lastImu_ = IMU();
00170 imagesProcessed_ = 0;
00171 previousPose_.setIdentity();
00172
00173 delete okvisCallbackHandler_;
00174 okvisCallbackHandler_ = new OkvisCallbackHandler();
00175 }
00176 initGravity_ = false;
00177 #endif
00178 }
00179
00180
00181 Transform OdometryOkvis::computeTransform(
00182 SensorData & data,
00183 const Transform & guess,
00184 OdometryInfo * info)
00185 {
00186 UDEBUG("");
00187 Transform t;
00188 #ifdef RTABMAP_OKVIS
00189 UTimer timer;
00190
00191 okvis::Time timeOkvis = okvis::Time(data.stamp());
00192
00193 bool imuUpdated = false;
00194 if(!data.imu().empty())
00195 {
00196 UDEBUG("IMU update stamp=%f acc=%f %f %f gyr=%f %f %f", data.stamp(),
00197 data.imu().linearAcceleration()[0],
00198 data.imu().linearAcceleration()[1],
00199 data.imu().linearAcceleration()[2],
00200 data.imu().angularVelocity()[0],
00201 data.imu().angularVelocity()[1],
00202 data.imu().angularVelocity()[2]);
00203 if(okvisEstimator_ != 0)
00204 {
00205 Eigen::Vector3d acc(data.imu().linearAcceleration()[0], data.imu().linearAcceleration()[1], data.imu().linearAcceleration()[2]);
00206 Eigen::Vector3d ang(data.imu().angularVelocity()[0], data.imu().angularVelocity()[1], data.imu().angularVelocity()[2]);
00207 imuUpdated = okvisEstimator_->addImuMeasurement(timeOkvis, acc, ang);
00208 }
00209 else
00210 {
00211 UWARN("Ignoring IMU, waiting for an image to initialize...");
00212 lastImu_ = data.imu();
00213 }
00214 }
00215
00216 bool imageUpdated = false;
00217 if(!data.imageRaw().empty())
00218 {
00219 UDEBUG("Image update stamp=%f", data.stamp());
00220 std::vector<cv::Mat> images;
00221 std::vector<CameraModel> models;
00222 if(data.stereoCameraModel().isValidForProjection())
00223 {
00224 images.push_back(data.imageRaw());
00225 images.push_back(data.rightRaw());
00226 CameraModel mleft = data.stereoCameraModel().left();
00227
00228 mleft.setLocalTransform(lastImu_.localTransform().inverse()*mleft.localTransform());
00229 models.push_back(mleft);
00230 CameraModel mright = data.stereoCameraModel().right();
00231
00232
00233 if(!imagesAlreadyRectified())
00234 {
00235 cv::Mat R = data.stereoCameraModel().R();
00236 cv::Mat T = data.stereoCameraModel().T();
00237 UASSERT(R.cols==3 && R.rows == 3);
00238 UASSERT(T.cols==1 && T.rows == 3);
00239 Transform extrinsics(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), T.at<double>(0,0),
00240 R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), T.at<double>(1,0),
00241 R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), T.at<double>(2,0));
00242 mright.setLocalTransform(mleft.localTransform() * extrinsics.inverse());
00243 }
00244 else
00245 {
00246 Transform extrinsics(1, 0, 0, 0,
00247 0, 1, 0, data.stereoCameraModel().baseline(),
00248 0, 0, 1, 0);
00249 mright.setLocalTransform(extrinsics * mleft.localTransform());
00250 }
00251
00252
00253 models.push_back(mright);
00254 }
00255 else
00256 {
00257 UASSERT(int((data.imageRaw().cols/data.cameraModels().size())*data.cameraModels().size()) == data.imageRaw().cols);
00258 int subImageWidth = data.imageRaw().cols/data.cameraModels().size();
00259 for(unsigned int i=0; i<data.cameraModels().size(); ++i)
00260 {
00261 if(data.cameraModels()[i].isValidForProjection())
00262 {
00263 images.push_back(cv::Mat(data.imageRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, data.imageRaw().rows)));
00264 CameraModel m = data.cameraModels()[i];
00265
00266 m.setLocalTransform(lastImu_.localTransform().inverse()*m.localTransform());
00267 models.push_back(m);
00268 }
00269 }
00270 }
00271
00272 if(images.size())
00273 {
00274
00275 if(okvisEstimator_ == 0)
00276 {
00277 UDEBUG("Initialization");
00278 if(lastImu_.empty())
00279 {
00280 UWARN("Ignoring Image, waiting for imu to initialize...");
00281 return t;
00282 }
00283
00284 okvis::VioParameters parameters;
00285 if(configFilename_.empty())
00286 {
00287 UERROR("OKVIS config file is empty (%s)!", Parameters::kOdomOKVISConfigPath().c_str());
00288 return t;
00289 }
00290 else
00291 {
00292 okvis::VioParametersReader vio_parameters_reader(configFilename_);
00293 vio_parameters_reader.getParameters(parameters);
00294 if(parameters.nCameraSystem.numCameras() > 0)
00295 {
00296 UWARN("Camera calibration included in OKVIS is ignored as calibration from received images will be used instead.");
00297 }
00298 parameters.nCameraSystem = okvis::cameras::NCameraSystem();
00299 }
00300
00301 parameters.publishing.publishRate = parameters.imu.rate;
00302 parameters.publishing.publishLandmarks = true;
00303 parameters.publishing.publishImuPropagatedState = true;
00304 parameters.publishing.landmarkQualityThreshold = 1.0e-2;
00305 parameters.publishing.maxLandmarkQuality = 0.05;
00306 parameters.publishing.trackedBodyFrame = okvis::FrameName::B;
00307 parameters.publishing.velocitiesFrame = okvis::FrameName::B;
00308
00309
00310 parameters.imu.T_BS = okvis::kinematics::Transformation(lastImu_.localTransform().toEigen4d());
00311 UINFO("Images are already rectified = %s", imagesAlreadyRectified()?"true":"false");
00312 for(unsigned int i=0; i<models.size(); ++i)
00313 {
00314 okvis::cameras::NCameraSystem::DistortionType distType = okvis::cameras::NCameraSystem::NoDistortion;
00315 std::shared_ptr<const okvis::cameras::CameraBase> cam;
00316 if(!imagesAlreadyRectified())
00317 {
00318 if(models[i].D_raw().cols == 8)
00319 {
00320 okvis::cameras::RadialTangentialDistortion8 dist(
00321 models[i].D_raw().at<double>(0,0),
00322 models[i].D_raw().at<double>(0,1),
00323 models[i].D_raw().at<double>(0,2),
00324 models[i].D_raw().at<double>(0,3),
00325 models[i].D_raw().at<double>(0,4),
00326 models[i].D_raw().at<double>(0,5),
00327 models[i].D_raw().at<double>(0,6),
00328 models[i].D_raw().at<double>(0,7));
00329 cam.reset(
00330 new okvis::cameras::PinholeCamera<okvis::cameras::RadialTangentialDistortion8>(
00331 models[i].imageWidth(),
00332 models[i].imageHeight(),
00333 models[i].K_raw().at<double>(0,0),
00334 models[i].K_raw().at<double>(1,1),
00335 models[i].K_raw().at<double>(0,2),
00336 models[i].K_raw().at<double>(1,2),
00337 dist));
00338 distType = okvis::cameras::NCameraSystem::RadialTangential8;
00339 UINFO("RadialTangential8");
00340 }
00341 else if(models[i].D_raw().cols == 6)
00342 {
00343 okvis::cameras::EquidistantDistortion dist(
00344 models[i].D_raw().at<double>(0,0),
00345 models[i].D_raw().at<double>(0,1),
00346 models[i].D_raw().at<double>(0,4),
00347 models[i].D_raw().at<double>(0,5));
00348 cam.reset(new okvis::cameras::PinholeCamera<okvis::cameras::EquidistantDistortion>(
00349 models[i].imageWidth(),
00350 models[i].imageHeight(),
00351 models[i].K_raw().at<double>(0,0),
00352 models[i].K_raw().at<double>(1,1),
00353 models[i].K_raw().at<double>(0,2),
00354 models[i].K_raw().at<double>(1,2),
00355 dist));
00356 distType = okvis::cameras::NCameraSystem::Equidistant;
00357 UINFO("Equidistant");
00358 }
00359 else if(models[i].D_raw().cols >= 4)
00360 {
00361
00362 okvis::cameras::RadialTangentialDistortion dist(
00363 models[i].D_raw().at<double>(0,0),
00364 models[i].D_raw().at<double>(0,1),
00365 models[i].D_raw().at<double>(0,2),
00366 models[i].D_raw().at<double>(0,3));
00367 cam.reset(
00368 new okvis::cameras::PinholeCamera<okvis::cameras::RadialTangentialDistortion>(
00369 models[i].imageWidth(),
00370 models[i].imageHeight(),
00371 models[i].K_raw().at<double>(0,0),
00372 models[i].K_raw().at<double>(1,1),
00373 models[i].K_raw().at<double>(0,2),
00374 models[i].K_raw().at<double>(1,2),
00375 dist));
00376 distType = okvis::cameras::NCameraSystem::RadialTangential;
00377 UINFO("RadialTangential");
00378 }
00379 }
00380 else
00381 {
00382 okvis::cameras::RadialTangentialDistortion dist(0,0,0,0);
00383 cam.reset(
00384 new okvis::cameras::PinholeCamera<okvis::cameras::RadialTangentialDistortion>(
00385 models[i].imageWidth(),
00386 models[i].imageHeight(),
00387 models[i].K().at<double>(0,0),
00388 models[i].K().at<double>(1,1),
00389 models[i].K().at<double>(0,2),
00390 models[i].K().at<double>(1,2),
00391 dist));
00392 distType = okvis::cameras::NCameraSystem::RadialTangential;
00393 }
00394
00395 if(cam.get())
00396 {
00397 UINFO("model %d: %s", i, models[i].localTransform().prettyPrint().c_str());
00398
00399 Eigen::Vector3d r(models[i].localTransform().x(), models[i].localTransform().y(), models[i].localTransform().z());
00400 parameters.nCameraSystem.addCamera(
00401 std::shared_ptr<const okvis::kinematics::Transformation>(new okvis::kinematics::Transformation(r, models[i].localTransform().getQuaterniond().normalized())),
00402 cam,
00403 distType);
00404 }
00405 }
00406
00407 okvisEstimator_ = new okvis::ThreadedKFVio(parameters);
00408
00409 okvisEstimator_->setFullStateCallback(
00410 std::bind(&OkvisCallbackHandler::fullStateCallback, okvisCallbackHandler_,
00411 std::placeholders::_1, std::placeholders::_2,
00412 std::placeholders::_3, std::placeholders::_4));
00413
00414 okvisEstimator_->setLandmarksCallback(
00415 std::bind(&OkvisCallbackHandler::landmarksCallback, okvisCallbackHandler_,
00416 std::placeholders::_1, std::placeholders::_2,
00417 std::placeholders::_3));
00418
00419 okvisEstimator_->setBlocking(true);
00420 }
00421
00422 for(unsigned int i=0; i<images.size(); ++i)
00423 {
00424 cv::Mat gray;
00425 if(images[i].type() == CV_8UC3)
00426 {
00427 cv::cvtColor(images[i], gray, CV_BGR2GRAY);
00428 }
00429 else if(images[i].type() == CV_8UC1)
00430 {
00431 gray = images[i];
00432 }
00433 else
00434 {
00435 UFATAL("Not supported color type!");
00436 }
00437
00438 imageUpdated = okvisEstimator_->addImage(timeOkvis, i, gray);
00439 if(!imageUpdated)
00440 {
00441 UWARN("Image update with stamp %f delayed...", data.stamp());
00442 }
00443 }
00444 if(imageUpdated)
00445 {
00446 ++imagesProcessed_;
00447 }
00448 }
00449 }
00450
00451 if((imageUpdated || imuUpdated) && imagesProcessed_ > 10)
00452 {
00453 Transform fixPos(-1,0,0,0, 0,-1,0,0, 0,0,1,0);
00454 Transform fixRot(0,0,1,0, 0,-1,0,0, 1,0,0,0);
00455 Transform p = okvisCallbackHandler_->getLastTransform();
00456 if(!p.isNull())
00457 {
00458 p = fixPos * p * fixRot;
00459
00460 if(this->getPose().rotation().isIdentity())
00461 {
00462 initGravity_ = true;
00463 this->reset(this->getPose()*p.rotation());
00464 }
00465
00466 if(previousPose_.isIdentity())
00467 {
00468 previousPose_ = p;
00469 }
00470
00471
00472 t = previousPose_.inverse()*p;
00473 previousPose_ = p;
00474
00475 if(info)
00476 {
00477 info->reg.covariance = cv::Mat::eye(6,6, CV_64FC1);
00478 info->reg.covariance *= this->framesProcessed() == 0?9999:0.0001;
00479
00480
00481
00482
00483
00484
00485
00486
00487 }
00488 }
00489 if(imageUpdated)
00490 {
00491 UINFO("Odom update time = %fs p=%s", timer.elapsed(), p.prettyPrint().c_str());
00492 }
00493 }
00494 #else
00495 UERROR("RTAB-Map is not built with OKVIS support! Select another visual odometry approach.");
00496 #endif
00497 return t;
00498 }
00499
00500 }