pose transform

3D pose

Eigen transform

核心函数：

• inverse
• pose3 * pose3
• pose3 * vector3

#pragma once

#include <math.h>
#include <string>
#include <Eigen/Eigen>

template <typename FloatType>
class Rigid3 {
 public:
  using Vector = Eigen::Matrix<FloatType, 3, 1>;
  using Quaternion = Eigen::Quaternion<FloatType>;
  using AngleAxis = Eigen::AngleAxis<FloatType>;

  Rigid3() : translation_(Vector::Zero()), rotation_(Quaternion::Identity()) {}
  Rigid3(const Vector& translation, const Quaternion& rotation)
      : translation_(translation), rotation_(rotation) {}
  Rigid3(const Vector& translation, const AngleAxis& rotation)
      : translation_(translation), rotation_(rotation) {}

  static Rigid3 Rotation(const AngleAxis& angle_axis) {
    return Rigid3(Vector::Zero(), Quaternion(angle_axis));
  }

  static Rigid3 Rotation(const Quaternion& rotation) {
    return Rigid3(Vector::Zero(), rotation);
  }

  static Rigid3 Translation(const Vector& vector) {
    return Rigid3(vector, Quaternion::Identity());
  }

  static Rigid3 FromArrays(const std::array<FloatType, 4>& rotation,
                           const std::array<FloatType, 3>& translation) {
    return Rigid3(Eigen::Map<const Vector>(translation.data()),
                  Eigen::Quaternion<FloatType>(rotation[0], rotation[1],
                                               rotation[2], rotation[3]));
  }

  static Rigid3<FloatType> Identity() { return Rigid3<FloatType>(); }

  template <typename OtherType>
  Rigid3<OtherType> cast() const {
    return Rigid3<OtherType>(translation_.template cast<OtherType>(),
                             rotation_.template cast<OtherType>());
  }

  const Vector& translation() const { return translation_; }
  const Quaternion& rotation() const { return rotation_; }


  ///< pose3 inverse 
  Rigid3 inverse() const {
    const Quaternion rotation = rotation_.conjugate();
    const Vector translation = -(rotation * translation_);
    return Rigid3(translation, rotation);
  }

  std::string DebugString() const {

    std::stringstream str;
    str << "{ t: [" << translation().x() <<", "<<translation().y()<<", "<<translation().z()<<
           "], r: ["<<rotation().w()<<", "<<rotation().x()<<", "<<rotation().y()<<", "<<rotation().z()<<"] }";
    return str.str();
  }

  bool IsValid() const {
    return !std::isnan(translation_.x()) && !std::isnan(translation_.y()) &&
           !std::isnan(translation_.z()) &&
           std::abs(FloatType(1) - rotation_.norm()) < FloatType(1e-3);
  }

 private:
  Vector translation_;
  Quaternion rotation_;
};

// pose3 * pose3
template <typename FloatType>
Rigid3<FloatType> operator*(const Rigid3<FloatType>& lhs,
                            const Rigid3<FloatType>& rhs) {
  return Rigid3<FloatType>(
      lhs.rotation() * rhs.translation() + lhs.translation(),
      (lhs.rotation() * rhs.rotation()).normalized());
}

///< pose3 * vector3
template <typename FloatType>
typename Rigid3<FloatType>::Vector operator*(
    const Rigid3<FloatType>& rigid,
    const typename Rigid3<FloatType>::Vector& point) {
  return rigid.rotation() * point + rigid.translation();
}

// This is needed for gmock.
template <typename T>
std::ostream& operator<<(std::ostream& os,
                         const Rigid3<T>& rigid) {
  os << rigid.DebugString();
  return os;
}

using Rigid3d = Rigid3<double>;
using Rigid3f = Rigid3<float>;

C++ transform

核心函数：

• inverse
• pose3 * pose3
• pose3 * vector3

#pragma once

#include <math.h>
#include <string>

namespace Base {

template <typename Scalar>
class Vector
{
public:
  Vector(const Scalar& x,const Scalar& y,const Scalar& z):
    _x(x),_y(y),_z(z){
  }
  Vector(): _x(Scalar(0)),_y(Scalar(0)),_z(Scalar(0)){}
  Vector(const Vector& other) {
    _x = other.x();
    _y = other.y();
    _z = other.z();
  }


