diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index f282d53c73..c1f2263373 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -1472,6 +1472,43 @@ typedef gtsam::PinholePose PinholePoseCal3Unified; typedef gtsam::PinholePose PinholePoseCal3Bundler; typedef gtsam::PinholePose PinholePoseCal3Fisheye; +#include +class SphericalCamera { + // Standard Constructors + SphericalCamera(); + SphericalCamera(const gtsam::Pose3& pose); + SphericalCamera(const gtsam::Pose3& pose, + const gtsam::EmptyCal::shared_ptr& cal); + SphericalCamera(const gtsam::Vector& v); + + // Testable + bool equals(const gtsam::SphericalCamera& camera, double tol = 1e-9) const; + void print(const std::string& s = "SphericalCamera") const; + + // Standard Interface + gtsam::Pose3 pose() const; + const gtsam::Rot3& rotation() const; + const gtsam::Point3& translation() const; + + // Transformations and measurement functions + pair projectSafe(const gtsam::Point3& pw) const; + gtsam::Unit3 project(const gtsam::Point3& point) const; + gtsam::Unit3 project2(const gtsam::Point3& point) const; + gtsam::Unit3 project2(const gtsam::Unit3& pwu) const; + gtsam::Point3 backproject(const gtsam::Unit3& p, double depth) const; + gtsam::Unit3 backprojectPointAtInfinity(const gtsam::Unit3& p) const; + gtsam::Vector2 reprojectionError(const gtsam::Point3& point, + const gtsam::Unit3& measured) const; + + // Manifold + gtsam::SphericalCamera retract(const gtsam::Vector6& d) const; + gtsam::Vector6 localCoordinates(const gtsam::SphericalCamera& p) const; + static gtsam::SphericalCamera Identity(); + + // enabling serialization functionality + void serialize() const; +}; + #include class Similarity2 { // Standard Constructors @@ -1851,6 +1888,22 @@ gtsam::TriangulationResult triangulateSafe( const gtsam::Point2Vector& measurements, const gtsam::TriangulationParameters& params); +// Spherical versions +gtsam::Point3 triangulatePoint3( + const gtsam::CameraSet& cameras, + const gtsam::SphericalCamera::MeasurementVector& measurements, + double rank_tol, bool optimize, + const gtsam::SharedNoiseModel& model = nullptr, + const bool useLOST = false); +gtsam::Point3 triangulateNonlinear( + const gtsam::CameraSet& cameras, + const gtsam::SphericalCamera::MeasurementVector& measurements, + const gtsam::Point3& initialEstimate); +gtsam::TriangulationResult triangulateSafe( + const gtsam::CameraSet& cameras, + const gtsam::SphericalCamera::MeasurementVector& measurements, + const gtsam::TriangulationParameters& params); + #include template diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 0838523e0a..49c44b9261 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -20,6 +20,7 @@ namespace gtsam { #include #include #include +#include #include #include #include @@ -630,6 +631,7 @@ template , gtsam::PinholeCamera, gtsam::PinholeCamera, + gtsam::SphericalCamera, gtsam::NavState, gtsam::imuBias::ConstantBias}> virtual class PriorFactor : gtsam::NoiseModelFactor { @@ -738,6 +740,7 @@ template , gtsam::PinholeCamera, gtsam::PinholeCamera, + gtsam::SphericalCamera, gtsam::imuBias::ConstantBias}> virtual class NonlinearEquality : gtsam::NoiseModelFactor { // Constructor - forces exact evaluation diff --git a/gtsam/nonlinear/values.i b/gtsam/nonlinear/values.i index bb85275629..ba8c21797a 100644 --- a/gtsam/nonlinear/values.i +++ b/gtsam/nonlinear/values.i @@ -30,6 +30,7 @@ namespace gtsam { #include #include #include +#include #include #include @@ -113,6 +114,7 @@ class Values { void insert(gtsam::Key j, const gtsam::PinholePose& camera); void insert(gtsam::Key j, const gtsam::PinholePose& camera); void insert(gtsam::Key j, const gtsam::PinholePose& camera); + void insert(gtsam::Key j, const gtsam::SphericalCamera& camera); void insert(gtsam::Key j, const gtsam::imuBias::ConstantBias& constant_bias); void insert(gtsam::Key j, const gtsam::NavState& nav_state); void insert(gtsam::Key j, double c); @@ -157,6 +159,7 @@ class Values { void update(gtsam::Key j, const gtsam::PinholePose& camera); void update(gtsam::Key j, const gtsam::PinholePose& camera); void update(gtsam::Key j, const gtsam::PinholePose& camera); + void update(gtsam::Key j, const gtsam::SphericalCamera& camera); void update(gtsam::Key j, const gtsam::imuBias::ConstantBias& constant_bias); void update(gtsam::Key j, const gtsam::NavState& nav_state); void update(gtsam::Key j, double c); @@ -201,6 +204,7 @@ class Values { void insert_or_assign(gtsam::Key j, const gtsam::PinholePose& camera); void insert_or_assign(gtsam::Key j, const gtsam::PinholePose& camera); void insert_or_assign(gtsam::Key j, const gtsam::PinholePose& camera); + void insert_or_assign(gtsam::Key j, const gtsam::SphericalCamera& camera); void insert_or_assign(gtsam::Key j, const gtsam::imuBias::ConstantBias& constant_bias); void insert_or_assign(gtsam::Key j, const gtsam::NavState& nav_state); void insert_or_assign(gtsam::Key j, double c); @@ -243,6 +247,7 @@ class Values { gtsam::PinholePose, gtsam::PinholePose, gtsam::PinholePose, + gtsam::SphericalCamera, gtsam::imuBias::ConstantBias, gtsam::NavState, double}> diff --git a/gtsam/slam/GeneralSFMFactor.h b/gtsam/slam/GeneralSFMFactor.h index 71ddac8887..e8dc931127 100644 --- a/gtsam/slam/GeneralSFMFactor.h +++ b/gtsam/slam/GeneralSFMFactor.h @@ -42,6 +42,7 @@ #endif #include #include +#include namespace boost { namespace serialization { @@ -62,14 +63,16 @@ class GeneralSFMFactor: public NoiseModelFactorN { GTSAM_CONCEPT_MANIFOLD_TYPE(CAMERA) GTSAM_CONCEPT_MANIFOLD_TYPE(LANDMARK) + using Measurement = typename CAMERA::Measurement; static const int DimC = FixedDimension::value; static const int DimL = FixedDimension::value; - typedef Eigen::Matrix JacobianC; - typedef Eigen::Matrix JacobianL; + static const int ZDim = traits::dimension; + typedef Eigen::Matrix JacobianC; + typedef Eigen::Matrix JacobianL; protected: - Point2 measured_; ///< the 2D measurement + Measurement measured_; ///< the measurement public: @@ -89,14 +92,18 @@ class GeneralSFMFactor: public NoiseModelFactorN { * @param cameraKey is the index of the camera * @param landmarkKey is the index of the landmark */ - GeneralSFMFactor(const Point2& measured, const SharedNoiseModel& model, + GeneralSFMFactor(const Measurement& measured, const SharedNoiseModel& model, Key cameraKey, Key landmarkKey) : Base(model, cameraKey, landmarkKey), measured_(measured) {} - GeneralSFMFactor() : measured_(0.0, 0.0) {} ///< default constructor - ///< constructor that takes a Point2 + GeneralSFMFactor() : measured_(Measurement()) {} ///< default constructor + ///< constructor that takes a Point2 (only enabled for Point2 measurements) + template ::value, int>::type = 0> GeneralSFMFactor(const Point2& p) : measured_(p) {} ///< constructor that takes doubles x,y to make a Point2 + template ::value, int>::type = 0> GeneralSFMFactor(double x, double y) : measured_(x, y) {} ~GeneralSFMFactor() override {} ///< destructor @@ -113,7 +120,7 @@ class GeneralSFMFactor: public NoiseModelFactorN { */ void print(const std::string& s = "SFMFactor", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { Base::print(s, keyFormatter); - traits::Print(measured_, s + ".z"); + traits::Print(measured_, s + ".z"); } /** @@ -121,20 +128,21 @@ class GeneralSFMFactor: public NoiseModelFactorN { */ bool equals(const NonlinearFactor &p, double tol = 1e-9) const override { const This* e = dynamic_cast(&p); - return e && Base::equals(p, tol) && traits::Equals(this->measured_, e->measured_, tol); + return e && Base::equals(p, tol) && + traits::Equals(this->measured_, e->measured_, tol); } /** h(x)-z */ Vector evaluateError(const CAMERA& camera, const LANDMARK& point, OptionalMatrixType H1, OptionalMatrixType H2) const override { try { - return camera.project2(point,H1,H2) - measured_; - } - catch( CheiralityException& e [[maybe_unused]]) { + Measurement predicted = camera.project2(point, H1, H2); + return traits::Local(measured_, predicted); + } catch (CheiralityException& e [[maybe_unused]]) { if (H1) *H1 = JacobianC::Zero(); if (H2) *H2 = JacobianL::Zero(); //TODO Print the exception via logging - return Z_2x1; + return Vector::Zero(ZDim); } } @@ -144,17 +152,19 @@ class GeneralSFMFactor: public NoiseModelFactorN { if (!this->active(values)) return std::shared_ptr(); const Key key1 = this->key1(), key2 = this->key2(); - JacobianC H1; - JacobianL H2; - Vector2 b; + JacobianC Dcamera; + JacobianL Dlandmark; + Vector b; try { - const CAMERA& camera = values.atRef(key1); - const LANDMARK& point = values.atRef(key2); - b = measured() - camera.project2(point, H1, H2); + const CAMERA& camera = values.at(key1); + const LANDMARK& point = values.at(key2); + Measurement predicted = camera.project2(point, &Dcamera, &Dlandmark); + + b = -Vector(traits::Local(measured_, predicted)); } catch (CheiralityException& e [[maybe_unused]]) { - H1.setZero(); - H2.setZero(); - b.setZero(); + Dcamera.setZero(); + Dlandmark.setZero(); + b = Vector::Zero(ZDim); //TODO Print the exception via logging } @@ -163,8 +173,8 @@ class GeneralSFMFactor: public NoiseModelFactorN { if (noiseModel && !noiseModel->isUnit()) { // TODO: implement WhitenSystem for fixed size matrices and include // above - H1 = noiseModel->Whiten(H1); - H2 = noiseModel->Whiten(H2); + Dcamera = noiseModel->Whiten(Dcamera); + Dlandmark = noiseModel->Whiten(Dlandmark); b = noiseModel->Whiten(b); } @@ -174,11 +184,13 @@ class GeneralSFMFactor: public NoiseModelFactorN { model = std::static_pointer_cast(noiseModel)->unit(); } - return std::make_shared >(key1, H1, key2, H2, b, model); + return std::make_shared>(key1, Dcamera, + key2, Dlandmark, + b, model); } /** return the measured */ - inline const Point2 measured() const { + inline const Measurement measured() const { return measured_; } diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index 745589c251..068680b2fa 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -10,6 +10,7 @@ namespace gtsam { #include #include #include +#include // Following header defines PinholeCamera{Cal3_S2|Cal3DS2|Cal3Bundler|Cal3Fisheye|Cal3Unified} #include @@ -106,10 +107,10 @@ typedef gtsam::GenericProjectionFactor template virtual class GeneralSFMFactor : gtsam::NoiseModelFactor { - GeneralSFMFactor(const gtsam::Point2& measured, + GeneralSFMFactor(const CAMERA::Measurement& measured, const gtsam::noiseModel::Base* model, gtsam::Key cameraKey, gtsam::Key landmarkKey); - gtsam::Point2 measured() const; + CAMERA::Measurement measured() const; }; typedef gtsam::GeneralSFMFactor, gtsam::Point3> @@ -142,6 +143,8 @@ typedef gtsam::GeneralSFMFactor, typedef gtsam::GeneralSFMFactor, gtsam::Point3> GeneralSFMFactorPoseCal3Unified; +typedef gtsam::GeneralSFMFactor + GeneralSFMFactorSphericalCamera; template @@ -157,19 +160,20 @@ virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor { #include -// Currently not wrapping SphericalCamera, since measurement type is not Point2 but Unit3 template < CAMERA = {gtsam::PinholeCameraCal3_S2, gtsam::PinholeCameraCal3DS2, gtsam::PinholeCameraCal3Bundler, gtsam::PinholeCameraCal3Fisheye, gtsam::PinholeCameraCal3Unified, gtsam::PinholePoseCal3_S2, gtsam::PinholePoseCal3DS2, gtsam::PinholePoseCal3Bundler, - gtsam::PinholePoseCal3Fisheye, gtsam::PinholePoseCal3Unified}> + gtsam::PinholePoseCal3Fisheye, gtsam::PinholePoseCal3Unified, + gtsam::SphericalCamera}> virtual class SmartFactorBase : gtsam::NonlinearFactor { - void add(const gtsam::Point2& measured, gtsam::Key key); - void add(const gtsam::Point2Vector& measurements, const gtsam::KeyVector& cameraKeys); + void add(const CAMERA::Measurement& measured, gtsam::Key key); + void add(const CAMERA::MeasurementVector& measurements, + const gtsam::KeyVector& cameraKeys); size_t dim() const; - const gtsam::Point2Vector& measured() const; - gtsam::CameraSet cameras(const gtsam::Values& values) const; + const CAMERA::MeasurementVector& measured() const; + std::vector cameras(const gtsam::Values& values) const; void print(const std::string& s = "", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; @@ -205,7 +209,8 @@ template < gtsam::PinholeCameraCal3Bundler, gtsam::PinholeCameraCal3Fisheye, gtsam::PinholeCameraCal3Unified, gtsam::PinholePoseCal3_S2, gtsam::PinholePoseCal3DS2, gtsam::PinholePoseCal3Bundler, - gtsam::PinholePoseCal3Fisheye, gtsam::PinholePoseCal3Unified}> + gtsam::PinholePoseCal3Fisheye, gtsam::PinholePoseCal3Unified, + gtsam::SphericalCamera}> virtual class SmartProjectionFactor : gtsam::SmartFactorBase { SmartProjectionFactor(); @@ -290,11 +295,12 @@ virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor { }; #include -// Only for PinholePose cameras -> PinholeCamera is not supported +// Only for pose-only cameras (e.g., PinholePose or SphericalCamera) template + gtsam::PinholePoseCal3Unified, + gtsam::SphericalCamera}> virtual class SmartProjectionRigFactor : gtsam::SmartProjectionFactor { SmartProjectionRigFactor(); @@ -303,10 +309,10 @@ virtual class SmartProjectionRigFactor : gtsam::SmartProjectionFactor { const gtsam::CameraSet* cameraRig, const gtsam::SmartProjectionParams& params = gtsam::SmartProjectionParams()); - void add(const gtsam::Point2& measured, const gtsam::Key& poseKey, + void add(const CAMERA::Measurement& measured, const gtsam::Key& poseKey, const size_t& cameraId = 0); - void add(const gtsam::Point2Vector& measurements, const gtsam::KeyVector& poseKeys, + void add(const CAMERA::MeasurementVector& measurements, const gtsam::KeyVector& poseKeys, const gtsam::FastVector& cameraIds = gtsam::FastVector()); const gtsam::KeyVector& nonUniqueKeys() const; diff --git a/gtsam_unstable/slam/serialization.cpp b/gtsam_unstable/slam/serialization.cpp index bee69ab830..3a840ed6cf 100644 --- a/gtsam_unstable/slam/serialization.cpp +++ b/gtsam_unstable/slam/serialization.cpp @@ -25,6 +25,7 @@ #include #include #include +#include //#include using namespace gtsam; @@ -82,6 +83,8 @@ typedef GenericProjectionFactor GenericProjectionFactorC typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3_S2; typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3DS2; +typedef gtsam::GeneralSFMFactor + GeneralSFMFactorSphericalCamera; typedef gtsam::GeneralSFMFactor2 GeneralSFMFactor2Cal3_S2; @@ -173,6 +176,8 @@ BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3DS2, "gtsam::GenericProjectio BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3_S2, "gtsam::GeneralSFMFactorCal3_S2"); BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3DS2, "gtsam::GeneralSFMFactorCal3DS2"); +BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorSphericalCamera, + "gtsam::GeneralSFMFactorSphericalCamera"); BOOST_CLASS_EXPORT_GUID(GeneralSFMFactor2Cal3_S2, "gtsam::GeneralSFMFactor2Cal3_S2"); @@ -288,4 +293,3 @@ Values::shared_ptr gtsam::deserializeValuesFromXMLFile(const std::string& fname, /* ************************************************************************* */ - diff --git a/python/gtsam/examples/SFMExample_SphericalCamera.py b/python/gtsam/examples/SFMExample_SphericalCamera.py new file mode 100644 index 0000000000..f979eba848 --- /dev/null +++ b/python/gtsam/examples/SFMExample_SphericalCamera.py @@ -0,0 +1,152 @@ +""" +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 + +A structure-from-motion example using SphericalCamera. + +SphericalCamera measures bearing vectors (Unit3) instead of pixel coordinates, +making it suitable for omnidirectional cameras, fisheye cameras with very wide +field of view, or any setup where direction measurements are more natural. +""" + +import matplotlib.pyplot as plt +import numpy as np + +import gtsam +from gtsam import symbol_shorthand + +L = symbol_shorthand.L +X = symbol_shorthand.X + +from gtsam.examples import SFMdata +from gtsam.utils import plot + +from gtsam import (DoglegOptimizer, GeneralSFMFactorSphericalCamera, + Marginals, NonlinearFactorGraph, SphericalCamera, + PriorFactorPoint3, PriorFactorSphericalCamera, Values, Unit3) + + +def main(): + """ + Structure-from-Motion using SphericalCamera. + + Unlike PinholeCamera which measures pixel coordinates (Point2), + SphericalCamera measures bearing vectors (Unit3) - directions on the + unit sphere. This is the natural measurement model for omnidirectional + cameras or when working with calibrated bearing measurements. + + Key differences from standard SFM: + - No camera calibration needed (SphericalCamera uses EmptyCal) + - Measurements are Unit3 (2 DOF direction) instead of Point2 (2 DOF pixel) + - Uses GeneralSFMFactorSphericalCamera instead of GenericProjectionFactor + """ + + # Create the set of ground-truth landmarks (cube corners) + points = SFMdata.createPoints() + + # Create the set of ground-truth poses (circular trajectory around landmarks) + poses = SFMdata.createPoses() + + # Define the camera observation noise model + # Unit3 has 2 DOF, so we use a 2D isotropic noise model + # The sigma represents angular uncertainty in radians + measurement_noise = gtsam.noiseModel.Isotropic.Sigma(2, 0.01) # ~0.6 degrees + + # Create a factor graph + graph = NonlinearFactorGraph() + + # Add a prior on camera x0. This indirectly specifies where the origin is. + # SphericalCamera has 6 DOF (same as Pose3): 0.1 rad std on roll,pitch,yaw and 0.1m on x,y,z + camera_noise = gtsam.noiseModel.Diagonal.Sigmas( + np.array([0.1, 0.1, 0.1, 0.1, 0.1, 0.1]) + ) + graph.push_back(PriorFactorSphericalCamera(X(0), SphericalCamera(poses[0]), camera_noise)) + + # Simulated measurements from each camera pose, adding them to the factor graph + for i, pose in enumerate(poses): + camera = SphericalCamera(pose) + for j, point in enumerate(points): + # project2 returns Unit3 (bearing vector) + measurement = camera.project2(point) + factor = GeneralSFMFactorSphericalCamera( + measurement, measurement_noise, X(i), L(j) + ) + graph.push_back(factor) + + # Add a prior on the first landmark to fix the scale + # (SFM has scale ambiguity, this fixes it) + point_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) + graph.push_back(PriorFactorPoint3(L(0), points[0], point_noise)) + + print("Factor Graph:") + print(f" Number of factors: {graph.size()}") + print(f" Number of poses: {len(poses)}") + print(f" Number of landmarks: {len(points)}") + + # Create initial estimates with some noise + initial_estimate = Values() + rng = np.random.default_rng(42) # Fixed seed for reproducibility + + for i, pose in enumerate(poses): + # Add noise to initial pose estimates + noisy_pose = pose.retract(0.1 * rng.standard_normal(6)) + initial_estimate.insert(X(i), SphericalCamera(noisy_pose)) + + for j, point in enumerate(points): + # Add noise to initial landmark estimates + noisy_point = point + 0.1 * rng.standard_normal(3) + initial_estimate.insert(L(j), noisy_point) + + print(f"\nInitial error: {graph.error(initial_estimate):.6f}") + + # Optimize the graph using Dogleg optimizer + params = gtsam.DoglegParams() + params.setVerbosity("TERMINATION") + optimizer = DoglegOptimizer(graph, initial_estimate, params) + + print("\nOptimizing...") + result = optimizer.optimize() + + print(f"Final error: {graph.error(result):.6f}") + print(f"Iterations: {optimizer.iterations()}") + + # Print some results + print("\n=== Results ===") + print("\nOptimized Landmark Positions:") + for j in range(len(points)): + estimated = result.atPoint3(L(j)) + ground_truth = points[j] + error = np.linalg.norm(estimated - ground_truth) + print(f" L{j}: estimated={estimated}, error={error:.4f}m") + + print("\nOptimized Camera Poses (translation only):") + for i in range(len(poses)): + estimated_camera = result.atSphericalCamera(X(i)) + estimated_trans = estimated_camera.pose().translation() + ground_truth_trans = poses[i].translation() + error = np.linalg.norm(estimated_trans - ground_truth_trans) + print(f" X{i}: estimated={estimated_trans}, error={error:.4f}m") + + # Compute marginals for uncertainty visualization + marginals = Marginals(graph, result) + + # Extract poses from SphericalCamera results for plotting + result_poses = Values() + for i in range(len(poses)): + camera = result.atSphericalCamera(X(i)) + result_poses.insert(X(i), camera.pose()) + + # Plot the results + plot.plot_3d_points(1, result, marginals=marginals) + plot.plot_trajectory(1, result_poses, marginals=None, scale=8) + plot.set_axes_equal(1) + plt.title("SphericalCamera SFM Result") + plt.show() + + +if __name__ == "__main__": + main() diff --git a/python/gtsam/preamble/geometry.h b/python/gtsam/preamble/geometry.h index 40b841abc1..ad4b20e60a 100644 --- a/python/gtsam/preamble/geometry.h +++ b/python/gtsam/preamble/geometry.h @@ -15,3 +15,4 @@ PYBIND11_MAKE_OPAQUE(gtsam::CameraSet>); PYBIND11_MAKE_OPAQUE(gtsam::CameraSet>); PYBIND11_MAKE_OPAQUE(gtsam::CameraSet>); PYBIND11_MAKE_OPAQUE(gtsam::CameraSet>); +PYBIND11_MAKE_OPAQUE(gtsam::CameraSet); diff --git a/python/gtsam/specializations/geometry.h b/python/gtsam/specializations/geometry.h index 4aea1b5cf2..4469ea481d 100644 --- a/python/gtsam/specializations/geometry.h +++ b/python/gtsam/specializations/geometry.h @@ -21,3 +21,5 @@ py::bind_vector>>( m_, "CameraSetCal3Unified"); py::bind_vector>>( m_, "CameraSetCal3Fisheye"); +py::bind_vector>( + m_, "CameraSetSpherical"); diff --git a/python/gtsam/tests/test_SphericalCamera.py b/python/gtsam/tests/test_SphericalCamera.py new file mode 100644 index 0000000000..388ae653c6 --- /dev/null +++ b/python/gtsam/tests/test_SphericalCamera.py @@ -0,0 +1,65 @@ +""" +GTSAM Copyright 2010-2021, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +SphericalCamera unit tests. +Author: auto-generated +""" +import unittest + +import numpy as np + +import gtsam +from gtsam import Point3, Pose3, Rot3, SphericalCamera, Unit3 +from gtsam.utils.test_case import GtsamTestCase + +# Camera pose: Rot3 with diagonal (1, -1, -1), translation (0, 0, 0.5) +pose = Pose3(Rot3(np.diag([1., -1., -1.])), Point3(0, 0, 0.5)) +camera = SphericalCamera(pose) + +# Test points +point1 = Point3(-0.08, -0.08, 0.0) +point2 = Point3(-0.08, 0.08, 0.0) +point3 = Point3(0.08, 0.08, 0.0) +point4 = Point3(0.08, -0.08, 0.0) + +# Manually computed bearing vectors (from Matlab) +bearing1 = Unit3(np.array([-0.156054862928174, 0.156054862928174, 0.975342893301088])) +bearing2 = Unit3(np.array([-0.156054862928174, -0.156054862928174, 0.975342893301088])) +bearing3 = Unit3(np.array([0.156054862928174, -0.156054862928174, 0.975342893301088])) +bearing4 = Unit3(np.array([0.156054862928174, 0.156054862928174, 0.975342893301088])) + +depth = 0.512640224719052 + + +class TestSphericalCamera(GtsamTestCase): + + def test_constructor(self): + """Test construction from Pose3 and pose() accessor.""" + self.gtsamAssertEquals(camera.pose(), pose) + + def test_project(self): + """Test projection of 4 points to bearing vectors.""" + self.gtsamAssertEquals(camera.project(point1), bearing1) + self.gtsamAssertEquals(camera.project(point2), bearing2) + self.gtsamAssertEquals(camera.project(point3), bearing3) + self.gtsamAssertEquals(camera.project(point4), bearing4) + + def test_backproject(self): + """Test back-projection from bearing + depth to 3D point.""" + self.gtsamAssertEquals(camera.backproject(bearing1, depth), point1) + self.gtsamAssertEquals(camera.backproject(bearing2, depth), point2) + self.gtsamAssertEquals(camera.backproject(bearing3, depth), point3) + self.gtsamAssertEquals(camera.backproject(bearing4, depth), point4) + + def test_reprojection_error(self): + """Test that reprojection error is zero at ground truth.""" + result = camera.reprojectionError(point1, bearing1) + np.testing.assert_allclose(result, np.zeros(2), atol=1e-9) + + +if __name__ == "__main__": + unittest.main()