Update/pose generation

rct_optimizations/CMakeLists.txt

@@ -56,6 +56,7 @@ target_link_libraries(${PROJECT_NAME}
 if(CATKIN_ENABLE_TESTING)
   # Build the test support library
   add_library(${PROJECT_NAME}_test_support
+    test/src/pose_generator.cpp
     test/src/observation_creator.cpp
     test/src/utilities.cpp)
   target_link_libraries(${PROJECT_NAME}_test_support ${PROJECT_NAME})

rct_optimizations/test/extrinsic_camera_on_wrist_utest.cpp

@@ -5,6 +5,7 @@
 // Test utilities
 #include <rct_optimizations_tests/utilities.h>
 #include <rct_optimizations_tests/observation_creator.h>
+#include <rct_optimizations_tests/pose_generator.h>
 
 using namespace rct_optimizations;
 
@@ -31,9 +32,14 @@ enum class InitialConditions
   PERFECT, IDENTITY_TARGET, RANDOM_AROUND_ANSWER
 };
 
+enum class CameraPattern
+{
+  HEMISPHERE, CONE, GRID
+};
+
 } // namespace anonymous
 
-void run_test(InitialConditions condition)
+void run_test(InitialConditions condition, CameraPattern pattern)
 {
 
   auto camera = test::makeKinectCamera();
@@ -52,34 +58,70 @@ void run_test(InitialConditions condition)
   // Create some number of "test" images...
   //    We'll take pictures in a grid above the origin of the target
   std::vector<Eigen::Isometry3d> wrist_poses;
-  std::vector<CorrespondenceSet> correspondences;
-  for (int i = -5; i < 5; ++i)
+
+  //std::vector<Eigen::Affine3d> wrist_poses;
+  std::vector<rct_optimizations::CorrespondenceSet> correspondences;
+
+  std::vector<Eigen::Isometry3d>camera_poses;
+
+  if(pattern == CameraPattern::HEMISPHERE)
   {
-    for (int j = -5; j < 5; ++j)
-    {
-      Eigen::Vector3d center_point = true_base_to_target.translation() + Eigen::Vector3d(i * 0.025, j * 0.025, 1.0);
-      Eigen::Isometry3d camera_pose = test::lookAt(center_point,
-                                                   true_base_to_target.translation(),
-                                                   Eigen::Vector3d(1, 0, 0));
-      Eigen::Isometry3d wrist_pose = camera_pose * true_wrist_to_camera.inverse();
-
-      // Attempt to generate points
-      try
-      {
-        CorrespondenceSet corr = getCorrespondences(camera_pose,
-                                                    true_base_to_target,
-                                                    camera,
-                                                    grid,
-                                                    true);
-        correspondences.push_back(corr);
-        wrist_poses.push_back(wrist_pose);
-      }
-      catch (const std::exception& ex)
-      {
-        continue;
-      }
-    }
+    const double radius = 2.0;
+    const unsigned int theta_cnt = 10;
+    const unsigned int phi_cnt = 10;
+
+    //hemisphere poses: radius 2, 10 rows of 10 observations
+    camera_poses = rct_optimizations::test::genHemispherePose(true_base_to_target.translation(),
+                                                             radius,
+                                                             theta_cnt,
+                                                             phi_cnt
+                                                             );
   }
+  if(pattern == CameraPattern::CONE)
+  {
+    const double radius = 1.0;
+    const double h = 2.0;
+    const unsigned int observations = 20;
+    //conical poses: 20 poses in a 1 meter radius cone at a distance of 2 meters
+    camera_poses = rct_optimizations::test::genConicalPose(true_base_to_target.translation(),
+                                                           observations,
+                                                           radius,
+                                                           h
+                                                           );
+  }
+  else //GRID
+  {
+    const double spacing = 0.2;
+    const double h = 2.0;
+    unsigned int grid_side = 10;
+    //grid poses: 100 poses at a distance of 2 meters with 0.2 meter spacing
+    camera_poses = rct_optimizations::test::genGridPose(true_base_to_target.translation(),
+                                                           grid_side,
+                                                           spacing,
+                                                           h
+                                                           );
+  }
+ for (auto& pose : camera_poses)
+ {
+     Eigen::Isometry3d wrist_pose = pose * true_wrist_to_camera.inverse();
+
+       // Attempt to generate points
+       try
+       {
+         CorrespondenceSet corr = getCorrespondences(pose,
+                                                     true_base_to_target,
+                                                     camera,
+                                                     grid,
+                                                     true);
+         correspondences.push_back(corr);
+         wrist_poses.push_back(wrist_pose);
+       }
+       catch (const std::exception& ex)
+       {
+         continue;
+       }
+  }
+
 
   // Fill out the calibration
   ExtrinsicCameraOnWristProblem problem;
@@ -120,21 +162,42 @@ void run_test(InitialConditions condition)
   printResults(result);
 }
 