  inline Scalar x() const {return this->_x;}
  inline Scalar y() const {return this->_y;}
  inline Scalar z() const {return this->_z;}

  inline Scalar& x()  {
    return this->_x;}
  inline Scalar& y()  {return this->_y;}
  inline Scalar& z()  {return this->_z;}

  Vector<Scalar> operator=(const Vector<Scalar>& other) {
    return Base::Vector<Scalar>(other.x(),other.y(),other.z());
  }

  inline Vector<Scalar> operator+(const Vector<Scalar>& other) {
    return Base::Vector<Scalar>(other.x() + _x,other.y() + _y,other.z()+ _z);
  }

  inline Vector<Scalar> operator-() {
    return Base::Vector<Scalar>(-_x,-_y,-_z);
  }

  static inline Vector<Scalar> Zero() { return Vector<Scalar>(Scalar(0), Scalar(0), Scalar(0)); }

  template<typename NewScalarType>
  inline Vector<NewScalarType> cast() const
  {
    return Base::Vector<NewScalarType>(_x,_y,_z);
  }

private:
  Scalar _x;
  Scalar _y;
  Scalar _z;
};


template <typename Scalar>
class Quaternion
{
public:
  Quaternion(const Scalar& w,const Scalar& x,const Scalar& y,const Scalar& z):
    _w(w),_x(x),_y(y),_z(z){}
  Quaternion(): _w(Scalar(1)),_x(Scalar(0)),_y(Scalar(0)),_z(Scalar(0)){}
  Quaternion(const Quaternion& other) {
    _w = other.w();
    _x = other.x();
    _y = other.y();
    _z = other.z();
  }

  inline Scalar w() const {return _w;}
  inline Scalar x() const {return _x;}
  inline Scalar y() const {return _y;}
  inline Scalar z() const {return _z;}

  inline Scalar& w()  {return this->_w;}
  inline Scalar& x()  {return this->_x;}
  inline Scalar& y()  {return this->_y;}
  inline Scalar& z()  {return this->_z;}

  Quaternion<Scalar> operator=(const Quaternion<Scalar>& other) {
    return Base::Quaternion<Scalar>(other.w(),other.x(),other.y(),other.z());
  }

  Quaternion<Scalar> operator*(const Quaternion<Scalar>& other) const {

    Quaternion<Scalar> result;

    result.w() = _w * other.w() - _x * other.x() - _y * other.y() - _z * other.z();
    result.x() = _w * other.x() + _x * other.w() + _y * other.z() - _z * other.y();
    result.y() = _w * other.y() - _x * other.z() + _y * other.w() + _z * other.x();
    result.z() = _w * other.z() + _x * other.y() - _y * other.x() + _z * other.w();

    return result;
  }

  static inline Quaternion<Scalar> Identity() { return Quaternion<Scalar>(Scalar(1),Scalar(0), Scalar(0), Scalar(0)); }

//  四元数   Q(p,v)   v =(x,y,z)共轭  即为： Q*(p,-v);轴和 四元数  是反向的
//  四元数   Q(p,v)   v =(x,y,z)逆为：  Q*/四元数长度
//  注：四元数的逆就是  与其相乘 为1  ，这样就很明显了，  Q*Q*  =q^2  +V^2 ,
//  而四元数长度  即为  q^2 +v^2   这样相除就是1,达到了逆的效果

  inline Quaternion inverse() const {

    Scalar normSq = _w*_w + _x*_x + _y*_y + _z*_z;

    if(normSq > Scalar(0)) {
      return Base::Quaternion<Scalar>(_w/normSq,-_x/normSq,-_y/normSq,-_z/normSq);
    } else {
      return Quaternion::Identity();
    }
  }

  inline Quaternion conjugate() const {
     return Quaternion(_w,-_x,-_y,-_z);
  }
  inline Scalar norm() const {
    Scalar normSq = _w*_w + _x*_x + _y*_y + _z*_z;
    return Scalar(std::sqrt(normSq));
  }

  inline Quaternion normalized() const {
    Scalar norm = this->norm();
    if(norm > Scalar(0)) {
      return Quaternion(_w/norm,_x/norm,_y/norm,_z/norm);
    }
    return *this;
  }

