Added unit tests for kinematic calibration observation generators

Jmeyer1292 · Jul 22, 2020 · a53b32c · a53b32c
1 parent bf1df66
commit a53b32c
Showing 1 changed file with 92 additions and 13 deletions.
diff --git a/rct_optimizations/test/dh_parameter_utest.cpp b/rct_optimizations/test/dh_parameter_utest.cpp
@@ -62,33 +62,112 @@ TEST(DHChain, generateObservations3D)
   EXPECT_EQ(observations.size(), n);
 }
 
-TEST(DHChain, generateObservations2D)
+class DHChainObservationGeneration : public ::testing::Test
 {
-  DHChain camera_robot = test::createABBIRB2400();
-  DHChain target_robot({});
-
-  // Create a transform to the tool0 of the robot at it's all-zero position
-  Eigen::Isometry3d camera_base_to_target_base = camera_robot.getFK<double>(Eigen::VectorXd::Zero(6));
+  public:
+  DHChainObservationGeneration()
+    : camera_chain(test::createABBIRB2400())
+    , target_chain({})
+    , target(7, 5, 0.025)
+  {
+    // Create a transform to the tool0 of the robot at it's all-zero position
+    camera_base_to_target_base = camera_chain.getFK<double>(Eigen::VectorXd::Zero(6));
 
-  // Place the target 0.5m in front of the robot's all-zero position, facing back at the robot
-  camera_base_to_target_base.translate(Eigen::Vector3d(0.0, 0.0, 0.5));
-  camera_base_to_target_base.rotate(Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitY()));
+    // Place the target 0.5m in front of the robot's all-zero position, facing back at the robot
+    camera_base_to_target_base.translate(Eigen::Vector3d(0.0, 0.0, 0.5));
+    camera_base_to_target_base.rotate(Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitY()));
+  }
 
+  protected:
+  DHChain camera_chain;
+  DHChain target_chain;
+  Eigen::Isometry3d camera_base_to_target_base;
   const std::size_t n = 100;
-  const test::Target target(5, 5, 0.025);
+  test::Target target;
+};
+
+TEST_F(DHChainObservationGeneration, generateObservations2D)
+{
   const test::Camera camera = test::makeKinectCamera();
   Observation2D3D::Set observations;
-  EXPECT_NO_THROW(observations = test::createObservations(camera_robot,
-                                                          target_robot,
+  EXPECT_NO_THROW(observations = test::createObservations(camera_chain,
+                                                          target_chain,
                                                           Eigen::Isometry3d::Identity(),
                                                           Eigen::Isometry3d::Identity(),
                                                           camera_base_to_target_base,
-                                                          test::Target(5, 5, 0.025),
+                                                          target,
                                                           camera,
                                                           n));
   EXPECT_GE(observations.size(), n);
 }
 
+TEST_F(DHChainObservationGeneration, generateObservations3D)
+{
+  Observation3D3D::Set observations;
+  EXPECT_NO_THROW(observations = test::createObservations(camera_chain,
+                                                          target_chain,
+                                                          Eigen::Isometry3d::Identity(),
+                                                          Eigen::Isometry3d::Identity(),
+                                                          camera_base_to_target_base,
+                                                          target,
+                                                          n));
+  EXPECT_EQ(observations.size(), n);
+}
+
+TEST_F(DHChainObservationGeneration, generateKinematicObservations2D)
+{
+  KinObservation2D3D::Set observations;
+  test::Camera camera = test::makeKinectCamera();
+  EXPECT_NO_THROW(observations = test::createKinematicObservations(camera_chain,
+                                                                   target_chain,
+                                                                   Eigen::Isometry3d::Identity(),
+                                                                   Eigen::Isometry3d::Identity(),
+                                                                   camera_base_to_target_base,
+                                                                   target,
+                                                                   camera,
+                                                                   n));
+  EXPECT_GE(observations.size(), n);
+}
+
+TEST_F(DHChainObservationGeneration, generateKinematicObservations3D)
+{
+  KinObservation3D3D::Set observations;
+  EXPECT_NO_THROW(observations = test::createKinematicObservations(camera_chain,
+                                                                   target_chain,
+                                                                   Eigen::Isometry3d::Identity(),
+                                                                   Eigen::Isometry3d::Identity(),
+                                                                   camera_base_to_target_base,
+                                                                   target,
+                                                                   n));
+  EXPECT_EQ(observations.size(), n);
+}
+
+TEST(DHChain, generateKinematicMeasurements)
+{
+  // Set up a scenario in which the camera is static and the target is the robot tool0 frame
+  // Create the DH chains
+  DHChain target_chain = test::createABBIRB2400();
+  DHChain camera_chain({});
+
+  Eigen::Isometry3d camera_base_to_target_base(Eigen::Isometry3d::Identity());
+  // Move the target chain in front of the camera in X
+  camera_base_to_target_base.translate(Eigen::Vector3d(2.0, 0.0, 0.0));
+  // Rotate the target chain 180 degrees to face the camera
+  camera_base_to_target_base.rotate(Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitZ()));
+
+  // Create the measurements
+  const std::size_t n = 100;
+  KinematicMeasurement::Set measurements
+    = test::createKinematicMeasurements(camera_chain,
+                                        target_chain,
+                                        Eigen::Isometry3d::Identity(),
+                                        Eigen::Isometry3d::Identity(),
+                                        camera_base_to_target_base,
+                                        n);
+
+  EXPECT_EQ(measurements.size(), n);
+}
+
 int main(int argc, char **argv)
 {
   testing::InitGoogleTest(&argc, argv);