예제 #1
0
//==============================================================================
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();
}
예제 #2
0
//==============================================================================
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);
}
예제 #3
0
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);
}
예제 #4
0
파일: pose.hpp 프로젝트: caomw/slam-4
	pose operator- () const {
            Eigen::Rotation2Dd inverse_rot = rotation.inverse();
            return { inverse_rot*(-translation), inverse_rot };
	}
예제 #5
0
파일: pose.hpp 프로젝트: caomw/slam-4
	double bearing () const { return rotation.angle(); }