在ceres中使用eigen quaternion作为优化参数

最新推荐文章于 2024-04-25 11:27:18 发布

原创最新推荐文章于 2024-04-25 11:27:18 发布 · 1k 阅读

1 ·

CC 4.0 BY-SA版权

文章标签：

#程序人生

SLAM 专栏收录该内容

38 篇文章

订阅专栏

本文介绍如何在Ceres Solver中利用Eigen库定义四元数并进行姿态估计。通过构建四元数误差模型，实现对传感器数据的优化处理。具体展示了如何使用模板函数实现残差计算。

ceres中支持定义Eigen::Vector<T, n, 1> 这种类型，因此只需要将四元数的初始值(x0, y0, z0, w0)传进来，然后使用Eigen::Quaternion<T, 4, 1> 构造一个quaternion，然后后续的操作就是跟旋转矩阵一样了。

template <typename T>
	bool operator()(const T *q, const T *t, T *residual) const
	{
		Eigen::Matrix<T, 3, 1> cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};
		Eigen::Quaternion<T> q_last_curr{q[3], q[0], q[1], q[2]};
		Eigen::Matrix<T, 3, 1> t_last_curr{T(s) * t[0], T(s) * t[1], T(s) * t[2]};
		lp = q_last_curr * cp + t_last_curr;
		residual[0] = nu.x() / de.norm();
		residual[1] = nu.y() / de.norm();
		residual[2] = nu.z() / de.norm();
		return true;
	}

参考：lidar factor