From 3571420010a332743293a8903daeba0afc927d90 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Tue, 3 May 2022 13:54:25 -0400 Subject: [PATCH] Added PinholePose in wrap and FromPose3 in EssentialMatrix --- gtsam/geometry/geometry.i | 71 ++++++++++++++++++++++++++++++++++++- gtsam/nonlinear/nonlinear.i | 16 +++++++++ 2 files changed, 86 insertions(+), 1 deletion(-) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 415aa0dc4..63fd7056a 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -547,6 +547,12 @@ class EssentialMatrix { // Standard Constructors EssentialMatrix(const gtsam::Rot3& aRb, const gtsam::Unit3& aTb); + // Constructors from Pose3 + gtsam::EssentialMatrix FromPose3(const gtsam::Pose3& _1P2_); + + gtsam::EssentialMatrix FromPose3(const gtsam::Pose3& _1P2_, + Eigen::Ref H); + // Testable void print(string s = "") const; bool equals(const gtsam::EssentialMatrix& pose, double tol) const; @@ -904,6 +910,12 @@ class PinholeCamera { Eigen::Ref Dresult_dp, Eigen::Ref Dresult_ddepth, Eigen::Ref Dresult_dcal); + + gtsam::Point2 reprojectionError(const gtsam::Point3& pw, const gtsam::Point2& measured, + Eigen::Ref Dpose, + Eigen::Ref Dpoint, + Eigen::Ref Dcal); + double range(const gtsam::Point3& point); double range(const gtsam::Point3& point, Eigen::Ref Dcamera, Eigen::Ref Dpoint); @@ -914,7 +926,58 @@ class PinholeCamera { // enabling serialization functionality void serialize() const; }; - + +#include +template +class PinholePose { + // Standard Constructors and Named Constructors + PinholePose(); + PinholePose(const gtsam::PinholePose other); + PinholePose(const gtsam::Pose3& pose); + PinholePose(const gtsam::Pose3& pose, const CALIBRATION* K); + static This Level(const gtsam::Pose2& pose, double height); + static This Lookat(const gtsam::Point3& eye, const gtsam::Point3& target, + const gtsam::Point3& upVector, const CALIBRATION* K); + + // Testable + void print(string s = "PinholePose") const; + bool equals(const This& camera, double tol) const; + + // Standard Interface + gtsam::Pose3 pose() const; + CALIBRATION calibration() const; + + // Manifold + This retract(Vector d) const; + Vector localCoordinates(const This& T2) const; + size_t dim() const; + static size_t Dim(); + + // Transformations and measurement functions + static gtsam::Point2 Project(const gtsam::Point3& cameraPoint); + pair projectSafe(const gtsam::Point3& pw) const; + gtsam::Point2 project(const gtsam::Point3& point); + gtsam::Point2 project(const gtsam::Point3& point, + Eigen::Ref Dpose, + Eigen::Ref Dpoint, + Eigen::Ref Dcal); + gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const; + gtsam::Point3 backproject(const gtsam::Point2& p, double depth, + Eigen::Ref Dresult_dpose, + Eigen::Ref Dresult_dp, + Eigen::Ref Dresult_ddepth, + Eigen::Ref Dresult_dcal); + double range(const gtsam::Point3& point); + double range(const gtsam::Point3& point, Eigen::Ref Dcamera, + Eigen::Ref Dpoint); + double range(const gtsam::Pose3& pose); + double range(const gtsam::Pose3& pose, Eigen::Ref Dcamera, + Eigen::Ref Dpose); + + // enabling serialization functionality + void serialize() const; +}; + #include class Similarity2 { // Standard Constructors @@ -971,6 +1034,12 @@ typedef gtsam::PinholeCamera PinholeCameraCal3Unified; typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; typedef gtsam::PinholeCamera PinholeCameraCal3Fisheye; +typedef gtsam::PinholePose PinholePoseCal3_S2; +typedef gtsam::PinholePose PinholePoseCal3DS2; +typedef gtsam::PinholePose PinholePoseCal3Unified; +typedef gtsam::PinholePose PinholePoseCal3Bundler; +typedef gtsam::PinholePose PinholePoseCal3Fisheye; + template class CameraSet { CameraSet(); diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 3fff71978..30181e08d 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -226,6 +226,10 @@ class Values { void insert(size_t j, const gtsam::PinholeCamera& camera); void insert(size_t j, const gtsam::PinholeCamera& camera); void insert(size_t j, const gtsam::PinholeCamera& camera); + void insert(size_t j, const gtsam::PinholePose& camera); + void insert(size_t j, const gtsam::PinholePose& camera); + void insert(size_t j, const gtsam::PinholePose& camera); + void insert(size_t j, const gtsam::PinholePose& camera); void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); void insert(size_t j, const gtsam::NavState& nav_state); void insert(size_t j, double c); @@ -269,6 +273,10 @@ class Values { void update(size_t j, const gtsam::PinholeCamera& camera); void update(size_t j, const gtsam::PinholeCamera& camera); void update(size_t j, const gtsam::PinholeCamera& camera); + void update(size_t j, const gtsam::PinholePose& camera); + void update(size_t j, const gtsam::PinholePose& camera); + void update(size_t j, const gtsam::PinholePose& camera); + void update(size_t j, const gtsam::PinholePose& camera); void update(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); void update(size_t j, const gtsam::NavState& nav_state); void update(size_t j, Vector vector); @@ -310,6 +318,10 @@ class Values { void insert_or_assign(size_t j, const gtsam::PinholeCamera& camera); void insert_or_assign(size_t j, const gtsam::PinholeCamera& camera); void insert_or_assign(size_t j, const gtsam::PinholeCamera& camera); + void insert_or_assign(size_t j, const gtsam::PinholePose& camera); + void insert_or_assign(size_t j, const gtsam::PinholePose& camera); + void insert_or_assign(size_t j, const gtsam::PinholePose& camera); + void insert_or_assign(size_t j, const gtsam::PinholePose& camera); void insert_or_assign(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); void insert_or_assign(size_t j, const gtsam::NavState& nav_state); void insert_or_assign(size_t j, Vector vector); @@ -351,6 +363,10 @@ class Values { gtsam::PinholeCamera, gtsam::PinholeCamera, gtsam::PinholeCamera, + gtsam::PinholePose, + gtsam::PinholePose, + gtsam::PinholePose, + gtsam::PinholePose, gtsam::imuBias::ConstantBias, gtsam::NavState, Vector,