文章目录
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));
}
}