手写VIO（一）

第二题：四元数和李代数更新的区别

#include <iostream>
#include <Eigen/Dense>
#include <unsupported/Eigen/MatrixFunctions>

using namespace std;

Eigen::Matrix3d skew_symmetric(Eigen::Vector3d &w)
{
	Eigen::Matrix3d mat;
	mat << 0, -w.z(), w.y(), w.z(), 0, -w.x(), -w.y(), w.x(), 0;
	return mat;
}

int main() {
	Eigen::Vector4d q = Eigen::Vector4d::Random();//生成随机四元数
	q.normalize();//归一化
	Eigen::Quaterniond q_init(q[0], q[1], q[2], q[3]);
	Eigen::Matrix3d R_init = q_init.toRotationMatrix();//将四元数变为旋转矩阵
	Eigen::Vector3d w(0.01, 0.02, 0.03);
	Eigen::Quaterniond q_delta(1, 0.5 * w.x(), 0.5 * w.y(), 0.5 * w.z());
	Eigen::Quaterniond q_update= q_init * q_delta;
	q_update.normalize();
	Eigen::Matrix3d R_update = R_init * skew_symmetric(w).exp();
	Eigen::Quaterniond q_R(R_update);

	cout << "q_init = " << q_init.w() << " " << q_init.x() << " " << q_init.y() << " " << q_init.z() << endl;
	c