//============================================================================== void SE2::logMap(const StateSpace::State* _in, Eigen::VectorXd& _tangent) const { if (_tangent.rows() != 3) _tangent.resize(3); auto in = static_cast<const State*>(_in); Isometry2d transform = getIsometry(in); _tangent.head<2>() = transform.translation(); Eigen::Rotation2Dd rotation = Eigen::Rotation2Dd::Identity(); rotation.fromRotationMatrix(transform.rotation()); _tangent[2] = rotation.angle(); }
//============================================================================== void SE2::print(const StateSpace::State* _state, std::ostream& _os) const { auto state = static_cast<const State*>(_state); auto transform = getIsometry(state); Eigen::IOFormat cleanFmt( Eigen::StreamPrecision, Eigen::DontAlignCols, ",", ",", "", "", "[", "]"); Eigen::Rotation2Dd rotation = Eigen::Rotation2Dd::Identity(); rotation.fromRotationMatrix(transform.rotation()); _os << Eigen::Vector3d( transform.translation()[0], transform.translation()[1], rotation.angle()) .format(cleanFmt); }
IGL_INLINE void igl::fit_rigid( const Eigen::MatrixXd & A, const Eigen::MatrixXd & B, Eigen::Rotation2Dd & R, Eigen::RowVector2d & t) { using namespace Eigen; Matrix2d Rmat; procrustes(A,B,Rmat,t); R.fromRotationMatrix(Rmat); }
pose operator- () const { Eigen::Rotation2Dd inverse_rot = rotation.inverse(); return { inverse_rot*(-translation), inverse_rot }; }
double bearing () const { return rotation.angle(); }