  Scalar angle() {
      //四元数转角度
      return Scalar(atan2(2.0 * (_w*_z + _x*_y), 1. - 2. * (_y*_y + _z*_z)));
  }
  template<typename NewScalarType>
  inline Quaternion<NewScalarType> cast() const
  {
    return Quaternion<NewScalarType>(_w,_x,_y,_z);
  }

private:
  Scalar _w;
  Scalar _x;
  Scalar _y;
  Scalar _z;
};

template <typename Scalar>
Vector<Scalar> operator*(const Quaternion<Scalar>& p,const Vector<Scalar>& v) {

  Scalar dcm[3][3];
  const Scalar a = p.w();
  const Scalar b = p.x();
  const Scalar c = p.y();
  const Scalar d = p.z();
  const Scalar aa = a * a;
  const Scalar ab = a * b;
  const Scalar ac = a * c;
  const Scalar ad = a * d;
  const Scalar bb = b * b;
  const Scalar bc = b * c;
  const Scalar bd = b * d;
  const Scalar cc = c * c;
  const Scalar cd = c * d;
  const Scalar dd = d * d;
  dcm[0][0] = aa + bb - cc - dd;
  dcm[0][1] = 2 * (bc - ad);
  dcm[0][2] = 2 * (ac + bd);
  dcm[1][0] = 2 * (bc + ad);
  dcm[1][1] = aa - bb + cc - dd;
  dcm[1][2] = 2 * (cd - ab);
  dcm[2][0] = 2 * (bd - ac);
  dcm[2][1] = 2 * (ab + cd);
  dcm[2][2] = aa - bb - cc + dd;

  Vector<Scalar> result;

  result.x() = dcm[0][0]*v.x() + dcm[0][1]*v.y() + dcm[0][2]*v.z();
  result.y() = dcm[1][0]*v.x() + dcm[1][1]*v.y() + dcm[1][2]*v.z();
  result.z() = dcm[2][0]*v.x() + dcm[2][1]*v.y() + dcm[2][2]*v.z();

  return result;
}
}

template <typename Scalar>
class Rigid3 {
 public:
  using Vector = Base::Vector<Scalar>;
  using Quaternion = Base::Quaternion<Scalar>;

  Rigid3() : translation_(Vector::Zero()), rotation_(Quaternion::Identity()) {}
  Rigid3(const Vector& translation, const Quaternion& rotation)
      : translation_(translation), rotation_(rotation) {}

  static Rigid3 Rotation(const Quaternion& rotation) {
    return Rigid3(Vector::Zero(), rotation);
  }

  static Rigid3 Translation(const Vector& vector) {
    return Rigid3(vector, Quaternion::Identity());
  }

  static Rigid3<Scalar> Identity() { return Rigid3<Scalar>(); }

  template <typename OtherType>
  Rigid3<OtherType> cast() const {
    return Rigid3<OtherType>(translation_.template cast<OtherType>(),
                             rotation_.template cast<OtherType>());
  }

  const Vector& translation() const { return translation_; }
  const Quaternion& rotation() const { return rotation_; }


  ///< pose3 inverse
  Rigid3 inverse() const {
    const Quaternion rotation = rotation_.conjugate();
    const Vector translation = -(rotation * translation_);
    return Rigid3(translation, rotation);
  }

  std::string DebugString() const {

    std::stringstream str;
    str << "{ t: [" << translation().x() <<", "<<translation().y()<<", "<<translation().z()<<
           "], r: ["<<rotation().w()<<", "<<rotation().x()<<", "<<rotation().y()<<", "<<rotation().z()<<"] }";
    return str.str();
  }

  bool IsValid() const {
    return !std::isnan(translation_.x()) && !std::isnan(translation_.y()) &&
           !std::isnan(translation_.z()) &&
           std::abs(Scalar(1) - rotation_.norm()) < Scalar(1e-3);
  }

 private:
  Vector translation_;
  Quaternion rotation_;
};

// pose3 * pose3
template <typename Scalar>
Rigid3<Scalar> operator*(const Rigid3<Scalar>& lhs,
                            const Rigid3<Scalar>& rhs) {
  return Rigid3<Scalar>(
      lhs.rotation() * rhs.translation() + lhs.translation(),
      (lhs.rotation() * rhs.rotation()).normalized());
}

