12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request #623 from borglab/fix/621 

Jacobians for Camera models
release/4.3a0
Varun Agrawal 2020-12-01 17:27:52 -05:00 committed by GitHub
parent 0b045fdc59 06d4933e1b
commit 5c7383a118
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
17 changed files with 238 additions and 91 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

66
gtsam/geometry/Cal3.h Normal file
View File

@ -0,0 +1,66 @@
/* ----------------------------------------------------------------------------
* 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 Cal3.h
* @brief Common code for all Calibration models.
* @author Varun Agrawal
*/
/**
* @addtogroup geometry
*/
#pragma once
#include <gtsam/geometry/Point2.h>
namespace gtsam {
/**
* Function which makes use of the Implicit Function Theorem to compute the
* Jacobians of `calibrate` using `uncalibrate`.
* This is useful when there are iterative operations in the `calibrate`
* function which make computing jacobians difficult.
*
* Given f(pi, pn) = uncalibrate(pn) - pi, and g(pi) = calibrate, we can
* easily compute the Jacobians:
* df/pi = -I (pn and pi are independent args)
* Dp = -inv(H_uncal_pn) * df/pi = -inv(H_uncal_pn) * (-I) = inv(H_uncal_pn)
* Dcal = -inv(H_uncal_pn) * df/K = -inv(H_uncal_pn) * H_uncal_K
*
* @tparam Cal Calibration model.
* @tparam Dim The number of parameters in the calibration model.
* @param p Calibrated point.
* @param Dcal optional 2*p Jacobian wrpt `p` Cal3DS2 parameters.
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates.
*/
template <typename Cal, size_t Dim>
void calibrateJacobians(const Cal& calibration, const Point2& pn,
OptionalJacobian<2, Dim> Dcal = boost::none,
OptionalJacobian<2, 2> Dp = boost::none) {
if (Dcal || Dp) {
Eigen::Matrix<double, 2, Dim> H_uncal_K;
Matrix22 H_uncal_pn, H_uncal_pn_inv;
// Compute uncalibrate Jacobians
calibration.uncalibrate(pn, Dcal ? &H_uncal_K : nullptr, H_uncal_pn);
H_uncal_pn_inv = H_uncal_pn.inverse();
if (Dp) *Dp = H_uncal_pn_inv;
if (Dcal) *Dcal = -H_uncal_pn_inv * H_uncal_K;
}
}
//TODO(Varun) Make common base class for all calibration models.
} // \ namespace gtsam

19
gtsam/geometry/Cal3Bundler.cpp
View File

@ -121,24 +121,7 @@ Point2 Cal3Bundler::calibrate(const Point2& pi,
throw std::runtime_error(
"Cal3Bundler::calibrate fails to converge. need a better initialization");
// We make use of the Implicit Function Theorem to compute the Jacobians from uncalibrate
// Given f(pi, pn) = uncalibrate(pn) - pi, and g(pi) = calibrate, we can easily compute the Jacobians
// df/pi = -I (pn and pi are independent args)
// Dcal = -inv(H_uncal_pn) * df/pi = -inv(H_uncal_pn) * (-I) = inv(H_uncal_pn)
// Dp = -inv(H_uncal_pn) * df/K = -inv(H_uncal_pn) * H_uncal_K
Matrix23 H_uncal_K;
Matrix22 H_uncal_pn, H_uncal_pn_inv;
if (Dcal || Dp) {
// Compute uncalibrate Jacobians
uncalibrate(pn, Dcal ? &H_uncal_K : nullptr, H_uncal_pn);
H_uncal_pn_inv = H_uncal_pn.inverse();
if (Dp) *Dp = H_uncal_pn_inv;
if (Dcal) *Dcal = -H_uncal_pn_inv * H_uncal_K;
}
calibrateJacobians<Cal3Bundler, dimension>(*this, pn, Dcal, Dp);
return pn;
}

1
gtsam/geometry/Cal3Bundler.h
View File

