Function TEST_F

tests/rvt_test.cpp:123–187

Run tests ------------------------------------------------------------------------------ */ Test rpy conversion

/* Run tests ------------------------------------------------------------------------------ */
// Test rpy conversion
TEST_F(RVTTest, test_rpy_conversions)
{
  // Identity conversions with RPY
  Eigen::Isometry3d expected_affine = Eigen::Isometry3d::Identity();
  std::vector<double> xyzrpy;
  visual_tools_->convertToXYZRPY(expected_affine, xyzrpy);
  std::vector<double> expected_vector;
  expected_vector.push_back(0);
  expected_vector.push_back(0);
  expected_vector.push_back(0);
  expected_vector.push_back(0);
  expected_vector.push_back(0);
  expected_vector.push_back(0);
  EXPECT_TRUE(testVector("Identity: ", expected_vector, xyzrpy));

  // Identity conversion back to Eigen
  Eigen::Isometry3d expected_affine2 = visual_tools_->convertFromXYZRPY(xyzrpy, rvt::XYZ);
  EXPECT_TRUE(testIsometry3d("Identity convert back", expected_affine, expected_affine2));

  // -------------------------------------------------------------------
  // Translation conversions to RPY
  expected_affine.translation().x() = 1;
  expected_affine.translation().y() = 2;
  expected_affine.translation().z() = 3;
  visual_tools_->convertToXYZRPY(expected_affine, xyzrpy);
  expected_vector[0] = 1;
  expected_vector[1] = 2;
  expected_vector[2] = 3;
  EXPECT_TRUE(testVector("123: ", expected_vector, xyzrpy));

  // Translation convertion back to Eigen
  expected_affine2 = visual_tools_->convertFromXYZRPY(xyzrpy, rvt::XYZ);
  EXPECT_TRUE(testIsometry3d("123 convert back", expected_affine, expected_affine2));

  // Translation convertion back to Eigen via long function
  expected_affine2 = visual_tools_->convertFromXYZRPY(xyzrpy[0], xyzrpy[1], xyzrpy[2], xyzrpy[3],
                                                      xyzrpy[4], xyzrpy[5], rvt::XYZ);
  EXPECT_TRUE(testIsometry3d("123 convert back long", expected_affine, expected_affine2));

  // Translation convertion back to Eigen via NEW long function
  expected_affine2 = visual_tools_->convertFromXYZRPY(xyzrpy[0], xyzrpy[1], xyzrpy[2], xyzrpy[3],
                                                      xyzrpy[4], xyzrpy[5], rvt::XYZ);
  EXPECT_TRUE(testIsometry3d("123 convert back new long", expected_affine, expected_affine2));

  // -------------------------------------------------------------------
  // Rotation conversions to RPY
  expected_affine = expected_affine * Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX()) *
                    Eigen::AngleAxisd(0.5 * M_PI, Eigen::Vector3d::UnitZ()) *
                    Eigen::AngleAxisd(-0.5 * M_PI, Eigen::Vector3d::UnitX());
  visual_tools_->convertToXYZRPY(expected_affine, xyzrpy);

  // Rotation convertion back to Eigen
  expected_affine2 = visual_tools_->convertFromXYZRPY(xyzrpy, rvt::XYZ);
  EXPECT_TRUE(testIsometry3d("123 convert back", expected_affine, expected_affine2));

  // Rotation convertion back to Eigen via long function
  expected_affine2 = visual_tools_->convertFromXYZRPY(xyzrpy[0], xyzrpy[1], xyzrpy[2], xyzrpy[3],
                                                      xyzrpy[4], xyzrpy[5], rvt::XYZ);