OpenVINS 代码解读(1) - 数据类型Types

OpenVINS是黄国权老师团队的开源VISLAM，其基于MSCKF框架，加入了SLAM的局部地图提升前端VIO定位精度的思路值得大家学习。这里将陆续梳理和分享源代码的解读，因为时间有限难免有误，欢迎大家指正。

目录

0.class Type

1. 位姿四元素 class JPLQuat : public Type

2. 3维向量 class Vec : public Type （平移，ba，bg，velocity）

3. 位姿状态，class PoseJPL : public Type

4. IMU状态 class IMU : public Type

5. SLAM的路标点，class Landmark : public Vec

 

正文

可见，PoseJPL中 定义了两个基本类型的JPLQuat 和Vec，PoseJPL的对象调用set_value和set_fej的时候，这两个基本类型会分别设置父类Type中定义的两个变量 _value和_fej。 同理，IMU中定义了四个基本类型PoseJPL和Vec，IMU的对象调用set_value和set_fej的时候，这四个基本类型会分别设置父类Type中定义的两个变量 _value和_fej

 

父类——

0.class Type

/// First-estimate

Eigen::MatrixXd _fej;

/// Current best estimate

Eigen::MatrixXd _value;

virtual void set_local_id(int new_id) {

virtual void set_value(const Eigen::MatrixXd new_value) {

virtual void set_fej(const Eigen::MatrixXd new_value) {

virtual void update(const Eigen::VectorXd dx) = 0;

 

子类——

1. 位姿四元素 class JPLQuat : public Type

定义参考：

http://mars.cs.umn.edu/tr/reports/Trawny05b.pdf

ftp://naif.jpl.nasa.gov/pub/naif/misc/Quaternion_White_Paper/Quaternions_White_Paper.pdf

大小为3 JPLQuat() : Type(3)

function 1 存储位姿的四元素

@brief Sets the value of the estimate and recomputes the internal rotation matrixparam new_value New value for the quaternion estimate

void set_value(const Eigen::MatrixXd new_value) override {

assert(new_value.rows() == 4);

assert(new_value.cols() == 1);

_value = new_value;

//compute associated rotation

_R = quat_2_Rot(new_value);

}

function 2 存储FEJ

@brief Sets the fej value and recomputes the fej rotation matrix param new_value New value for the quaternion estimate

void set_fej(const Eigen::MatrixXd new_value) override {

assert(new_value.rows() == 4);

assert(new_value.cols() == 1);

_fej = new_value;

//compute associated rotation

_Rfej = quat_2_Rot(new_value);

}

function 3 更新位姿状态

@brief Implements update operation by left-multiplying the current quaternion with a quaternion built from a small axis-angle perturbation.

所以

@param dx Axis-angle representation of the perturbing quaternion

void update(const Eigen::VectorXd dx) override {

assert(dx.rows() == _size);

//Build perturbing quaternion

Eigen::Matrix<double, 4, 1> dq;

dq << .5 * dx, 1.0;

dq = quatnorm(dq); // 归一化

//Update estimate and recompute R

set_value(quat_multiply(dq, _value)); // dx 扰动

}

 

其中四元素的乘法：

inline Eigen::Matrix<double, 4, 1> quat_multiply(const Eigen::Matrix<double, 4, 1> &q, const Eigen::Matrix<double, 4, 1> &p) {

Eigen::Matrix<double, 4, 1> q_t;

Eigen::Matrix<double, 4, 4> Qm;

// create big L matrix

Qm.block(0, 0, 3, 3) = q(3, 0) * Eigen::MatrixXd::Identity(3, 3) - skew_x(q.block(0, 0, 3, 1));

Qm.block(0, 3, 3, 1) = q.block(0, 0, 3, 1);

Qm.block(3, 0, 1, 3) = -q.block(0, 0, 3, 1).transpose();

Qm(3, 3) = q(3, 0);

q_t = Qm * p;

// ensure unique by forcing q_4 to be >0

if (q_t(3, 0) < 0) {

q_t *= -1;

}

// normalize and return

return q_t / q_t.norm();

}

 

2. 3维向量 class Vec : public Type （平移，ba，bg，velocity）

大小不定，可调整Vec(int dim) : Type(dim)

function 1 设置 set_value和 set_fej 都是调用父类Type 的虚函数函数

* @brief Overwrite value of state's estimate

* @param new_value New value that will overwrite state's value

virtual void set_value(const Eigen::MatrixXd new_value) {

assert(_value.rows()==new_value.rows());

assert(_value.cols()==new_value.cols());

_value = new_value;

}

/**

* @brief Overwrite value of first-estimate

* @param new_value New value that will overwrite state's fej

*/

virtual void set_fej(const Eigen::MatrixXd new_value) {

assert(_fej.rows()==new_value.rows());

assert(_fej.cols()==new_value.cols());

_fej = new_value;

}

function 2 更新状态

* @brief Implements the update operation through standard vector addition

* @param dx Additive error state correction

void update(const Eigen::VectorXd dx) override {

assert(dx.rows() == _size);

set_value(_value + dx);

}

 

3. 位姿状态，class PoseJPL : public Type

大小为6 PoseJPL() : Type(6)

function 1 初始化 位姿

PoseJPL() : Type(6) {

//Initialize subvariables

_q = new JPLQuat();

_p = new Vec(3);

Eigen::Matrix<double, 7, 1> pose0;

pose0.setZero();

pose0(3) = 1.0;

set_value(pose0);

set_fej(pose0);

}

function 2 设置位姿

* @brief Sets the value of the estimate

* @param new_value New value we should set

*/

void set_value(const Eigen::MatrixXd new_value) override {

assert(new_value.rows() == 7);

assert(new_value.cols() == 1);

//Set orientation value

_q->set_value(new_value.block(0, 0, 4, 1));

//Set position value

_p->set_value(new_value.block(4, 0, 3, 1));

_value = new_value;

}

function 3 设置位姿 FEJ

/**

* @brief Sets the value of the first estimate

* @param new_value New value we should set

*/

void set_fej(const Eigen::MatrixXd new_value) override {

assert(new_value.rows() == 7);

assert(new_value.cols() == 1);

//Set orientation fej value

_q->set_fej(new_value.block(0, 0, 4, 1));

//Set position fej value

_p->set_fej(new_value.block(4, 0, 3, 1));

_fej = new_value;

}

function 4 更新位姿

/**

* @brief Update q and p using a the JPLQuat update for orientation and vector update for position

* @param dx Correction vector (orientation then position)

void update(const Eigen::VectorXd dx) override {

assert(dx.rows() == _size);

Eigen::Matrix<double, 7, 1> newX = _value;

Eigen::Matrix<double, 4, 1> dq;

dq << .5 * dx.block(0, 0, 3, 1), 1.0;

dq = quatnorm(dq);

//Update orientation

newX.block(0, 0, 4, 1) = quat_multiply(dq, quat());

//Update position

newX.block(4, 0, 3, 1) += dx.block(3, 0, 3, 1);

set_value(newX);

}

4. IMU状态 class IMU : public Type

大小 15 IMU() : Type(15)

function 1 初始化 位姿，速度v，ba和bg

IMU() : Type(15) {

_pose = new PoseJPL();

_v = new Vec(3);

_bg = new Vec(3);

_ba = new Vec(3);

Eigen::Matrix<double, 16, 1> imu0;

imu0.setZero();

imu0(3) = 1.0;

set_value(imu0);

set_fej(imu0);

}

function 2 设置位姿

* @brief Sets the value of the estimate

* @param new_value New value we should set

*/

void set_value(const Eigen::MatrixXd new_value) override {

assert(new_value.rows() == 16);

assert(new_value.cols() == 1);

_pose->set_value(new_value.block(0, 0, 7, 1));

_v->set_value(new_value.block(7, 0, 3, 1));

_bg->set_value(new_value.block(10, 0, 3, 1));

_ba->set_value(new_value.block(13, 0, 3, 1));

_value = new_value;

}

function 3 设置位姿 FEJ

* @brief Sets the value of the first estimate

* @param new_value New value we should set

*/

void set_fej(const Eigen::MatrixXd new_value) override {

assert(new_value.rows() == 16);

assert(new_value.cols() == 1);

_pose->set_fej(new_value.block(0, 0, 7, 1));

_v->set_fej(new_value.block(7, 0, 3, 1));

_bg->set_fej(new_value.block(10, 0, 3, 1));

_ba->set_fej(new_value.block(13, 0, 3, 1));

_fej = new_value;

}

function 4 更新位姿

* @brief Performs update operation using JPLQuat update for orientation, then vector updates for position, velocity, gyro bias, and accel bias (in that order).

* @param dx 15 DOF vector encoding update using the following order (q, p, v, bg, ba)

 

void update(const Eigen::VectorXd dx) override {

assert(dx.rows() == _size);

Eigen::Matrix<double, 16, 1> newX = _value;

Eigen::Matrix<double, 4, 1> dq;

dq << .5 * dx.block(0, 0, 3, 1), 1.0;

dq = quatnorm(dq);

 

newX.block(0, 0, 4, 1) = quat_multiply(dq, quat());

newX.block(4, 0, 3, 1) += dx.block(3, 0, 3, 1);

newX.block(7, 0, 3, 1) += dx.block(6, 0, 3, 1);

newX.block(10, 0, 3, 1) += dx.block(9, 0, 3, 1);

newX.block(13, 0, 3, 1) += dx.block(12, 0, 3, 1);

set_value(newX);

}

5. SLAM的路标点，class Landmark : public Vec

* @brief Will return the position of the feature in the global frame of reference.

* @param getfej Set to true to get the landmark FEJ value

* @return Position of feature either in global or anchor frame

*/

Eigen::Matrix<double,3,1> get_xyz(bool getfej);

 

* @brief Will set the current value based on the representation.

* @param p_FinG Position of the feature either in global or anchor frame

* @param isfej Set to true to set the landmark FEJ value

*/

void set_from_xyz(Eigen::Matrix<double,3,1> p_FinG, bool isfej);