@ -18,6 +18,7 @@
#pragma once
#include <gtsam/geometry/Cal3.h>
#include <gtsam/geometry/Point2.h>
namespace gtsam {

4
gtsam/geometry/Cal3DS2.h
View File

@ -44,8 +44,8 @@ public:
Cal3DS2() : Base() {}
Cal3DS2(double fx, double fy, double s, double u0, double v0,
double k1, double k2, double p1 = 0.0, double p2 = 0.0) :
Base(fx, fy, s, u0, v0, k1, k2, p1, p2) {}
double k1, double k2, double p1 = 0.0, double p2 = 0.0, double tol = 1e-5) :
Base(fx, fy, s, u0, v0, k1, k2, p1, p2, tol) {}
virtual ~Cal3DS2() {}

54
gtsam/geometry/Cal3DS2_Base.cpp
View File

@ -13,6 +13,7 @@
* @file Cal3DS2_Base.cpp
* @date Feb 28, 2010
* @author ydjian
* @author Varun Agrawal
*/
#include <gtsam/base/Vector.h>
@ -24,8 +25,16 @@
namespace gtsam {
/* ************************************************************************* */
Cal3DS2_Base::Cal3DS2_Base(const Vector &v):
fx_(v[0]), fy_(v[1]), s_(v[2]), u0_(v[3]), v0_(v[4]), k1_(v[5]), k2_(v[6]), p1_(v[7]), p2_(v[8]){}
Cal3DS2_Base::Cal3DS2_Base(const Vector& v)
: fx_(v(0)),
fy_(v(1)),
s_(v(2)),
u0_(v(3)),
v0_(v(4)),
k1_(v(5)),
k2_(v(6)),
p1_(v(7)),
p2_(v(8)) {}
/* ************************************************************************* */
Matrix3 Cal3DS2_Base::K() const {
@ -94,9 +103,8 @@ static Matrix2 D2dintrinsic(double x, double y, double rr,
}
/* ************************************************************************* */
Point2 Cal3DS2_Base::uncalibrate(const Point2& p,
OptionalJacobian<2,9> H1, OptionalJacobian<2,2> H2) const {
Point2 Cal3DS2_Base::uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal,
OptionalJacobian<2, 2> Dp) const {
// rr = x^2 + y^2;
// g = (1 + k(1)*rr + k(2)*rr^2);
// dp = [2*k(3)*x*y + k(4)*(rr + 2*x^2); 2*k(4)*x*y + k(3)*(rr + 2*y^2)];
@ -115,37 +123,44 @@ Point2 Cal3DS2_Base::uncalibrate(const Point2& p,
const double pny = g * y + dy;
Matrix2 DK;
if (H1 || H2) DK << fx_, s_, 0.0, fy_;
if (Dcal || Dp) {
DK << fx_, s_, 0.0, fy_;
}
// Derivative for calibration
if (H1)
*H1 = D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
if (Dcal) {
*Dcal = D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
}
// Derivative for points
if (H2)
*H2 = D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK);
if (Dp) {
*Dp = D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK);
}
// Regular uncalibrate after distortion
return Point2(fx_ * pnx + s_ * pny + u0_, fy_ * pny + v0_);
}
/* ************************************************************************* */
Point2 Cal3DS2_Base::calibrate(const Point2& pi, const double tol) const {
Point2 Cal3DS2_Base::calibrate(const Point2& pi, OptionalJacobian<2, 9> 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_));
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
// 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 x = pn.x(), y = pn.y(), xy = x * y, xx = x * x, yy = y * y;
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 g = (1 + k1_ * rr + k2_ * rr * rr);
const double dx = 2 * p1_ * xy + p2_ * (rr + 2 * xx);
@ -153,8 +168,11 @@ Point2 Cal3DS2_Base::calibrate(const Point2& pi, const double tol) const {
pn = (invKPi - Point2(dx, dy)) / g;
}
if ( iteration >= maxIterations )
throw std::runtime_error("Cal3DS2::calibrate fails to converge. need a better initialization");
if (iteration >= maxIterations)
throw std::runtime_error(
"Cal3DS2::calibrate fails to converge. need a better initialization");
calibrateJacobians<Cal3DS2_Base, dimension>(*this, pn, Dcal, Dp);
return pn;
}

52
gtsam/geometry/Cal3DS2_Base.h
View File