-TEST(CameraOnWrist, perfect_start)
+TEST(CameraOnWrist, perfect_start_hemisphere)
 {
-  run_test(InitialConditions::PERFECT);
+  run_test(InitialConditions::PERFECT, CameraPattern::HEMISPHERE);
 }
 
-TEST(CameraOnWrist, identity_target_start)
+TEST(CameraOnWrist, perfect_start_cone)
 {
-  run_test(InitialConditions::IDENTITY_TARGET);
+  run_test(InitialConditions::PERFECT, CameraPattern::CONE);
 }
 
+TEST(CameraOnWrist, perfect_start_grid)
+{
+  run_test(InitialConditions::PERFECT, CameraPattern::GRID);
+}
+
+TEST(CameraOnWrist, identity_target_start_hemisphere)
+{
+  run_test(InitialConditions::IDENTITY_TARGET, CameraPattern::HEMISPHERE);
+}
+
+TEST(CameraOnWrist, identity_target_start_cone)
+{
+  run_test(InitialConditions::IDENTITY_TARGET, CameraPattern::CONE);
+}
+
+//TEST(CameraOnWrist, identity_target_start_grid)
+//{
+//  run_test(InitialConditions::IDENTITY_TARGET, CameraPattern::GRID);
+//}
+
+
 TEST(CameraOnWrist, perturbed_start)
 {
   // Run 10 random tests
   for (int i = 0; i < 10; ++i)
-    run_test(InitialConditions::RANDOM_AROUND_ANSWER);
+    run_test(InitialConditions::RANDOM_AROUND_ANSWER, CameraPattern::HEMISPHERE);
 }
 
 int main(int argc, char **argv)

rct_optimizations/test/include/rct_optimizations_tests/pose_generator.h

@@ -0,0 +1,41 @@
+#pragma once
+
+#include "rct_optimizations_tests/observation_creator.h"
+#include <rct_optimizations_tests/utilities.h>
+#include <Eigen/Geometry>
+#include <vector>
+
+namespace rct_optimizations
+{
+namespace test
+{
+/**
+ * @brief genHemispherePose Generates camera poses in a hemisphere pattern
+ * @param target_pose The position of the target
+ * @param r Radius of the hemisphere
+ * @param theta_cnt The number of points in the theta-wise direction
+ * @param phi_cnt The number of points in the phi-wise direction
+ * @return A vector of camera positions & orientations
+ */
+std::vector<Eigen::Isometry3d> genHemispherePose(const Eigen::Vector3d& target_pose, const double r, const unsigned int theta_cnt, const unsigned int phi_cnt);
+/**
+ * @brief genConicalPose Generates camera positions using a conical template, with the target at the 'point'
+ * @param target_pose The position of the target
+ * @param r Radius of the cone
+ * @param h Height of the cone (distance to target)
+ * @param theta_cnt Number of observations in a theta direction
+ * @return A vector of camera positions & orientations
+ */
+std::vector<Eigen::Isometry3d> genConicalPose(const Eigen::Vector3d& target_pose, const unsigned int observations, const double r, const double h);
+
+/**
+ * @brief genGridPose Generates camera positions in a grid pattern
+ * @param target_pose The position of the target
+ * @param grid_side number of columns & rows to go into grid
+ * @param spacing Distance betweeon points
+ * @param h Grid distance to target
+ * @return A vector of camera positions & orientations
+ */
+std::vector<Eigen::Isometry3d> genGridPose(const Eigen::Vector3d& target_pose, const unsigned int grid_side, const double spacing, const double h);
+}  // namespace test
+}  // namespace rct_optimizations

rct_optimizations/test/src/pose_generator.cpp