///< pose3 * vector3
template <typename FloatType>
typename Rigid3<FloatType>::Vector operator*(
    const Rigid3<FloatType>& rigid,
    const typename Rigid3<FloatType>::Vector& point) {
  return rigid.rotation() * point + rigid.translation();
}

// This is needed for gmock.
template <typename T>
std::ostream& operator<<(std::ostream& os,
                         const Rigid3<T>& rigid) {
  os << rigid.DebugString();
  return os;
}

using Rigid3d = Rigid3<double>;
using Rigid3f = Rigid3<float>;

Test

vector Test

核心函数：

• inverse
• pose3 * pose3
• pose3 * vector3

double ToYaw(const std::vector<double>& q) {

    const double& w = q[0];
    const double& x = q[1];
    const double& y = q[2];
    const double& z = q[3];

    //四元数转角度
    return std::atan2(2.0 * (w*z + x*y), 1. - 2. * (y*y + z*z));
}

std::vector<double> QuaternionInverse(const std::vector<double>& q) {

    double normSq =  q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3];
    return std::vector<double>{
                    q[0]/normSq,
                    -q[1]/normSq,
                    -q[2]/normSq,
                    -q[3]/normSq
                    };
}


std::vector<double> FromYaw(double yaw, double pitch = 0, double roll = 0) {
    // yaw (Z), pitch (Y), roll (X)
    double cy = cos(yaw * 0.5);
    double sy = sin(yaw * 0.5);
    double cp = cos(pitch * 0.5);
    double sp = sin(pitch * 0.5);
    double cr = cos(roll * 0.5);
    double sr = sin(roll * 0.5);
    //角度转四元数
    double w = (cr * cp * cy + sr * sp * sy);
    double x = (sr * cp * cy - cr * sp * sy);
    double y = (cr * sp * cy + sr * cp * sy);
    double z = (cr * cp * sy - sr * sp * cy);

    return std::vector<double>{w,x,y,z};
}


std::vector<double> QuaternionXQVector3d(const std::vector<double>& p,
                                         const std::vector<double>& v) {

  double dcm[3][3];
  const double a = p[0];
  const double b = p[1];
  const double c = p[2];
  const double d = p[3];
  const double aa = a * a;
  const double ab = a * b;
  const double ac = a * c;
  const double ad = a * d;
  const double bb = b * b;
  const double bc = b * c;
  const double bd = b * d;
  const double cc = c * c;
  const double cd = c * d;
  const double dd = d * d;
  dcm[0][0] = aa + bb - cc - dd;
  dcm[0][1] = 2 * (bc - ad);
  dcm[0][2] = 2 * (ac + bd);
  dcm[1][0] = 2 * (bc + ad);
  dcm[1][1] = aa - bb + cc - dd;
  dcm[1][2] = 2 * (cd - ab);
  dcm[2][0] = 2 * (bd - ac);
  dcm[2][1] = 2 * (ab + cd);
  dcm[2][2] = aa - bb - cc + dd;

  // transform vector
  std::vector<double> res(3);
  res[0] = dcm[0][0]*v[0] + dcm[0][1]*v[1] + dcm[0][2]*v[2];
  res[1] = dcm[1][0]*v[0] + dcm[1][1]*v[1] + dcm[1][2]*v[2];
  res[2] = dcm[2][0]*v[0] + dcm[2][1]*v[1] + dcm[2][2]*v[2];

  return res;
}

std::vector<double> Pose3dInverse(const std::vector<double>& pose) {
  CHECK(pose.size() == 7);

  std::vector<double> q_inverse = QuaternionInverse(
        std::vector<double>{pose[3],pose[4],pose[5],pose[6]});

  std::vector<double> pose_v = QuaternionXQVector3d(q_inverse,
                             std::vector<double>{pose[0],pose[1],pose[2]});

  return std::vector<double>{-pose_v[0],-pose_v[1],-pose_v[2],
        q_inverse[0],q_inverse[1],q_inverse[2],q_inverse[3]};
}



