Eigen-based PnP Cost Function and Unit Test

rct_examples/src/tools/camera_on_wrist_extrinsic.cpp

@@ -7,7 +7,7 @@
 #include <rct_image_tools/image_utils.h>
 // The calibration function for 'moving camera' on robot wrist
 #include <rct_optimizations/extrinsic_hand_eye.h>
-#include <rct_optimizations/experimental/pnp.h>
+#include <rct_optimizations/pnp.h>
 
 // For display of found targets
 #include <opencv2/highgui/highgui.hpp>

rct_examples/src/tools/solve_pnp.cpp

@@ -10,7 +10,7 @@
 
 #include <rct_image_tools/image_observation_finder.h>
 #include <rct_image_tools/image_utils.h>
-#include <rct_optimizations/experimental/pnp.h>
+#include <rct_optimizations/pnp.h>
 #include <rct_ros_tools/parameter_loaders.h>
 #include <rct_ros_tools/print_utils.h>
 

rct_examples/src/tools/static_camera_extrinsic.cpp

@@ -13,7 +13,7 @@
 
 #include <opencv2/imgproc.hpp>
 #include <rct_optimizations/ceres_math_utilities.h>
-#include <rct_optimizations/experimental/pnp.h>
+#include <rct_optimizations/pnp.h>
 
 static void reproject(const Eigen::Isometry3d& wrist_to_target, const Eigen::Isometry3d& base_to_camera,
                       const rct_optimizations::Observation2D3D& obs, const rct_optimizations::CameraIntrinsics& intr,

rct_optimizations/include/rct_optimizations/ceres_math_utilities.h

@@ -61,6 +61,36 @@ inline void projectPoint(const CameraIntrinsics& intr, const T point[3], T xy_im
   xy_image[1] = intr.fy() * yp + intr.cy();
 }
 
+template<typename T>
+inline Eigen::Matrix<T, 2, 1> projectPoint(const rct_optimizations::CameraIntrinsics &intr,
+                                           const Eigen::Matrix<T, 3, 1>& point)
+{
+  // Scale the input point by its distance from the camera (i.e. z-coordinate)
+  Eigen::Matrix<T, 3, 1> scaled_point(point);
+  scaled_point(2) = T(1.0);
+
+  // Avoid divide by zero
+  if (std::abs(point.z() - T(0.0) > T(1.e-10)))
+  {
+    scaled_point.x() = point.x() / point.z();
+    scaled_point.y() = point.y() / point.z();
+  }
+
+  /* Create the camera parameters matrix
+   * | fx  0   cx | * | x/z | = | u |
+   * | 0   fy  cy | * | y/z | = | v |
+   * | 0   0   1  | * |  1  | = | 1 |
+   */
+  Eigen::Matrix3d camera_matrix;
+  camera_matrix << intr.fx(), 0.0, intr.cx(), 0.0, intr.fy(), intr.cy(), 0.0, 0.0, 1.0;
+
+  // Perform projection using focal length and camera optical center into image plane
+  Eigen::Matrix<T, 3, 1> image_pt = camera_matrix.cast<T>() * scaled_point;
+
+  // Return only the top two elements of the vector
+  return image_pt.template head<2>();
 }
 
+} // namespace rct_optimizations
+
 #endif // RCT_CERES_MATH_UTILITIES_H

rct_optimizations/include/rct_optimizations/local_parameterization.h