@ -14,10 +14,12 @@
* @brief Calibration of a camera with radial distortion
* @date Feb 28, 2010
* @author ydjian
* @author Varun Agrawal
*/
#pragma once
#include <gtsam/geometry/Cal3.h>
#include <gtsam/geometry/Point2.h>
namespace gtsam {
@ -39,22 +41,43 @@ namespace gtsam {
class GTSAM_EXPORT Cal3DS2_Base {
protected:
double fx_, fy_, s_, u0_, v0_ ; // focal length, skew and principal point
double k1_, k2_ ; // radial 2nd-order and 4th-order
double p1_, p2_ ; // tangential distortion
double fx_, fy_, s_, u0_, v0_; // focal length, skew and principal point
double k1_, k2_; // radial 2nd-order and 4th-order
double p1_, p2_; // tangential distortion
double tol_ = 1e-5; // tolerance value when calibrating
public:
enum { dimension = 9 };
/// @name Standard Constructors
/// @{
/// Default Constructor with only unit focal length
Cal3DS2_Base() : fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), p1_(0), p2_(0) {}
/// Default Constructor with only unit focal length
Cal3DS2_Base()
: fx_(1),
fy_(1),
s_(0),
u0_(0),
v0_(0),
k1_(0),
k2_(0),
p1_(0),
p2_(0),
tol_(1e-5) {}
Cal3DS2_Base(double fx, double fy, double s, double u0, double v0,
double k1, double k2, double p1 = 0.0, double p2 = 0.0) :
fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0), k1_(k1), k2_(k2), p1_(p1), p2_(p2) {}
Cal3DS2_Base(double fx, double fy, double s, double u0, double v0, double k1,
double k2, double p1 = 0.0, double p2 = 0.0, double tol = 1e-5)
: fx_(fx),
fy_(fy),
s_(s),
u0_(u0),
v0_(v0),
k1_(k1),
k2_(k2),
p1_(p1),
p2_(p2),
tol_(tol) {}
virtual ~Cal3DS2_Base() {}
@ -72,7 +95,7 @@ public:
virtual void print(const std::string& s = "") const;
/// assert equality up to a tolerance
bool equals(const Cal3DS2_Base& K, double tol = 10e-9) const;
bool equals(const Cal3DS2_Base& K, double tol = 1e-8) const;
/// @}
/// @name Standard Interface
@ -121,12 +144,12 @@ public:
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in (distorted) image coordinates
*/
Point2 uncalibrate(const Point2& p,
OptionalJacobian<2,9> Dcal = boost::none,
OptionalJacobian<2,2> Dp = boost::none) const ;
Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none,
OptionalJacobian<2, 2> Dp = boost::none) const;
/// Convert (distorted) image coordinates uv to intrinsic coordinates xy
Point2 calibrate(const Point2& p, const double tol=1e-5) const;
Point2 calibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none,
OptionalJacobian<2, 2> Dp = boost::none) const;
/// Derivative of uncalibrate wrpt intrinsic coordinates
Matrix2 D2d_intrinsic(const Point2& p) const ;
@ -164,6 +187,7 @@ private:
ar & BOOST_SERIALIZATION_NVP(k2_);
ar & BOOST_SERIALIZATION_NVP(p1_);
ar & BOOST_SERIALIZATION_NVP(p2_);
ar & BOOST_SERIALIZATION_NVP(tol_);
}
/// @}

9
gtsam/geometry/Cal3Fisheye.cpp
View File

