From a204f6d5088f1cea60a70d71e789012f0de63764 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Tue, 20 Jul 2021 20:46:43 -0400 Subject: [PATCH] amended --- gtsam/geometry/Pose3.cpp | 4 - gtsam/geometry/Pose3.h | 9 - .../slam/ProjectionFactorRollingShutter.h | 22 +- .../testProjectionFactorRollingShutter.cpp | 233 ++++++++++++++++++ 4 files changed, 244 insertions(+), 24 deletions(-) create mode 100644 gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index b2cdd0c87..c183e32ed 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -423,8 +423,4 @@ std::ostream &operator<<(std::ostream &os, const Pose3& pose) { return os; } -Pose3 pose3_interp(const Pose3& X, const Pose3& Y, double t, Matrix& Hx, Matrix& Hy) { - return X.interp(t, Y, Hx, Hy); -} - } // namespace gtsam diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 00c6fa8af..d76e1b41a 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -353,15 +353,6 @@ public: return std::make_pair(0, 2); } - /** - * @brief Spherical Linear interpolation between *this and other - * @param s a value between 0 and 1.5 - * @param other final point of iterpolation geodesic on manifold - * @param Hx jacobian of the interpolation on this - & @param Hy jacobian of the interpolation on other - */ - Pose3 interp(double t, const Pose3& other, OptionalJacobian<6, 6> Hx = boost::none, OptionalJacobian<6, 6> Hy = boost::none) const; - /// Output stream operator GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Pose3& p); diff --git a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h index 69b7e7376..499f1cec3 100644 --- a/gtsam_unstable/slam/ProjectionFactorRollingShutter.h +++ b/gtsam_unstable/slam/ProjectionFactorRollingShutter.h @@ -10,8 +10,8 @@ * -------------------------------------------------------------------------- */ /** - * @file RollingShutterProjectionFactor.h - * @brief Basic bearing factor from 2D measurement for rolling shutter cameras + * @file ProjectionFactorRollingShutter.h + * @brief Basic projection factor for rolling shutter cameras * @author Yotam Stern */ @@ -34,7 +34,7 @@ namespace gtsam { * @addtogroup SLAM */ - class RollingShutterProjectionFactor: public NoiseModelFactor3 { + class ProjectionFactorRollingShutter: public NoiseModelFactor3 { protected: // Keep a copy of measurement and calibration for I/O @@ -53,13 +53,13 @@ namespace gtsam { typedef NoiseModelFactor3 Base; /// shorthand for this class - typedef RollingShutterProjectionFactor This; + typedef ProjectionFactorRollingShutter This; /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; /// Default constructor - RollingShutterProjectionFactor() : + ProjectionFactorRollingShutter() : measured_(0, 0), interp_param_(0), throwCheirality_(false), verboseCheirality_(false) { } @@ -74,7 +74,7 @@ namespace gtsam { * @param K shared pointer to the constant calibration * @param body_P_sensor is the transform from body to sensor frame (default identity) */ - RollingShutterProjectionFactor(const Point2& measured, double interp_param, const SharedNoiseModel& model, + ProjectionFactorRollingShutter(const Point2& measured, double interp_param, const SharedNoiseModel& model, Key poseKey_a, Key poseKey_b, Key pointKey, const boost::shared_ptr& K, boost::optional body_P_sensor = boost::none) : Base(model, poseKey_a, poseKey_b, pointKey), measured_(measured), interp_param_(interp_param), K_(K), body_P_sensor_(body_P_sensor), @@ -93,7 +93,7 @@ namespace gtsam { * @param verboseCheirality determines whether exceptions are printed for Cheirality * @param body_P_sensor is the transform from body to sensor frame (default identity) */ - RollingShutterProjectionFactor(const Point2& measured, double interp_param, const SharedNoiseModel& model, + ProjectionFactorRollingShutter(const Point2& measured, double interp_param, const SharedNoiseModel& model, Key poseKey_a, Key poseKey_b, Key pointKey, const boost::shared_ptr& K, bool throwCheirality, bool verboseCheirality, boost::optional body_P_sensor = boost::none) : @@ -101,7 +101,7 @@ namespace gtsam { throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {} /** Virtual destructor */ - virtual ~RollingShutterProjectionFactor() {} + virtual ~ProjectionFactorRollingShutter() {} /// @return a deep copy of this factor virtual gtsam::NonlinearFactor::shared_ptr clone() const { @@ -114,7 +114,7 @@ namespace gtsam { * @param keyFormatter optional formatter useful for printing Symbols */ void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const { - std::cout << s << "RollingShutterProjectionFactor, z = "; + std::cout << s << "ProjectionFactorRollingShutter, z = "; traits::Print(measured_); std::cout << " rolling shutter interpolation param = " << interp_param_; if(this->body_P_sensor_) @@ -141,7 +141,7 @@ namespace gtsam { gtsam::Matrix Hprj; //pose = interpolate(pose_a, pose_b, interp_param_, H1, H2); - pose = pose_a.interp(interp_param_, pose_b, H1, H2); + pose = interpolate(pose_a, pose_b, interp_param_, H1, H2); try { if(body_P_sensor_) { if(H1 && H2) { @@ -211,4 +211,4 @@ namespace gtsam { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; // rolling shutter projection factor -} //namespace gtsam \ No newline at end of file +} //namespace gtsam diff --git a/gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp new file mode 100644 index 000000000..b0fcb23b1 --- /dev/null +++ b/gtsam_unstable/slam/tests/testProjectionFactorRollingShutter.cpp @@ -0,0 +1,233 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testProjectionFactorRollingShutter.cpp + * @brief Unit tests for ProjectionFactorRollingShutter Class + * @author Luca Carlone + * @date July 2021 + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +using namespace std::placeholders; +using namespace std; +using namespace gtsam; + +// make a realistic calibration matrix +static double fov = 60; // degrees +static size_t w=640,h=480; +static Cal3_S2::shared_ptr K(new Cal3_S2(fov,w,h)); + +// Create a noise model for the pixel error +static SharedNoiseModel model(noiseModel::Unit::Create(2)); + +// Convenience for named keys +using symbol_shorthand::X; +using symbol_shorthand::L; +using symbol_shorthand::T; + +//typedef ProjectionFactorPPP TestProjectionFactor; + +///// traits +//namespace gtsam { +//template<> +//struct traits : public Testable { +//}; +//} +// +///* ************************************************************************* */ +//TEST( ProjectionFactorPPP, nonStandard ) { +// ProjectionFactorPPP f; +//} +// +///* ************************************************************************* */ +//TEST( ProjectionFactorPPP, Constructor) { +// Key poseKey(X(1)); +// Key transformKey(T(1)); +// Key pointKey(L(1)); +// +// Point2 measurement(323.0, 240.0); +// +// TestProjectionFactor factor(measurement, model, poseKey, transformKey, pointKey, K); +//} +// +///* ************************************************************************* */ +//TEST( ProjectionFactorPPP, ConstructorWithTransform) { +// Key poseKey(X(1)); +// Key transformKey(T(1)); +// Key pointKey(L(1)); +// +// Point2 measurement(323.0, 240.0); +// TestProjectionFactor factor(measurement, model, poseKey, transformKey, pointKey, K); +//} +// +///* ************************************************************************* */ +//TEST( ProjectionFactorPPP, Equals ) { +// // Create two identical factors and make sure they're equal +// Point2 measurement(323.0, 240.0); +// +// TestProjectionFactor factor1(measurement, model, X(1), T(1), L(1), K); +// TestProjectionFactor factor2(measurement, model, X(1), T(1), L(1), K); +// +// CHECK(assert_equal(factor1, factor2)); +//} +// +///* ************************************************************************* */ +//TEST( ProjectionFactorPPP, EqualsWithTransform ) { +// // Create two identical factors and make sure they're equal +// Point2 measurement(323.0, 240.0); +// Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); +// +// TestProjectionFactor factor1(measurement, model, X(1), T(1), L(1), K); +// TestProjectionFactor factor2(measurement, model, X(1), T(1), L(1), K); +// +// CHECK(assert_equal(factor1, factor2)); +//} +// +///* ************************************************************************* */ +//TEST( ProjectionFactorPPP, Error ) { +// // Create the factor with a measurement that is 3 pixels off in x +// Key poseKey(X(1)); +// Key transformKey(T(1)); +// Key pointKey(L(1)); +// Point2 measurement(323.0, 240.0); +// TestProjectionFactor factor(measurement, model, poseKey, transformKey, pointKey, K); +// +// // Set the linearization point +// Pose3 pose(Rot3(), Point3(0,0,-6)); +// Point3 point(0.0, 0.0, 0.0); +// +// // Use the factor to calculate the error +// Vector actualError(factor.evaluateError(pose, Pose3(), point)); +// +// // The expected error is (-3.0, 0.0) pixels / UnitCovariance +// Vector expectedError = Vector2(-3.0, 0.0); +// +// // Verify we get the expected error +// CHECK(assert_equal(expectedError, actualError, 1e-9)); +//} +// +///* ************************************************************************* */ +//TEST( ProjectionFactorPPP, ErrorWithTransform ) { +// // Create the factor with a measurement that is 3 pixels off in x +// Key poseKey(X(1)); +// Key transformKey(T(1)); +// Key pointKey(L(1)); +// Point2 measurement(323.0, 240.0); +// Pose3 transform(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); +// TestProjectionFactor factor(measurement, model, poseKey,transformKey, pointKey, K); +// +// // Set the linearization point. The vehicle pose has been selected to put the camera at (-6, 0, 0) +// Pose3 pose(Rot3(), Point3(-6.25, 0.10 , -1.0)); +// Point3 point(0.0, 0.0, 0.0); +// +// // Use the factor to calculate the error +// Vector actualError(factor.evaluateError(pose, transform, point)); +// +// // The expected error is (-3.0, 0.0) pixels / UnitCovariance +// Vector expectedError = Vector2(-3.0, 0.0); +// +// // Verify we get the expected error +// CHECK(assert_equal(expectedError, actualError, 1e-9)); +//} +// +///* ************************************************************************* */ +//TEST( ProjectionFactorPPP, Jacobian ) { +// // Create the factor with a measurement that is 3 pixels off in x +// Key poseKey(X(1)); +// Key transformKey(T(1)); +// Key pointKey(L(1)); +// Point2 measurement(323.0, 240.0); +// TestProjectionFactor factor(measurement, model, poseKey, transformKey, pointKey, K); +// +// // Set the linearization point +// Pose3 pose(Rot3(), Point3(0,0,-6)); +// Point3 point(0.0, 0.0, 0.0); +// +// // Use the factor to calculate the Jacobians +// Matrix H1Actual, H2Actual, H3Actual; +// factor.evaluateError(pose, Pose3(), point, H1Actual, H2Actual, H3Actual); +// +// // The expected Jacobians +// Matrix H1Expected = (Matrix(2, 6) << 0., -554.256, 0., -92.376, 0., 0., 554.256, 0., 0., 0., -92.376, 0.).finished(); +// Matrix H3Expected = (Matrix(2, 3) << 92.376, 0., 0., 0., 92.376, 0.).finished(); +// +// // Verify the Jacobians are correct +// CHECK(assert_equal(H1Expected, H1Actual, 1e-3)); +// CHECK(assert_equal(H3Expected, H3Actual, 1e-3)); +// +// // Verify H2 with numerical derivative +// Matrix H2Expected = numericalDerivative32( +// std::function( +// std::bind(&TestProjectionFactor::evaluateError, &factor, +// std::placeholders::_1, std::placeholders::_2, +// std::placeholders::_3, boost::none, boost::none, +// boost::none)), +// pose, Pose3(), point); +// +// CHECK(assert_equal(H2Expected, H2Actual, 1e-5)); +//} +// +///* ************************************************************************* */ +//TEST( ProjectionFactorPPP, JacobianWithTransform ) { +// // Create the factor with a measurement that is 3 pixels off in x +// Key poseKey(X(1)); +// Key transformKey(T(1)); +// Key pointKey(L(1)); +// Point2 measurement(323.0, 240.0); +// Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0)); +// TestProjectionFactor factor(measurement, model, poseKey, transformKey, pointKey, K); +// +// // Set the linearization point. The vehicle pose has been selected to put the camera at (-6, 0, 0) +// Pose3 pose(Rot3(), Point3(-6.25, 0.10 , -1.0)); +// Point3 point(0.0, 0.0, 0.0); +// +// // Use the factor to calculate the Jacobians +// Matrix H1Actual, H2Actual, H3Actual; +// factor.evaluateError(pose, body_P_sensor, point, H1Actual, H2Actual, H3Actual); +// +// // The expected Jacobians +// Matrix H1Expected = (Matrix(2, 6) << -92.376, 0., 577.350, 0., 92.376, 0., -9.2376, -577.350, 0., 0., 0., 92.376).finished(); +// Matrix H3Expected = (Matrix(2, 3) << 0., -92.376, 0., 0., 0., -92.376).finished(); +// +// // Verify the Jacobians are correct +// CHECK(assert_equal(H1Expected, H1Actual, 1e-3)); +// CHECK(assert_equal(H3Expected, H3Actual, 1e-3)); +// +// // Verify H2 with numerical derivative +// Matrix H2Expected = numericalDerivative32( +// std::function( +// std::bind(&TestProjectionFactor::evaluateError, &factor, +// std::placeholders::_1, std::placeholders::_2, +// std::placeholders::_3, boost::none, boost::none, +// boost::none)), +// pose, body_P_sensor, point); +// +// CHECK(assert_equal(H2Expected, H2Actual, 1e-5)); +// +// +//} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ +