我有以下代码:
Eigen::Matrix4f transformation = myMethod(); // homogenous transformation matrix
std::cout << "transformation = " << std::endl << transformation << std::endl << std::endl;
Eigen::Isometry3f estimate = Eigen::Isometry3f(transformation);
r = estimate.rotation(); // rotation matrix
t = estimate.translation(); // translation matrix
std::cout<<"r = " << std::endl << r << std::endl << std::endl;
std::cout<<"t = " << std::endl << t << std::endl << std::endl;
打印:
transformation =
1.0000002384185791 4.9265406687482027e-07 -4.5500800638365035e-07 -2.384185791015625e-07
-5.2062489430682035e-07 1.0000003576278687 5.4357258250092855e-07 -3.5762786865234375e-07
4.9866633844430908e-07 -4.6970637868071208e-07 1.0000002384185791 -2.384185791015625e-07
0 0 0 1
r =
1.0000001192092896 7.2572328235764871e-07 -3.9811814644963306e-07
-6.7004600623477018e-07 0.99999994039535522 4.5586514829665248e-07
5.422648428066168e-07 -4.4537293319990567e-07 0.99999994039535522
t =
-2.384185791015625e-07
-3.5762786865234375e-07
-2.384185791015625e-07
在这里,我试图从齐次变换矩阵中获取旋转和平移矩阵。我猜平移矩阵是齐次矩阵最后一列的前三个元素,旋转矩阵是左上角的 3x3 矩阵。上面正确返回了翻译矩阵。但旋转矩阵并不完全是齐次矩阵的左上角 3x3 矩阵。
我想我在这里缺少一些基本概念。做错了什么?
Transform
的文档描述了不同的存储模式:
inverse()
和 rotation()
。这实际上发生了,
Isometry
只是返回存储在构造函数中的内容:
template <typename Scalar, int Dim, int Mode, int Options>
EIGEN_DEVICE_FUNC typename Transform<Scalar, Dim, Mode, Options>::RotationReturnType
Transform<Scalar, Dim, Mode, Options>::rotation() const {
return internal::transform_rotation_impl<Mode>::run(*this);
}
template <int Mode>
struct transform_rotation_impl {
template <typename TransformType>
EIGEN_DEVICE_FUNC static inline const typename TransformType::LinearMatrixType run(const TransformType& t) {
typedef typename TransformType::LinearMatrixType LinearMatrixType;
LinearMatrixType result;
t.computeRotationScaling(&result, (LinearMatrixType*)0);
return result;
}
};
template <>
struct transform_rotation_impl<Isometry> {
template <typename TransformType>
EIGEN_DEVICE_FUNC static inline typename TransformType::ConstLinearPart run(const TransformType& t) {
return t.linear(); <===================== here
}
};
但是,这是版本v3.4。正如@chtz在评论中指出的,这个描述和优化本身并不存在于v3.3中,
rotation
不使用“总机”,而是直接调用固定函数:
template<typename Scalar, int Dim, int Mode, int Options>
EIGEN_DEVICE_FUNC const typename Transform<Scalar,Dim,Mode,Options>::LinearMatrixType
Transform<Scalar,Dim,Mode,Options>::rotation() const
{
LinearMatrixType result;
computeRotationScaling(&result, (LinearMatrixType*)0);
return result;
}
computeRotationScaling
(链接方法正下方的方法)然后计算 SVD,这就是可能引入数值误差的地方:
template<typename Scalar, int Dim, int Mode, int Options>
template<typename RotationMatrixType, typename ScalingMatrixType>
EIGEN_DEVICE_FUNC void Transform<Scalar,Dim,Mode,Options>::computeRotationScaling(RotationMatrixType *rotation, ScalingMatrixType *scaling) const
{
JacobiSVD<LinearMatrixType> svd(linear(), ComputeFullU | ComputeFullV);
Scalar x = (svd.matrixU() * svd.matrixV().adjoint()).determinant(); // so x has absolute value 1
VectorType sv(svd.singularValues());
sv.coeffRef(0) *= x;
if(scaling) scaling->lazyAssign(svd.matrixV() * sv.asDiagonal() * svd.matrixV().adjoint());
if(rotation)
{
LinearMatrixType m(svd.matrixU());
m.col(0) /= x;
rotation->lazyAssign(m * svd.matrixV().adjoint());
}
}
v3.3 中的 Isometry
实际上是一个 Affine
,其中某些部分被关闭(例如 scale
用断言阻止你,还有更多)。
我想我在这里缺少一些基本概念。做错了什么?
看来您正在使用旧版本的库。考虑尝试使用 v3.4 的代码。