I have found several great sources on VO, namely
this,
this, and
this. Since I would like to work on monocular VO, I use
this first. In this blog, I will write about its implementation.
The original code is written for windows, and I want to port it to Ubantu in ROS catkin workspace. So I do not follow the file structure but use the one for catkin. Whenever a third party library is needed, I include it in the cmake.
At first, we have to detect features using FAST. We need to define a class called frame to process the images. Several third party libraries are needed, similar to
SVO. To use Eigen, I copy the CMakeModules folder in SVO to my src folder, which contains FindEigen.cmake. Regarding Sophus, I encounter an error saying that Sophus.cmake cannot be found. This is wired because in SVO it finds the library without any problem. After debugging, it turns out that I should use FIND_PACKAGE(Sophus REQUIRED) in cmake instead of FIND_PACKAGE(sophus REQUIRED). The name should be exactly the same and the first letter should be CAPITAL.
For the constructor in the frame class, : key_pts_(5) means to set key_pts_ to 5, as mentioned
here. In Frame.cpp, it turns out that we could not use #include "feature.h", which leads to re-definition of frame class.
We also have to add
IF(CMAKE_COMPILER_IS_GNUCC)
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
ELSE()
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
ENDIF()
as in SVO cmake to be able to use lambda expression, eg.
std::for_each(key_pts_.begin(), key_pts_.end(), [&](Feature* ftr){ ftr = NULL; });
Since I am using Ubantu, I could not find _aligned_malloc, which seems to be only available in windows VS. So I delete the SSE2 related part in image_utils.h.
Another issue often encountered is the re-declaration problem. For instance, in the point3d.h, we need to add
#ifndef POINT3D_H_
#define POINT3D_H_
and #endif
to avoid this.
In frame.h, when it says Vector3d does not name a type, we have to include abstract_camera.h, and using namespace Eigen; solves this problem.
When I run the initial_position, it gives core dumped error, and it seems c2f is not executed. It also turns out that px_ref_it->x - px_cur_it->x, px_ref_it->y - px_cur_it->y are all 0. This is because I use two identical images for the frame 1 and 2. But even if I use different images, it still gives me segmentation fault. The reason is that I forget to initialize cam_(cam) in frame.cpp
Take note that vector is in namespace std, not cv.
Next problem is that scale is nan. The implementation seems to be problematic, and I probably should implement my own VO step by step.
In the FAST keypoint detection, the image is divided into several cells, and only one keypoint in every cell with the highest shi-tomasi score is selected, as shown in the following code
if (score > corners.at(k).score)
corners.at(k) = Corner(xy.x*scale, xy.y*scale, score, L, 0.0f);
There is an angle in the Corner defined in feature.h, which should hold the gradient information of the keypoint, but it does not seem to be in the code.
