文章目錄

• 目錄

  文章目錄

  前言

  一、VIO系統(tǒng)數(shù)據(jù)的獲取：圖像傳感器選型、IMU傳感器選型

  二、建立linux系統(tǒng)+ROS環(huán)境

  三、如何讀取圖像數(shù)據(jù)

  四、如何讀取IMU傳感器數(shù)據(jù)

  五、標(biāo)定圖像和IMU數(shù)據(jù)的外參、相機(jī)的內(nèi)參

  六、移植VINS-MONO或者VINS-FUSION：主要調(diào)試獲取圖像和imu的訂閱數(shù)據(jù)

  七、實(shí)踐VIO中遇到的問題

  總結(jié)

  ---

前言

分享一些自己學(xué)習(xí)+實(shí)踐 VIO視覺融合慣導(dǎo)的定位算法系統(tǒng)工程化過程

一、VIO系統(tǒng)數(shù)據(jù)的獲取：圖像傳感器選型、IMU傳感器選型

二、建立linux系統(tǒng)+ROS環(huán)境

三、如何讀取圖像數(shù)據(jù)

四、如何讀取IMU傳感器數(shù)據(jù)

五、標(biāo)定圖像和IMU數(shù)據(jù)的外參、相機(jī)的內(nèi)參

六、移植VINS-MONO或者VINS-FUSION：主要調(diào)試獲取圖像和imu的訂閱數(shù)據(jù)

七、實(shí)踐VIO中遇到的問題

1、在運(yùn)行vins-mono中發(fā)現(xiàn)，Eigen報(bào)錯(cuò)，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加速，在內(nèi)存上分配了128位的指針，涉及字節(jié)對(duì)齊問題，該問題在編譯時(shí)不會(huì)報(bào)錯(cuò)，只在運(yùn)行時(shí)報(bào)錯(cuò)。

原因1：類中有Eigen類型的成員變量
class Foo
{
? //...
? Eigen::Vector2d v;
? //...
};
//...
Foo *foo = new Foo;
?在生成定長(zhǎng)的Matrix或Vector對(duì)象時(shí)，需要開辟內(nèi)存，調(diào)用默認(rèn)構(gòu)造函數(shù)，通常x86下的指針是32位，內(nèi)存位數(shù)沒對(duì)齊就會(huì)導(dǎo)致程序運(yùn)行出錯(cuò)。而對(duì)于動(dòng)態(tài)變量(例如Eigen::VectorXd)會(huì)動(dòng)態(tài)分配內(nèi)存，因此會(huì)自動(dòng)地進(jìn)行內(nèi)存對(duì)齊。

解決方法：在public下寫一個(gè)宏EIGEN_MAKE_ALIGNED_OPERATOR_NEW

class Foo
{
? ...
? Eigen::Vector2d v;
? ...
public:
? EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
...
Foo *foo = new Foo;
這個(gè)宏在new一個(gè)對(duì)象時(shí)會(huì)總是返回一個(gè)對(duì)齊的指針。

如果想要通過模板參數(shù)來定義一個(gè)定長(zhǎng)變量，可以使用宏EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)。若NeedsToAlign是True，則進(jìn)行對(duì)齊運(yùn)算；若NeedsToAlign為False，則采用默認(rèn)的對(duì)齊方式。

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)禁用對(duì)齊

class Foo
{
? ...
? Eigen::Matrix<double,2,1,Eigen::DontAlign> v;
? ...
};
這種方法在將變量賦值到一個(gè)臨時(shí)的對(duì)齊向量時(shí)，仍然有可能重新啟用矢量化。

void Foo::bar()
{
? Eigen::Vector2d av(v);
? // use av instead of v
? ...
? // if av changed, then do:
? v = av;
}
2)私有結(jié)構(gòu)體

將定長(zhǎng)的對(duì)象存儲(chǔ)到私有結(jié)構(gòu)體中，在構(gòu)建主對(duì)象時(shí)固定分配內(nèi)存。這里的明顯優(yōu)勢(shì)是類Foo在對(duì)齊問題上保持不變。缺點(diǎn)是無論如何都需要堆分配。

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數(shù)據(jù)結(jié)構(gòu)
使用Eigen::aligned_allocator告訴容器進(jìn)行16字節(jié)對(duì)齊。

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：通過值傳遞定長(zhǎng)Eigen對(duì)象
void func(Eigen::Vector4d v);
在c++中使用值傳遞都是一個(gè)很糟糕的選項(xiàng)，意味著很多無用拷貝，這兒還會(huì)造成程序崩潰。通常采用引用傳遞：

void my_function(const Eigen::Vector2d& v);
?原因4：編譯器對(duì)棧對(duì)齊做了錯(cuò)誤假設(shè)（例如Windows上的GCC）
void foo()
{
? Eigen::Quaternionf q;
? //...
}
?GCC中編譯器假設(shè)堆棧是16字節(jié)對(duì)齊，但在Windows中堆棧只能保證4字節(jié)對(duì)齊。當(dāng)從另一個(gè)線程或另一個(gè)編譯器編譯的二進(jìn)制文件調(diào)用函數(shù)時(shí)，可能會(huì)破壞堆棧對(duì)齊。

有三種方法可供參考：

1）局部

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

-mincoming-stack-boundary=2
相當(dāng)于告訴GCC堆棧需要2*2=4字節(jié)對(duì)齊

3） 全局

-mstackrealign
相當(dāng)于向所有函數(shù)添加force_align_arg_pointer

如果不想要進(jìn)行向量化，可以在編譯時(shí)使用EIGEN_DONT_ALIGN_STATICALLY選項(xiàng)禁用所有16字節(jié)靜態(tài)對(duì)齊，但保持16位堆對(duì)齊。或者同時(shí)使用EIGEN_DONT_VECTORIZE和EIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT，這保持了16位的對(duì)齊代碼，但不支持向量化。
?

2、ubuntu 16.4 的 ros kinetic 依賴的是boost158版本，使用167版本編譯程序，就會(huì)報(bào)?or -rpath-link)
/usr/bin/ld: warning: libboost_thread.

---

總結(jié)

1、基礎(chǔ)矩陣、單一性矩陣、本征矩陣為什么能計(jì)算出相機(jī)的旋轉(zhuǎn)和平移；其原理是什么
2、相機(jī)旋轉(zhuǎn)的時(shí)候?yàn)槭裁茨苡肐MU進(jìn)行測(cè)量其旋轉(zhuǎn)的角度

總結(jié)

以上是生活随笔為你收集整理的VIO-slam 系统构建的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。