@@ -0,0 +1,76 @@
+#pragma once
+
+#include <ceres/rotation.h>
+#include <Eigen/Dense>
+
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2015 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+//   this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+//   this list of conditions and the following disclaimer in the documentation
+//   and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+//   used to endorse or promote products derived from this software without
+//   specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: sameeragarwal@google.com (Sameer Agarwal)
+
+namespace rct_optimizations
+{
+/**
+ * @brief Local parameterization of an Eigen quaternion
+ *
+ * As of version 1.12.0, Ceres does not provide implementations of auto-diff local parameterizations out of the box
+ * However, Ceres uses them in unit its own unit tests. This struct is adapted directly from the following Ceres unit test:
+ *
+ * https://ceres-solver.googlesource.com/ceres-solver/+/master/internal/ceres/local_parameterization_test.cc#345
+ * blob: 636cac29a78648e823758e154681583cd81d5b25
+ *
+ */
+struct EigenQuaternionPlus
+{
+  template<typename T>
+  bool operator()(const T *x, const T *delta, T *x_plus_delta) const
+  {
+    const T norm_delta = sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]);
+    Eigen::Quaternion<T> q_delta;
+    if (norm_delta > T(0.0))
+    {
+      const T sin_delta_by_delta = sin(norm_delta) / norm_delta;
+      q_delta.coeffs() << sin_delta_by_delta * delta[0], sin_delta_by_delta * delta[1],
+        sin_delta_by_delta * delta[2], cos(norm_delta);
+    }
+    else
+    {
+      // We do not just use q_delta = [0,0,0,1] here because that is a
+      // constant and when used for automatic differentiation will
+      // lead to a zero derivative. Instead we take a first order
+      // approximation and evaluate it at zero.
+      q_delta.coeffs() << delta[0], delta[1], delta[2], T(1.0);
+    }
+    Eigen::Map<Eigen::Quaternion<T>> x_plus_delta_ref(x_plus_delta);
+    Eigen::Map<const Eigen::Quaternion<T>> x_ref(x);
+    x_plus_delta_ref = q_delta * x_ref;
+    return true;
+  }
+};
+
+} // namespace rct_optimizations

rct_optimizations/src/rct_optimizations/camera_intrinsic.cpp

@@ -1,5 +1,5 @@
 #include "rct_optimizations/experimental/camera_intrinsic.h"
-#include <rct_optimizations/experimental/pnp.h>
+#include <rct_optimizations/pnp.h>
 
 #include <rct_optimizations/ceres_math_utilities.h>
 #include <rct_optimizations/eigen_conversions.h>

rct_optimizations/src/rct_optimizations/pnp.cpp

@@ -1,37 +1,36 @@
-#include "rct_optimizations/experimental/pnp.h"
+#include "rct_optimizations/pnp.h"
 #include "rct_optimizations/ceres_math_utilities.h"