std::vector<double> QuaternionXQuaternion(const std::vector<double> &p,
                                          const std::vector<double> &q) {
    std::vector<double> r(4);
    r[0] = p[0] * q[0] - p[1] * q[1] - p[2] * q[2] - p[3] * q[3];
    r[1] = p[0] * q[1] + p[1] * q[0] + p[2] * q[3] - p[3] * q[2];
    r[2] = p[0] * q[2] - p[1] * q[3] + p[2] * q[0] + p[3] * q[1];
    r[3] = p[0] * q[3] + p[1] * q[2] - p[2] * q[1] + p[3] * q[0];
    return r;
}

std::vector<double> Pose3dXPose3d(const std::vector<double>& pose_1,
                                  const std::vector<double>& pose_2) {

  std::vector<double> v = QuaternionXQVector3d(std::vector<double>{pose_1[3],pose_1[4],pose_1[5],pose_1[6]}
                                               ,std::vector<double>{pose_2[0],pose_2[1],pose_2[2]});
  v = {v[0]+pose_1[0],v[1]+pose_1[1],v[2]+pose_1[2]};
  std::vector<double> q = QuaternionXQuaternion(std::vector<double>{pose_1[3],pose_1[4],pose_1[5],pose_1[6]}
                                                ,std::vector<double>{pose_2[3],pose_2[4],pose_2[5],pose_2[6]});

  return std::vector<double>{v[0],v[1],v[2],q[0],q[1],q[2],q[3]};
}

std::vector<double> Pose3dXVector3d(const std::vector<double>& pose_1,
                                  const std::vector<double>& pose_2) {

  std::vector<double> v = QuaternionXQVector3d(std::vector<double>{pose_1[3],pose_1[4],pose_1[5],pose_1[6]}
                                               ,std::vector<double>{pose_2[0],pose_2[1],pose_2[2]});
  v = {v[0]+pose_1[0],v[1]+pose_1[1],v[2]+pose_1[2]};

  return std::vector<double>{v[0],v[1],v[2]};
}

Test

void LOGPOSE(const std::vector<double>& pose,const std::string& str) {
  CHECK(pose.size() == 7 || pose.size() ==3);
  if(pose.size() == 7) {
    LOG(ERROR)<<str<<" pose: "<<pose[0]<<" "<<pose[1]<<" "<<pose[2]<<" "<<pose[3]<<" "
             <<pose[4]<<" "<<pose[5]<<" "<<pose[6];
  } else if(pose.size() == 3) {
    LOG(ERROR)<<str<<" point: "<<pose[0]<<" "<<pose[1]<<" "<<pose[2];
  }

}

std::vector<double> Rigid3dPose3d(const megbot_transform::Rigid3d& pose) {
  return std::vector<double> {
      pose.translation().x()
      ,pose.translation().y()
      ,pose.translation().z()

      ,pose.rotation().w()
      ,pose.rotation().x()
      ,pose.rotation().y()
      ,pose.rotation().z()
  };
}

Rigid3d MegbotRigid3d(const megbot_transform::Rigid3d& pose) {

  double x = pose.translation().x();
  double y = pose.translation().y();
  double z = pose.translation().z();

  double q_w = pose.rotation().w();
  double q_x = pose.rotation().x();
  double q_y = pose.rotation().y();
  double q_z = pose.rotation().z();

  return Rigid3d(Rigid3d::Vector(x,y,z),Rigid3d::Quaternion(q_w,q_x,q_y,q_z) );
}

      // test data
      const megbot_transform::Rigid3d pose_1 = megbot_transform::Rigid3d(Eigen::Vector3d{1,2.5,3.5},
                                            megbot_transform::RollPitchYaw(0.3,0.2,0.1));

      const megbot_transform::Rigid3d pose_2 = megbot_transform::Rigid3d(Eigen::Vector3d{5,7.5,4.5},
                                            megbot_transform::RollPitchYaw(0.5,0.1,0.6));

      const Eigen::Vector3d vector3 = {1,2,4};

      // test debugstring
      LOG(ERROR)<<"x_oring x1: "<<(pose_1).DebugString();
      LOG(ERROR)<<"x_new   x1: "<<(MegbotRigid3d(pose_1)).DebugString();

      // pose1 * pose 2
      LOG(ERROR)<<"x_oring x: "<<(pose_1 * pose_2).DebugString();
      LOG(ERROR)<<"x_new   x: "<<(MegbotRigid3d(pose_1) * MegbotRigid3d(pose_2)).DebugString();

      // pose1.invers
      LOG(ERROR)<<"x_oring invs: "<<(pose_1.inverse()).DebugString();
      LOG(ERROR)<<"x_new   invs: "<<(MegbotRigid3d(pose_1).inverse()).DebugString();
      // pose2.invers
      LOG(ERROR)<<"x_oring invs: "<<(pose_2.inverse()).DebugString();
      LOG(ERROR)<<"x_new   invs: "<<(MegbotRigid3d(pose_2).inverse()).DebugString();

      //pose1 * vector3
      LOG(ERROR)<<"x_oring invs: "<<(megbot_transform::Rigid3d::Translation(pose_1 * vector3)).DebugString();
      LOG(ERROR)<<"x_new   invs: "<<(Rigid3d::Translation(MegbotRigid3d(pose_1) * Rigid3d::Vector(1,2,4))).DebugString();