@ -122,14 +122,15 @@ Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1,
}
/* ************************************************************************* */
Point2 Cal3Fisheye::calibrate(const Point2& uv, const double tol) const {
Point2 Cal3Fisheye::calibrate(const Point2& uv, OptionalJacobian<2, 9> Dcal,
OptionalJacobian<2, 2> Dp) const {
// initial gues just inverts the pinhole model
const double u = uv.x(), v = uv.y();
const double yd = (v - v0_) / fy_;
const double xd = (u - s_ * yd - u0_) / fx_;
Point2 pi(xd, yd);
// Perform newtons method, break when solution converges past tol,
// Perform newtons method, break when solution converges past tol_,
// throw exception if max iterations are reached
const int maxIterations = 10;
int iteration;
@ -140,7 +141,7 @@ Point2 Cal3Fisheye::calibrate(const Point2& uv, const double tol) const {
const Point2 uv_hat = uncalibrate(pi, boost::none, jac);
// Test convergence
if ((uv_hat - uv).norm() < tol) break;
if ((uv_hat - uv).norm() < tol_) break;
// Newton's method update step
pi = pi - jac.inverse() * (uv_hat - uv);
@ -151,6 +152,8 @@ Point2 Cal3Fisheye::calibrate(const Point2& uv, const double tol) const {
"Cal3Fisheye::calibrate fails to converge. need a better "
"initialization");
calibrateJacobians<Cal3Fisheye, dimension>(*this, pi, Dcal, Dp);
return pi;
}

13
gtsam/geometry/Cal3Fisheye.h
View File

@ -14,10 +14,12 @@
* @brief Calibration of a fisheye camera
* @date Apr 8, 2020
* @author ghaggin
* @author Varun Agrawal
*/
#pragma once
#include <gtsam/geometry/Cal3.h>
#include <gtsam/geometry/Point2.h>
#include <string>
@ -48,6 +50,7 @@ class GTSAM_EXPORT Cal3Fisheye {
private:
double fx_, fy_, s_, u0_, v0_; // focal length, skew and principal point
double k1_, k2_, k3_, k4_; // fisheye distortion coefficients
double tol_ = 1e-5; // tolerance value when calibrating
public:
enum { dimension = 9 };
@ -59,11 +62,11 @@ class GTSAM_EXPORT Cal3Fisheye {
/// Default Constructor with only unit focal length
Cal3Fisheye()
: fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), k3_(0), k4_(0) {}
: fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), k3_(0), k4_(0), tol_(1e-5) {}
Cal3Fisheye(const double fx, const double fy, const double s, const double u0,
const double v0, const double k1, const double k2,
const double k3, const double k4)
const double k3, const double k4, double tol = 1e-5)
: fx_(fx),
fy_(fy),
s_(s),
@ -72,7 +75,8 @@ class GTSAM_EXPORT Cal3Fisheye {
k1_(k1),
k2_(k2),
k3_(k3),
k4_(k4) {}
k4_(k4),
tol_(tol) {}
virtual ~Cal3Fisheye() {}
@ -139,7 +143,8 @@ class GTSAM_EXPORT Cal3Fisheye {
/// Convert (distorted) image coordinates [u;v] to intrinsic coordinates [x_i,
/// y_i]
Point2 calibrate(const Point2& p, const double tol = 1e-5) const;
Point2 calibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none,
OptionalJacobian<2, 2> Dp = boost::none) const;
/// @}
/// @name Testable

25
gtsam/geometry/Cal3Unified.cpp
View File

@ -13,6 +13,7 @@
* @file Cal3Unified.cpp
* @date Mar 8, 2014
* @author Jing Dong
* @author Varun Agrawal
*/
#include <gtsam/base/Vector.h>
@ -54,8 +55,8 @@ bool Cal3Unified::equals(const Cal3Unified& K, double tol) const {
/* ************************************************************************* */
// todo: make a fixed sized jacobian version of this
Point2 Cal3Unified::uncalibrate(const Point2& p,
OptionalJacobian<2,10> H1,
OptionalJacobian<2,2> H2) const {
OptionalJacobian<2,10> Dcal,
OptionalJacobian<2,2> Dp) const {
// this part of code is modified from Cal3DS2,
// since the second part of this model (after project to normalized plane)
@ -78,16 +79,16 @@ Point2 Cal3Unified::uncalibrate(const Point2& p,
Point2 puncalib = Base::uncalibrate(m, H1base, H2base);
// Inlined derivative for calibration
if (H1) {
if (Dcal) {
// part1
Vector2 DU;
DU << -xs * sqrt_nx * xi_sqrt_nx2, //
-ys * sqrt_nx * xi_sqrt_nx2;
*H1 << H1base, H2base * DU;
*Dcal << H1base, H2base * DU;
}
// Inlined derivative for points
if (H2) {
if (Dp) {
// part1
const double denom = 1.0 * xi_sqrt_nx2 / sqrt_nx;
const double mid = -(xi * xs*ys) * denom;
@ -95,20 +96,24 @@ Point2 Cal3Unified::uncalibrate(const Point2& p,
DU << (sqrt_nx + xi*(ys*ys + 1)) * denom, mid, //
mid, (sqrt_nx + xi*(xs*xs + 1)) * denom;
*H2 << H2base * DU;
*Dp << H2base * DU;
}
return puncalib;
}
/* ************************************************************************* */
Point2 Cal3Unified::calibrate(const Point2& pi, const double tol) const {
Point2 Cal3Unified::calibrate(const Point2& pi, OptionalJacobian<2, 10> Dcal,
OptionalJacobian<2, 2> Dp) const {
// calibrate point to Nplane use base class::calibrate()
Point2 pnplane = Base::calibrate(pi, tol);
Point2 pnplane = Base::calibrate(pi);
// call nplane to space
return this->nPlaneToSpace(pnplane);
Point2 pn = this->nPlaneToSpace(pnplane);
calibrateJacobians<Cal3Unified, dimension>(*this, pn, Dcal, Dp);
return pn;
}
/* ************************************************************************* */
Point2 Cal3Unified::nPlaneToSpace(const Point2& p) const {

4
gtsam/geometry/Cal3Unified.h
View File

@ -14,6 +14,7 @@
* @brief Unified Calibration Model, see Mei07icra for details
* @date Mar 8, 2014
* @author Jing Dong
* @author Varun Agrawal
*/
/**
@ -99,7 +100,8 @@ public:
OptionalJacobian<2,2> Dp = boost::none) const ;
/// Conver a pixel coordinate to ideal coordinate
Point2 calibrate(const Point2& p, const double tol=1e-5) const;
Point2 calibrate(const Point2& p, OptionalJacobian<2, 10> Dcal = boost::none,
OptionalJacobian<2, 2> Dp = boost::none) const;
/// Convert a 3D point to normalized unit plane
Point2 spaceToNPlane(const Point2& p) const;

5
gtsam/geometry/SimpleCamera.h
View File

@ -31,9 +31,8 @@ namespace gtsam {
/// Also needed as forward declarations in the wrapper.
using PinholeCameraCal3_S2 = gtsam::PinholeCamera<gtsam::Cal3_S2>;
using PinholeCameraCal3Bundler = gtsam::PinholeCamera<gtsam::Cal3Bundler>;
//TODO Need to fix issue 621 for this to work with wrapper
// using PinholeCameraCal3DS2 = gtsam::PinholeCamera<gtsam::Cal3DS2>;
// using PinholeCameraCal3Unified = gtsam::PinholeCamera<gtsam::Cal3Unified>;
using PinholeCameraCal3DS2 = gtsam::PinholeCamera<gtsam::Cal3DS2>;
using PinholeCameraCal3Unified = gtsam::PinholeCamera<gtsam::Cal3Unified>;
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
/**

17
gtsam/geometry/tests/testCal3DFisheye.cpp
View File

@ -181,6 +181,23 @@ TEST(Cal3Fisheye, calibrate3) {
CHECK(assert_equal(xi_hat, xi));
}
Point2 calibrate_(const Cal3Fisheye& k, const Point2& pt) {
return k.calibrate(pt);
}
/* ************************************************************************* */
TEST(Cal3Fisheye, Dcalibrate)
{
Point2 p(0.5, 0.5);
Point2 pi = K.uncalibrate(p);
Matrix Dcal, Dp;
K.calibrate(pi, Dcal, Dp);
Matrix numerical1 = numericalDerivative21(calibrate_, K, pi);
CHECK(assert_equal(numerical1,Dcal,1e-5));
Matrix numerical2 = numericalDerivative22(calibrate_, K, pi);
CHECK(assert_equal(numerical2,Dp,1e-5));
}
/* ************************************************************************* */
int main() {
TestResult tr;

22
gtsam/geometry/tests/testCal3DS2.cpp
View File

@ -74,12 +74,26 @@ TEST( Cal3DS2, Duncalibrate2)
CHECK(assert_equal(numerical,separate,1e-5));
}
/* ************************************************************************* */
TEST( Cal3DS2, assert_equal)
{
CHECK(assert_equal(K,K,1e-5));
Point2 calibrate_(const Cal3DS2& k, const Point2& pt) {
return k.calibrate(pt);
}
/* ************************************************************************* */
TEST( Cal3DS2, 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, assert_equal) { CHECK(assert_equal(K, K, 1e-5)); }
/* ************************************************************************* */
TEST( Cal3DS2, retract)
{

16
gtsam/geometry/tests/testCal3Unified.cpp
View File

@ -82,6 +82,22 @@ TEST( Cal3Unified, Duncalibrate2)
CHECK(assert_equal(numerical,computed,1e-6));
}
Point2 calibrate_(const Cal3Unified& k, const Point2& pt) {
return k.calibrate(pt);
}
/* ************************************************************************* */
TEST( Cal3Unified, Dcalibrate)
{
Point2 pi = K.uncalibrate(p);
Matrix Dcal, Dp;
K.calibrate(pi, Dcal, Dp);
Matrix numerical1 = numericalDerivative21(calibrate_, K, pi);
CHECK(assert_equal(numerical1,Dcal,1e-5));
Matrix numerical2 = numericalDerivative22(calibrate_, K, pi);
CHECK(assert_equal(numerical2,Dp,1e-5));
}
/* ************************************************************************* */
TEST( Cal3Unified, assert_equal)
{

10
gtsam/gtsam.i
View File

@ -881,7 +881,7 @@ virtual class Cal3DS2_Base {
// Action on Point2
gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const;
gtsam::Point2 calibrate(const gtsam::Point2& p) const;
// enabling serialization functionality
void serialize() const;
@ -1064,9 +1064,8 @@ class PinholeCamera {
// Some typedefs for common camera types
// PinholeCameraCal3_S2 is the same as SimpleCamera above
typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
//TODO (Issue 621) due to lack of jacobians of Cal3DS2_Base::calibrate, PinholeCamera does not apply to Cal3DS2/Unified
//typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2;
//typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2;
typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
#include <gtsam/geometry/StereoCamera.h>
@ -2634,8 +2633,7 @@ virtual class GeneralSFMFactor : gtsam::NoiseModelFactor {
gtsam::Point2 measured() const;
};
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3_S2, gtsam::Point3> GeneralSFMFactorCal3_S2;
//TODO (Issue 621) due to lack of jacobians of Cal3DS2_Base::calibrate, GeneralSFMFactor does not apply to Cal3DS2
//typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCamera<gtsam::Cal3Bundler>, gtsam::Point3> GeneralSFMFactorCal3Bundler;
template<CALIBRATION = {gtsam::Cal3_S2}>

6
gtsam_unstable/slam/serialization.cpp
View File

@ -86,8 +86,7 @@ typedef GenericProjectionFactor<Pose3, Point3, Cal3_S2> GenericProjectionFactorC
typedef GenericProjectionFactor<Pose3, Point3, Cal3DS2> GenericProjectionFactorCal3DS2;
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3_S2, gtsam::Point3> GeneralSFMFactorCal3_S2;
//TODO fix issue 621
//typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
typedef gtsam::GeneralSFMFactor2<gtsam::Cal3_S2> GeneralSFMFactor2Cal3_S2;
@ -184,8 +183,7 @@ BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3_S2, "gtsam::GenericProjectio
BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3DS2, "gtsam::GenericProjectionFactorCal3DS2");
BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3_S2, "gtsam::GeneralSFMFactorCal3_S2");
//TODO Fix issue 621
//BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3DS2, "gtsam::GeneralSFMFactorCal3DS2");
BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3DS2, "gtsam::GeneralSFMFactorCal3DS2");
BOOST_CLASS_EXPORT_GUID(GeneralSFMFactor2Cal3_S2, "gtsam::GeneralSFMFactor2Cal3_S2");

6
tests/testSerializationSLAM.cpp
View File

@ -100,8 +100,7 @@ typedef GenericProjectionFactor<Pose3, Point3, Cal3_S2> GenericProjectionFactorC
typedef GenericProjectionFactor<Pose3, Point3, Cal3DS2> GenericProjectionFactorCal3DS2;
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3_S2, gtsam::Point3> GeneralSFMFactorCal3_S2;
//TODO Fix issue 621 for this to work
//typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
typedef gtsam::GeneralSFMFactor2<gtsam::Cal3_S2> GeneralSFMFactor2Cal3_S2;
@ -198,8 +197,7 @@ BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3_S2, "gtsam::GenericProjectio
BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3DS2, "gtsam::GenericProjectionFactorCal3DS2");
BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3_S2, "gtsam::GeneralSFMFactorCal3_S2");
//TODO fix issue 621
//BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3DS2, "gtsam::GeneralSFMFactorCal3DS2");
BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3DS2, "gtsam::GeneralSFMFactorCal3DS2");
BOOST_CLASS_EXPORT_GUID(GeneralSFMFactor2Cal3_S2, "gtsam::GeneralSFMFactor2Cal3_S2");