//============================================================================== 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); }
double bearing () const { return rotation.angle(); }