从零开始手写VIO第二讲vio_data_simulation-master源码阅读
程序员文章站
2022-04-16 21:25:02
...
前言
这是高博的课程《从零开始手写VIO》里面第二讲vio_data_simulation-master的源码阅读（主要为源码注释）。一些代码比如存取文件的函数，不是重点，这里直接黑箱化了。
项目结构
主要文件(Readme.md中内容)：
main/gener_alldata.cpp : 用于生成imu数据，相机轨迹，特征点像素坐标，特征点的3d坐标
src/param.cpp：imu噪声参数，imu频率，相机内参数等等
src/camera_model.cpp：相机模型，调用的svo， 目前代码里这个文件删掉了
python_tool/：文件夹里为可视化工具，draw_points.py就是动态绘制相机轨迹和观测到的特征点。如果是ubuntu不需额外安装，windows需要安装python matplot等依赖项。
Param.h
#ifndef IMUSIM_PARAM_H
#define IMUSIM_PARAM_H
#include <eigen3/Eigen/Core>
class Param{
public:
Param();
//频率、时间间隔、开始结束时间
// time
int imu_frequency = 200;
int cam_frequency = 30;
double imu_timestep = 1./imu_frequency;
double cam_timestep = 1./cam_frequency;
double t_start = 0.;
double t_end = 20; // 20 s
//陀螺仪和加速度计的噪声方差（bias随机游走噪声方差及高斯白噪声方差）
// noise
double gyro_bias_sigma = 1.0e-5;
double acc_bias_sigma = 0.0001;
double gyro_noise_sigma = 0.015; // rad/s
double acc_noise_sigma = 0.019; // m/(s^2)
double pixel_noise = 1; // 1 pixel noise
//相机内参、图像高宽
// cam f
double fx = 460;
double fy = 460;
double cx = 255;
double cy = 255;
double image_w = 640;
double image_h = 640;
// 外参数（旋转，平移）
Eigen::Matrix3d R_bc; // cam to body
Eigen::Vector3d t_bc; // cam to body
};
#endif //IMUSIM_PARAM_H
param.cpp
#include "param.h"
Param::Param()
{
Eigen::Matrix3d R; // 把body坐标系朝向旋转一下,得到相机坐标系，好让它看到landmark, 相机坐标系的轴在body坐标系中的表示
// 相机朝着轨迹里面看， 特征点在轨迹外部， 这里我们采用这个
R << 0, 0, -1,
-1, 0, 0,
0, 1, 0;
R_bc = R;
t_bc = Eigen::Vector3d(0.05,0.04,0.03);
}
imu.h
#ifndef IMUSIMWITHPOINTLINE_IMU_H
#define IMUSIMWITHPOINTLINE_IMU_H
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Geometry>
#include <iostream>
#include <vector>
#include "param.h"
//构建结构体类型，包含时间戳、body系到world系的变换矩阵、加速度计与陀螺仪数据、bias噪声以及加速度计速度
struct MotionData
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
double timestamp;
Eigen::Matrix3d Rwb;
Eigen::Vector3d twb;
Eigen::Vector3d imu_acc;
Eigen::Vector3d imu_gyro;
Eigen::Vector3d imu_gyro_bias;
Eigen::Vector3d imu_acc_bias;
Eigen::Vector3d imu_velocity;
};
// euler2Rotation: body frame to interitail frame
Eigen::Matrix3d euler2Rotation( Eigen::Vector3d eulerAngles);
Eigen::Matrix3d eulerRates2bodyRates(Eigen::Vector3d eulerAngles);
class IMU
{
public:
IMU(Param p);
Param param_;
Eigen::Vector3d gyro_bias_;
Eigen::Vector3d acc_bias_;
Eigen::Vector3d init_velocity_;
Eigen::Vector3d init_twb_;
Eigen::Matrix3d init_Rwb_;
MotionData MotionModel(double t);
void addIMUnoise(MotionData& data);
void testImu(std::string src, std::string dist); // imu数据进行积分，用来看imu轨迹
};
#endif //IMUSIMWITHPOINTLINE_IMU_H
imu.cpp
1、Eigen::Matrix3d euler2Rotation( Eigen::Vector3d eulerAngles)
惯性系下一个点转到body系，用欧拉角表示为：
而要从body系转到惯性系，对RbI取逆得到RIb。
2、Eigen::Matrix3d eulerRates2bodyRates(Eigen::Vector3d eulerAngles)
惯性系下的欧拉角速度转换到body系
#include "imu.h"
#include "utilities.h"
// euler2Rotation:body frame to interitail frame
Eigen::Matrix3d euler2Rotation( Eigen::Vector3d eulerAngles)
{
double roll = eulerAngles(0);
double pitch = eulerAngles(1);
double yaw = eulerAngles(2);
double cr = cos(roll); double sr = sin(roll);
double cp = cos(pitch); double sp = sin(pitch);
double cy = cos(yaw); double sy = sin(yaw);
Eigen::Matrix3d RIb;
RIb<< cy*cp , cy*sp*sr - sy*cr, sy*sr + cy* cr*sp,
sy*cp, cy *cr + sy*sr*sp, sp*sy*cr - cy*sr,
-sp, cp*sr, cp*cr;
return RIb;
}
Eigen::Matrix3d eulerRates2bodyRates(Eigen::Vector3d eulerAngles)
{
double roll = eulerAngles(0);
double pitch = eulerAngles(1);
double cr = cos(roll); double sr = sin(roll);
double cp = cos(pitch); double sp = sin(pitch);
Eigen::Matrix3d R;
R<< 1, 0, -sp,
0, cr, sr*cp,
0, -sr, cr*cp;
return R;
}
//初始化，将输入参数p赋值给param_
IMU::IMU(Param p): param_(p)
{
gyro_bias_ = Eigen::Vector3d::Zero();
acc_bias_ = Eigen::Vector3d::Zero();
}
//添加IMU噪声函数，输入参数为MotionData结构体类型的地址
void IMU::addIMUnoise(MotionData& data)
{
/*
要得到一个我们最常需要的、符合一定分布规律的且随机质量较高的随机数，我们要做的是：
1定义random_device对象(rd)
2选择随机引擎（默认、线性、梅森、斐波那契）的实现类(默认)，将random_device的随机结果(rd)传入作为种子
3选择要的分布，创建分布对象，将引擎传入作为种子，让分布对象输出随机数。
*/
std::random_device rd;//产生一个种子
std::default_random_engine generator_(rd());// Create random number generator with rd() as seed
std::normal_distribution<double> noise(0.0, 1.0);//正态分布模板类，创建一个均值为0方差为1的分布对象
Eigen::Vector3d noise_gyro(noise(generator_),noise(generator_),noise(generator_));//陀螺仪噪声，这里产生的应该是3个一样随机数的向量？
Eigen::Matrix3d gyro_sqrt_cov = param_.gyro_noise_sigma * Eigen::Matrix3d::Identity();//陀螺仪方差乘以3x3单位阵，陀螺仪3个方向上的噪声
data.imu_gyro = data.imu_gyro + gyro_sqrt_cov * noise_gyro / sqrt( param_.imu_timestep ) + gyro_bias_;//给陀螺仪数据增加噪声，即陀螺仪误差模型，高斯白噪声从连续到离散差了个1/根号t，这里Sg为单位阵，gyro_sqrt_cov * noise_gyro表示陀螺方差乘以正态分布，得到以陀螺方差的高斯分布。
//同理
Eigen::Vector3d noise_acc(noise(generator_),noise(generator_),noise(generator_));
Eigen::Matrix3d acc_sqrt_cov = param_.acc_noise_sigma * Eigen::Matrix3d::Identity();
data.imu_acc = data.imu_acc + acc_sqrt_cov * noise_acc / sqrt( param_.imu_timestep ) + acc_bias_;//加速度计误差模型，这里的imu_acc已经做了减法a-g
// gyro_bias update
Eigen::Vector3d noise_gyro_bias(noise(generator_),noise(generator_),noise(generator_));
gyro_bias_ += param_.gyro_bias_sigma * sqrt(param_.imu_timestep ) * noise_gyro_bias;//bias随机游走的噪声方差从连续时间到离散时间需要乘以根号t
data.imu_gyro_bias = gyro_bias_;
// acc_bias update， 同理
Eigen::Vector3d noise_acc_bias(noise(generator_),noise(generator_),noise(generator_));
acc_bias_ += param_.acc_bias_sigma * sqrt(param_.imu_timestep ) * noise_acc_bias;
data.imu_acc_bias = acc_bias_;
}
MotionData IMU::MotionModel(double t)
{
MotionData data;
// param
float ellipse_x = 15;
float ellipse_y = 20;
float z = 1; // z轴做sin运动
float K1 = 10; // z轴的正弦频率是x，y的k1倍
float K = M_PI/ 10; // 20 * K = 2pi 由于我们采取的是时间是20s, 系数K控制yaw正好旋转一圈，运动一周
// translation
// twb: body frame in world frame
Eigen::Vector3d position( ellipse_x * cos( K * t) + 5, ellipse_y * sin( K * t) + 5, z * sin( K1 * K * t ) + 5);
Eigen::Vector3d dp(- K * ellipse_x * sin(K*t), K * ellipse_y * cos(K*t), z*K1*K * cos(K1 * K * t)); // position导数 in world frame，求一次导得速度
double K2 = K*K;
Eigen::Vector3d ddp( -K2 * ellipse_x * cos(K*t), -K2 * ellipse_y * sin(K*t), -z*K1*K1*K2 * sin(K1 * K * t)); // position二阶导数，求二次导为加速度
// Rotation
double k_roll = 0.1;
double k_pitch = 0.2;
Eigen::Vector3d eulerAngles(k_roll * cos(t) , k_pitch * sin(t) , K*t ); // roll ~ [-0.2, 0.2], pitch ~ [-0.3, 0.3], yaw ~ [0,2pi]
Eigen::Vector3d eulerAnglesRates(-k_roll * sin(t) , k_pitch * cos(t) , K); // euler angles 的导数，得欧拉角速度
// Eigen::Vector3d eulerAngles(0.0,0.0, K*t ); // roll ~ 0, pitch ~ 0, yaw ~ [0,2pi]
// Eigen::Vector3d eulerAnglesRates(0.,0. , K); // euler angles 的导数
Eigen::Matrix3d Rwb = euler2Rotation(eulerAngles); // body frame to world frame
Eigen::Vector3d imu_gyro = eulerRates2bodyRates(eulerAngles) * eulerAnglesRates; // euler rates trans to body gyro， 欧拉角速度转换为陀螺角速度
Eigen::Vector3d gn (0,0,-9.81); // gravity in navigation frame(ENU) ENU (0,0,-9.81) NED(0,0,9,81)
Eigen::Vector3d imu_acc = Rwb.transpose() * ( ddp - gn ); // Rbw * Rwn * gn = gs
data.imu_gyro = imu_gyro;
data.imu_acc = imu_acc;
data.Rwb = Rwb;
data.twb = position;
data.imu_velocity = dp;
data.timestamp = t;
return data;
}
//读取生成的imu数据并用imu动力学模型对数据进行计算，最后保存imu积分以后的轨迹，
//用来验证数据以及模型的有效性。
void IMU::testImu(std::string src, std::string dist)
{
std::vector<MotionData>imudata;
LoadPose(src,imudata);
std::ofstream save_points;
save_points.open(dist);
double dt = param_.imu_timestep;
Eigen::Vector3d Pwb = init_twb_; // position : from imu measurements
Eigen::Quaterniond Qwb(init_Rwb_); // quaterniond: from imu measurements
Eigen::Vector3d Vw = init_velocity_; // velocity : from imu measurements
Eigen::Vector3d gw(0,0,-9.81); // ENU frame
Eigen::Vector3d temp_a;
Eigen::Vector3d theta;
for (int i = 1; i < imudata.size(); ++i) {
MotionData imupose = imudata[i];
//delta_q = [1 , 1/2 * thetax , 1/2 * theta_y, 1/2 * theta_z]
Eigen::Quaterniond dq;
Eigen::Vector3d dtheta_half = imupose.imu_gyro * dt /2.0;
dq.w() = 1;
dq.x() = dtheta_half.x();
dq.y() = dtheta_half.y();
dq.z() = dtheta_half.z();
/// imu 动力学模型 欧拉积分
// Eigen::Vector3d acc_w = Qwb * (imupose.imu_acc) + gw; // aw = Rwb * ( acc_body - acc_bias ) + gw
// Qwb = Qwb * dq;
// Vw = Vw + acc_w * dt;
// Pwb = Pwb + Vw * dt + 0.5 * dt * dt * acc_w;
/// 中值积分 （课程PPT公式）
Eigen::Vector3d acc_w = 1/2 * (Qwb * imudata[i-1].imu_acc + gw * Qwb * (imupose.imu_acc) + gw);
Qwb = Qwb * dq;
Vw = Vw + acc_w * dt;
Pwb = Pwb + Vw * dt + 0.5 * dt * dt * acc_w;
// 按着imu postion, imu quaternion , cam postion, cam quaternion 的格式存储，由于没有cam，所以imu存了两次
save_points<<imupose.timestamp<<" "
<<Qwb.w()<<" "
<<Qwb.x()<<" "
<<Qwb.y()<<" "
<<Qwb.z()<<" "
<<Pwb(0)<<" "
<<Pwb(1)<<" "
<<Pwb(2)<<" "
<<Qwb.w()<<" "
<<Qwb.x()<<" "
<<Qwb.y()<<" "
<<Qwb.z()<<" "
<<Pwb(0)<<" "
<<Pwb(1)<<" "
<<Pwb(2)<<" "
<<std::endl;
}
std::cout<<"test end"<<std::endl;
}
utilities.h
里面是用于存取文件的函数，这里直接黑箱处理。
在这里插入代码片
上一篇： 手写VIO作业（一）
下一篇： 乐视视频真实地址解析