测试结果：

 x_oring x1: { t: [1, 2.5, 3.5], q: [0.983347, 0.143572, 0.106021, 0.0342708] }
 x_new   x1: { t: [1, 2.5, 3.5], r: [0.983347, 0.143572, 0.106021, 0.0342708] }
 x_oring x: { t: [6.58125, 8.92447, 8.89219], q: [0.8588, 0.376288, 0.183936, 0.295021] }
 x_new   x: { t: [6.58125, 8.92447, 8.89219], r: [0.8588, 0.376288, 0.183936, 0.295021] }
 x_oring invs: { t: [-0.524436, -3.36781, -2.80764], q: [0.983347, -0.143572, -0.106021, -0.0342708] }
 x_new   invs: { t: [-0.524436, -3.36781, -2.80764], r: [0.983347, -0.143572, -0.106021, -0.0342708] }
 x_oring invs: { t: [-7.87047, -5.50149, -3.04783], q: [0.928135, -0.221748, -0.119284, -0.274163] }
 x_new   invs: { t: [-7.87047, -5.50149, -3.04783], r: [0.928135, -0.221748, -0.119284, -0.274163] }
 x_oring invs: { t: [2.77466, 3.41031, 7.62576], q: [1, 0, 0, 0] }
 x_new   invs: { t: [2.77466, 3.41031, 7.62576], r: [1, 0, 0, 0] }

2D Pose

Eigen transform

template <typename FloatType>
class Rigid2 {
 public:
  using Vector = Eigen::Matrix<FloatType, 2, 1>;
  using Rotation2D = Eigen::Rotation2D<FloatType>;

  Rigid2() : translation_(Vector::Zero()), rotation_(Rotation2D::Identity()) {}
  Rigid2(const Vector& translation, const Rotation2D& rotation)
      : translation_(translation), rotation_(rotation) {}
  Rigid2(const Vector& translation, const double rotation)
      : translation_(translation), rotation_(rotation) {}

  static Rigid2 Rotation(const double rotation) {
    return Rigid2(Vector::Zero(), rotation);
  }

  static Rigid2 Rotation(const Rotation2D& rotation) {
    return Rigid2(Vector::Zero(), rotation);
  }

  static Rigid2 Translation(const Vector& vector) {
    return Rigid2(vector, Rotation2D::Identity());
  }

  static Rigid2<FloatType> Identity() { return Rigid2<FloatType>(); }

  template <typename OtherType>
  Rigid2<OtherType> cast() const {
    return Rigid2<OtherType>(translation_.template cast<OtherType>(),
                             rotation_.template cast<OtherType>());
  }

  const Vector& translation() const { return translation_; }

  Rotation2D rotation() const { return rotation_; }

  FloatType normalized_angle() const {
    
    FloatType & difference =  rotation().angle();
    const FloatType kPi = FloatType(M_PI);
    while (difference > kPi) difference -= 2. * kPi;
    while (difference < -kPi) difference += 2. * kPi;
    return difference;
  }

  Rigid2 inverse() const {
    const Rotation2D rotation = rotation_.inverse();
    const Vector translation = -(rotation * translation_);
    return Rigid2(translation, rotation);
  }

  std::string DebugString() const {
    std::stringstream str;
    str << "{ t: [" << translation().x() <<", "<<translation().y()<<"], r: ["<<rotation().angle()<<"] }";
    return str.str();
  }

