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

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