jingpang/learnviorb

learnviorb's Issues

Loop closing not working?

On the same data, ORB_SLAM2 is able to detect the loop and correct the map, but VIORB does not. Seems strange. Any clues?

Unable to build the RT branch

OS: Ubuntu 16.04
ROS: kinetic
Eigen: 3.1.0
OpenCV: 3.3.1 [ From /opt/ros/kinetic ]

Here is the screen shot of the error.

Coordinate of IMU input and camera pose output

I have some question about the coordinate of VIORB SLAM in both input and output.

1. the [IMUData] (https://github.com/jingpang/LearnVIORB/blob/RT/Examples/ROS/ORB_VIO/src/ros_vio.cc#L136) is body frame coordinate isn't it? (x point to the front of the sensor)
2. the output mTcw coordinate is camera coordinate where z point to the front of the camera (right handed rule), isn't it?

Thank you in advance for your help.

Shows segmentation fault while trying to run on Ubuntu 16.04

Hi
While trying to run the code, I run into SIGSEGV error (probably segmentation fault) and the program exits. I tried debugging in qtcreator and found that it runs into the above problem at line 160 in ros_vio.cc "SLAM.TrackMonoVI(im, vimuData, imageMsg->header.stamp.toSec() - imageMsgDelaySec);" . I am new to this and have not been yet able to figure out what is causing this. Any help would be appreciated .

./build.sh error

cc@cc-K55VD:~/catkin_ws/src/LearnVIORB$ ./build.sh
Configuring and building Thirdparty/DBoW2 ...
mkdir: cannot create directory ‘build’: File exists
-- Configuring done
-- Generating done
-- Build files have been written to: /home/cc/catkin_ws/src/LearnVIORB/Thirdparty/DBoW2/build
[100%] Built target DBoW2
Configuring and building Thirdparty/g2o ...
mkdir: cannot create directory ‘build’: File exists
-- BUILD TYPE:Release
-- Compiling on Unix
-- Configuring done
-- Generating done
-- Build files have been written to: /home/cc/catkin_ws/src/LearnVIORB/Thirdparty/g2o/build
[100%] Built target g2o
Uncompress vocabulary ...
Configuring and building ORB_SLAM2 ...
mkdir: cannot create directory ‘build’: File exists
Build type: Release
-- Using flag -std=c++11.
-- Could NOT find CHOLMOD (missing: CHOLMOD_INCLUDE_DIR CHOLMOD_LIBRARIES)
CMake Error: The following variables are used in this project, but they are set to NOTFOUND.
Please set them or make sure they are set and tested correctly in the CMake files:
CHOLMOD_INCLUDE_DIR
used as include directory in directory /home/cc/catkin_ws/src/LearnVIORB
used as include directory in directory /home/cc/catkin_ws/src/LearnVIORB
used as include directory in directory /home/cc/catkin_ws/src/LearnVIORB
CHOLMOD_LIBRARY
linked by target "ORB_SLAM2" in directory /home/cc/catkin_ws/src/LearnVIORB

-- Configuring incomplete, errors occurred!
See also "/home/cc/catkin_ws/src/LearnVIORB/build/CMakeFiles/CMakeOutput.log".
make: *** No targets specified and no makefile found. Stop.
Build ROS node ...
mkdir: cannot create directory ‘build’: File exists
[rosbuild] Building package ORB_VIO
[rosbuild] using multiarch 'x86_64-linux-gnu' for finding Boost
-- Using CATKIN_DEVEL_PREFIX: /home/cc/catkin_ws/src/LearnVIORB/Examples/ROS/ORB_VIO/build/devel
-- Using CMAKE_PREFIX_PATH: /home/cc/catkin_ws/devel;/opt/ros/kinetic
-- This workspace overlays: /home/cc/catkin_ws/devel;/opt/ros/kinetic
-- Using PYTHON_EXECUTABLE: /usr/bin/python
-- Using Debian Python package layout
-- Using empy: /usr/bin/empy
-- Using CATKIN_ENABLE_TESTING: ON
-- Skip enable_testing() for dry packages
-- Using CATKIN_TEST_RESULTS_DIR: /home/cc/catkin_ws/src/LearnVIORB/Examples/ROS/ORB_VIO/build/test_results
-- Found gtest sources under '/usr/src/gtest': gtests will be built
-- Using Python nosetests: /usr/bin/nosetests-2.7
-- catkin 0.7.6
-- Using these message generators: gencpp;geneus;genlisp;gennodejs;genpy
[rosbuild] Including /opt/ros/kinetic/share/roslisp/rosbuild/roslisp.cmake
[rosbuild] Including /opt/ros/kinetic/share/roscpp/rosbuild/roscpp.cmake
[rosbuild] Including /opt/ros/kinetic/share/rospy/rosbuild/rospy.cmake
Build type: Release
-- Using flag -std=c++11.
-- Configuring done
-- Generating done
-- Build files have been written to: /home/cc/catkin_ws/src/LearnVIORB/Examples/ROS/ORB_VIO/build
[ 0%] Built target rospack_genmsg_libexe
[ 0%] Built target rosbuild_precompile
make[2]: *** No rule to make target '../../../../lib/libORB_SLAM2.so', needed by '../VIO'. Stop.
CMakeFiles/Makefile2:163: recipe for target 'CMakeFiles/VIO.dir/all' failed
make[1]: *** [CMakeFiles/VIO.dir/all] Error 2
Makefile:127: recipe for target 'all' failed
make: *** [all] Error 2

Launch file in Examples/ROS/ORB_VIO/launch.
Modify the configuration file config/euroc.yaml
Run as: roslaunch ORB_VIO testeuroc.launch

I fund a bug when I use my device with slow and small vision change motion at the beginnig

Hi,
When I run my own device with a slow movement at the beginning, the VIORB always died. And I finally found the problem and solve it in the code.

sorry, for convenience, Iet me explain it in chinese :)

