Paper
Author: Raúl Mur-Artal and Juan D. Tardós
Title: Probabilistic Semi-Dense Mapping from Highly Accurate Feature-Based Monocular SLAM
How to run ORB-SLAM in Indigo
Simply remove line
1 | <depend package="opencv2"/> |
from manifest.xml and still compile the package using
. The reason for it is because the
opencv2 package doesn’t exist in ROS Indigo.1
rosmake
Code in Depth
ROS
-
Extract configuration paths under package using package API
1
ros::package::getPath("<pkg name>")+"/"+"<sub dir>"
OpenCV
- Avoid changing parameters in each compilation, using OpenCV yml parser
The format of the configuration file is below:1
cv::FileStorage fs(<path>.c_str(), cv::FileStorage::READ);
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 | %YAML:1.0 # Camera Parameters. Adjust them! # Camera calibration parameters (OpenCV) Camera.fx: 609.2855 Camera.fy: 609.3422 Camera.cx: 351.4274 Camera.cy: 237.7324 # Camera distortion paremeters (OpenCV) -- Camera.k1: -0.3492 Camera.k2: 0.1363 Camera.p1: 0.0 Camera.p2: 0.0 # Camera frames per second Camera.fps: 30.0 ... ... # Constant Velocity Motion Model (0 - disabled, 1 - enabled [recommended]) UseMotionModel: 1 |
and the parameters can be parsed in runtime. Another option is using parameter parser in ROS.
- How to convert
/1
g2o::SE3Quat
to Eigen matrix to1
g2o::Sim3
1
cv::Mat
Conversion between
format and 1
g2o
1
OpenCV
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 | cv::Mat Converter::toCvMat(const g2o::SE3Quat &SE3) { Eigen::Matrix<double,4,4> eigMat = SE3.to_homogeneous_matrix(); return toCvMat(eigMat); } cv::Mat Converter::toCvMat(const Eigen::Matrix<double,3,1> &m) { cv::Mat cvMat(3,1,CV_32F); for(int i=0;i<3;i++) cvMat.at<float>(i)=m(i); return cvMat.clone(); } cv::Mat Converter::toCvMat(const g2o::Sim3 &Sim3) { Eigen::Matrix3d eigR = Sim3.rotation().toRotationMatrix(); Eigen::Vector3d eigt = Sim3.translation(); double s = Sim3.scale(); return toCvSE3(s*eigR,eigt); } |
To be continue …