C++ (Cpp) VectorXd::isApproxの例

コード例 #1

ファイル: test_Rn.cpp プロジェクト: personalrobotics/aikido

//==============================================================================
TEST(Rn, LogMapRx)
{
  Rn rvss(3);

  auto state = rvss.createState();
  rvss.setValue(state, Eigen::Vector3d(1, 2, 3));

  Eigen::VectorXd out;
  rvss.logMap(state, out);
  EXPECT_TRUE(out.isApprox(Eigen::Vector3d(1, 2, 3)));

  rvss.expMap(Eigen::Vector3d(4, 5, 7), state);
  rvss.logMap(state, out);
  EXPECT_TRUE(out.isApprox(Eigen::Vector3d(4, 5, 7)));
}

コード例 #2

ファイル: test_SO3.cpp プロジェクト: personalrobotics/aikido

TEST(SO3, LogMap)
{
  SO3 so3;
  auto state = so3.createState();
  so3.setQuaternion(
      state,
      Eigen::Quaterniond(Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d::UnitZ())));

  Eigen::VectorXd out;
  so3.logMap(state, out);
  EXPECT_TRUE(Eigen::Vector3d(0, 0, M_PI_2).isApprox(out));

  so3.expMap(Eigen::Vector3d(M_PI_2, M_PI_2, M_PI / 5), state);
  so3.logMap(state, out);
  EXPECT_TRUE(out.isApprox(Eigen::Vector3d(M_PI_2, M_PI_2, M_PI / 5)));
}