diff --git a/gtsam/base/base.i b/gtsam/base/base.i index 31d09cb3d5..86b1b82eab 100644 --- a/gtsam/base/base.i +++ b/gtsam/base/base.i @@ -6,6 +6,7 @@ namespace gtsam { #include #include +#include #include #include #include @@ -102,7 +103,7 @@ virtual class Value { #include template virtual class GenericValue : gtsam::Value { diff --git a/gtsam/geometry/Cal3DS2_k3.cpp b/gtsam/geometry/Cal3DS2_k3.cpp new file mode 100644 index 0000000000..dfd0a83ecd --- /dev/null +++ b/gtsam/geometry/Cal3DS2_k3.cpp @@ -0,0 +1,59 @@ +/* ---------------------------------------------------------------------------- + + * 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 Cal3DS2_k3.cpp + * @date Dec 19, 2023 + * @author demul + * @author Hyeonjin Jeong + */ + +#include +#include +#include +#include +#include + +namespace gtsam +{ +/* ************************************************************************* */ +std::ostream& operator<<(std::ostream& os, const Cal3DS2_k3& cal) +{ + os << (Cal3DS2_k3_Base&)cal; + return os; +} + +/* ************************************************************************* */ +void Cal3DS2_k3::print(const std::string& s_) const +{ + Base::print(s_); +} + +/* ************************************************************************* */ +bool Cal3DS2_k3::equals(const Cal3DS2_k3& K, double tol) const +{ + const Cal3DS2_k3_Base* base = dynamic_cast(&K); + return Cal3DS2_k3_Base::equals(*base, tol); +} + +/* ************************************************************************* */ +Cal3DS2_k3 Cal3DS2_k3::retract(const Vector& d) const +{ + return Cal3DS2_k3(vector() + d); +} + +/* ************************************************************************* */ +Vector Cal3DS2_k3::localCoordinates(const Cal3DS2_k3& T2) const +{ + return T2.vector() - vector(); +} +} // namespace gtsam +/* ************************************************************************* */ diff --git a/gtsam/geometry/Cal3DS2_k3.h b/gtsam/geometry/Cal3DS2_k3.h new file mode 100644 index 0000000000..fe8324c2c1 --- /dev/null +++ b/gtsam/geometry/Cal3DS2_k3.h @@ -0,0 +1,150 @@ +/* ---------------------------------------------------------------------------- + + * 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 Cal3DS2_k3.h + * @brief Calibration of a camera with radial distortion whose coefficients are not only k1, k2 but also k3, + * calculations in base class Cal3DS2_Base_k3 + * @date Dec 19, 2023 + * @author demul + * @author Hyeonjin Jeong + */ + +#pragma once + +#include + +namespace gtsam +{ +/** + * @brief Calibration of a camera with radial distortion whose coefficients are not only k1, k2 but also k3 + * Lie-group behaviors for optimization. + * \sa Cal3DS2_Base_k3 + * @addtogroup geometry + * \nosubgrouping + */ +class GTSAM_EXPORT Cal3DS2_k3 : public Cal3DS2_k3_Base +{ + using Base = Cal3DS2_k3_Base; + + public: + enum + { + dimension = 10 + }; + + /// @name Standard Constructors + /// @{ + + /// Default Constructor with only unit focal length + Cal3DS2_k3() = default; + + Cal3DS2_k3( + double fx, + double fy, + double s, + double u0, + double v0, + double k1, + double k2, + double p1 = 0.0, + double p2 = 0.0, + double k3 = 0.0, + double tol = + 1e-5) // The order in which the coefficients are sorted follows the same order as the one used in opencv + : Base(fx, fy, s, u0, v0, k1, k2, p1, p2, k3, tol) + { + } + + ~Cal3DS2_k3() override + { + } + + /// @} + /// @name Advanced Constructors + /// @{ + + Cal3DS2_k3(const Vector10& v) : Base(v) + { + } + + /// @} + /// @name Testable + /// @{ + + /// Output stream operator + GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const Cal3DS2_k3& cal); + + /// print with optional string + void print(const std::string& s = "") const override; + + /// assert equality up to a tolerance + bool equals(const Cal3DS2_k3& K, double tol = 10e-9) const; + + /// @} + /// @name Manifold + /// @{ + + /// Given delta vector, update calibration + Cal3DS2_k3 retract(const Vector& d) const; + + /// Given a different calibration, calculate update to obtain it + Vector localCoordinates(const Cal3DS2_k3& T2) const; + + /// Return dimensions of calibration manifold object + size_t dim() const override + { + return Dim(); + } + + /// Return dimensions of calibration manifold object + inline static size_t Dim() + { + return dimension; + } + + /// @} + /// @name Clone + /// @{ + + /// @return a deep copy of this object + boost::shared_ptr clone() const override + { + return boost::shared_ptr(new Cal3DS2_k3(*this)); + } + + /// @} + + private: + /// @name Advanced Interface + /// @{ + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive& ar, const unsigned int /*version*/) + { + ar& boost::serialization::make_nvp("Cal3DS2_k3", boost::serialization::base_object(*this)); + } + + /// @} +}; + +template <> +struct traits : public internal::Manifold +{ +}; + +template <> +struct traits : public internal::Manifold +{ +}; +} // namespace gtsam diff --git a/gtsam/geometry/Cal3DS2_k3_Base.cpp b/gtsam/geometry/Cal3DS2_k3_Base.cpp new file mode 100644 index 0000000000..1c637255a6 --- /dev/null +++ b/gtsam/geometry/Cal3DS2_k3_Base.cpp @@ -0,0 +1,214 @@ +/* ---------------------------------------------------------------------------- + + * 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 Cal3DS2_k3_Base.cpp + * @date Dec 18, 2023 + * @author demul + * @author Hyeonjin Jeong + */ + +#include +#include +#include +#include +#include + +namespace gtsam { +/* ************************************************************************* */ +Vector10 Cal3DS2_k3_Base::vector() const { + Vector10 v; + v << fx_, fy_, s_, u0_, v0_, k1_, k2_, p1_, p2_, k3_; + return v; +} + +/* ************************************************************************* */ +std::ostream& operator<<(std::ostream& os, const Cal3DS2_k3_Base& cal) { + os << (Cal3&)cal; + os << ", k1: " << cal.k1() << ", k2: " << cal.k2() << ", p1: " << cal.p1() << ", p2: " << cal.p2() + << ", k3: " << cal.k3(); + return os; +} + +/* ************************************************************************* */ +void Cal3DS2_k3_Base::print(const std::string& s_) const { + gtsam::print((Matrix)K(), s_ + ".K"); + gtsam::print(Vector(k()), s_ + ".k"); +} + +/* ************************************************************************* */ +bool Cal3DS2_k3_Base::equals(const Cal3DS2_k3_Base& K, double tol) const { + const Cal3* base = dynamic_cast(&K); + return Cal3::equals(*base, tol) && std::fabs(k1_ - K.k1_) < tol && std::fabs(k2_ - K.k2_) < tol && + std::fabs(p1_ - K.p1_) < tol && std::fabs(p2_ - K.p2_) < tol && + std::fabs(k3_ - K.k3_) < tol; +} + +/* ************************************************************************* */ +Eigen::Matrix D2dcalibration(double x, + double y, + double xx, + double yy, + double xy, + double rr, + double r4, + double r6, + double pnx, + double pny, + const Matrix2& DK) { + Matrix25 DR1; + DR1 << pnx, 0.0, pny, 1.0, 0.0, 0.0, pny, 0.0, 0.0, 1.0; + Matrix25 DR2; + DR2 << x * rr, x * r4, 2 * xy, rr + 2 * xx, x * r6, // + y * rr, y * r4, rr + 2 * yy, 2 * xy, y * r6; + Eigen::Matrix D; + D << DR1, DK * DR2; + return D; +} + +/* ************************************************************************* */ +static Matrix2 D2dintrinsic(double x, + double y, + double rr, + double r4, + double g, + double k1, + double k2, + double p1, + double p2, + double k3, + const Matrix2& DK) { + const double drdx = 2. * x; + const double drdy = 2. * y; + const double dgdx = k1 * drdx + k2 * 2. * rr * drdx + k3 * 3 * r4 * drdx; + const double dgdy = k1 * drdy + k2 * 2. * rr * drdy + k3 * 3 * r4 * drdy; + + // Dx = 2*p1*xy + p2*(rr+2*xx); + // Dy = 2*p2*xy + p1*(rr+2*yy); + const double dDxdx = 2. * p1 * y + p2 * (drdx + 4. * x); + const double dDxdy = 2. * p1 * x + p2 * drdy; + const double dDydx = 2. * p2 * y + p1 * drdx; + const double dDydy = 2. * p2 * x + p1 * (drdy + 4. * y); + + Matrix2 DR; + DR << g + x * dgdx + dDxdx, x * dgdy + dDxdy, // + y * dgdx + dDydx, g + y * dgdy + dDydy; + + return DK * DR; +} + +/* ************************************************************************* */ +Point2 Cal3DS2_k3_Base::uncalibrate(const Point2& p, + OptionalJacobian<2, 10> Dcal, + OptionalJacobian<2, 2> Dp) const { + // r² = x² + y²; + // g = (1 + k(1)*r² + k(2)*r⁴ + k(5)*r⁶); + // dp = [2*k(3)*x*y + k(4)*(r² + 2*x²); 2*k(4)*x*y + k(3)*(r² + 2*y²)]; + // pi(:,i) = g * pn(:,i) + dp; + const double x = p.x(), y = p.y(), xy = x * y, xx = x * x, yy = y * y; + const double rr = xx + yy; + const double r4 = rr * rr; + const double r6 = rr * rr * rr; + const double g = 1. + k1_ * rr + k2_ * r4 + k3_ * r6; // scaling factor + + // tangential component + const double dx = 2. * p1_ * xy + p2_ * (rr + 2. * xx); + const double dy = 2. * p2_ * xy + p1_ * (rr + 2. * yy); + + // Radial and tangential distortion applied + const double pnx = g * x + dx; + const double pny = g * y + dy; + + Matrix2 DK; + if (Dcal || Dp) { + DK << fx_, s_, 0.0, fy_; + } + + // Derivative for calibration + if (Dcal) { + *Dcal = D2dcalibration(x, y, xx, yy, xy, rr, r4, r6, pnx, pny, DK); + } + + // Derivative for points + if (Dp) { + *Dp = D2dintrinsic(x, y, rr, r4, g, k1_, k2_, p1_, p2_, k3_, DK); + } + + // Regular uncalibrate after distortion + return Point2(fx_ * pnx + s_ * pny + u0_, fy_ * pny + v0_); +} + +/* ************************************************************************* */ +Point2 Cal3DS2_k3_Base::calibrate(const Point2& pi, + OptionalJacobian<2, 10> Dcal, + OptionalJacobian<2, 2> Dp) const { + // Use the following fixed point iteration to invert the radial distortion. + // pn_{t+1} = (inv(K)*pi - dp(pn_{t})) / g(pn_{t}) + + const Point2 invKPi((1 / fx_) * (pi.x() - u0_ - (s_ / fy_) * (pi.y() - v0_)), + (1 / fy_) * (pi.y() - v0_)); + + // initialize by ignoring the distortion at all, might be problematic for + // pixels around boundary + Point2 pn = invKPi; + + // iterate until the uncalibrate is close to the actual pixel coordinate + const int maxIterations = 10; + int iteration; + for (iteration = 0; iteration < maxIterations; ++iteration) { + if (distance2(uncalibrate(pn), pi) <= tol_) break; + const double px = pn.x(), py = pn.y(), xy = px * py, xx = px * px, yy = py * py; + const double rr = xx + yy; + const double r4 = rr * rr; + const double r6 = rr * rr * rr; + const double g = (1 + k1_ * rr + k2_ * r4 + k3_ * r6); + const double dx = 2 * p1_ * xy + p2_ * (rr + 2 * xx); + const double dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy); + pn = (invKPi - Point2(dx, dy)) / g; + } + + if (iteration >= maxIterations) + throw std::runtime_error("Cal3DS2::calibrate fails to converge. need a better initialization"); + + calibrateJacobians(*this, pn, Dcal, Dp); + + return pn; +} + +/* ************************************************************************* */ +Matrix2 Cal3DS2_k3_Base::D2d_intrinsic(const Point2& p) const { + const double x = p.x(), y = p.y(), xx = x * x, yy = y * y; + const double rr = xx + yy; + const double r4 = rr * rr; + const double r6 = rr * rr * rr; + const double g = (1 + k1_ * rr + k2_ * r4 + k3_ * r6); + Matrix2 DK; + DK << fx_, s_, 0.0, fy_; + return D2dintrinsic(x, y, rr, r4, g, k1_, k2_, p1_, p2_, k3_, DK); +} + +/* ************************************************************************* */ +Eigen::Matrix Cal3DS2_k3_Base::D2d_calibration(const Point2& p) const { + const double x = p.x(), y = p.y(), xx = x * x, yy = y * y, xy = x * y; + const double rr = xx + yy; + const double r4 = rr * rr; + const double r6 = rr * rr * rr; + const double g = (1 + k1_ * rr + k2_ * r4 + k3_ * r6); + const double dx = 2 * p1_ * xy + p2_ * (rr + 2 * xx); + const double dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy); + const double pnx = g * x + dx; + const double pny = g * y + dy; + Matrix2 DK; + DK << fx_, s_, 0.0, fy_; + return D2dcalibration(x, y, xx, yy, xy, rr, r4, r6, pnx, pny, DK); +} +} // namespace gtsam +/* ************************************************************************* */ diff --git a/gtsam/geometry/Cal3DS2_k3_Base.h b/gtsam/geometry/Cal3DS2_k3_Base.h new file mode 100644 index 0000000000..c07ecd63af --- /dev/null +++ b/gtsam/geometry/Cal3DS2_k3_Base.h @@ -0,0 +1,217 @@ +/* ---------------------------------------------------------------------------- + + * 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 Cal3DS2_k3.h + * @brief Calibration of a camera with radial distortion whose coefficients are not only k1, k2 but also k3. + * @date Dec 18, 2023 + * @author demul + * @author Hyeonjin Jeong + */ + +#pragma once + +#include + +namespace gtsam +{ +/** + * @brief Calibration of a camera with radial distortion whose coefficients are not only k1, k2 but also k3. + * @ingroup geometry + * \nosubgrouping + * + * Uses same distortionmodel as OpenCV, with + * http://docs.opencv.org/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html + * but using only k1,k2,k3,p1, and p2 coefficients. + * K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ] + * r² = P.x² + P.y² + * P̂ = (1 + k1*r² + k2*r⁴ + k3*r⁶) P + [ (2*p1 P.x P.y) + p2 (r² + 2 Pn.x²) + * p1 (r² + 2 Pn.y²) + (2*p2 Pn.x Pn.y) ] + * pi = K*P̂ + */ + +class GTSAM_EXPORT Cal3DS2_k3_Base : public Cal3 +{ + protected: + double k1_ = 0.0f, + k2_ = 0.0f; ///< radial 2nd-order and 4th-order + double p1_ = 0.0f, + p2_ = 0.0f; ///< tangential distortion + double k3_ = 0.0f; ///< radial 6th-order + double tol_ = 1e-5; ///< tolerance value when calibrating + + public: + enum + { + dimension = 10 + }; + + /// @name Standard Constructors + /// @{ + + /// Default Constructor with only unit focal length + Cal3DS2_k3_Base() = default; + + Cal3DS2_k3_Base( + double fx, + double fy, + double s, + double u0, + double v0, + double k1, + double k2, + double p1, + double p2, + double k3, + double tol = + 1e-5) // The order in which the coefficients are sorted follows the same order as the one used in opencv + : Cal3(fx, fy, s, u0, v0), k1_(k1), k2_(k2), p1_(p1), p2_(p2), k3_(k3), tol_(tol) + { + } + + ~Cal3DS2_k3_Base() override + { + } + + /// @} + /// @name Advanced Constructors + /// @{ + + Cal3DS2_k3_Base(const Vector10& v) + : Cal3(v(0), v(1), v(2), v(3), v(4)), k1_(v(5)), k2_(v(6)), p1_(v(7)), p2_(v(8)), k3_(v(9)) + { + } + + /// @} + /// @name Testable + /// @{ + + /// Output stream operator + GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const Cal3DS2_k3_Base& cal); + + /// print with optional string + void print(const std::string& s = "") const override; + + /// assert equality up to a tolerance + bool equals(const Cal3DS2_k3_Base& K, double tol = 1e-8) const; + + /// @} + /// @name Standard Interface + /// @{ + + /// First distortion coefficient + inline double k1() const + { + return k1_; + } + + /// Second distortion coefficient + inline double k2() const + { + return k2_; + } + + /// First tangential distortion coefficient + inline double p1() const + { + return p1_; + } + + /// Second tangential distortion coefficient + inline double p2() const + { + return p2_; + } + + /// Third distortion coefficient + inline double k3() const + { + return k3_; + } + + /// return distortion parameter vector + Vector5 k() const + { + return (Vector(5) << k1_, k2_, p1_, p2_, k3_).finished(); + } + + /// Return all parameters as a vector + Vector10 vector() const; + + /** + * convert intrinsic coordinates xy to (distorted) image coordinates uv + * @param p point in intrinsic coordinates + * @param Dcal optional 2*10 Jacobian wrpt Cal3DS2_k3 parameters + * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates + * @return point in (distorted) image coordinates + */ + Point2 uncalibrate( + const Point2& p, + OptionalJacobian<2, 10> Dcal = {}, + OptionalJacobian<2, 2> Dp = {}) const; + + /// Convert (distorted) image coordinates uv to intrinsic coordinates xy + Point2 calibrate( + const Point2& p, + OptionalJacobian<2, 10> Dcal = {}, + OptionalJacobian<2, 2> Dp = {}) const; + + /// Derivative of uncalibrate wrpt intrinsic coordinates + Matrix2 D2d_intrinsic(const Point2& p) const; + + /// Derivative of uncalibrate wrpt the calibration parameters + Eigen::Matrix D2d_calibration(const Point2& p) const; + + /// return DOF, dimensionality of tangent space + size_t dim() const override + { + return Dim(); + } + + /// return DOF, dimensionality of tangent space + inline static size_t Dim() + { + return dimension; + } + + /// @} + /// @name Clone + /// @{ + + /// @return a deep copy of this object + virtual boost::shared_ptr clone() const + { + return boost::shared_ptr(new Cal3DS2_k3_Base(*this)); + } + + /// @} + + private: + /// @name Advanced Interface + /// @{ + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive& ar, const unsigned int /*version*/) + { + ar& boost::serialization::make_nvp("Cal3DS2_k3_Base", boost::serialization::base_object(*this)); + ar& BOOST_SERIALIZATION_NVP(k1_); + ar& BOOST_SERIALIZATION_NVP(k2_); + ar& BOOST_SERIALIZATION_NVP(p1_); + ar& BOOST_SERIALIZATION_NVP(p2_); + ar& BOOST_SERIALIZATION_NVP(k3_); + ar& BOOST_SERIALIZATION_NVP(tol_); + } + + /// @} +}; +} // namespace gtsam diff --git a/gtsam/geometry/SimpleCamera.h b/gtsam/geometry/SimpleCamera.h index 7944514423..a21b9f5e4e 100644 --- a/gtsam/geometry/SimpleCamera.h +++ b/gtsam/geometry/SimpleCamera.h @@ -21,6 +21,7 @@ #include #include #include +#include #include #include #include @@ -34,6 +35,7 @@ namespace gtsam { using PinholeCameraCal3_S2 = gtsam::PinholeCamera; using PinholeCameraCal3Bundler = gtsam::PinholeCamera; using PinholeCameraCal3DS2 = gtsam::PinholeCamera; + using PinholeCameraCal3DS2_k3 = gtsam::PinholeCamera; using PinholeCameraCal3Unified = gtsam::PinholeCamera; using PinholeCameraCal3Fisheye = gtsam::PinholeCamera; diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 01161817b3..fc119f35b8 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -741,6 +741,56 @@ virtual class Cal3DS2 : gtsam::Cal3DS2_Base { void serialize() const; }; +#include +virtual class Cal3DS2_k3_Base { + // Standard Constructors + Cal3DS2_k3_Base(); + + // Testable + void print(string s = "") const; + + // Standard Interface + double fx() const; + double fy() const; + double skew() const; + double px() const; + double py() const; + double k1() const; + double k2() const; + double k3() const; + Matrix K() const; + Vector k() const; + Vector vector() const; + + // Action on Point2 + gtsam::Point2 uncalibrate(const gtsam::Point2& p) const; + gtsam::Point2 calibrate(const gtsam::Point2& p) const; + + // enabling serialization functionality + void serialize() const; +}; + +#include +virtual class Cal3DS2_k3 : gtsam::Cal3DS2_k3_Base { + // Standard Constructors + Cal3DS2_k3(); + Cal3DS2_k3(double fx, double fy, double s, double u0, double v0, double k1, + double k2, double p1, double p2, double k3); + Cal3DS2_k3(Vector v); + + // Testable + bool equals(const gtsam::Cal3DS2_k3& rhs, double tol) const; + + // Manifold + size_t dim() const; + static size_t Dim(); + gtsam::Cal3DS2_k3 retract(Vector v) const; + Vector localCoordinates(const gtsam::Cal3DS2_k3& c) const; + + // enabling serialization functionality + void serialize() const; +}; + #include virtual class Cal3Unified : gtsam::Cal3DS2_Base { // Standard Constructors @@ -1004,6 +1054,7 @@ class PinholeCamera { // PinholeCameraCal3_S2 is the same as SimpleCamera above typedef gtsam::PinholeCamera PinholeCameraCal3_S2; typedef gtsam::PinholeCamera PinholeCameraCal3DS2; +typedef gtsam::PinholeCamera PinholeCameraCal3DS2_k3; typedef gtsam::PinholeCamera PinholeCameraCal3Unified; typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; typedef gtsam::PinholeCamera PinholeCameraCal3Fisheye; @@ -1061,6 +1112,7 @@ class PinholePose { typedef gtsam::PinholePose PinholePoseCal3_S2; typedef gtsam::PinholePose PinholePoseCal3DS2; +typedef gtsam::PinholePose PinholePoseCal3DS2_k3; typedef gtsam::PinholePose PinholePoseCal3Unified; typedef gtsam::PinholePose PinholePoseCal3Bundler; typedef gtsam::PinholePose PinholePoseCal3Fisheye; @@ -1241,6 +1293,29 @@ gtsam::TriangulationResult triangulateSafe( const gtsam::Point2Vector& measurements, const gtsam::TriangulationParameters& params); +// Cal3DS2_k3 versions +gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, + gtsam::Cal3DS2_k3* sharedCal, + const gtsam::Point2Vector& measurements, + double rank_tol, bool optimize, + const gtsam::SharedNoiseModel& model = nullptr); +gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3DS2_k3& cameras, + const gtsam::Point2Vector& measurements, + double rank_tol, bool optimize, + const gtsam::SharedNoiseModel& model = nullptr, + const bool useLOST = false); +gtsam::Point3 triangulateNonlinear(const gtsam::Pose3Vector& poses, + gtsam::Cal3DS2_k3* sharedCal, + const gtsam::Point2Vector& measurements, + const gtsam::Point3& initialEstimate); +gtsam::Point3 triangulateNonlinear(const gtsam::CameraSetCal3DS2_k3& cameras, + const gtsam::Point2Vector& measurements, + const gtsam::Point3& initialEstimate); +gtsam::TriangulationResult triangulateSafe( + const gtsam::CameraSetCal3DS2_k3& cameras, + const gtsam::Point2Vector& measurements, + const gtsam::TriangulationParameters& params); + // Cal3Bundler versions gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, gtsam::Cal3Bundler* sharedCal, diff --git a/gtsam/geometry/tests/testCal3DS2_k3.cpp b/gtsam/geometry/tests/testCal3DS2_k3.cpp new file mode 100644 index 0000000000..93a7db4d76 --- /dev/null +++ b/gtsam/geometry/tests/testCal3DS2_k3.cpp @@ -0,0 +1,130 @@ +/* ---------------------------------------------------------------------------- + + * 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 testCal3DS2_k3.cpp + * @brief Unit tests for Cal3DS2_k3 calibration model. + */ + +#include +#include +#include +#include +#include + +using namespace gtsam; + +GTSAM_CONCEPT_TESTABLE_INST(Cal3DS2_k3) +GTSAM_CONCEPT_MANIFOLD_INST(Cal3DS2_k3) + +static Cal3DS2_k3 K(500, 100, 0.1, 320, 240, 1e-3, 2.0 * 1e-3, 3.0 * 1e-3, 4.0 * 1e-3, 5.0 * 1e-3); +static Point2 p(2, 3); + +/* ************************************************************************* */ +TEST(Cal3DS2_k3, Uncalibrate) { + Vector k = K.k(); + double r = p.x() * p.x() + p.y() * p.y(); + double g = 1 + k[0] * r + k[1] * r * r + k[4] * r * r * r; + double tx = 2 * k[2] * p.x() * p.y() + k[3] * (r + 2 * p.x() * p.x()); + double ty = k[2] * (r + 2 * p.y() * p.y()) + 2 * k[3] * p.x() * p.y(); + Vector v_hat = (Vector(3) << g * p.x() + tx, g * p.y() + ty, 1.0).finished(); + Vector v_i = K.K() * v_hat; + Point2 p_i(v_i(0) / v_i(2), v_i(1) / v_i(2)); + Point2 q = K.uncalibrate(p); + CHECK(assert_equal(q, p_i)); +} + +TEST(Cal3DS2_k3, Calibrate) { + Point2 pn(0.5, 0.5); + Point2 pi = K.uncalibrate(pn); + Point2 pn_hat = K.calibrate(pi); + CHECK(traits::Equals(pn, pn_hat, 1e-5)); +} + +Point2 uncalibrate_(const Cal3DS2_k3& k, const Point2& pt) { return k.uncalibrate(pt); } + +/* ************************************************************************* */ +TEST(Cal3DS2_k3, Duncalibrate1) { + Matrix computed; + K.uncalibrate(p, computed, {}); + Matrix numerical = numericalDerivative21(uncalibrate_, K, p, 1e-6); + CHECK(assert_equal(numerical, computed, 1e-5)); + Matrix separate = K.D2d_calibration(p); + CHECK(assert_equal(numerical, separate, 1e-5)); +} + +/* ************************************************************************* */ +TEST(Cal3DS2_k3, Duncalibrate2) { + Matrix computed; + K.uncalibrate(p, {}, computed); + Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-6); + CHECK(assert_equal(numerical, computed, 1e-5)); + Matrix separate = K.D2d_intrinsic(p); + CHECK(assert_equal(numerical, separate, 1e-5)); +} + +Point2 calibrate_(const Cal3DS2_k3& k, const Point2& pt) { return k.calibrate(pt); } + +/* ************************************************************************* */ +TEST(Cal3DS2_k3, Dcalibrate) { + Point2 pn(0.5, 0.5); + Point2 pi = K.uncalibrate(pn); + Matrix Dcal, Dp; + K.calibrate(pi, Dcal, Dp); + Matrix numerical1 = numericalDerivative21(calibrate_, K, pi, 1e-7); + CHECK(assert_equal(numerical1, Dcal, 1e-5)); + Matrix numerical2 = numericalDerivative22(calibrate_, K, pi, 1e-7); + CHECK(assert_equal(numerical2, Dp, 1e-5)); +} + +/* ************************************************************************* */ +TEST(Cal3DS2_k3, Equal) { CHECK(assert_equal(K, K, 1e-5)); } + +/* ************************************************************************* */ +TEST(Cal3DS2_k3, Retract) { + Cal3DS2_k3 expected(500 + 1, + 100 + 2, + 0.1 + 3, + 320 + 4, + 240 + 5, + 1e-3 + 6, + 2.0 * 1e-3 + 7, + 3.0 * 1e-3 + 8, + 4.0 * 1e-3 + 9, + 5.0 * 1e-3 + 10); + + EXPECT_LONGS_EQUAL(Cal3DS2_k3::Dim(), 10); + EXPECT_LONGS_EQUAL(expected.dim(), 10); + + Vector10 d; + d << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10; + Cal3DS2_k3 actual = K.retract(d); + CHECK(assert_equal(expected, actual, 1e-7)); + CHECK(assert_equal(d, K.localCoordinates(actual), 1e-7)); +} + +/* ************************************************************************* */ +TEST(Cal3DS2_k3, Print) { + Cal3DS2_k3 cal(1, 2, 3, 4, 5, 6, 7, 8, 9, 10); + std::stringstream os; + os << "fx: " << cal.fx() << ", fy: " << cal.fy() << ", s: " << cal.skew() << ", px: " << cal.px() + << ", py: " << cal.py() << ", k1: " << cal.k1() << ", k2: " << cal.k2() << ", p1: " << cal.p1() + << ", p2: " << cal.p2() << ", k3: " << cal.k3(); + + EXPECT(assert_stdout_equal(os.str(), cal)); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testSerializationGeometry.cpp b/gtsam/geometry/tests/testSerializationGeometry.cpp index aa111c3ae9..d9772e4b58 100644 --- a/gtsam/geometry/tests/testSerializationGeometry.cpp +++ b/gtsam/geometry/tests/testSerializationGeometry.cpp @@ -24,6 +24,7 @@ #include #include #include +#include #include #include #include @@ -51,6 +52,7 @@ static Cal3_S2Stereo cal4(1.0, 2.0, 3.0, 4.0, 5.0, 6.0); static Cal3_S2Stereo::shared_ptr cal4ptr(new Cal3_S2Stereo(cal4)); static CalibratedCamera cal5(Pose3(rt3, pt3)); static Cal3Unified cal6(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0); +static Cal3DS2_k3 cal7(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0); static PinholeCamera cam1(pose3, cal1); static StereoCamera cam2(pose3, cal4ptr); @@ -75,6 +77,7 @@ TEST (Serialization, text_geometry) { EXPECT(equalsObj(cal4)); EXPECT(equalsObj(cal5)); EXPECT(equalsObj(cal6)); + EXPECT(equalsObj(cal7)); EXPECT(equalsObj(cam1)); EXPECT(equalsObj(cam2)); @@ -99,6 +102,7 @@ TEST (Serialization, xml_geometry) { EXPECT(equalsXML(cal3)); EXPECT(equalsXML(cal4)); EXPECT(equalsXML(cal5)); + EXPECT(equalsXML(cal7)); EXPECT(equalsXML(cam1)); EXPECT(equalsXML(cam2)); @@ -123,6 +127,7 @@ TEST (Serialization, binary_geometry) { EXPECT(equalsBinary(cal3)); EXPECT(equalsBinary(cal4)); EXPECT(equalsBinary(cal5)); + EXPECT(equalsBinary(cal7)); EXPECT(equalsBinary(cam1)); EXPECT(equalsBinary(cam2)); diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index d82da3459a..3b145b623f 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -26,6 +26,7 @@ #include #include #include +#include #include #include #include @@ -757,6 +758,7 @@ TriangulationResult triangulateSafe(const CameraSet& cameras, using CameraSetCal3Bundler = CameraSet>; using CameraSetCal3_S2 = CameraSet>; using CameraSetCal3DS2 = CameraSet>; +using CameraSetCal3DS2_k3 = CameraSet>; using CameraSetCal3Fisheye = CameraSet>; using CameraSetCal3Unified = CameraSet>; using CameraSetSpherical = CameraSet; diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 3f5fb1dd54..c7a7f2a48f 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -6,6 +6,7 @@ namespace gtsam { #include #include +#include #include #include #include @@ -528,7 +529,7 @@ class ISAM2 { gtsam::Values calculateEstimate() const; template , gtsam::PinholeCamera, gtsam::PinholeCamera, @@ -594,6 +595,7 @@ template + gtsam::Cal3DS2_k3, Vector, Matrix}> VALUE calculateEstimate(size_t key) const; }; diff --git a/gtsam/nonlinear/values.i b/gtsam/nonlinear/values.i index 1b0322699d..8869d52282 100644 --- a/gtsam/nonlinear/values.i +++ b/gtsam/nonlinear/values.i @@ -6,6 +6,7 @@ namespace gtsam { #include #include +#include #include #include #include @@ -82,6 +83,7 @@ class Values { void insert(size_t j, const gtsam::Unit3& unit3); void insert(size_t j, const gtsam::Cal3_S2& cal3_s2); void insert(size_t j, const gtsam::Cal3DS2& cal3ds2); + void insert(size_t j, const gtsam::Cal3DS2_k3& cal3ds2_k3); void insert(size_t j, const gtsam::Cal3Bundler& cal3bundler); void insert(size_t j, const gtsam::Cal3Fisheye& cal3fisheye); void insert(size_t j, const gtsam::Cal3Unified& cal3unified); @@ -117,6 +119,7 @@ class Values { void update(size_t j, const gtsam::Unit3& unit3); void update(size_t j, const gtsam::Cal3_S2& cal3_s2); void update(size_t j, const gtsam::Cal3DS2& cal3ds2); + void update(size_t j, const gtsam::Cal3DS2_k3& cal3ds2_k3); void update(size_t j, const gtsam::Cal3Bundler& cal3bundler); void update(size_t j, const gtsam::Cal3Fisheye& cal3fisheye); void update(size_t j, const gtsam::Cal3Unified& cal3unified); @@ -149,6 +152,7 @@ class Values { void insert_or_assign(size_t j, const gtsam::Unit3& unit3); void insert_or_assign(size_t j, const gtsam::Cal3_S2& cal3_s2); void insert_or_assign(size_t j, const gtsam::Cal3DS2& cal3ds2); + void insert_or_assign(size_t j, const gtsam::Cal3DS2_k3& cal3ds2_k3); void insert_or_assign(size_t j, const gtsam::Cal3Bundler& cal3bundler); void insert_or_assign(size_t j, const gtsam::Cal3Fisheye& cal3fisheye); void insert_or_assign(size_t j, const gtsam::Cal3Unified& cal3unified); @@ -187,6 +191,7 @@ class Values { gtsam::Unit3, gtsam::Cal3_S2, gtsam::Cal3DS2, + gtsam::Cal3DS2_k3, gtsam::Cal3Bundler, gtsam::Cal3Fisheye, gtsam::Cal3Unified, diff --git a/gtsam_unstable/gtsam_unstable.i b/gtsam_unstable/gtsam_unstable.i index 9323255adf..8cdbc6ea7c 100644 --- a/gtsam_unstable/gtsam_unstable.i +++ b/gtsam_unstable/gtsam_unstable.i @@ -59,7 +59,15 @@ class PoseRTV { PoseRTV(const gtsam::Rot3& rot, const gtsam::Point3& pt, const Vector& vel); PoseRTV(const gtsam::Pose3& pose, const Vector& vel); PoseRTV(const gtsam::Pose3& pose); - PoseRTV(double roll, double pitch, double yaw, double x, double y, double z, double vx, double vy, double vz); + PoseRTV(double roll, + double pitch, + double yaw, + double x, + double y, + double z, + double vx, + double vy, + double vz); // testable bool equals(const gtsam::PoseRTV& other, double tol) const; @@ -92,14 +100,20 @@ class PoseRTV { gtsam::PoseRTV transformed_from(const gtsam::Pose3& trans) const; // IMU/dynamics - gtsam::PoseRTV planarDynamics(double vel_rate, double heading_rate, double max_accel, double dt) const; - gtsam::PoseRTV flyingDynamics(double pitch_rate, double heading_rate, double lift_control, double dt) const; + gtsam::PoseRTV planarDynamics(double vel_rate, + double heading_rate, + double max_accel, + double dt) const; + gtsam::PoseRTV flyingDynamics(double pitch_rate, + double heading_rate, + double lift_control, + double dt) const; gtsam::PoseRTV generalDynamics(Vector accel, Vector gyro, double dt) const; Vector imuPrediction(const gtsam::PoseRTV& x2, double dt) const; gtsam::Point3 translationIntegration(const gtsam::PoseRTV& x2, double dt) const; Vector translationIntegrationVec(const gtsam::PoseRTV& x2, double dt) const; - void serializable() const; // enabling serialization functionality + void serializable() const; // enabling serialization functionality }; #include @@ -137,8 +151,8 @@ class Pose3Upright { static gtsam::Pose3Upright Expmap(Vector xi); static Vector Logmap(const gtsam::Pose3Upright& p); - void serializable() const; // enabling serialization functionality -}; // \class Pose3Upright + void serializable() const; // enabling serialization functionality +}; // \class Pose3Upright #include class BearingS2 { @@ -159,10 +173,9 @@ class BearingS2 { gtsam::BearingS2 retract(Vector v) const; Vector localCoordinates(const gtsam::BearingS2& p2) const; - void serializable() const; // enabling serialization functionality + void serializable() const; // enabling serialization functionality }; - #include class SimWall2D { SimWall2D(); @@ -188,149 +201,189 @@ class SimWall2D { #include class SimPolygon2D { - static void seedGenerator(size_t seed); - static gtsam::SimPolygon2D createTriangle(const gtsam::Point2& pA, const gtsam::Point2& pB, const gtsam::Point2& pC); - static gtsam::SimPolygon2D createRectangle(const gtsam::Point2& p, double height, double width); - - static gtsam::SimPolygon2D randomTriangle(double side_len, double mean_side_len, double sigma_side_len, - double min_vertex_dist, double min_side_len, const gtsam::SimPolygon2DVector& existing_polys); - static gtsam::SimPolygon2D randomRectangle(double side_len, double mean_side_len, double sigma_side_len, - double min_vertex_dist, double min_side_len, const gtsam::SimPolygon2DVector& existing_polys); - - gtsam::Point2 landmark(size_t i) const; - size_t size() const; - gtsam::Point2Vector vertices() const; - - bool equals(const gtsam::SimPolygon2D& p, double tol) const; - void print(string s) const; - - gtsam::SimWall2DVector walls() const; - bool contains(const gtsam::Point2& p) const; - bool overlaps(const gtsam::SimPolygon2D& p) const; - - static bool anyContains(const gtsam::Point2& p, const gtsam::SimPolygon2DVector& obstacles); - static bool anyOverlaps(const gtsam::SimPolygon2D& p, const gtsam::SimPolygon2DVector& obstacles); - static bool insideBox(double s, const gtsam::Point2& p); - static bool nearExisting(const gtsam::Point2Vector& S, - const gtsam::Point2& p, double threshold); - - static gtsam::Point2 randomPoint2(double s); - static gtsam::Rot2 randomAngle(); - static double randomDistance(double mu, double sigma); - static double randomDistance(double mu, double sigma, double min_dist); - static gtsam::Point2 randomBoundedPoint2(double boundary_size, - const gtsam::Point2Vector& landmarks, double min_landmark_dist); - static gtsam::Point2 randomBoundedPoint2(double boundary_size, - const gtsam::Point2Vector& landmarks, - const gtsam::SimPolygon2DVector& obstacles, double min_landmark_dist); - static gtsam::Point2 randomBoundedPoint2(double boundary_size, - const gtsam::SimPolygon2DVector& obstacles); - static gtsam::Point2 randomBoundedPoint2( - const gtsam::Point2& LL_corner, const gtsam::Point2& UR_corner, - const gtsam::Point2Vector& landmarks, - const gtsam::SimPolygon2DVector& obstacles, double min_landmark_dist); - static gtsam::Pose2 randomFreePose(double boundary_size, const gtsam::SimPolygon2DVector& obstacles); - }; - - // std::vector - class SimWall2DVector - { - //Capacity - size_t size() const; - size_t max_size() const; - void resize(size_t sz); - size_t capacity() const; - bool empty() const; - void reserve(size_t n); - - //Element access - gtsam::SimWall2D at(size_t n) const; - gtsam::SimWall2D front() const; - gtsam::SimWall2D back() const; - - //Modifiers - void assign(size_t n, const gtsam::SimWall2D& u); - void push_back(const gtsam::SimWall2D& x); - void pop_back(); - }; - - // std::vector - class SimPolygon2DVector - { - //Capacity - size_t size() const; - size_t max_size() const; - void resize(size_t sz); - size_t capacity() const; - bool empty() const; - void reserve(size_t n); - - //Element access - gtsam::SimPolygon2D at(size_t n) const; - gtsam::SimPolygon2D front() const; - gtsam::SimPolygon2D back() const; - - //Modifiers - void assign(size_t n, const gtsam::SimPolygon2D& u); - void push_back(const gtsam::SimPolygon2D& x); - void pop_back(); - }; + static void seedGenerator(size_t seed); + static gtsam::SimPolygon2D createTriangle(const gtsam::Point2& pA, + const gtsam::Point2& pB, + const gtsam::Point2& pC); + static gtsam::SimPolygon2D createRectangle(const gtsam::Point2& p, double height, double width); + + static gtsam::SimPolygon2D randomTriangle(double side_len, + double mean_side_len, + double sigma_side_len, + double min_vertex_dist, + double min_side_len, + const gtsam::SimPolygon2DVector& existing_polys); + static gtsam::SimPolygon2D randomRectangle(double side_len, + double mean_side_len, + double sigma_side_len, + double min_vertex_dist, + double min_side_len, + const gtsam::SimPolygon2DVector& existing_polys); + + gtsam::Point2 landmark(size_t i) const; + size_t size() const; + gtsam::Point2Vector vertices() const; + + bool equals(const gtsam::SimPolygon2D& p, double tol) const; + void print(string s) const; + + gtsam::SimWall2DVector walls() const; + bool contains(const gtsam::Point2& p) const; + bool overlaps(const gtsam::SimPolygon2D& p) const; + + static bool anyContains(const gtsam::Point2& p, const gtsam::SimPolygon2DVector& obstacles); + static bool anyOverlaps(const gtsam::SimPolygon2D& p, const gtsam::SimPolygon2DVector& obstacles); + static bool insideBox(double s, const gtsam::Point2& p); + static bool nearExisting(const gtsam::Point2Vector& S, const gtsam::Point2& p, double threshold); + + static gtsam::Point2 randomPoint2(double s); + static gtsam::Rot2 randomAngle(); + static double randomDistance(double mu, double sigma); + static double randomDistance(double mu, double sigma, double min_dist); + static gtsam::Point2 randomBoundedPoint2(double boundary_size, + const gtsam::Point2Vector& landmarks, + double min_landmark_dist); + static gtsam::Point2 randomBoundedPoint2(double boundary_size, + const gtsam::Point2Vector& landmarks, + const gtsam::SimPolygon2DVector& obstacles, + double min_landmark_dist); + static gtsam::Point2 randomBoundedPoint2(double boundary_size, + const gtsam::SimPolygon2DVector& obstacles); + static gtsam::Point2 randomBoundedPoint2(const gtsam::Point2& LL_corner, + const gtsam::Point2& UR_corner, + const gtsam::Point2Vector& landmarks, + const gtsam::SimPolygon2DVector& obstacles, + double min_landmark_dist); + static gtsam::Pose2 randomFreePose(double boundary_size, + const gtsam::SimPolygon2DVector& obstacles); +}; + +// std::vector +class SimWall2DVector { + // Capacity + size_t size() const; + size_t max_size() const; + void resize(size_t sz); + size_t capacity() const; + bool empty() const; + void reserve(size_t n); + + // Element access + gtsam::SimWall2D at(size_t n) const; + gtsam::SimWall2D front() const; + gtsam::SimWall2D back() const; + + // Modifiers + void assign(size_t n, const gtsam::SimWall2D& u); + void push_back(const gtsam::SimWall2D& x); + void pop_back(); +}; + +// std::vector +class SimPolygon2DVector { + // Capacity + size_t size() const; + size_t max_size() const; + void resize(size_t sz); + size_t capacity() const; + bool empty() const; + void reserve(size_t n); + + // Element access + gtsam::SimPolygon2D at(size_t n) const; + gtsam::SimPolygon2D front() const; + gtsam::SimPolygon2D back() const; + + // Modifiers + void assign(size_t n, const gtsam::SimPolygon2D& u); + void push_back(const gtsam::SimPolygon2D& x); + void pop_back(); +}; // Nonlinear factors from gtsam, for our Value types #include -template +template virtual class PriorFactor : gtsam::NoiseModelFactor { PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); - void serializable() const; // enabling serialization functionality + void serializable() const; // enabling serialization functionality }; #include -template +template virtual class BetweenFactor : gtsam::NoiseModelFactor { - BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel); + BetweenFactor(size_t key1, + size_t key2, + const T& relativePose, + const gtsam::noiseModel::Base* noiseModel); - void serializable() const; // enabling serialization functionality + void serializable() const; // enabling serialization functionality }; #include -template +template virtual class BetweenFactorEM : gtsam::NonlinearFactor { - BetweenFactorEM(size_t key1, size_t key2, const T& relativePose, - const gtsam::noiseModel::Gaussian* model_inlier, const gtsam::noiseModel::Gaussian* model_outlier, - double prior_inlier, double prior_outlier); - - BetweenFactorEM(size_t key1, size_t key2, const T& relativePose, - const gtsam::noiseModel::Gaussian* model_inlier, const gtsam::noiseModel::Gaussian* model_outlier, - double prior_inlier, double prior_outlier, bool flag_bump_up_near_zero_probs); + BetweenFactorEM(size_t key1, + size_t key2, + const T& relativePose, + const gtsam::noiseModel::Gaussian* model_inlier, + const gtsam::noiseModel::Gaussian* model_outlier, + double prior_inlier, + double prior_outlier); + + BetweenFactorEM(size_t key1, + size_t key2, + const T& relativePose, + const gtsam::noiseModel::Gaussian* model_inlier, + const gtsam::noiseModel::Gaussian* model_outlier, + double prior_inlier, + double prior_outlier, + bool flag_bump_up_near_zero_probs); Vector whitenedError(const gtsam::Values& x); Vector unwhitenedError(const gtsam::Values& x); Vector calcIndicatorProb(const gtsam::Values& x); - gtsam::Pose2 measured(); // TODO: figure out how to output a template instead + gtsam::Pose2 measured(); // TODO: figure out how to output a template instead void set_flag_bump_up_near_zero_probs(bool flag); bool get_flag_bump_up_near_zero_probs() const; void updateNoiseModels(const gtsam::Values& values, const gtsam::NonlinearFactorGraph& graph); - void updateNoiseModels_givenCovs(const gtsam::Values& values, Matrix cov1, Matrix cov2, Matrix cov12); + void updateNoiseModels_givenCovs(const gtsam::Values& values, + Matrix cov1, + Matrix cov2, + Matrix cov12); Matrix get_model_inlier_cov(); Matrix get_model_outlier_cov(); - void serializable() const; // enabling serialization functionality + void serializable() const; // enabling serialization functionality }; #include -template +template virtual class TransformBtwRobotsUnaryFactorEM : gtsam::NonlinearFactor { - TransformBtwRobotsUnaryFactorEM(size_t key, const T& relativePose, size_t keyA, size_t keyB, - const gtsam::Values& valA, const gtsam::Values& valB, - const gtsam::noiseModel::Gaussian* model_inlier, const gtsam::noiseModel::Gaussian* model_outlier, - double prior_inlier, double prior_outlier); - - TransformBtwRobotsUnaryFactorEM(size_t key, const T& relativePose, size_t keyA, size_t keyB, - const gtsam::Values& valA, const gtsam::Values& valB, - const gtsam::noiseModel::Gaussian* model_inlier, const gtsam::noiseModel::Gaussian* model_outlier, - double prior_inlier, double prior_outlier, bool flag_bump_up_near_zero_probs, bool start_with_M_step); + TransformBtwRobotsUnaryFactorEM(size_t key, + const T& relativePose, + size_t keyA, + size_t keyB, + const gtsam::Values& valA, + const gtsam::Values& valB, + const gtsam::noiseModel::Gaussian* model_inlier, + const gtsam::noiseModel::Gaussian* model_outlier, + double prior_inlier, + double prior_outlier); + + TransformBtwRobotsUnaryFactorEM(size_t key, + const T& relativePose, + size_t keyA, + size_t keyB, + const gtsam::Values& valA, + const gtsam::Values& valB, + const gtsam::noiseModel::Gaussian* model_inlier, + const gtsam::noiseModel::Gaussian* model_outlier, + double prior_inlier, + double prior_outlier, + bool flag_bump_up_near_zero_probs, + bool start_with_M_step); Vector whitenedError(const gtsam::Values& x); Vector unwhitenedError(const gtsam::Values& x); @@ -338,25 +391,32 @@ virtual class TransformBtwRobotsUnaryFactorEM : gtsam::NonlinearFactor { void setValAValB(const gtsam::Values valA, const gtsam::Values valB); void updateNoiseModels(const gtsam::Values& values, const gtsam::NonlinearFactorGraph& graph); - void updateNoiseModels_givenCovs(const gtsam::Values& values, Matrix cov1, Matrix cov2, Matrix cov12); + void updateNoiseModels_givenCovs(const gtsam::Values& values, + Matrix cov1, + Matrix cov2, + Matrix cov12); Matrix get_model_inlier_cov(); Matrix get_model_outlier_cov(); - void serializable() const; // enabling serialization functionality + void serializable() const; // enabling serialization functionality }; #include -template +template virtual class TransformBtwRobotsUnaryFactor : gtsam::NonlinearFactor { - TransformBtwRobotsUnaryFactor(size_t key, const T& relativePose, size_t keyA, size_t keyB, - const gtsam::Values& valA, const gtsam::Values& valB, - const gtsam::noiseModel::Gaussian* model); + TransformBtwRobotsUnaryFactor(size_t key, + const T& relativePose, + size_t keyA, + size_t keyB, + const gtsam::Values& valA, + const gtsam::Values& valB, + const gtsam::noiseModel::Gaussian* model); Vector whitenedError(const gtsam::Values& x); Vector unwhitenedError(const gtsam::Values& x); void setValAValB(const gtsam::Values valA, const gtsam::Values valB); - void serializable() const; // enabling serialization functionality + void serializable() const; // enabling serialization functionality }; #include @@ -365,16 +425,15 @@ virtual class SmartRangeFactor : gtsam::NoiseModelFactor { void addRange(size_t key, double measuredRange); gtsam::Point2 triangulate(const gtsam::Values& x) const; - //void print(string s) const; - + // void print(string s) const; }; #include -template +template virtual class RangeFactor : gtsam::NoiseModelFactor { RangeFactor(size_t key1, size_t key2, double measured, const gtsam::noiseModel::Base* noiseModel); - void serializable() const; // enabling serialization functionality + void serializable() const; // enabling serialization functionality }; typedef gtsam::RangeFactor RangeFactorRTV; @@ -399,32 +458,38 @@ class TimeOfArrival { #include virtual class TOAFactor : gtsam::NonlinearFactor { // For now, because of overload issues, we only expose constructor with known sensor coordinates: - TOAFactor(size_t key1, gtsam::Point3 sensor, double measured, + TOAFactor(size_t key1, + gtsam::Point3 sensor, + double measured, const gtsam::noiseModel::Base* noiseModel); static void InsertEvent(size_t key, const gtsam::Event& event, gtsam::Values* values); }; #include -template +template virtual class NonlinearEquality : gtsam::NoiseModelFactor { // Constructor - forces exact evaluation NonlinearEquality(size_t j, const T& feasible); // Constructor - allows inexact evaluation NonlinearEquality(size_t j, const T& feasible, double error_gain); - void serializable() const; // enabling serialization functionality + void serializable() const; // enabling serialization functionality }; #include -template +template virtual class IMUFactor : gtsam::NoiseModelFactor { /** Standard constructor */ - IMUFactor(Vector accel, Vector gyro, - double dt, size_t key1, size_t key2, const gtsam::noiseModel::Base* model); + IMUFactor(Vector accel, + Vector gyro, + double dt, + size_t key1, + size_t key2, + const gtsam::noiseModel::Base* model); /** Full IMU vector specification */ - IMUFactor(Vector imu_vector, - double dt, size_t key1, size_t key2, const gtsam::noiseModel::Base* model); + IMUFactor( + Vector imu_vector, double dt, size_t key1, size_t key2, const gtsam::noiseModel::Base* model); Vector gyro() const; Vector accel() const; @@ -435,15 +500,19 @@ virtual class IMUFactor : gtsam::NoiseModelFactor { }; #include -template +template virtual class FullIMUFactor : gtsam::NoiseModelFactor { /** Standard constructor */ - FullIMUFactor(Vector accel, Vector gyro, - double dt, size_t key1, size_t key2, const gtsam::noiseModel::Base* model); + FullIMUFactor(Vector accel, + Vector gyro, + double dt, + size_t key1, + size_t key2, + const gtsam::noiseModel::Base* model); /** Single IMU vector - imu = [accel, gyro] */ - FullIMUFactor(Vector imu, - double dt, size_t key1, size_t key2, const gtsam::noiseModel::Base* model); + FullIMUFactor( + Vector imu, double dt, size_t key1, size_t key2, const gtsam::noiseModel::Base* model); Vector gyro() const; Vector accel() const; @@ -502,14 +571,16 @@ virtual class PendulumFactor2 : gtsam::NonlinearFactor { virtual class PendulumFactorPk : gtsam::NonlinearFactor { /** Standard constructor */ - PendulumFactorPk(size_t pk, size_t qk, size_t qk1, double h, double m, double r, double g, double alpha); + PendulumFactorPk( + size_t pk, size_t qk, size_t qk1, double h, double m, double r, double g, double alpha); Vector evaluateError(const double& pk, const double& qk, const double& qk1) const; }; virtual class PendulumFactorPk1 : gtsam::NonlinearFactor { /** Standard constructor */ - PendulumFactorPk1(size_t pk1, size_t qk, size_t qk1, double h, double m, double r, double g, double alpha); + PendulumFactorPk1( + size_t pk1, size_t qk, size_t qk1, double h, double m, double r, double g, double alpha); Vector evaluateError(const double& pk1, const double& qk, const double& qk1) const; }; @@ -522,8 +593,8 @@ virtual class Reconstruction : gtsam::NoiseModelFactor { }; virtual class DiscreteEulerPoincareHelicopter : gtsam::NoiseModelFactor { - DiscreteEulerPoincareHelicopter(size_t xiKey, size_t xiKey_1, size_t gKey, - double h, Matrix Inertia, Vector Fu, double m); + DiscreteEulerPoincareHelicopter( + size_t xiKey, size_t xiKey_1, size_t gKey, double h, Matrix Inertia, Vector Fu, double m); Vector evaluateError(Vector xiK, Vector xiK_1, const gtsam::Pose3& gK) const; }; @@ -580,8 +651,11 @@ virtual class ConcurrentBatchFilter : gtsam::ConcurrentFilter { gtsam::ConcurrentBatchFilterResult update(); gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors); - gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); - gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyList& keysToMove); + gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, + const gtsam::Values& newTheta); + gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, + const gtsam::Values& newTheta, + const gtsam::KeyList& keysToMove); gtsam::Values calculateEstimate() const; }; @@ -605,7 +679,8 @@ virtual class ConcurrentBatchSmoother : gtsam::ConcurrentSmoother { gtsam::ConcurrentBatchSmootherResult update(); gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors); - gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta); + gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors, + const gtsam::Values& newTheta); gtsam::Values calculateEstimate() const; }; @@ -613,13 +688,15 @@ virtual class ConcurrentBatchSmoother : gtsam::ConcurrentSmoother { // slam //************************************************************************* #include -virtual class RelativeElevationFactor: gtsam::NoiseModelFactor { +virtual class RelativeElevationFactor : gtsam::NoiseModelFactor { RelativeElevationFactor(); - RelativeElevationFactor(size_t poseKey, size_t pointKey, double measured, - const gtsam::noiseModel::Base* model); + RelativeElevationFactor(size_t poseKey, + size_t pointKey, + double measured, + const gtsam::noiseModel::Base* model); double measured() const; - //void print(string s) const; + // void print(string s) const; }; #include @@ -629,28 +706,44 @@ virtual class DummyFactor : gtsam::NonlinearFactor { #include virtual class InvDepthFactorVariant1 : gtsam::NoiseModelFactor { - InvDepthFactorVariant1(size_t poseKey, size_t landmarkKey, const gtsam::Point2& measured, const gtsam::Cal3_S2* K, const gtsam::noiseModel::Base* model); + InvDepthFactorVariant1(size_t poseKey, + size_t landmarkKey, + const gtsam::Point2& measured, + const gtsam::Cal3_S2* K, + const gtsam::noiseModel::Base* model); }; #include virtual class InvDepthFactorVariant2 : gtsam::NoiseModelFactor { - InvDepthFactorVariant2(size_t poseKey, size_t landmarkKey, const gtsam::Point2& measured, const gtsam::Cal3_S2* K, const gtsam::Point3& referencePoint, const gtsam::noiseModel::Base* model); + InvDepthFactorVariant2(size_t poseKey, + size_t landmarkKey, + const gtsam::Point2& measured, + const gtsam::Cal3_S2* K, + const gtsam::Point3& referencePoint, + const gtsam::noiseModel::Base* model); }; #include virtual class InvDepthFactorVariant3a : gtsam::NoiseModelFactor { - InvDepthFactorVariant3a(size_t poseKey, size_t landmarkKey, const gtsam::Point2& measured, const gtsam::Cal3_S2* K, const gtsam::noiseModel::Base* model); + InvDepthFactorVariant3a(size_t poseKey, + size_t landmarkKey, + const gtsam::Point2& measured, + const gtsam::Cal3_S2* K, + const gtsam::noiseModel::Base* model); }; virtual class InvDepthFactorVariant3b : gtsam::NoiseModelFactor { - InvDepthFactorVariant3b(size_t poseKey1, size_t poseKey2, size_t landmarkKey, const gtsam::Point2& measured, const gtsam::Cal3_S2* K, const gtsam::noiseModel::Base* model); + InvDepthFactorVariant3b(size_t poseKey1, + size_t poseKey2, + size_t landmarkKey, + const gtsam::Point2& measured, + const gtsam::Cal3_S2* K, + const gtsam::noiseModel::Base* model); }; - #include class Mechanization_bRn2 { Mechanization_bRn2(); - Mechanization_bRn2(gtsam::Rot3& initial_bRn, Vector initial_x_g, - Vector initial_x_a); + Mechanization_bRn2(gtsam::Rot3& initial_bRn, Vector initial_x_g, Vector initial_x_a); Vector b_g(double g_e); gtsam::Rot3 bRn(); Vector x_g(); @@ -658,55 +751,82 @@ class Mechanization_bRn2 { static gtsam::Mechanization_bRn2 initialize(Matrix U, Matrix F, double g_e); gtsam::Mechanization_bRn2 correct(Vector dx) const; gtsam::Mechanization_bRn2 integrate(Vector u, double dt) const; - //void print(string s) const; + // void print(string s) const; }; #include class AHRS { AHRS(Matrix U, Matrix F, double g_e); pair initialize(double g_e); - pair integrate(const gtsam::Mechanization_bRn2& mech, gtsam::GaussianDensity* state, Vector u, double dt); - pair aid(const gtsam::Mechanization_bRn2& mech, gtsam::GaussianDensity* state, Vector f, bool Farrel); - pair aidGeneral(const gtsam::Mechanization_bRn2& mech, gtsam::GaussianDensity* state, Vector f, Vector f_expected, const gtsam::Rot3& increment); - //void print(string s) const; + pair integrate( + const gtsam::Mechanization_bRn2& mech, gtsam::GaussianDensity* state, Vector u, double dt); + pair aid( + const gtsam::Mechanization_bRn2& mech, gtsam::GaussianDensity* state, Vector f, bool Farrel); + pair aidGeneral( + const gtsam::Mechanization_bRn2& mech, + gtsam::GaussianDensity* state, + Vector f, + Vector f_expected, + const gtsam::Rot3& increment); + // void print(string s) const; }; // Tectonic SAM Factors #include -//typedef gtsam::NoiseModelFactorN NLPosePose; +// typedef gtsam::NoiseModelFactorN NLPosePose; virtual class DeltaFactor : gtsam::NoiseModelFactor { - DeltaFactor(size_t i, size_t j, const gtsam::Point2& measured, - const gtsam::noiseModel::Base* noiseModel); - //void print(string s) const; + DeltaFactor(size_t i, + size_t j, + const gtsam::Point2& measured, + const gtsam::noiseModel::Base* noiseModel); + // void print(string s) const; }; -//typedef gtsam::NoiseModelFactorN NLPosePosePosePoint; +// typedef gtsam::NoiseModelFactorN NLPosePosePosePoint; virtual class DeltaFactorBase : gtsam::NoiseModelFactor { - DeltaFactorBase(size_t b1, size_t i, size_t b2, size_t j, - const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel); - //void print(string s) const; + DeltaFactorBase(size_t b1, + size_t i, + size_t b2, + size_t j, + const gtsam::Point2& measured, + const gtsam::noiseModel::Base* noiseModel); + // void print(string s) const; }; -//typedef gtsam::NoiseModelFactorN NLPosePosePosePose; +// typedef gtsam::NoiseModelFactorN NLPosePosePosePose; virtual class OdometryFactorBase : gtsam::NoiseModelFactor { - OdometryFactorBase(size_t b1, size_t i, size_t b2, size_t j, - const gtsam::Pose2& measured, const gtsam::noiseModel::Base* noiseModel); - //void print(string s) const; + OdometryFactorBase(size_t b1, + size_t i, + size_t b2, + size_t j, + const gtsam::Pose2& measured, + const gtsam::noiseModel::Base* noiseModel); + // void print(string s) const; }; #include -#include #include -template +#include +template virtual class ProjectionFactorPPP : gtsam::NoiseModelFactor { - ProjectionFactorPPP(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, - size_t poseKey, size_t transformKey, size_t pointKey, const CALIBRATION* k); - - ProjectionFactorPPP(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, - size_t poseKey, size_t transformKey, size_t pointKey, const CALIBRATION* k, bool throwCheirality, bool verboseCheirality); + ProjectionFactorPPP(const gtsam::Point2& measured, + const gtsam::noiseModel::Base* noiseModel, + size_t poseKey, + size_t transformKey, + size_t pointKey, + const CALIBRATION* k); + + ProjectionFactorPPP(const gtsam::Point2& measured, + const gtsam::noiseModel::Base* noiseModel, + size_t poseKey, + size_t transformKey, + size_t pointKey, + const CALIBRATION* k, + bool throwCheirality, + bool verboseCheirality); gtsam::Point2 measured() const; CALIBRATION* calibration() const; @@ -716,18 +836,31 @@ virtual class ProjectionFactorPPP : gtsam::NoiseModelFactor { // enabling serialization functionality void serialize() const; }; -typedef gtsam::ProjectionFactorPPP ProjectionFactorPPPCal3_S2; -typedef gtsam::ProjectionFactorPPP ProjectionFactorPPPCal3DS2; -typedef gtsam::ProjectionFactorPPP ProjectionFactorPPPCal3Fisheye; +typedef gtsam::ProjectionFactorPPP + ProjectionFactorPPPCal3_S2; +typedef gtsam::ProjectionFactorPPP + ProjectionFactorPPPCal3DS2; +typedef gtsam::ProjectionFactorPPP + ProjectionFactorPPPCal3Fisheye; #include -template +template virtual class ProjectionFactorPPPC : gtsam::NoiseModelFactor { - ProjectionFactorPPPC(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, - size_t poseKey, size_t transformKey, size_t pointKey, size_t calibKey); - - ProjectionFactorPPPC(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, - size_t poseKey, size_t transformKey, size_t pointKey, size_t calibKey, bool throwCheirality, bool verboseCheirality); + ProjectionFactorPPPC(const gtsam::Point2& measured, + const gtsam::noiseModel::Base* noiseModel, + size_t poseKey, + size_t transformKey, + size_t pointKey, + size_t calibKey); + + ProjectionFactorPPPC(const gtsam::Point2& measured, + const gtsam::noiseModel::Base* noiseModel, + size_t poseKey, + size_t transformKey, + size_t pointKey, + size_t calibKey, + bool throwCheirality, + bool verboseCheirality); gtsam::Point2 measured() const; bool verboseCheirality() const; @@ -736,25 +869,52 @@ virtual class ProjectionFactorPPPC : gtsam::NoiseModelFactor { // enabling serialization functionality void serialize() const; }; -typedef gtsam::ProjectionFactorPPPC ProjectionFactorPPPCCal3_S2; -typedef gtsam::ProjectionFactorPPPC ProjectionFactorPPPCCal3DS2; -typedef gtsam::ProjectionFactorPPPC ProjectionFactorPPPCCal3Fisheye; +typedef gtsam::ProjectionFactorPPPC + ProjectionFactorPPPCCal3_S2; +typedef gtsam::ProjectionFactorPPPC + ProjectionFactorPPPCCal3DS2; +typedef gtsam::ProjectionFactorPPPC + ProjectionFactorPPPCCal3Fisheye; #include virtual class ProjectionFactorRollingShutter : gtsam::NoiseModelFactor { - ProjectionFactorRollingShutter(const gtsam::Point2& measured, double alpha, const gtsam::noiseModel::Base* noiseModel, - size_t poseKey_a, size_t poseKey_b, size_t pointKey, const gtsam::Cal3_S2* K); - - ProjectionFactorRollingShutter(const gtsam::Point2& measured, double alpha, const gtsam::noiseModel::Base* noiseModel, - size_t poseKey_a, size_t poseKey_b, size_t pointKey, const gtsam::Cal3_S2* K, gtsam::Pose3& body_P_sensor); - - ProjectionFactorRollingShutter(const gtsam::Point2& measured, double alpha, const gtsam::noiseModel::Base* noiseModel, - size_t poseKey_a, size_t poseKey_b, size_t pointKey, const gtsam::Cal3_S2* K, bool throwCheirality, - bool verboseCheirality); - - ProjectionFactorRollingShutter(const gtsam::Point2& measured, double alpha, const gtsam::noiseModel::Base* noiseModel, - size_t poseKey_a, size_t poseKey_b, size_t pointKey, const gtsam::Cal3_S2* K, bool throwCheirality, - bool verboseCheirality, gtsam::Pose3& body_P_sensor); + ProjectionFactorRollingShutter(const gtsam::Point2& measured, + double alpha, + const gtsam::noiseModel::Base* noiseModel, + size_t poseKey_a, + size_t poseKey_b, + size_t pointKey, + const gtsam::Cal3_S2* K); + + ProjectionFactorRollingShutter(const gtsam::Point2& measured, + double alpha, + const gtsam::noiseModel::Base* noiseModel, + size_t poseKey_a, + size_t poseKey_b, + size_t pointKey, + const gtsam::Cal3_S2* K, + gtsam::Pose3& body_P_sensor); + + ProjectionFactorRollingShutter(const gtsam::Point2& measured, + double alpha, + const gtsam::noiseModel::Base* noiseModel, + size_t poseKey_a, + size_t poseKey_b, + size_t pointKey, + const gtsam::Cal3_S2* K, + bool throwCheirality, + bool verboseCheirality); + + ProjectionFactorRollingShutter(const gtsam::Point2& measured, + double alpha, + const gtsam::noiseModel::Base* noiseModel, + size_t poseKey_a, + size_t poseKey_b, + size_t pointKey, + const gtsam::Cal3_S2* K, + bool throwCheirality, + bool verboseCheirality, + gtsam::Pose3& body_P_sensor); gtsam::Point2 measured() const; double alpha() const; @@ -766,4 +926,4 @@ virtual class ProjectionFactorRollingShutter : gtsam::NoiseModelFactor { void serialize() const; }; -} //\namespace gtsam +} // namespace gtsam diff --git a/gtsam_unstable/slam/serialization.cpp b/gtsam_unstable/slam/serialization.cpp index 4b1ecae54d..12b031b5ff 100644 --- a/gtsam_unstable/slam/serialization.cpp +++ b/gtsam_unstable/slam/serialization.cpp @@ -24,6 +24,7 @@ #include #include #include +#include //#include using namespace gtsam; @@ -38,6 +39,7 @@ typedef PriorFactor PriorFactorPose2; typedef PriorFactor PriorFactorPose3; typedef PriorFactor PriorFactorCal3_S2; typedef PriorFactor PriorFactorCal3DS2; +typedef PriorFactor PriorFactorCal3DS2_k3; typedef PriorFactor PriorFactorCalibratedCamera; typedef PriorFactor PriorFactorPinholeCameraCal3_S2; typedef PriorFactor PriorFactorStereoCamera; @@ -58,6 +60,7 @@ typedef NonlinearEquality NonlinearEqualityPose2; typedef NonlinearEquality NonlinearEqualityPose3; typedef NonlinearEquality NonlinearEqualityCal3_S2; typedef NonlinearEquality NonlinearEqualityCal3DS2; +typedef NonlinearEquality NonlinearEqualityCal3DS2_k3; typedef NonlinearEquality NonlinearEqualityCalibratedCamera; typedef NonlinearEquality NonlinearEqualityPinholeCameraCal3_S2; typedef NonlinearEquality NonlinearEqualityStereoCamera; @@ -76,6 +79,7 @@ typedef BearingRangeFactor BearingRangeFactor3D; typedef GenericProjectionFactor GenericProjectionFactorCal3_S2; typedef GenericProjectionFactor GenericProjectionFactorCal3DS2; +typedef GenericProjectionFactor GenericProjectionFactorCal3DS2_k3; typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3_S2; typedef gtsam::GeneralSFMFactor GeneralSFMFactorCal3DS2; @@ -113,6 +117,7 @@ GTSAM_VALUE_EXPORT(gtsam::Pose2); GTSAM_VALUE_EXPORT(gtsam::Pose3); GTSAM_VALUE_EXPORT(gtsam::Cal3_S2); GTSAM_VALUE_EXPORT(gtsam::Cal3DS2); +GTSAM_VALUE_EXPORT(gtsam::Cal3DS2_k3); GTSAM_VALUE_EXPORT(gtsam::Cal3_S2Stereo); GTSAM_VALUE_EXPORT(gtsam::CalibratedCamera); GTSAM_VALUE_EXPORT(gtsam::PinholeCameraCal3_S2);