手写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;
	cout << "q_update = " << q_update.w() << " " << q_update.x() << " " << q_update.y() << " " << q_update.z() << endl;
	cout << "q_R = " << q_R.w() << " " << q_R.x() << " " << q_R.y() << " " << q_R.z() << endl;
 
	return 0;
}

这里有点疑惑，为什么有些算出来会差个负号

 第三题：右扰动求导