文章目錄

前言

分享一些自己學習+實踐 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.

---

總結

1、基礎矩陣、單一性矩陣、本征矩陣為什么能計算出相機的旋轉和平移；其原理是什么
2、相機旋轉的時候為什么能用IMU進行測量其旋轉的角度