@@ -0,0 +1,106 @@
+#include "rct_optimizations_tests/observation_creator.h"
+#include "rct_optimizations_tests/pose_generator.h"
+#include <stdexcept>
+
+// hemisphere
+
+std::vector<Eigen::Isometry3d>
+rct_optimizations::test::genHemispherePose(const Eigen::Vector3d& target_pose, const double r, const unsigned int theta_cnt, const unsigned int phi_cnt)
+{
+    std::size_t position_cnt = theta_cnt * phi_cnt;
+
+  // all in the target coordinate system, with z forward
+  std::vector<Eigen::Isometry3d> camera_positions;
+  camera_positions.reserve(position_cnt);
+
+  Eigen::VectorXd theta_range = Eigen::VectorXd::LinSpaced(theta_cnt, 0.0, M_PI);
+  Eigen::VectorXd phi_range = Eigen::VectorXd::LinSpaced(phi_cnt, 0.0, M_PI);
+
+  for (std::size_t theta_it = 0; theta_it < theta_cnt; ++theta_it)
+  {
+    for (std::size_t phi_it = 0; phi_it < phi_cnt; ++phi_it)
+    {
+      double theta_cur = theta_range(theta_it);
+      double phi_cur = phi_range(phi_it);
+
+      // position in target coordinate frame
+      Eigen::Isometry3d camera = Eigen::Isometry3d::Identity();
+      camera.translation() = target_pose;
+
+      Eigen::Vector3d position;
+      position(0) = r * std::cos(theta_cur) * std::sin(phi_cur);
+      position(1) = r * std::sin(theta_cur) * std::sin(phi_cur);
+      position(2) = r * std::cos(phi_cur);
+
+      camera.translate(position);
+
+      // x is 'up' in target frame
+      Eigen::Isometry3d camera_oriented =
+          rct_optimizations::test::lookAt(camera.translation(), target_pose, Eigen::Vector3d(1, 0, 0));
+
+      // this vector is still in target spatial coordinates
+      camera_positions.push_back(camera_oriented);
+    }
+  }
+
+  return camera_positions;
+}
+
+std::vector<Eigen::Isometry3d>
+rct_optimizations::test::genConicalPose(const Eigen::Vector3d& target_pose, const unsigned int observations, const double r, const double h)
+{
+  std::vector<Eigen::Isometry3d> camera_positions;
+  camera_positions.reserve(observations);
+
+  // Generates positions in target frame; need to convert to world frame
+  double dt = 2.0f * M_PI / double(observations);  // delta theta about cone base
+
+  for (int i = 0; i < observations; ++i)
+  {
+    Eigen::Isometry3d camera_pose = Eigen::Isometry3d::Identity();
+    camera_pose.translation() = target_pose;
+
+    // preserving target spatial coordinate frame:
+    camera_pose.translate(Eigen::Vector3d{ r * cos(i * dt), r * sin(i * dt), h });
+
+    // change orientation to look at target
+    Eigen::Isometry3d camera_oriented =
+        rct_optimizations::test::lookAt(camera_pose.translation(), target_pose, Eigen::Vector3d(1, 0, 0));
+
+    camera_positions.push_back(camera_oriented);
+  }
+
+  return camera_positions;
+}
+
+std::vector<Eigen::Isometry3d>
+rct_optimizations::test::genGridPose(const Eigen::Vector3d& target_pose, const unsigned int grid_side, const double spacing, const double h)
+{
+  // Generates positions in target frame; need to convert to world frame
+  std::vector<Eigen::Isometry3d> camera_positions;
+  camera_positions.reserve(grid_side * grid_side);
+
+
+  double end_point = (static_cast<double>(grid_side - 1) / 2.0) * spacing;
+  Eigen::VectorXd grid_coords = Eigen::VectorXd::LinSpaced(grid_side, -1 * end_point, end_point);
+
+  for (std::size_t i = 0; i < grid_side; ++i)
+  {
+    for (std::size_t j = 0; j < grid_side; ++j)
+    {
+      Eigen::Isometry3d camera_pose = Eigen::Isometry3d::Identity();
+      camera_pose.translation()  = target_pose;
+      camera_pose.translate(Eigen::Vector3d{grid_coords(i), grid_coords(j), h });
+
+      // change orientation to look at target
+      Eigen::Isometry3d camera_oriented = rct_optimizations::test::lookAt(
+          camera_pose.translation(), target_pose, Eigen::Vector3d(1, 0, 0));
+
+      camera_positions.push_back(camera_oriented);
+    }
+  }
+
+
+
+  return camera_positions;
+}