在LocalMapping线程里，函数AddToLocalWindow(KeyFrame* pKF)在往mlLocalKeyFrames里添加新的关键帧时候，有种情况没有考虑到，会导致系统直接报错退出。就是在系统刚运行的时候，由于相机移动缓慢且视觉变化不大，在LocalMapping线程KeyFrameCulling()中会剔除冗余的关键帧，那么伴随着函数DeleteBadInLocalWindow()，mlLocalKeyFrames里的关键帧也会被剔除，这样会有很大的几率导致mlLocalKeyFrames里面的关键帧从一个接近窗口大小或等于窗口大小的数量在一次关键帧剔除后减少到只剩几帧，于是在新的一次AddToLocalWindow(KeyFrame* pKF)时，会从LocalMapping.cc的864行开始执行，也就是循环地向mlLocalKeyFrames还剩的关键帧的最前的关键帧那帧的前面，插入它前一帧关键帧(有点绕...)，直到数量超过窗口阈值或者是前一帧是空的(NULL)，注意，这个条件是允许把系统最最开始的第一帧插进去的，在这种情况下，mlLocalKeyFrames的最前一帧关键帧是整个系统的第一帧关键帧。那么，问题来了，在局部优化的时候，是需要取mlLocalKeyFrames最前一帧关键帧的前一关键帧作为顶点添加IMU相关约束进行优化的，如果出现以上情况，mlLocalKeyFrames最前一帧关键帧的前一帧是空的，那么就会出问题。具体体现在Optimizer.cc的247行或者是1094行开始向g2o里添加顶点时，系统直接就退出了。

说了这么多，就是在描述一种会出现错误的情况，我的解决办法很简单，就是在AddToLocalWindow(KeyFrame* pKF)函数最后几行里增加一个条件，避免把系统最开始的那帧关键帧也插入mlLocalKeyFrames中去。
while(mlLocalKeyFrames.size() < mnLocalWindowSize && pKF0->GetPrevKeyFrame()!=NULL)
{
pKF0 = pKF0->GetPrevKeyFrame();
if(pKF0->mnId != 0) //新增加的语句
mlLocalKeyFrames.push_front(pKF0);
}

最后还想说一下，因为自己用的设备是廉价的IMU，实际跑了一下VIORB，发现会出现尺度漂移的现象，所以闭环检测时，计算的还是相似变换，这样就能闭上了，否则几何校验这块过不去，闭环没法闭...

Save the ORB_SLAM map, how to use offline map tracking and locating

Hi:
When I save ORB-SlAM map, can I use offline map tracking and positioning directly?Direct use of ORB_SLAM relocation mode is easy to locate failed。

Is this algorithm be tested?

Too awesome.
I wonder if this algorithm was compared with the groundtruth in Euroc, KITTI or TUM RGB-D dataset?
Thank you!

Is it possible to replace g2o with ceres as the backend and will it be faster or have better accuracy?

I notice that there are some jobs taking ceres as the backend optimization framework, such as OKVIS and HKUST-Mono. Here in your code, g2o is used. I have always wondered between g2o and ceres, which may behaves better. So can I know the reason why g2o is used in your implementation? And Is it possible to replace g2o with ceres as the backend and will it be faster or have better accuracy?

process has died when run offline dataset on ubuntu 16

[VIO-2] process has died [pid 12821, exit code -11, cmd /home/luozhongfei/LearnVIORB/Examples/ROS/ORB_VIO/VIO /home/luozhongfei/LearnVIORB/Examples/ROS/ORB_VIO/../../../Vocabulary/ORBvoc.bin /home/luozhongfei/LearnVIORB/Examples/ROS/ORB_VIO/../../../config/euroc.yaml __name:=VIO __log:=/home/luozhongfei/.ros/log/b0b00082-5fd3-11e7-bf6c-484d7ec87cb6/VIO-2.log].
log file: /home/luozhongfei/.ros/log/b0b00082-5fd3-11e7-bf6c-484d7ec87cb6/VIO-2*.log

No image input

Hi,
Thanks for your work! But when I roslaunch ORB_VIO testeuroc.launch, why there is no image input? I have already modified the the path in config/euroc.yaml and build the project successfully.

camera's pose may be lost after vins was initialized？

Hi~
Thank you for your great work.
the system vins will be initialized after the system runs with only mono vision for 15s. But after map scale updated, the camera's pose may be lost, and then the system ran relocation module.Sometimes, relocation failed.

Anybody knows why ?
Thanks in advance.

Test Initial Scale?

