//==============================================================================
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(); }