-#include "rct_optimizations/eigen_conversions.h"
-
+#include "rct_optimizations/local_parameterization.h"
 #include <ceres/ceres.h>
 
 namespace
 {
-
 struct SolvePnPCostFunc
 {
-public:
-  SolvePnPCostFunc(const rct_optimizations::CameraIntrinsics& intr, const Eigen::Vector3d& pt_in_target,
+  public:
+  SolvePnPCostFunc(const rct_optimizations::CameraIntrinsics& intr,
+                   const Eigen::Vector3d& pt_in_target,
                    const Eigen::Vector2d& pt_in_image)
     : intr_(intr), in_target_(pt_in_target), in_image_(pt_in_image)
-  {}
+  {
+  }
 
   template<typename T>
-  bool operator()(const T* const target_pose, T* const residual) const
+  bool operator()(const T *const target_q, const T *const target_t, T *const residual) const
   {
-    const T* target_angle_axis = target_pose + 0;
-    const T* target_position = target_pose + 3;
+    using Isometry3 = Eigen::Transform<T, 3, Eigen::Isometry>;
+    using Vector3 = Eigen::Matrix<T, 3, 1>;
+    using Vector2 = Eigen::Matrix<T, 2, 1>;
 
-    // Transform points into camera coordinates
-    T target_pt[3];
-    target_pt[0] = T(in_target_(0));
-    target_pt[1] = T(in_target_(1));
-    target_pt[2] = T(in_target_(2));
+    Eigen::Map<const Eigen::Quaternion<T>> q(target_q);
+    Eigen::Map<const Vector3> t(target_t);
+    Isometry3 camera_to_target = Isometry3::Identity();
+    camera_to_target = Eigen::Translation<T, 3>(t) * q;
 
-    T camera_point[3];  // Point in camera coordinates
-    rct_optimizations::transformPoint(target_angle_axis, target_position, target_pt, camera_point);
+    // Transform points into camera coordinates
+    Vector3 camera_pt = camera_to_target.inverse() * in_target_.cast<T>();
 
-    T xy_image[2];
-    rct_optimizations::projectPoint(intr_, camera_point, xy_image);
+    Vector2 xy_image = projectPoint(intr_, camera_pt);
 
     residual[0] = xy_image[0] - in_image_.x();
     residual[1] = xy_image[1] - in_image_.y();
@@ -44,39 +43,50 @@ struct SolvePnPCostFunc
   Eigen::Vector2d in_image_;
 };
 
-}
+} // namespace anonymous
 
-rct_optimizations::PnPResult rct_optimizations::optimize(const rct_optimizations::PnPProblem& params)
+namespace rct_optimizations
 {
-  using namespace rct_optimizations;
-  Pose6d internal_camera_to_target = poseEigenToCal(params.camera_to_target_guess);
+PnPResult optimize(const PnPProblem &params)
+{
+  // Create the optimization variables from the input guess
+  Eigen::Quaterniond q(params.camera_to_target_guess.rotation());
+  Eigen::Vector3d t(params.camera_to_target_guess.translation());
 
   ceres::Problem problem;
 
-  for (std::size_t i = 0; i < params.correspondences.size(); ++i) // For each 3D point seen in the 2D image
+  // For each 3D point seen in the 2D image
+  for (std::size_t i = 0; i < params.correspondences.size(); ++i)
   {
     // Define
-    const auto& img_obs = params.correspondences[i].in_image;
-    const auto& point_in_target = params.correspondences[i].in_target;
+    const auto &img_obs = params.correspondences[i].in_image;
+    const auto &point_in_target = params.correspondences[i].in_target;
 
     // Allocate Ceres data structures - ownership is taken by the ceres
     // Problem data structure
-    auto* cost_fn = new SolvePnPCostFunc(params.intr, point_in_target, img_obs);
+    auto *cost_fn = new SolvePnPCostFunc(params.intr, point_in_target, img_obs);
 
-    auto* cost_block = new ceres::AutoDiffCostFunction<SolvePnPCostFunc, 2, 6>(cost_fn);
+    auto *cost_block = new ceres::AutoDiffCostFunction<SolvePnPCostFunc, 2, 4, 3>(cost_fn);
 
-    problem.AddResidualBlock(cost_block, NULL, internal_camera_to_target.values.data());
+    problem.AddResidualBlock(cost_block, nullptr, q.coeffs().data(), t.data());
   }
 
-  ceres::Solver::Options options;
+  ceres::LocalParameterization *q_param
+    = new ceres::AutoDiffLocalParameterization<EigenQuaternionPlus, 4, 3>();
+  problem.SetParameterization(q.coeffs().data(), q_param);
+
   ceres::Solver::Summary summary;
+  ceres::Solver::Options options;
   ceres::Solve(options, &problem, &summary);
 
   PnPResult result;
   result.converged = summary.termination_type == ceres::CONVERGENCE;
   result.initial_cost_per_obs = summary.initial_cost / summary.num_residuals;
   result.final_cost_per_obs = summary.final_cost / summary.num_residuals;
-  result.camera_to_target = poseCalToEigen(internal_camera_to_target);
+  result.camera_to_target = Eigen::Translation3d(t) * q;
 
   return result;
 }
+
+} // namespace rct_optimizations
+

rct_optimizations/test/CMakeLists.txt

@@ -51,6 +51,13 @@ add_executable(${PROJECT_NAME}_covariance_tests covariance_utest.cpp)
 target_link_libraries(${PROJECT_NAME}_covariance_tests PRIVATE ${PROJECT_NAME} GTest::GTest GTest::Main)
 add_dependencies(${PROJECT_NAME}_covariance_tests ${PROJECT_NAME})
 
+# PnP
+add_executable(${PROJECT_NAME}_pnp_tests pnp_utest.cpp)
+target_link_libraries(${PROJECT_NAME}_pnp_tests PRIVATE ${PROJECT_NAME}_test_support GTest::GTest GTest::Main)
+rct_gtest_discover_tests(${PROJECT_NAME}_pnp_tests)
+add_dependencies(${PROJECT_NAME}_pnp_tests ${PROJECT_NAME})
+add_dependencies(run_tests ${PROJECT_NAME}_pnp_tests)
+
 # Install the test executables so they can be run independently later if needed
 install(
   TARGETS
@@ -60,6 +67,7 @@ install(
     ${PROJECT_NAME}_extrinsic_hand_eye_tests
     ${PROJECT_NAME}_dh_parameter_tests
     ${PROJECT_NAME}_extrinsic_hand_eye_dh_chain_tests
+    ${PROJECT_NAME}_pnp_tests
   RUNTIME DESTINATION bin/tests
   LIBRARY DESTINATION lib/tests
   ARCHIVE DESTINATION lib/tests

rct_optimizations/test/pnp_utest.cpp

@@ -0,0 +1,110 @@
+#include <gtest/gtest.h>
+#include <rct_optimizations/pnp.h>
+#include <rct_optimizations_tests/observation_creator.h>
+#include <rct_optimizations_tests/utilities.h>
+
+using namespace rct_optimizations;
+
+TEST(PNP_2D, PerfectInitialConditions)
+{
+  test::Camera camera = test::makeKinectCamera();
+
+  unsigned target_rows = 5;
+  unsigned target_cols = 7;
+  double spacing = 0.025;
+  test::Target target(target_rows, target_cols, spacing);
+
+  Eigen::Isometry3d camera_to_target(Eigen::Isometry3d::Identity());
+  double x = static_cast<double>(target_rows - 1) * spacing / 2.0;
+  double y = static_cast<double>(target_cols - 1) * spacing / 2.0;
+  camera_to_target.translate(Eigen::Vector3d(x, y, 1.0));
+  camera_to_target.rotate(Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX()));
+
+  PnPProblem problem;
+  problem.camera_to_target_guess = camera_to_target;
+  problem.intr = camera.intr;
+  EXPECT_NO_THROW(problem.correspondences =
+                      test::getCorrespondences(camera_to_target, Eigen::Isometry3d::Identity(), camera, target, true));
+
+  PnPResult result = optimize(problem);
+  EXPECT_TRUE(result.converged);
+  EXPECT_TRUE(result.camera_to_target.isApprox(camera_to_target));
+  EXPECT_LT(result.initial_cost_per_obs, 1.0e-15);
+  EXPECT_LT(result.final_cost_per_obs, 1.0e-15);
+}
+
+TEST(PNP_2D, PerturbedInitialCondition)
+{
+  test::Camera camera = test::makeKinectCamera();
+
+  unsigned target_rows = 5;
+  unsigned target_cols = 7;
+  double spacing = 0.025;
+  test::Target target(target_rows, target_cols, spacing);
+
+  Eigen::Isometry3d camera_to_target(Eigen::Isometry3d::Identity());
+  double x = static_cast<double>(target_rows) * spacing / 2.0;
+  double y = static_cast<double>(target_cols) * spacing / 2.0;
+  camera_to_target.translate(Eigen::Vector3d(x, y, 1.0));
+  camera_to_target.rotate(Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX()));
+
+  PnPProblem problem;
+  problem.camera_to_target_guess = test::perturbPose(camera_to_target, 0.05, 0.05);
+  problem.intr = camera.intr;
+  problem.correspondences = test::getCorrespondences(camera_to_target,
+                                                     Eigen::Isometry3d::Identity(),
+                                                     camera,
+                                                     target,
+                                                     true);
+
+  PnPResult result = optimize(problem);
+  EXPECT_TRUE(result.converged);
+  EXPECT_TRUE(result.camera_to_target.isApprox(camera_to_target, 1.0e-8));
+  EXPECT_LT(result.final_cost_per_obs, 1.0e-15);
+}
+
+TEST(PNP_2D, BadIntrinsicParameters)
+{
+  test::Camera camera = test::makeKinectCamera();
+
+  unsigned target_rows = 5;
+  unsigned target_cols = 7;
+  double spacing = 0.025;
+  test::Target target(target_rows, target_cols, spacing);
+
+  Eigen::Isometry3d camera_to_target(Eigen::Isometry3d::Identity());
+  double x = static_cast<double>(target_rows) * spacing / 2.0;
+  double y = static_cast<double>(target_cols) * spacing / 2.0;
+  camera_to_target.translate(Eigen::Vector3d(x, y, 1.0));
+  camera_to_target.rotate(Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX()));
+
+  PnPProblem problem;
+  problem.camera_to_target_guess = camera_to_target;
+  problem.correspondences = test::getCorrespondences(camera_to_target,
+                                                     Eigen::Isometry3d::Identity(),
+                                                     camera,
+                                                     target,
+                                                     true);
+
+  // Tweak the camera intrinsics such that we are optimizing with different values (+/- 1%)
+  // than were used to generate the observations
+  problem.intr = camera.intr;
+  problem.intr.fx() *= 1.01;
+  problem.intr.fy() *= 0.99;
+  problem.intr.cx() *= 1.01;
+  problem.intr.cy() *= 0.99;
+
+  PnPResult result = optimize(problem);
+
+  // The optimization should still converge by moving the camera further away from the target,
+  // but the residual error and error in the resulting transform should be much higher
+  EXPECT_TRUE(result.converged);
+  EXPECT_FALSE(result.camera_to_target.isApprox(camera_to_target, 1.0e-3));
+  EXPECT_GT(result.final_cost_per_obs, 1.0e-3);
+}
+
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}