Add Cal3DS2_k3 distortion model

gtsam/base/base.i

@@ -6,6 +6,7 @@ namespace gtsam {
 
 #include <gtsam/geometry/Cal3Bundler.h>
 #include <gtsam/geometry/Cal3DS2.h>
+#include <gtsam/geometry/Cal3DS2_k3.h>
 #include <gtsam/geometry/Cal3Fisheye.h>
 #include <gtsam/geometry/Cal3Unified.h>
 #include <gtsam/geometry/Cal3_S2.h>
@@ -102,7 +103,7 @@ virtual class Value {
 #include <gtsam/base/GenericValue.h>
 template <T = {Vector, Matrix, gtsam::Point2, gtsam::Point3, gtsam::Rot2,
                gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::StereoPoint2,
-               gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler,
+               gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3DS2_k3, gtsam::Cal3Bundler,
                gtsam::Cal3Fisheye, gtsam::Cal3Unified, gtsam::EssentialMatrix,
                gtsam::CalibratedCamera, gtsam::imuBias::ConstantBias}>
 virtual class GenericValue : gtsam::Value {

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 <gtsam/base/Matrix.h>
+#include <gtsam/base/Vector.h>
+#include <gtsam/geometry/Cal3DS2_k3.h>
+#include <gtsam/geometry/Point2.h>
+#include <gtsam/geometry/Point3.h>
+
+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<const Cal3DS2_k3_Base*>(&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
+/* ************************************************************************* */

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 <gtsam/geometry/Cal3DS2_k3_Base.h>
+
+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<Base> clone() const override
+    {
+        return boost::shared_ptr<Base>(new Cal3DS2_k3(*this));
+    }
+
+    /// @}
+
+   private:
+    /// @name Advanced Interface
+    /// @{
+
+    /** Serialization function */
+    friend class boost::serialization::access;
+    template <class Archive>
+    void serialize(Archive& ar, const unsigned int /*version*/)
+    {
+        ar& boost::serialization::make_nvp("Cal3DS2_k3", boost::serialization::base_object<Cal3DS2_k3_Base>(*this));
+    }
+
+    /// @}
+};
+
+template <>
+struct traits<Cal3DS2_k3> : public internal::Manifold<Cal3DS2_k3>
+{
+};
+
+template <>
+struct traits<const Cal3DS2_k3> : public internal::Manifold<Cal3DS2_k3>
+{
+};
+}  // namespace gtsam

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 <gtsam/base/Matrix.h>
+#include <gtsam/base/Vector.h>
+#include <gtsam/geometry/Cal3DS2_k3_Base.h>
+#include <gtsam/geometry/Point2.h>
+#include <gtsam/geometry/Point3.h>
+
+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<const Cal3*>(&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<double, 2, 10> 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<double, 2, 10> 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<Cal3DS2_k3_Base, dimension>(*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<double, 2, 10> 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
+/* ************************************************************************* */

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 <gtsam/geometry/Cal3DS2_Base.h>
+
+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<double, 2, 10> 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<Cal3DS2_k3_Base> clone() const
+    {
+        return boost::shared_ptr<Cal3DS2_k3_Base>(new Cal3DS2_k3_Base(*this));
+    }
+
+    /// @}
+
+   private:
+    /// @name Advanced Interface
+    /// @{
+
+    /** Serialization function */
+    friend class boost::serialization::access;
+    template <class Archive>
+    void serialize(Archive& ar, const unsigned int /*version*/)
+    {
+        ar& boost::serialization::make_nvp("Cal3DS2_k3_Base", boost::serialization::base_object<Cal3>(*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

gtsam/geometry/SimpleCamera.h

@@ -21,6 +21,7 @@
 #include <gtsam/geometry/BearingRange.h>
 #include <gtsam/geometry/Cal3Bundler.h>
 #include <gtsam/geometry/Cal3DS2.h>
+#include <gtsam/geometry/Cal3DS2_k3.h>
 #include <gtsam/geometry/Cal3Fisheye.h>
 #include <gtsam/geometry/Cal3Unified.h>
 #include <gtsam/geometry/Cal3Fisheye.h>
@@ -34,6 +35,7 @@ namespace gtsam {
   using PinholeCameraCal3_S2 = gtsam::PinholeCamera<gtsam::Cal3_S2>;
   using PinholeCameraCal3Bundler = gtsam::PinholeCamera<gtsam::Cal3Bundler>;
   using PinholeCameraCal3DS2 = gtsam::PinholeCamera<gtsam::Cal3DS2>;
+  using PinholeCameraCal3DS2_k3 = gtsam::PinholeCamera<gtsam::Cal3DS2_k3>;
   using PinholeCameraCal3Unified = gtsam::PinholeCamera<gtsam::Cal3Unified>;
   using PinholeCameraCal3Fisheye = gtsam::PinholeCamera<gtsam::Cal3Fisheye>;
 

gtsam/geometry/geometry.i

@@ -741,6 +741,56 @@ virtual class Cal3DS2 : gtsam::Cal3DS2_Base {
   void serialize() const;
 };
 
+#include <gtsam/geometry/Cal3DS2_k3_Base.h>
+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 <gtsam/geometry/Cal3DS2_k3.h>
+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 <gtsam/geometry/Cal3Unified.h>
 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<gtsam::Cal3_S2> PinholeCameraCal3_S2;
 typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2;
+typedef gtsam::PinholeCamera<gtsam::Cal3DS2_k3> PinholeCameraCal3DS2_k3;
 typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
 typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
 typedef gtsam::PinholeCamera<gtsam::Cal3Fisheye> PinholeCameraCal3Fisheye;
@@ -1061,6 +1112,7 @@ class PinholePose {
 
 typedef gtsam::PinholePose<gtsam::Cal3_S2> PinholePoseCal3_S2;
 typedef gtsam::PinholePose<gtsam::Cal3DS2> PinholePoseCal3DS2;
+typedef gtsam::PinholePose<gtsam::Cal3DS2_k3> PinholePoseCal3DS2_k3;
 typedef gtsam::PinholePose<gtsam::Cal3Unified> PinholePoseCal3Unified;
 typedef gtsam::PinholePose<gtsam::Cal3Bundler> PinholePoseCal3Bundler;
 typedef gtsam::PinholePose<gtsam::Cal3Fisheye> 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,

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 <CppUnitLite/TestHarness.h>
+#include <gtsam/base/Testable.h>
+#include <gtsam/base/TestableAssertions.h>
+#include <gtsam/base/numericalDerivative.h>
+#include <gtsam/geometry/Cal3DS2_k3.h>
+
+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<Point2>::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);
+}
+/* ************************************************************************* */

gtsam/geometry/tests/testSerializationGeometry.cpp

@@ -24,6 +24,7 @@
 #include <gtsam/geometry/CalibratedCamera.h>
 #include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/Cal3DS2.h>
+#include <gtsam/geometry/Cal3DS2_k3.h>
 #include <gtsam/geometry/Cal3Bundler.h>
 #include <gtsam/geometry/Cal3Unified.h>
 #include <gtsam/geometry/StereoCamera.h>
@@ -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<Cal3_S2> 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));

gtsam/geometry/triangulation.h

@@ -26,6 +26,7 @@
 #include <gtsam/geometry/Cal3Unified.h>
 #include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/geometry/Cal3DS2.h>
+#include <gtsam/geometry/Cal3DS2_k3.h>
 #include <gtsam/geometry/CameraSet.h>
 #include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/geometry/SphericalCamera.h>
@@ -757,6 +758,7 @@ TriangulationResult triangulateSafe(const CameraSet<CAMERA>& cameras,
 using CameraSetCal3Bundler = CameraSet<PinholeCamera<Cal3Bundler>>;
 using CameraSetCal3_S2 = CameraSet<PinholeCamera<Cal3_S2>>;
 using CameraSetCal3DS2 = CameraSet<PinholeCamera<Cal3DS2>>;
+using CameraSetCal3DS2_k3 = CameraSet<PinholeCamera<Cal3DS2_k3>>;
 using CameraSetCal3Fisheye = CameraSet<PinholeCamera<Cal3Fisheye>>;
 using CameraSetCal3Unified = CameraSet<PinholeCamera<Cal3Unified>>;
 using CameraSetSpherical = CameraSet<SphericalCamera>;

gtsam/nonlinear/nonlinear.i

@@ -6,6 +6,7 @@ namespace gtsam {
 
 #include <gtsam/geometry/Cal3Bundler.h>
 #include <gtsam/geometry/Cal3DS2.h>
+#include <gtsam/geometry/Cal3DS2_k3.h>
 #include <gtsam/geometry/Cal3Fisheye.h>
 #include <gtsam/geometry/Cal3Unified.h>
 #include <gtsam/geometry/Cal3_S2.h>
@@ -528,7 +529,7 @@ class ISAM2 {
   gtsam::Values calculateEstimate() const;
   template <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
                      gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
-                     gtsam::Cal3Bundler, gtsam::EssentialMatrix,
+                     gtsam::Cal3DS2_k3, gtsam::Cal3Bundler, gtsam::EssentialMatrix,
                      gtsam::PinholeCamera<gtsam::Cal3_S2>,
                      gtsam::PinholeCamera<gtsam::Cal3Bundler>,
                      gtsam::PinholeCamera<gtsam::Cal3Fisheye>,
@@ -594,6 +595,7 @@ template <T = {double,
                gtsam::Unit3,
                gtsam::Cal3_S2,
                gtsam::Cal3DS2,
+               gtsam::Cal3DS2_k3,
                gtsam::Cal3Bundler,
                gtsam::Cal3Fisheye,
                gtsam::Cal3Unified,
@@ -700,7 +702,7 @@ virtual class BatchFixedLagSmoother : gtsam::FixedLagSmoother {
   gtsam::LevenbergMarquardtParams params() const;
   template <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
                      gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
-                     Vector, Matrix}>
+                     gtsam::Cal3DS2_k3, Vector, Matrix}>
   VALUE calculateEstimate(size_t key) const;
 };
 

gtsam/nonlinear/values.i

@@ -6,6 +6,7 @@ namespace gtsam {
 
 #include <gtsam/geometry/Cal3Bundler.h>
 #include <gtsam/geometry/Cal3DS2.h>
+#include <gtsam/geometry/Cal3DS2_k3.h>
 #include <gtsam/geometry/Cal3Fisheye.h>
 #include <gtsam/geometry/Cal3Unified.h>
 #include <gtsam/geometry/Cal3_S2.h>
@@ -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,

gtsam_unstable/slam/serialization.cpp

@@ -24,6 +24,7 @@
 #include <gtsam/geometry/Pose2.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/geometry/Cal3DS2.h>
+#include <gtsam/geometry/Cal3DS2_k3.h>
 //#include <gtsam/geometry/Cal3_S2Stereo.h>
 
 using namespace gtsam;
@@ -38,6 +39,7 @@ typedef PriorFactor<Pose2>                PriorFactorPose2;
 typedef PriorFactor<Pose3>                PriorFactorPose3;
 typedef PriorFactor<Cal3_S2>              PriorFactorCal3_S2;
 typedef PriorFactor<Cal3DS2>              PriorFactorCal3DS2;
+typedef PriorFactor<Cal3DS2_k3>           PriorFactorCal3DS2_k3;
 typedef PriorFactor<CalibratedCamera>     PriorFactorCalibratedCamera;
 typedef PriorFactor<PinholeCameraCal3_S2> PriorFactorPinholeCameraCal3_S2;
 typedef PriorFactor<StereoCamera>         PriorFactorStereoCamera;
@@ -58,6 +60,7 @@ typedef NonlinearEquality<Pose2>                  NonlinearEqualityPose2;
 typedef NonlinearEquality<Pose3>                  NonlinearEqualityPose3;
 typedef NonlinearEquality<Cal3_S2>                NonlinearEqualityCal3_S2;
 typedef NonlinearEquality<Cal3DS2>                NonlinearEqualityCal3DS2;
+typedef NonlinearEquality<Cal3DS2_k3>             NonlinearEqualityCal3DS2_k3;
 typedef NonlinearEquality<CalibratedCamera>       NonlinearEqualityCalibratedCamera;
 typedef NonlinearEquality<PinholeCameraCal3_S2>   NonlinearEqualityPinholeCameraCal3_S2;
 typedef NonlinearEquality<StereoCamera>           NonlinearEqualityStereoCamera;
@@ -76,6 +79,7 @@ typedef BearingRangeFactor<Pose3, Point3>  BearingRangeFactor3D;
 
 typedef GenericProjectionFactor<Pose3, Point3, Cal3_S2> GenericProjectionFactorCal3_S2;
 typedef GenericProjectionFactor<Pose3, Point3, Cal3DS2> GenericProjectionFactorCal3DS2;
+typedef GenericProjectionFactor<Pose3, Point3, Cal3DS2_k3> GenericProjectionFactorCal3DS2_k3;
 
 typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3_S2, gtsam::Point3> GeneralSFMFactorCal3_S2;
 typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> 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);