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

Common function to compute Jacobians of calibrate method

release/4.3a0
Varun Agrawal 2020-12-01 00:39:32 -05:00
parent cb3a766b30
commit 012820b7fa
8 changed files with 76 additions and 68 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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)
// 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
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/Cal3DS2_Base.h>
#include <gtsam/geometry/Point2.h>
namespace gtsam {

22
gtsam/geometry/Cal3DS2_Base.cpp
View File

@ -34,8 +34,7 @@ Cal3DS2_Base::Cal3DS2_Base(const Vector& v)
k1_(v(5)),
k2_(v(6)),
p1_(v(7)),
p2_(v(8)),
tol_(1e-5) {}
p2_(v(8)) {}
/* ************************************************************************* */
Matrix3 Cal3DS2_Base::K() const {
@ -173,24 +172,7 @@ Point2 Cal3DS2_Base::calibrate(const Point2& pi, OptionalJacobian<2, 9> Dcal,
throw std::runtime_error(
"Cal3DS2::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)
// 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
Matrix29 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<Cal3DS2_Base, dimension>(*this, pn, Dcal, Dp);
return pn;
}

44
gtsam/geometry/Cal3DS2_Base.h
View File

@ -23,6 +23,39 @@
namespace gtsam {
/**
* Function which makes use of the Implicit Function Theorem to compute the
* Jacobians of `calibrate` using `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)
* 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 T 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 T, size_t Dim>
void calibrateJacobians(const T& 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, H_uncal_K, 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;
}
}
/**
* @brief Calibration of a camera with radial distortion
* @addtogroup geometry
@ -40,14 +73,15 @@ 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 tol_; // tolerance value when calibrating
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
/// @{

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/Cal3DS2_Base.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

19
gtsam/geometry/Cal3Unified.cpp
View File

@ -111,24 +111,7 @@ Point2 Cal3Unified::calibrate(const Point2& pi, OptionalJacobian<2, 10> Dcal,
// call nplane to space
Point2 pn = this->nPlaneToSpace(pnplane);
// 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)
// 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
Eigen::Matrix<double, 2, 10> 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<Cal3Unified, dimension>(*this, pn, Dcal, Dp);
return pn;
}

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;