VIO-slam 系统构建

文章目录

目录

文章目录

前言

一、VIO系统数据的获取：图像传感器选型、IMU传感器选型

二、建立linux系统+ROS环境

三、如何读取图像数据

四、如何读取IMU传感器数据

五、标定图像和IMU数据的外参、相机的内参

六、移植VINS-MONO或者VINS-FUSION：主要调试获取图像和imu的订阅数据

七、实践VIO中遇到的问题

总结

前言

分享一些自己学习+实践 VIO视觉融合惯导的定位算法系统工程化过程

一、VIO系统数据的获取：图像传感器选型、IMU传感器选型

二、建立linux系统+ROS环境

三、如何读取图像数据

四、如何读取IMU传感器数据

五、标定图像和IMU数据的外参、相机的内参

六、移植VINS-MONO或者VINS-FUSION：主要调试获取图像和imu的订阅数据

七、实践VIO中遇到的问题

1、在运行vins-mono中发现，Eigen报错，vins_estimator: /usr/include/eigen3/Eigen/src/Core/DenseStorage.h:109: Eigen::internal::plain_array<T, Size, MatrixOrArrayOptions, 16>::plain_array() [with T = double; int Size = 270; int MatrixOrArrayOptions = 0]: Assertion `(internal::UIntPtr(eigen_unaligned_array_assert_workaround_gcc47(array)) & (15)) == 0 && "this assertion is explained here: " "http://eigen.tuxfamily.org/dox-devel/group__TopicUnalignedArrayAssert.html" " **** READ THIS WEB PAGE !!! ****"' failed.

解决问题：

1）查阅Eigen官方文档找到问题所在。原因是Eigen库为了使用SSE加速，在内存上分配了128位的指针，涉及字节对齐问题，该问题在编译时不会报错，只在运行时报错。

原因1：类中有Eigen类型的成员变量
class Foo
{
//...
Eigen::Vector2d v;
//...
};
//...
Foo *foo = new Foo;
在生成定长的Matrix或Vector对象时，需要开辟内存，调用默认构造函数，通常x86下的指针是32位，内存位数没对齐就会导致程序运行出错。而对于动态变量(例如Eigen::VectorXd)会动态分配内存，因此会自动地进行内存对齐。

解决方法：在public下写一个宏EIGEN_MAKE_ALIGNED_OPERATOR_NEW

class Foo
{
...
Eigen::Vector2d v;
...
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
...
Foo *foo = new Foo;
这个宏在new一个对象时会总是返回一个对齐的指针。

如果想要通过模板参数来定义一个定长变量，可以使用宏EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)。若NeedsToAlign是True，则进行对齐运算；若NeedsToAlign为False，则采用默认的对齐方式。

template<int n> class Foo
{
typedef Eigen::Matrix<float,n,1> Vector;
enum { NeedsToAlign = (sizeof(Vector)%16)==0 };
...
Vector v;
...
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
};
...
Foo<4> *foo4 = new Foo<4>; // foo4 is guaranteed to be 128bit-aligned
Foo<3> *foo3 = new Foo<3>; // foo3 has only the system default alignment guarantee
如果觉得到处放EIGEN_MAKE_ALIGNED_OPERATOR_NEW太过繁琐，还可以采用以下的两种措施。

1)禁用对齐

class Foo
{
...
Eigen::Matrix<double,2,1,Eigen::DontAlign> v;
...
};
这种方法在将变量赋值到一个临时的对齐向量时，仍然有可能重新启用矢量化。

void Foo::bar()
{
Eigen::Vector2d av(v);
// use av instead of v
...
// if av changed, then do:
v = av;
}
2)私有结构体

将定长的对象存储到私有结构体中，在构建主对象时固定分配内存。这里的明显优势是类Foo在对齐问题上保持不变。缺点是无论如何都需要堆分配。

struct Foo_d
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
Vector2d v;
...
};
struct Foo {
Foo() { init_d(); }
~Foo() { delete d; }
void bar()
{
// use d->v instead of v
...
}
private:
void init_d() { d = new Foo_d; }
Foo_d* d;
};
原因2：STL容器中的元素是Eigen数据结构
使用Eigen::aligned_allocator告诉容器进行16字节对齐。

std::map<int, Eigen::Vector4f>
将其改成

std::map<int, Eigen::Vector4f, std::less<int>,
Eigen::aligned_allocator<std::pair<const int, Eigen::Vector4f> > >
另一种方法，使用宏EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION

#include<Eigen/StdVector>
/* ... */
EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Matrix2d)
std::vector<Eigen::Vector2d>
原因3：通过值传递定长Eigen对象
void func(Eigen::Vector4d v);
在c++中使用值传递都是一个很糟糕的选项，意味着很多无用拷贝，这儿还会造成程序崩溃。通常采用引用传递：

void my_function(const Eigen::Vector2d& v);
原因4：编译器对栈对齐做了错误假设（例如Windows上的GCC）
void foo()
{
Eigen::Quaternionf q;
//...
}
GCC中编译器假设堆栈是16字节对齐，但在Windows中堆栈只能保证4字节对齐。当从另一个线程或另一个编译器编译的二进制文件调用函数时，可能会破坏堆栈对齐。

有三种方法可供参考：

1）局部

__attribute__((force_align_arg_pointer)) void foo()
{
Eigen::Quaternionf q;
//...
}
2）全局

-mincoming-stack-boundary=2
相当于告诉GCC堆栈需要2*2=4字节对齐

3）全局

-mstackrealign
相当于向所有函数添加force_align_arg_pointer

如果不想要进行向量化，可以在编译时使用EIGEN_DONT_ALIGN_STATICALLY选项禁用所有16字节静态对齐，但保持16位堆对齐。或者同时使用EIGEN_DONT_VECTORIZE和EIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT，这保持了16位的对齐代码，但不支持向量化。

2、ubuntu 16.4 的 ros kinetic 依赖的是boost158版本，使用167版本编译程序，就会报 or -rpath-link)
/usr/bin/ld: warning: libboost_thread.