Really thanks for sharing your code. I tried the initial part, but I didn't get the correct scale. What I did is using your code TryInitVIO after the number of key frames is larger than 10. Considering the error accumulation of IMU, every image frame is consider as key frame. I am not sure if I ignore other steps.
I'd like to know if you test the initial scale on Euroc dataset. Thanks.

How to interpret the scale.txt file

I got the scale.txt file as follows:

1403636580.963556 5.404152 5.401110
1403636581.113555 5.342806 5.317275
1403636581.263556 5.383121 5.398332
1403636581.413555 5.439227 5.319515
1403636581.563555 5.535811 5.424372
...
...
....
similar other values
....

I think the first is the timestamp for the keyframe, and what are the two values after that. I found that the first value is s_ and second value is sstar from code, but unable to understand the meaning and difference between them. And does the scale change with each keyframe, or is it constant after we initialize the scale from certain keyframes. Any help will be appreciated. Thank you.

Cannot pass EuRoC V1_02_medium

hi, can you pass EuRoC V1_02_medium dataset without any tracking lost? I run the code on my PC but lost so many times. How can I improve it?

Real time?

Hi, thanks for making this code available! In the comments, you say 'not real-time', what is it that is stopping this from being real-time capable?

Thanks!

code problem

In IMUPreintegrator.h,the function JacobianR calculates the right jacobian by the formula
JR = I - (1- cos( theta ) ) / theta * K + ( 1 - sin( theta ) / theta ) * K * K.But i saw the formula to do the same thing in Gao Xiang's blog was JR = sin( theta ) / theta * I - (1- cos( theta ) ) / theta * K + ( 1 - sin( theta ) / theta ) * k * k.transpose().I wonder if i was wrong or misunderstood somewhere.

Two questions about your code

Hi ! You really did a nice work, thanks so much !!!
There're two problems which trouble me, beg your help !!!