 private:
  Vector translation_;
  Rotation2D rotation_;
};

template <typename FloatType>
Rigid2<FloatType> operator*(const Rigid2<FloatType>& lhs,
                            const Rigid2<FloatType>& rhs) {
  return Rigid2<FloatType>(
      lhs.rotation() * rhs.translation() + lhs.translation(),
      lhs.rotation() * rhs.rotation());
}

template <typename FloatType>
typename Rigid2<FloatType>::Vector operator*(
    const Rigid2<FloatType>& rigid,
    const typename Rigid2<FloatType>::Vector& point) {
  return rigid.rotation() * point + rigid.translation();
}

// This is needed for gmock.
template <typename T>
std::ostream& operator<<(std::ostream& os,
                         const Rigid2<T>& rigid) {
  os << rigid.DebugString();
  return os;
}

using Rigid2d = Rigid2<double>;
using Rigid2f = Rigid2<float>;

C++ transform

  Eigen::Vector3d Inverse(const Eigen::Vector3d& pose) {

    double theta_inv = -pose(2);
    double x_inv = -(std::cos(pose(2))*pose(0)+std::sin(pose(2))*pose(1));
    double y_inv = -(-std::sin(pose(2))*pose(0)+std::cos(pose(2))*pose(1));
    return Eigen::Vector3d{x_inv,y_inv,theta_inv};
  }

  Eigen::Vector3d PoseDotPose(const Eigen::Vector3d& pose1,const Eigen::Vector3d& pose2) {

    double x = pose1(0) + std::cos(pose1(2))*pose2(0) - std::sin(pose1(2))* pose2(1);
    double y = pose1(1) + std::sin(pose1(2))*pose2(0) + std::cos(pose1(2))* pose2(1);
    double theta = pose1(2) + pose2(2);
    while (theta > M_PI) theta -= 2. * M_PI;
    while (theta < -M_PI) theta += 2. * M_PI;
    return Eigen::Vector3d{x,y,theta};
  }

python

# 取逆
def Pose2dInverse(x,y,yaw) :
    r_yaw = -yaw
    r_x = -(math.cos(yaw)*x + math.sin(yaw) * y)
    r_y = -(-math.sin(yaw)*x + math.cos(yaw) * y)
    return [r_x,r_y,r_yaw]

# 2d 点乘 2d  point1 * point2
def Pose2dDotPose2d(x1,y1,yaw1,x2,y2,yaw2) :
    x_r = x1 + x2*math.cos(yaw1) - y2*math.sin(yaw1)
    y_r = y1 + x2*math.sin(yaw1) + y2*math.cos(yaw1)
    yaw_r = yaw1 + yaw2
    return [x_r,y_r,yaw_r]

Test

TEST_F(TransformTest,transform2d) {

  common::Rigid2d origin(Eigen::Vector2d{0.1,0.2},60. / 180 * 3.1415926);
  common::Rigid2d inverse = origin.inverse();

  double theta_origin = origin.rotation().angle();
  double x_origin = origin.translation().x();
  double y_origin = origin.translation().y();

  /// 取逆
  auto inverse_o = Inverse(Eigen::Vector3d{x_origin,y_origin,theta_origin});
  CHECK_EQ(inverse.rotation().angle(),inverse_o(2));
  CHECK_EQ(inverse.translation().x(),inverse_o(0));
  CHECK_EQ(inverse.translation().y(),inverse_o(1));

  // ------------------------------------

  common::Rigid2d ss2 (Eigen::Vector2d{0.3,0.4},30. / 180 * 3.1415926);
  double theta_s2 = ss2.rotation().angle();
  double x_s2 = ss2.translation().x();
  double y_s2 = ss2.translation().y();

  common::Rigid2d s1s2 = origin*ss2;

  {
    auto s1ds2 = PoseDotPose(Eigen::Vector3d{x_origin,y_origin,theta_origin},
                             Eigen::Vector3d{x_s2,y_s2,theta_s2});
    CHECK_EQ(s1s2.rotation().angle(),s1ds2(2));
    CHECK_EQ(s1s2.translation().x(),s1ds2(0));
    CHECK_EQ(s1s2.translation().y(),s1ds2(1));
  }
}