37 #include <opencv2/imgproc/types_c.h> 48 #pragma GCC diagnostic push 49 #pragma GCC diagnostic ignored "-Wnon-virtual-dtor" 50 #pragma GCC diagnostic ignored "-Woverloaded-virtual" 51 #include <opencv2/opencv.hpp> 52 #pragma GCC diagnostic pop 53 #include <okvis/VioParametersReader.hpp> 54 #include <okvis/ThreadedKFVio.hpp> 55 #include <okvis/cameras/PinholeCamera.hpp> 56 #include <okvis/cameras/NoDistortion.hpp> 57 #include <okvis/cameras/RadialTangentialDistortion.hpp> 58 #include <okvis/cameras/EquidistantDistortion.hpp> 59 #include <okvis/cameras/RadialTangentialDistortion8.hpp> 60 #include <boost/filesystem.hpp> 66 class OkvisCallbackHandler
69 OkvisCallbackHandler()
73 Transform getLastTransform()
84 std::map<int, cv::Point3f> getLastLandmarks()
87 std::map<int, cv::Point3f> landmarks;
88 mutexLandmarks_.lock();
89 landmarks = landmarks_;
90 mutexLandmarks_.unlock();
95 void fullStateCallback(
96 const okvis::Time & t,
const okvis::kinematics::Transformation & T_WS,
97 const Eigen::Matrix<double, 9, 1> & ,
98 const Eigen::Matrix<double, 3, 1> & )
108 void landmarksCallback(
const okvis::Time & t,
109 const okvis::MapPointVector & landmarksVector,
110 const okvis::MapPointVector & )
113 mutexLandmarks_.lock();
115 for(
unsigned int i=0; i<landmarksVector.size(); ++i)
117 landmarks_.insert(std::make_pair((
int)landmarksVector[i].
id, cv::Point3f(landmarksVector[i].point[0], landmarksVector[i].point[1], landmarksVector[i].point[2])));
119 mutexLandmarks_.unlock();
125 Transform transform_;
126 std::map<int, cv::Point3f> landmarks_;
135 okvisCallbackHandler_(new OkvisCallbackHandler),
140 okvisParameters_(parameters),
148 UERROR(
"OKVIS config file is empty or doesn't exist (%s)!", Parameters::kOdomOKVISConfigPath().c_str());
157 delete okvisEstimator_;
158 delete okvisCallbackHandler_;
170 delete okvisEstimator_;
174 imagesProcessed_ = 0;
177 delete okvisCallbackHandler_;
178 okvisCallbackHandler_ =
new OkvisCallbackHandler();
180 initGravity_ =
false;
195 okvis::Time timeOkvis = okvis::Time(data.
stamp());
199 UDEBUG(
"IMU update stamp=%f acc=%f %f %f gyr=%f %f %f", data.
stamp(),
206 if(okvisEstimator_ != 0)
210 okvisEstimator_->addImuMeasurement(timeOkvis, acc, ang);
214 UWARN(
"Ignoring IMU, waiting for an image to initialize...");
222 std::vector<cv::Mat> images;
223 std::vector<CameraModel> models;
231 models.push_back(mleft);
239 UASSERT(R.cols==3 && R.rows == 3);
240 UASSERT(T.cols==1 && T.rows == 3);
241 Transform extrinsics(R.at<
double>(0,0), R.at<
double>(0,1), R.at<
double>(0,2), T.at<
double>(0,0),
242 R.at<
double>(1,0), R.at<
double>(1,1), R.at<
double>(1,2), T.at<
double>(1,0),
243 R.at<
double>(2,0), R.at<
double>(2,1), R.at<
double>(2,2), T.at<
double>(2,0));
255 models.push_back(mright);
261 for(
unsigned int i=0; i<data.
cameraModels().size(); ++i)
265 images.push_back(cv::Mat(data.
imageRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, data.
imageRaw().rows)));
274 bool imageUpdated =
false;
278 if(okvisEstimator_ == 0)
283 UWARN(
"Ignoring Image, waiting for imu to initialize...");
287 okvis::VioParameters parameters;
290 UERROR(
"OKVIS config file is empty or doesn't exist (%s)!", Parameters::kOdomOKVISConfigPath().c_str());
296 vio_parameters_reader.getParameters(parameters);
297 if(parameters.nCameraSystem.numCameras() > 0)
299 UWARN(
"Camera calibration included in OKVIS is ignored as calibration from received images will be used instead.");
301 parameters.nCameraSystem = okvis::cameras::NCameraSystem();
304 parameters.publishing.publishRate = parameters.imu.rate;
305 parameters.publishing.publishLandmarks =
true;
306 parameters.publishing.publishImuPropagatedState =
true;
307 parameters.publishing.landmarkQualityThreshold = 1.0e-2;
308 parameters.publishing.maxLandmarkQuality = 0.05;
309 parameters.publishing.trackedBodyFrame = okvis::FrameName::B;
310 parameters.publishing.velocitiesFrame = okvis::FrameName::B;
315 for(
unsigned int i=0; i<models.size(); ++i)
317 okvis::cameras::NCameraSystem::DistortionType distType = okvis::cameras::NCameraSystem::NoDistortion;
318 std::shared_ptr<const okvis::cameras::CameraBase>
cam;
321 if(models[i].D_raw().cols == 8)
323 okvis::cameras::RadialTangentialDistortion8 dist(
324 models[i].D_raw().at<double>(0,0),
325 models[i].D_raw().at<double>(0,1),
326 models[i].D_raw().at<double>(0,2),
327 models[i].D_raw().at<double>(0,3),
328 models[i].D_raw().at<double>(0,4),
329 models[i].D_raw().at<double>(0,5),
330 models[i].D_raw().at<double>(0,6),
331 models[i].D_raw().at<double>(0,7));
333 new okvis::cameras::PinholeCamera<okvis::cameras::RadialTangentialDistortion8>(
334 models[i].imageWidth(),
335 models[i].imageHeight(),
336 models[i].K_raw().at<double>(0,0),
337 models[i].K_raw().at<double>(1,1),
338 models[i].K_raw().at<double>(0,2),
339 models[i].K_raw().at<double>(1,2),
341 distType = okvis::cameras::NCameraSystem::RadialTangential8;
342 UINFO(
"RadialTangential8");
344 else if(models[i].D_raw().cols == 6)
346 okvis::cameras::EquidistantDistortion dist(
347 models[i].D_raw().at<double>(0,0),
348 models[i].D_raw().at<double>(0,1),
349 models[i].D_raw().at<double>(0,4),
350 models[i].D_raw().at<double>(0,5));
351 cam.reset(
new okvis::cameras::PinholeCamera<okvis::cameras::EquidistantDistortion>(
352 models[i].imageWidth(),
353 models[i].imageHeight(),
354 models[i].K_raw().at<double>(0,0),
355 models[i].K_raw().at<double>(1,1),
356 models[i].K_raw().at<double>(0,2),
357 models[i].K_raw().at<double>(1,2),
359 distType = okvis::cameras::NCameraSystem::Equidistant;
360 UINFO(
"Equidistant");
362 else if(models[i].D_raw().cols >= 4)
365 okvis::cameras::RadialTangentialDistortion dist(
366 models[i].D_raw().at<double>(0,0),
367 models[i].D_raw().at<double>(0,1),
368 models[i].D_raw().at<double>(0,2),
369 models[i].D_raw().at<double>(0,3));
371 new okvis::cameras::PinholeCamera<okvis::cameras::RadialTangentialDistortion>(
372 models[i].imageWidth(),
373 models[i].imageHeight(),
374 models[i].K_raw().at<double>(0,0),
375 models[i].K_raw().at<double>(1,1),
376 models[i].K_raw().at<double>(0,2),
377 models[i].K_raw().at<double>(1,2),
379 distType = okvis::cameras::NCameraSystem::RadialTangential;
380 UINFO(
"RadialTangential");
385 okvis::cameras::RadialTangentialDistortion dist(0,0,0,0);
387 new okvis::cameras::PinholeCamera<okvis::cameras::RadialTangentialDistortion>(
388 models[i].imageWidth(),
389 models[i].imageHeight(),
390 models[i].K().at<double>(0,0),
391 models[i].K().at<double>(1,1),
392 models[i].K().at<double>(0,2),
393 models[i].K().at<double>(1,2),
395 distType = okvis::cameras::NCameraSystem::RadialTangential;
400 UINFO(
"model %d: %s", i, models[i].localTransform().prettyPrint().c_str());
402 Eigen::Vector3d r(models[i].localTransform().x(), models[i].localTransform().y(), models[i].localTransform().z());
403 parameters.nCameraSystem.addCamera(
404 std::shared_ptr<const okvis::kinematics::Transformation>(
new okvis::kinematics::Transformation(r, models[i].localTransform().getQuaterniond().normalized())),
410 okvisEstimator_ =
new okvis::ThreadedKFVio(parameters);
412 okvisEstimator_->setFullStateCallback(
413 std::bind(&OkvisCallbackHandler::fullStateCallback, okvisCallbackHandler_,
414 std::placeholders::_1, std::placeholders::_2,
415 std::placeholders::_3, std::placeholders::_4));
417 okvisEstimator_->setLandmarksCallback(
418 std::bind(&OkvisCallbackHandler::landmarksCallback, okvisCallbackHandler_,
419 std::placeholders::_1, std::placeholders::_2,
420 std::placeholders::_3));
422 okvisEstimator_->setBlocking(
true);
425 for(
unsigned int i=0; i<images.size(); ++i)
428 if(images[i].type() == CV_8UC3)
430 cv::cvtColor(images[i], gray, CV_BGR2GRAY);
432 else if(images[i].type() == CV_8UC1)
438 UFATAL(
"Not supported color type!");
441 imageUpdated = okvisEstimator_->addImage(timeOkvis, i, gray);
444 UWARN(
"Image update with stamp %f delayed...", data.
stamp());
453 if(imageUpdated && imagesProcessed_ > 10)
455 Transform fixPos(-1,0,0,0, 0,-1,0,0, 0,0,1,0);
456 Transform fixRot(0,0,1,0, 0,-1,0,0, 1,0,0,0);
457 Transform p = okvisCallbackHandler_->getLastTransform();
460 p = fixPos * p * fixRot;
495 UERROR(
"RTAB-Map is not built with OKVIS support! Select another visual odometry approach.");
void setLocalTransform(const Transform &transform)
static std::string homeDir()
static bool parse(const ParametersMap ¶meters, const std::string &key, bool &value)
rtabmap::CameraThread * cam
unsigned int framesProcessed() const
virtual Transform computeTransform(SensorData &image, const Transform &guess=Transform(), OdometryInfo *info=0)
const cv::Mat & R() const
bool imagesAlreadyRectified() const
virtual void reset(const Transform &initialPose=Transform::getIdentity())
std::map< std::string, std::string > ParametersMap
const Transform & getPose() const
const cv::Mat & imageRaw() const
#define UASSERT(condition)
Wrappers of STL for convenient functions.
bool isValidForProjection() const
const CameraModel & left() const
const cv::Vec3d linearAcceleration() const
const cv::Mat & T() const
std::string UTILITE_EXP uReplaceChar(const std::string &str, char before, char after)
const std::vector< CameraModel > & cameraModels() const
virtual void reset(const Transform &initialPose=Transform::getIdentity())
const Transform & localTransform() const
const CameraModel & right() const
ULogger class and convenient macros.
const StereoCameraModel & stereoCameraModel() const
std::string configFilename_
OdometryOkvis(const rtabmap::ParametersMap ¶meters=rtabmap::ParametersMap())
const cv::Vec3d & angularVelocity() const
const Transform & localTransform() const