1. About IMU noise model

   Matrix3d IMUData::_gyrMeasCov = Matrix3d::Identity()1.7e-41.7e-4/0.005/*100/;
   Matrix3d IMUData::_accMeasCov = Matrix3d::Identity()2.0e-32.0e-3/0.005*100;
   Why did you multiply 100 at the last ?

2. in function void EdgeNavStatePVRPointXYZOnlyPose::linearizeOplus()

   Vector3d Paux = RcbRwb.transpose()(Pw-Pwb);
   Matrix<double,2,3> JdRwb = - Jpi * (Sophus::SO3::hat(Paux) * Rcb);
   There's transpose on Rwb, how to get the Jacobian W.R.T. Rwb ???
   Could you please give me some references ???

Thank you so much !!!

LocalBAPRVIDP

Sometimes the system crash in function LocalBAPRVIDP, any body know how to solve the issue?

How do you evaluate the results of VI-ORB against the groundtruth on EuRoC dataset?

It seems that the timestamps and FPS of the groundtruth and the output trojectory of VI-ORB are not the same. What's more, the groundtruth trojectory and the VI-ORB trojectory are based on different global coordinate systems(There is a transformation(rotation and translation) between the groundtruth and VI-ORB). So how do you evaluate the results of VI-ORB against the groundtruth or original ORB?

cannot launch node of type [ORB_VIO/VIO]

ERROR: cannot launch node of type [ORB_VIO/VIO]: can't locate node [VIO] in package [ORB_VIO]

Trouble understanding IMU Initialization

Thanks for integrating the IMU. I was going through your code and I have difficulty understanding the void EdgeGyrBias::linearizeOplus() in g2otypes.cc

According to your code:

Sophus::SO3_orb errR ( dRbij.transpose() * Rwbi.transpose() * Rwbj ); // dRij^T * Riw * Rwj
Matrix3d Jlinv = Sophus::SO3_orb::JacobianLInv(errR.log());
_jacobianOplusXi = - Jlinv * J_dR_bg;

Can you please elaborate on how you derived this or point to the resource? I couldn't find the corresponding equation in IMU Preintegration paper.

Crash on relocalization

Hi,
First of all, thanks for the work you put into this and making it available.

On to the issue. I was having issue crashing when the reloc. procedure was triggered. The crash itself was on the NavState constructor. I nailed it down to the std::vector of Frames used to compute the bias in Tracking.h , explanation:

Eigen requires some procedures when using STL containers for Eigen objects or classes that contain them. I can't at the moment provide a pull request due to being unable to compile without multiple alterations, but here's the fix:

Tracking.h

#include <Eigen/StdVector>
(...)
vector<Frame, Eigen::aligned_allocator<Frame> > mv20FramesReloc;

And adapting the function in Optimizer and its function call here

Cheers

make -j error

I checked and reinstalled eigen, but also encountered this error.I tryed eigen 3.3 and 3.4.
cc@cc-B250-HD3:~/catkin_ws/src/LearnVIORB-RT/build$ make -j
[ 3%] Building CXX object CMakeFiles/ORB_SLAM2.dir/src/Optimizer.cc.o
In file included from /usr/include/eigen3/Eigen/Core:297:0,
from /usr/include/eigen3/Eigen/Dense:1,
from /home/cc/catkin_ws/src/LearnVIORB-RT/src/IMU/imudata.h:4,
from /home/cc/catkin_ws/src/LearnVIORB-RT/include/Frame.h:35,
from /home/cc/catkin_ws/src/LearnVIORB-RT/include/KeyFrame.h:29,
from /home/cc/catkin_ws/src/LearnVIORB-RT/include/MapPoint.h:24,
from /home/cc/catkin_ws/src/LearnVIORB-RT/include/Map.h:24,
from /home/cc/catkin_ws/src/LearnVIORB-RT/include/Optimizer.h:24,
from /home/cc/catkin_ws/src/LearnVIORB-RT/src/Optimizer.cc:21:
/usr/include/eigen3/Eigen/src/Core/AssignEvaluator.h: In instantiation of ‘void Eigen::internal::call_assignment_no_alias(Dst&, const Src&, const Func&) [with Dst = Eigen::Matrix<int, -1, 1>; Src = Eigen::Matrix<long int, -1, 1, 0, -1, 1>; Func = Eigen::internal::assign_op]’:
/usr/include/eigen3/Eigen/src/Core/AssignEvaluator.h:712:27: required from ‘void Eigen::internal::call_assignment(Dst&, const Src&, const Func&, typename Eigen::internal::enable_if<(! Eigen::internal::evaluator_assume_aliasing::value), void*>::type) [with Dst = Eigen::Matrix<int, -1, 1>; Src = Eigen::Matrix<long int, -1, 1, 0, -1, 1>; Func = Eigen::internal::assign_op; typename Eigen::internal::enable_if<(! Eigen::internal::evaluator_assume_aliasing::value), void*>::type = void*]’
/usr/include/eigen3/Eigen/src/Core/AssignEvaluator.h:693:18: required from ‘void Eigen::internal::call_assignment(Dst&, const Src&) [with Dst = Eigen::Matrix<int, -1, 1>; Src = Eigen::Matrix<long int, -1, 1, 0, -1, 1>]’
/usr/include/eigen3/Eigen/src/Core/PlainObjectBase.h:682:32: required from ‘Derived& Eigen::PlainObjectBase::_set(const Eigen::DenseBase&) [with OtherDerived = Eigen::Matrix<long int, -1, 1, 0, -1, 1>; Derived = Eigen::Matrix<int, -1, 1>]’
/usr/include/eigen3/Eigen/src/Core/Matrix.h:225:24: required from ‘Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>& Eigen::Matrix<_Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols>::operator=(const Eigen::DenseBase&) [with OtherDerived = Eigen::Matrix<long int, -1, 1, 0, -1, 1>; _Scalar = int; int _Rows = -1; int _Cols = 1; int _Options = 0; int _MaxRows = -1; int _MaxCols = 1]’
/usr/include/eigen3/Eigen/src/Core/PermutationMatrix.h:367:17: required from ‘Eigen::PermutationMatrix<SizeAtCompileTime, MaxSizeAtCompileTime, IndexType>& Eigen::PermutationMatrix<SizeAtCompileTime, MaxSizeAtCompileTime, IndexType>::operator=(const Eigen::PermutationBase&) [with Other = Eigen::PermutationMatrix<-1, -1, long int>; int SizeAtCompileTime = -1; int MaxSizeAtCompileTime = -1; _StorageIndex = int]’
/home/cc/catkin_ws/src/LearnVIORB-RT/Thirdparty/g2o/g2o/solvers/linear_solver_eigen.h:68:18: required from ‘void g2o::LinearSolverEigen::CholeskyDecomposition::analyzePatternWithPermutation(g2o::LinearSolverEigen::SparseMatrix&, const PermutationMatrix&) [with MatrixType = Eigen::Matrix<double, 7, 7, 0, 7, 7>; g2o::LinearSolverEigen::SparseMatrix = Eigen::SparseMatrix<double, 0>; g2o::LinearSolverEigen::PermutationMatrix = Eigen::PermutationMatrix<-1, -1, long int>]’
/home/cc/catkin_ws/src/LearnVIORB-RT/Thirdparty/g2o/g2o/solvers/linear_solver_eigen.h:195:9: required from ‘void g2o::LinearSolverEigen::computeSymbolicDecomposition(const g2o::SparseBlockMatrix&) [with MatrixType = Eigen::Matrix<double, 7, 7, 0, 7, 7>]’
/home/cc/catkin_ws/src/LearnVIORB-RT/Thirdparty/g2o/g2o/solvers/linear_solver_eigen.h:100:37: required from ‘bool g2o::LinearSolverEigen::solve(const g2o::SparseBlockMatrix&, double*, double*) [with MatrixType = Eigen::Matrix<double, 7, 7, 0, 7, 7>]’
/home/cc/catkin_ws/src/LearnVIORB-RT/src/Optimizer.cc:4568:1: required from here
/usr/include/eigen3/Eigen/src/Core/util/StaticAssert.h:32:40: error: static assertion failed: YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY
#define EIGEN_STATIC_ASSERT(X,MSG) static_assert(X,#MSG);
^
/usr/include/eigen3/Eigen/src/Core/util/XprHelper.h:707:3: note: in expansion of macro ‘EIGEN_STATIC_ASSERT’
EIGEN_STATIC_ASSERT((internal::functor_is_product_like::ret
^
/usr/include/eigen3/Eigen/src/Core/AssignEvaluator.h:745:3: note: in expansion of macro ‘EIGEN_CHECK_BINARY_COMPATIBILIY’
EIGEN_CHECK_BINARY_COMPATIBILIY(Func,typename ActualDstTypeCleaned::Scalar,typename Src::Scalar);
^
CMakeFiles/ORB_SLAM2.dir/build.make:350: recipe for target 'CMakeFiles/ORB_SLAM2.dir/src/Optimizer.cc.o' failed
make[2]: *** [CMakeFiles/ORB_SLAM2.dir/src/Optimizer.cc.o] Error 1
CMakeFiles/Makefile2:67: recipe for target 'CMakeFiles/ORB_SLAM2.dir/all' failed
make[1]: *** [CMakeFiles/ORB_SLAM2.dir/all] Error 2
Makefile:83: recipe for target 'all' failed
make: *** [all] Error 2

Does it work in Windows

Hello, Thanks for your work.
I am trying to compile it with noROS(https://github.com/OpenSLAM/LearnViORB_NOROS/tree/master/master/LearnVIORB_NOROS) in Windows, but there seems something error in the g2o.

If I complie the g2o in the Thirdparty, and use the g2o.lib to complie the ORB_SLAM2, it would be error during LINKing like this:
.......
2>g2otypes.obj : error LNK2001: 无法解析的外部符号 "public: virtual bool __cdecl g2o::OptimizableGraph::Edge::setMeasurementFromState(void)" (?setMeasurementFromState@Edge@OptimizableGraph@g2o@@UEAA_NXZ)
2>Optimizer.obj : error LNK2019: 无法解析的外部符号 "public: void __cdecl g2o::OptimizableGraph::Edge::setRobustKernel(class g2o::RobustKernel *)" (?setRobustKernel@Edge@OptimizableGraph@g2o@@QEAAXPEAVRobustKernel@3@@z)，该符号在函数 "public: static void __cdecl ORB_SLAM2::Optimizer::BundleAdjustment(class std::vector<class ORB_SLAM2::KeyFrame *,class std::allocator<class ORB_SLAM2::KeyFrame *> > const &,class std::vector<class ORB_SLAM2::MapPoint *,class std::allocator<class ORB_SLAM2::MapPoint *> > const &,int,bool *,unsigned long,bool)" (?BundleAdjustment@Optimizer@ORB_SLAM2@@SAXAEBV?$vector@PEAVKeyFrame@ORB_SLAM2@@v?$allocator@PEAVKeyFrame@ORB_SLAM2@@@std@@@std@@aebv?$vector@PEAVMapPoint@ORB_SLAM2@@v?$allocator@PEAVMapPoint@ORB_SLAM2@@@std@@@4@HPEA_NK_N@Z) 中被引用
......

I think there are something wrong in the linear_solver_cholmod.h, because I can't find any reference of CholmodExt and LinearSolverCholmod in the g2o. So I add a file to instance a LinearSolverCholmod and recompile it. But it doesn't work.

2>e:\vs2015project\viorb\learnviorb_noros\thirdparty\g2o\g2o\solvers\linear_solver_cholmod.h(207): error C2065: “MatrixXD”: 未声明的标识符
2> e:\vs2015project\viorb\learnviorb_noros\thirdparty\g2o\g2o\solvers\linear_solver_cholmod.h(375): note: 参见对正在编译的类 模板 实例化“g2o::LinearSolverCholmod”的引用
2>e:\vs2015project\viorb\learnviorb_noros\thirdparty\g2o\g2o\solvers\linear_solver_cholmod.h(207): error C2923: “g2o::SparseBlockMatrix”: 对于参数“MatrixType”，“MatrixXD”不是有效的 模板 类型变量
2>e:\vs2015project\viorb\learnviorb_noros\thirdparty\g2o\g2o\solvers\linear_solver_cholmod.h(268): error C3646: “_scalarPermutation”: 未知重写说明符
2>e:\vs2015project\viorb\learnviorb_noros\thirdparty\g2o\g2o\solvers\linear_solver_cholmod.h(268): error C2059: 语法错误:“,”
2>e:\vs2015project\viorb\learnviorb_noros\thirdparty\g2o\g2o\solvers\linear_solver_cholmod.h(268): error C2238: 意外的标记位于“;”之前
.......

Any advise to fix it ?

pKFi->mvuRight[mit->second]<0

In Optimizer.cc, why pKFi->mvuRight[mit->second]<0 could decide sensor is stereo or Monocular？
if I use RGBD, where should i modified?

Test:: Scale initialize part work fine for my system

First of all, thank you for coding, Great work!!

Yesterday i try to use your code in part of VIN init scale. It work well for my system (MPU6050+sj4000wifi). Even in large scale, the estimator still following environment scale as show in my vid . By reducing the buffer of image callback, i got real-time estimate (this made lower fps).

However, when i used full IMU tracking and IMU error for optimization. It seem still have scale misalignment at the return point like this vid. I don't sure it cause by my camera time is not syncronized to IMU due to exposure time varied, or just normal limitation for monocular system. Have you try this case yet?

Cheer.

About gI definition during VIO initialization

In your code the gravity vector is set as
gI.at(2) = 1;
here is different from the paper(says -1). Is this correct? Also do you know what G in every equation means in the paper. I found you just ignore this item.
Thanks.

feature lost when after TryInitVIO return true

I run this program in realtime with RealSense Camaro，when the function LocalMapping::TryInitVIO() returns true , all features lost and can't be catched.
How to solve this issue? Thank you！

Also in this function，sometimes the scale and depth is negative，why？

program died afer initialized sometime

@jingpang
output error is as follow when run demo MH_01_easy.bag :

-----------here is the last KF in vScaleGravityKF------------

... Map scale updated ...


... Map NavState updated ...

finish global BA after vins init

... Map scale updated ...

Relocalized. id: 393
[VIO-2] process has died [pid 5714, exit code -11, cmd /home/zp/project/viorb/Examples/ROS/ORB_VIO/VIO /home/zp/project/viorb/Examples/ROS/ORB_VIO/../../../Vocabulary/ORBvoc.bin /home/zp/project/viorb/Examples/ROS/ORB_VIO/../../../config/euroc.yaml __name:=VIO __log:=/home/zp/.ros/log/bba769fe-696d-11e8-8ac7-6045cb8ab013/VIO-2.log].
log file: /home/zp/.ros/log/bba769fe-696d-11e8-8ac7-6045cb8ab013/VIO-2*.log

file open error in TryInitVIO

I meet some problems like "file open error in TryInitVIO", is there any problem with my test.InitVIOTmpPath: ?

static bool fopened = false;
static ofstream fgw,fscale,fbiasa,fcondnum,ftime,fbiasg;
string tmpfilepath = ConfigParam::getTmpFilePath();
if(!fopened)
{
// Need to modify this to correct path
fgw.open(tmpfilepath+"gw.txt");
fscale.open(tmpfilepath+"scale.txt");
fbiasa.open(tmpfilepath+"biasa.txt");
fcondnum.open(tmpfilepath+"condnum.txt");
ftime.open(tmpfilepath+"computetime.txt");
fbiasg.open(tmpfilepath+"biasg.txt");

i can't find these .txt files in my test.InitVIOTmpPath

RecomputeIMUBiasAndCurrentNavState()函数计算加速度零偏的理论依据？

它跟加速度零偏初始化时公式不一致，直接把尺度s和dthetaeig扔掉么？目前经常发现，重新计算后的加速度零偏特别大，可能是因为什么

Reinitialization problem

I have try your version in RT branch and connect it to mavlink to get imu from pixhawk and using image from monocular camera. the problem is after reinitialization complete the previous generated point cloud never come back as a part of new initialization. Is this part already implemented? and what can cause this problem?

Camera-imu frame transformation What is the parameter Tic Pc?

Hi:
I can't understand what the Tic Pc parameter means.

regarding imu bias during preintegration

Hi,

1st, thanks for the awesome work.

just wandering in file IMUPreintegrator.cpp, on lines 108-110, the computation of _delta_P, _delta_V and _delta_R seems ignoring the IMU bias terms, which is present according the Eq.35-37 in the IMU preintegration paper. Is it because you are initialising the imu bias to be 0 or this bias is considered in somewhere else in the code? Thanks for your answer in advance.

Have a nice weekend.

Cheers,

Su

some problems

solved

viorb without ros

https://github.com/OpenSLAM/LearnViORB_NOROS/blob/master/master/LearnVIORB_NOROS

Run roslaunch but no result ?

I modified config file and run roslaunch ORB_VIO testeuroc.launch ，then I play a bag，no VIO log or any result show. Only log below :

... logging to /home/liming/.ros/log/14c9cb86-6bb6-11e8-b487-e4a7a0c3f334/roslaunch-liming-Aero-14-8948.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://liming-Aero-14:39313/

SUMMARY
========

PARAMETERS
 * /rosdistro: kinetic
 * /rosversion: 1.12.13

NODES
  /
    VIO (ORB_VIO/VIO)

auto-starting new master
process[master]: started with pid [8958]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to 14c9cb86-6bb6-11e8-b487-e4a7a0c3f334
process[rosout-1]: started with pid [8971]
started core service [/rosout]
process[VIO-2]: started with pid [8974]

I'm sure I have changed "test.InitVIOTmpPath" and "bagfile" in config file.

Don't know what is wrong ?

two questions on PoseOptimization

Hi, thank you very much for sharing the code!
I am learning your code recently, and I have two questions on PoseOptimization functions, could you give me some instructions? Thanks in advance!

1. how you determine the values for Huber?
   for example "const float thHuberNavState = sqrt(30.5779);", how this 30.5779 is found?

2. about marginalization, it seems you marginalized vertexs of the current frame, shouldn't we marginalize vertexs of the last frame/keyframe instead?

   // Compute marginalized Hessian H and B, H*x=B, H/B can be used as prior for next optimization in PoseOptimization

   if(bComputeMarg)

   {

    std::vector<g2o::OptimizableGraph::Vertex*> margVerteces;
   
    margVerteces.push_back(optimizer.vertex(FramePVRId));
   
    margVerteces.push_back(optimizer.vertex(FrameBiasId));
   
   
   
    g2o::SparseBlockMatrixXd spinv;
   
    optimizer.computeMarginals(spinv, margVerteces);
   
    // spinv include 2 blocks, 9x9-(0,0) for PVR, 6x6-(1,1) for Bias
   
    Matrix<double,15,15> margCov = Matrix<double,15,15>::Zero();
   
    margCov.topLeftCorner(9,9) = spinv.block(0,0)->eval();
   
    margCov.topRightCorner(9,6) = spinv.block(0,1)->eval();
   
    margCov.bottomLeftCorner(6,9) = spinv.block(1,0)->eval();
   
    margCov.bottomRightCorner(6,6) = spinv.block(1,1)->eval();
   
    pFrame->mMargCovInv = margCov.inverse();
   
    pFrame->mNavStatePrior = ns_recov;
   
   
   
    //Debug log
   
    //cout<<"inv MargCov 2: "<<endl<<pFrame->mMargCovInv<<endl;
   

   }

About 'setMarginalized' and marginal covariance computation

I wonder if the optimization result is affected by setMarginalized(true). Or is this just related with computeCovariance()? I tried set it different to see the time cost. But the results seem to be same whatever it is true or false. I don't totally understand the algorithm for computing marginal convariance in g2o. What I want to do is to obtain camera pose covariance part. I did in following steps:
(1) add the current camera pose as a vertex
setId(0) ; setEstimate(initial value); setMarginalize(false); setFixed(false);
(2) add points
setId(i+1) ; setEstimate(initial value); setMarginalize(true); setFixed(true);
(3) add edge:
setMeasurements(position in image); setInformation(Identity); setRobustKernel(Huber);setDelta(sqrt(5.991)); set camera params(fx,fy,cx,cy)
(4) Get covariance after optimization by using the following code:
g2o::SparseBlockMatrixXd spinv;
optimizer.computeMarginals(spinv,optimizer.vertex(0));
The pose covariance is spinv.block(0,0)->eval().
I did not understand marginalize very well. I am not sure that setMarginalize as true affect the result of optimization or not.
I just worried I did something wrong. Is anyone familiar with this? Could you have a look to see if it is a problem anywhere?
Thanks very much for your help.

Why the right jacobian of SO(3) is different from the paper?

I think it is different.

Results comparision with ORB-SLAM

I run the ORB-SLAM with test the absolute trajectory error and got an error of 1.221392, and following plot.

I then run the VIORB and got an absolute trajectory error of 3.657186, and here is the output:

Then I took average value of scale as 5 [ from scale.txt file] and calculated the absolute trajectory error of 1.018193. And the following is the result.

But the difference with ORB SLAM with and without IMU seems to be very small and ORB-SLAM seems to work very good (even in terms of scale without IMU). Is that correct or am I doing something wrong.

Thank you.

A problem about the update of Map scale after running TryInitVIO()

Hi.
Here in the functionTryInitVIO(), the system had uptaded the map scales including all mappoints and keyframs, but in the next program, the map scales would be updated again.
What's the difference between these two updates?

            bool tmpbool = TryInitVIO();
                    SetVINSInited(tmpbool);
                    if(tmpbool)
                    {
                        // Update map scale
                       mpMap->UpdateScale(mnVINSInitScale);
                        // Set initialization flag
                        SetFirstVINSInited(true);
                    }

Thank you in advance.

Code is always on tryInitVIO mode

I run the code on EuRoC MH_01_easy dataset, and got a final scale of value: 5.899714. Is it the true scale for the MH_01_easy dataset ? Then I wanted to track the program flow with some cout statements on main methods and it seems like the code never goes out of the initialization state to trackingwithimu stage. Here is the initial and final output:

tart processing sequence ...
Images in the sequence: 3682

Load Image: 0
Done aggregating imu with n points: 2
Inside Tracking::TrackMonoVI()
Inside Tracking::GrabImageMonoVI
Inside Frame Constructor
Frame id: 0
Assigning features to grid
Inside Tracking::Track()
Inside Tracking::MonocularInitialization
clering mvIMUSinceLastKF and setting referenc frame
Inside Frame::GetNavState
Inside Frame::GetNavState
Inside Initializer::Initializer
Load Image: 1
Done aggregating imu with n points: 10
Inside Tracking::TrackMonoVI()
Inside Tracking::GrabImageMonoVI
Inside Frame Constructor
Assigning features to grid
Inside Tracking::Track()
Inside Tracking::MonocularInitialization
Trying to initialize
Initialize with nmatches: 401
Inside Initializer::Initialize
Load Image: 2
Inside Tracking::TrackMonoVI()
Inside Tracking::GrabImageMonoVI
Inside Frame Constructor
Frame id: 13
Assigning features to grid
Inside Tracking::Track()
mState: 1
Inside Tracking::MonocularInitialization
Trying to initialize
Initialize with nmatches: 282
Inside Initializer::Initialize
Inside Tracking::CreateInitialMapMonocular
Inside KeyFrame Constructor
Inside KeyFrame Constructor
dt = 0.000, this vs next time: 1403636579.864 vs 1403636579.864
New Map created with 239 points
Inside Frame::GetNavState
Load Image: 3
Inside Tracking::TrackMonoVI()
Inside Tracking::GrabImageMonoVI
Inside Frame Constructor
init done
opengl support available
Assigning features to grid
Inside Tracking::Track()
System initialized
Inside !mbOnlyTracking
Inside mState=ok and TRACK_WITH_IMU
Inside mVelocity.empty() || mCurrentFrame.mnId<mnLastRelocFrameId+2
Inside Tracking::TrackReferenceKeyFrame
Inside Optimizer::PoseOptimization(Frame *pFrame)
bOK at 7E: 1
Local Map tracking
Inside Tracking::TrackLocalMap
Inside Optimizer::PoseOptimization(Frame *pFrame)
bOK at 2E: 1
Inside Frame::GetNavState
Load Image: 4
Trying to init VIO
Inside LocalMapping::TryInitVIO
Keyframes: 2
mnLocalWindowSize: 10
Inside Tracking::TrackMonoVI()
Inside Tracking::GrabImageMonoVI
Inside Frame Constructor
Assigning features to grid
Inside Tracking::Track()
mState: 2
System initialized
Inside !mbOnlyTracking
Inside mState=ok and TRACK_WITH_IMU
!(Inside mVelocity.empty() || mCurrentFrame.mnId<mnLastRelocFrameId+2)
Inside Tracking::TrackWithMotionModel
Inlier: 195, vDepth: 195, vFeat: 195, matches: 195
This frame has matches: 157
Inside Optimizer::PoseOptimization(Frame *pFrame)
TrackWithMotionModel nmatchesMap : 124
bOK at 6E: 1
Local Map tracking
Inside Tracking::TrackLocalMap
Inside Optimizer::PoseOptimization(Frame *pFrame)
bOK at 2E: 1
Inside KeyFrame Constructor
Inside Frame::GetNavState
Inside KeyFrame::SetInitialNavStateAndBias
dt = 0.000, this vs next time: 1403636579.964 vs 1403636579.964
Inside Frame::GetNavState
......
........
.......
Load Image: 3339
Inside Tracking::TrackMonoVI()
Inside Tracking::GrabImageMonoVI
Inside Frame Constructor
Assigning features to grid
Inside Tracking::Track()
mState: 2
System initialized
Inside !mbOnlyTracking
Inside mState=ok and TRACK_WITH_IMU
!(Inside mVelocity.empty() || mCurrentFrame.mnId<mnLastRelocFrameId+2)
Inside Tracking::TrackWithMotionModel
Inlier: 382, vDepth: 382, vFeat: 382, matches: 382
This frame has matches: 252
Inside Optimizer::PoseOptimization(Frame *pFrame)
TrackWithMotionModel nmatchesMap : 197
bOK at 6E: 1
Local Map tracking
Inside Tracking::TrackLocalMap
dist = 0.963451
Trying to init VIO
Inside LocalMapping::TryInitVIO
dt = 0.000, this vs next time: 1403636580.264 vs 1403636580.264
dt = 0.000, this vs next time: 1403636580.714 vs 1403636580.714
dt = 0.000, this vs next time: 1403636581.164 vs 1403636581.164

So even after loading 3339 images, the system is still in the trying to init vio mode. But the gw value seems to be 9.7 and it seems to be fine. Also I was able to get the gyro and acc bias. Does it mean that the code doesnot have termination criteria for the IMU initialiazation. Thnaks for the reply.
Regards,
Dharma KC

g2o

Where did you get the library of the g2o library? Did you change it?

Unstable accelerometer bias estimation

Hi.
I have tested your implementation on EurocMAV dataset.
On all my tests I've got robust gyroscope & gravity etsimitation.
But unfotunately accelerometer is wrong on some scenes (V1_01_easy for example).
Also it loooks like it heavely depends on initial scene scale. What's wrong?
Also is there any implmentation of orb slam with GTSAM library?
It looks like you have converted some GTSAM factors to orb library and to g2o optimization.

In the RT version, the JdPwb express different from the master version?

@jingpang in the RT version ,
Matrix<double,3,3> JdPwb = - Jpi * (-Rcb*Rwb.transpose());
while, int the master version ,
Matrix<double,3,3> JdPwb = - Jpi * (-Rcb);

when i try change the expression in the master as the same with RT version ,i test mh_01_easy with a tracking lost.
what is the correct expression of JdPwb ?
Hope get your message,thanks!

Stereo and Monocular orbvio without ros

See https://github.com/ZuoJiaxing/LearnVIORBnorosgai2 for detail.

Can't run through EuRoC V102 dataset.

The program can initialize within 15 sec, and soon after initializing, the system is lost and no relocalization seems to be performed. How I can make it work on V102 dataset?

Building issue: "Could NOT find CHOLMOD"

Hi,

Thanks for sharing this repo!
I'm trying to test it, but when I execute the build.sh script I get the following error:

Could NOT find CHOLMOD (missing:  CHOLMOD_INCLUDE_DIR CHOLMOD_LIBRARIES) 
CMake Error: The following variables are used in this project, but they are set to NOTFOUND.
Please set them or make sure they are set and tested correctly in the CMake files:
CHOLMOD_INCLUDE_DIR
   used as include directory in directory /home/marco/viorb_ws/src/LearnVIORB
CHOLMOD_LIBRARY
    linked by target "ORB_SLAM2" in directory /home/marco/viorb_ws/src/LearnVIORB

-- Configuring incomplete, errors occurred!

Any idea why?

Thanks,
Marco.