坐标变换
- 位姿变换(位姿在不同坐标系中变换, 更常用):
PosesTorT,- 关键在于同一个位姿（实际位姿固定不变），但是在不同坐标系有不同的 表 示！
- 坐标系变换(坐标系本身变换), 并且
CoordsT:CoordsT = T.inverse().
注意: ROS tf 发布的变换是坐标系变换 CoordsT, 包括通过
tf2_ros static_transform_publisher, TransformBroadcaster.sendTransform()等
发布. 但是获取到的变换却是位姿变换 PosesT, 例如 Transformer.lookupTransform() !!
计算位姿变换 T
- 某个位姿在 a 坐标系中为:
Pa, e.g.(0, 0, 0) * RI,
该位姿在 b 坐标系中表示为Pb, e.g.(-1, 0, 0) * Rb,
那么 a 到 b 的位姿变换Ta2b = Pb * Pa.inverse(), e.g.(-1, 0, 0) * RT!! - 即 a 原点位姿在 b 中的位姿值, 就是 a 到 b 的位姿变换!!
计算 CoordsT
- 注意: 以同一个公共坐标系计算的是 CoordsT, e.g. 都在 a 坐标系中,
a 的原点为P1, b 的原点为P2, 那么CoordsT = P2 * P1.inverse(), and!
Ta2b = CoordsT.inverse() - 又或者都在 world 坐标系中, a 的原点为
Pw1, b 的原点为Pw2, 那么CoordsT =!
Pw2 * Pw1.inverse(), and Ta2b = CoordsT.inverse()
See also geometry: MakeTransform:
/**
* Make a transform: from @a from pose to @a to pose.
*
* If T is PosesT, then @a from usually should be (0, 0, 0) * RI,
* then @a to should be same real pose with @a from, but value is its value
* in coordinate of @a to.
*
* If T is CoordsT, then @a from pose and @a to pose should be values in the
* same coordinate, e.g. world or coordinate of @a from or coordinate of @a
* to or some other.
*
* And NOTE: CoordsT = T.inverse()!!
*
* @param from from pose to build transform.
* @param to to pose to build transform.
* @return the @a from to @a to transform (from --transform--> to).
*/
static inline Eigen::Affine3d MakeTransform(
Eigen::Affine3d const& from, Eigen::Affine3d const& to) noexcept
{
return to * from.inverse();
}
通过位姿变换 T 计算位姿
已知某个位姿在坐标系 a 中为: Pa, a 中位姿到 b 中位姿变换为 Ta2b, 那么,
Pa 在坐标系 b 中位姿 Pb = Ta2b * Pa.
已知某个位姿在坐标系 a 中为: Pa, b 中位姿到 a 中位姿变换为 Tb2a, 那么,
Pa 在坐标系 b 中位姿 Pb = Tb2a.inverse() * Pa
(Ta2b = Tb2a.inverse()).
坐标变换矩阵
r00 r01 r02 tx
r10 r11 r12 ty
r20 r21 r22 tz
0 0 0 1
仿射
表示位姿或者变换:
Affine3d = Translation3d(x, y, z) * Quaterniond(w, x, y, z)Affine2d = Translation2d(x, y) * Rotation2D<double>(angle))