diff --git a/rct_examples/src/tools/camera_on_wrist_extrinsic.cpp b/rct_examples/src/tools/camera_on_wrist_extrinsic.cpp index 06d64e56..6857e3ae 100644 --- a/rct_examples/src/tools/camera_on_wrist_extrinsic.cpp +++ b/rct_examples/src/tools/camera_on_wrist_extrinsic.cpp @@ -7,7 +7,7 @@ #include // The calibration function for 'moving camera' on robot wrist #include -#include +#include // For display of found targets #include diff --git a/rct_examples/src/tools/solve_pnp.cpp b/rct_examples/src/tools/solve_pnp.cpp index cb433c55..0e6726dc 100644 --- a/rct_examples/src/tools/solve_pnp.cpp +++ b/rct_examples/src/tools/solve_pnp.cpp @@ -10,7 +10,7 @@ #include #include -#include +#include #include #include diff --git a/rct_examples/src/tools/static_camera_extrinsic.cpp b/rct_examples/src/tools/static_camera_extrinsic.cpp index 0d1b454f..59f563bd 100644 --- a/rct_examples/src/tools/static_camera_extrinsic.cpp +++ b/rct_examples/src/tools/static_camera_extrinsic.cpp @@ -13,7 +13,7 @@ #include #include -#include +#include 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, diff --git a/rct_optimizations/include/rct_optimizations/ceres_math_utilities.h b/rct_optimizations/include/rct_optimizations/ceres_math_utilities.h index 98413857..1c6c3a02 100644 --- a/rct_optimizations/include/rct_optimizations/ceres_math_utilities.h +++ b/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 +inline Eigen::Matrix projectPoint(const rct_optimizations::CameraIntrinsics &intr, + const Eigen::Matrix& point) +{ + // Scale the input point by its distance from the camera (i.e. z-coordinate) + Eigen::Matrix 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 image_pt = camera_matrix.cast() * 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 diff --git a/rct_optimizations/include/rct_optimizations/local_parameterization.h b/rct_optimizations/include/rct_optimizations/local_parameterization.h new file mode 100644 index 00000000..11ca27a3 --- /dev/null +++ b/rct_optimizations/include/rct_optimizations/local_parameterization.h @@ -0,0 +1,76 @@ +#pragma once + +#include +#include + +// 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 + 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 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> x_plus_delta_ref(x_plus_delta); + Eigen::Map> x_ref(x); + x_plus_delta_ref = q_delta * x_ref; + return true; + } +}; + +} // namespace rct_optimizations diff --git a/rct_optimizations/include/rct_optimizations/experimental/pnp.h b/rct_optimizations/include/rct_optimizations/pnp.h similarity index 100% rename from rct_optimizations/include/rct_optimizations/experimental/pnp.h rename to rct_optimizations/include/rct_optimizations/pnp.h diff --git a/rct_optimizations/src/rct_optimizations/camera_intrinsic.cpp b/rct_optimizations/src/rct_optimizations/camera_intrinsic.cpp index 9d9acde5..29d0ec6d 100644 --- a/rct_optimizations/src/rct_optimizations/camera_intrinsic.cpp +++ b/rct_optimizations/src/rct_optimizations/camera_intrinsic.cpp @@ -1,5 +1,5 @@ #include "rct_optimizations/experimental/camera_intrinsic.h" -#include +#include #include #include diff --git a/rct_optimizations/src/rct_optimizations/pnp.cpp b/rct_optimizations/src/rct_optimizations/pnp.cpp index 0a3a3fc6..b6f8be54 100644 --- a/rct_optimizations/src/rct_optimizations/pnp.cpp +++ b/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 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 - 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; + using Vector3 = Eigen::Matrix; + using Vector2 = Eigen::Matrix; - // 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> q(target_q); + Eigen::Map t(target_t); + Isometry3 camera_to_target = Isometry3::Identity(); + camera_to_target = Eigen::Translation(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 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 ¶ms) +{ + // 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(cost_fn); + auto *cost_block = new ceres::AutoDiffCostFunction(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(); + 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 + diff --git a/rct_optimizations/test/CMakeLists.txt b/rct_optimizations/test/CMakeLists.txt index 111d48a2..bfea0408 100644 --- a/rct_optimizations/test/CMakeLists.txt +++ b/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 diff --git a/rct_optimizations/test/pnp_utest.cpp b/rct_optimizations/test/pnp_utest.cpp new file mode 100644 index 00000000..450fc559 --- /dev/null +++ b/rct_optimizations/test/pnp_utest.cpp @@ -0,0 +1,110 @@ +#include +#include +#include +#include + +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(target_rows - 1) * spacing / 2.0; + double y = static_cast(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(target_rows) * spacing / 2.0; + double y = static_cast(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(target_rows) * spacing / 2.0; + double y = static_cast(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(); +}