Frank Dellaert
GitHub
commit
6a23c476a1
|
|
@ -0,0 +1,129 @@
|
||||||
|
/* ----------------------------------------------------------------------------
|
||||||
|
|
||||||
|
* 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 FisheyeExample.cpp
|
||||||
|
* @brief A visualSLAM example for the structure-from-motion problem on a
|
||||||
|
* simulated dataset. This version uses a fisheye camera model and a GaussNewton
|
||||||
|
* solver to solve the graph in one batch
|
||||||
|
* @author ghaggin
|
||||||
|
* @Date Apr 9,2020
|
||||||
|
*/
|
||||||
|
|
||||||
|
/**
|
||||||
|
* A structure-from-motion example with landmarks
|
||||||
|
* - The landmarks form a 10 meter cube
|
||||||
|
* - The robot rotates around the landmarks, always facing towards the cube
|
||||||
|
*/
|
||||||
|
|
||||||
|
// For loading the data
|
||||||
|
#include "SFMdata.h"
|
||||||
|
|
||||||
|
// Camera observations of landmarks will be stored as Point2 (x, y).
|
||||||
|
#include <gtsam/geometry/Point2.h>
|
||||||
|
|
||||||
|
// Each variable in the system (poses and landmarks) must be identified with a
|
||||||
|
// unique key. We can either use simple integer keys (1, 2, 3, ...) or symbols
|
||||||
|
// (X1, X2, L1). Here we will use Symbols
|
||||||
|
#include <gtsam/inference/Symbol.h>
|
||||||
|
|
||||||
|
// Use GaussNewtonOptimizer to solve graph
|
||||||
|
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
|
||||||
|
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||||
|
#include <gtsam/nonlinear/Values.h>
|
||||||
|
|
||||||
|
// In GTSAM, measurement functions are represented as 'factors'. Several common
|
||||||
|
// factors have been provided with the library for solving robotics/SLAM/Bundle
|
||||||
|
// Adjustment problems. Here we will use Projection factors to model the
|
||||||
|
// camera's landmark observations. Also, we will initialize the robot at some
|
||||||
|
// location using a Prior factor.
|
||||||
|
#include <gtsam/geometry/Cal3Fisheye.h>
|
||||||
|
#include <gtsam/slam/PriorFactor.h>
|
||||||
|
#include <gtsam/slam/ProjectionFactor.h>
|
||||||
|
|
||||||
|
#include <fstream>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
|
using namespace std;
|
||||||
|
using namespace gtsam;
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
int main(int argc, char *argv[])
|
||||||
|
{
|
||||||
|
// Define the camera calibration parameters
|
||||||
|
// Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
|
||||||
|
boost::shared_ptr<Cal3Fisheye> K(new Cal3Fisheye(
|
||||||
|
278.66, 278.48, 0.0, 319.75, 241.96, -0.013721808247486035, 0.020727425669427896, -0.012786476702685545, 0.0025242267320687625));
|
||||||
|
|
||||||
|
// Define the camera observation noise model, 1 pixel stddev
|
||||||
|
auto measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0);
|
||||||
|
|
||||||
|
// Create the set of ground-truth landmarks
|
||||||
|
vector<Point3> points = createPoints();
|
||||||
|
|
||||||
|
// Create the set of ground-truth poses
|
||||||
|
vector<Pose3> poses = createPoses();
|
||||||
|
|
||||||
|
// Create a Factor Graph and Values to hold the new data
|
||||||
|
NonlinearFactorGraph graph;
|
||||||
|
Values initialEstimate;
|
||||||
|
|
||||||
|
// Add a prior on pose x0, 30cm std on x,y,z and 0.1 rad on roll,pitch,yaw
|
||||||
|
static auto kPosePrior = noiseModel::Diagonal::Sigmas(
|
||||||
|
(Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished());
|
||||||
|
graph.emplace_shared<PriorFactor<Pose3>>(Symbol('x', 0), poses[0], kPosePrior);
|
||||||
|
|
||||||
|
// Add a prior on landmark l0
|
||||||
|
static auto kPointPrior = noiseModel::Isotropic::Sigma(3, 0.1);
|
||||||
|
graph.emplace_shared<PriorFactor<Point3>>(Symbol('l', 0), points[0], kPointPrior);
|
||||||
|
|
||||||
|
// Add initial guesses to all observed landmarks
|
||||||
|
// Intentionally initialize the variables off from the ground truth
|
||||||
|
static Point3 kDeltaPoint(-0.25, 0.20, 0.15);
|
||||||
|
for (size_t j = 0; j < points.size(); ++j)
|
||||||
|
initialEstimate.insert<Point3>(Symbol('l', j), points[j] + kDeltaPoint);
|
||||||
|
|
||||||
|
// Loop over the poses, adding the observations to the graph
|
||||||
|
for (size_t i = 0; i < poses.size(); ++i)
|
||||||
|
{
|
||||||
|
// Add factors for each landmark observation
|
||||||
|
for (size_t j = 0; j < points.size(); ++j)
|
||||||
|
{
|
||||||
|
PinholeCamera<Cal3Fisheye> camera(poses[i], *K);
|
||||||
|
Point2 measurement = camera.project(points[j]);
|
||||||
|
graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3Fisheye>>(
|
||||||
|
measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Add an initial guess for the current pose
|
||||||
|
// Intentionally initialize the variables off from the ground truth
|
||||||
|
static Pose3 kDeltaPose(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
|
||||||
|
initialEstimate.insert(Symbol('x', i), poses[i] * kDeltaPose);
|
||||||
|
}
|
||||||
|
|
||||||
|
GaussNewtonParams params;
|
||||||
|
params.setVerbosity("TERMINATION");
|
||||||
|
params.maxIterations = 10000;
|
||||||
|
|
||||||
|
std::cout << "Optimizing the factor graph" << std::endl;
|
||||||
|
GaussNewtonOptimizer optimizer(graph, initialEstimate, params);
|
||||||
|
Values result = optimizer.optimize();
|
||||||
|
std::cout << "Optimization complete" << std::endl;
|
||||||
|
|
||||||
|
std::cout << "initial error=" << graph.error(initialEstimate) << std::endl;
|
||||||
|
std::cout << "final error=" << graph.error(result) << std::endl;
|
||||||
|
|
||||||
|
std::ofstream os("examples/vio_batch.dot");
|
||||||
|
graph.saveGraph(os, result);
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
/* ************************************************************************* */
|
||||||
|
|
@ -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 Cal3Fisheye.cpp
|
||||||
|
* @date Apr 8, 2020
|
||||||
|
* @author ghaggin
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <gtsam/base/Matrix.h>
|
||||||
|
#include <gtsam/base/Vector.h>
|
||||||
|
#include <gtsam/geometry/Cal3Fisheye.h>
|
||||||
|
#include <gtsam/geometry/Point2.h>
|
||||||
|
#include <gtsam/geometry/Point3.h>
|
||||||
|
|
||||||
|
namespace gtsam {
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
Cal3Fisheye::Cal3Fisheye(const Vector& v)
|
||||||
|
: fx_(v[0]),
|
||||||
|
fy_(v[1]),
|
||||||
|
s_(v[2]),
|
||||||
|
u0_(v[3]),
|
||||||
|
v0_(v[4]),
|
||||||
|
k1_(v[5]),
|
||||||
|
k2_(v[6]),
|
||||||
|
k3_(v[7]),
|
||||||
|
k4_(v[8]) {}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
Vector9 Cal3Fisheye::vector() const {
|
||||||
|
Vector9 v;
|
||||||
|
v << fx_, fy_, s_, u0_, v0_, k1_, k2_, k3_, k4_;
|
||||||
|
return v;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
Matrix3 Cal3Fisheye::K() const {
|
||||||
|
Matrix3 K;
|
||||||
|
K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0;
|
||||||
|
return K;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
static Matrix29 D2dcalibration(const double xd, const double yd,
|
||||||
|
const double xi, const double yi,
|
||||||
|
const double t3, const double t5,
|
||||||
|
const double t7, const double t9, const double r,
|
||||||
|
Matrix2& DK) {
|
||||||
|
// order: fx, fy, s, u0, v0
|
||||||
|
Matrix25 DR1;
|
||||||
|
DR1 << xd, 0.0, yd, 1.0, 0.0, 0.0, yd, 0.0, 0.0, 1.0;
|
||||||
|
|
||||||
|
// order: k1, k2, k3, k4
|
||||||
|
Matrix24 DR2;
|
||||||
|
DR2 << t3 * xi, t5 * xi, t7 * xi, t9 * xi, t3 * yi, t5 * yi, t7 * yi, t9 * yi;
|
||||||
|
DR2 /= r;
|
||||||
|
Matrix29 D;
|
||||||
|
D << DR1, DK * DR2;
|
||||||
|
return D;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
static Matrix2 D2dintrinsic(const double xi, const double yi, const double r,
|
||||||
|
const double td, const double t, const double tt,
|
||||||
|
const double t4, const double t6, const double t8,
|
||||||
|
const double k1, const double k2, const double k3,
|
||||||
|
const double k4, const Matrix2& DK) {
|
||||||
|
const double dr_dxi = xi / sqrt(xi * xi + yi * yi);
|
||||||
|
const double dr_dyi = yi / sqrt(xi * xi + yi * yi);
|
||||||
|
const double dt_dr = 1 / (1 + r * r);
|
||||||
|
const double dtd_dt =
|
||||||
|
1 + 3 * k1 * tt + 5 * k2 * t4 + 7 * k3 * t6 + 9 * k4 * t8;
|
||||||
|
const double dtd_dxi = dtd_dt * dt_dr * dr_dxi;
|
||||||
|
const double dtd_dyi = dtd_dt * dt_dr * dr_dyi;
|
||||||
|
|
||||||
|
const double rinv = 1 / r;
|
||||||
|
const double rrinv = 1 / (r * r);
|
||||||
|
const double dxd_dxi =
|
||||||
|
dtd_dxi * xi * rinv + td * rinv + td * xi * (-rrinv) * dr_dxi;
|
||||||
|
const double dxd_dyi = dtd_dyi * xi * rinv - td * xi * rrinv * dr_dyi;
|
||||||
|
const double dyd_dxi = dtd_dxi * yi * rinv - td * yi * rrinv * dr_dxi;
|
||||||
|
const double dyd_dyi =
|
||||||
|
dtd_dyi * yi * rinv + td * rinv + td * yi * (-rrinv) * dr_dyi;
|
||||||
|
|
||||||
|
Matrix2 DR;
|
||||||
|
DR << dxd_dxi, dxd_dyi, dyd_dxi, dyd_dyi;
|
||||||
|
|
||||||
|
return DK * DR;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1,
|
||||||
|
OptionalJacobian<2, 2> H2) const {
|
||||||
|
const double xi = p.x(), yi = p.y();
|
||||||
|
const double r = sqrt(xi * xi + yi * yi);
|
||||||
|
const double t = atan(r);
|
||||||
|
const double tt = t * t, t4 = tt * tt, t6 = tt * t4, t8 = t4 * t4;
|
||||||
|
const double td = t * (1 + k1_ * tt + k2_ * t4 + k3_ * t6 + k4_ * t8);
|
||||||
|
const double td_o_r = r > 1e-8 ? td / r : 1;
|
||||||
|
const double xd = td_o_r * xi, yd = td_o_r * yi;
|
||||||
|
Point2 uv(fx_ * xd + s_ * yd + u0_, fy_ * yd + v0_);
|
||||||
|
|
||||||
|
Matrix2 DK;
|
||||||
|
if (H1 || H2) DK << fx_, s_, 0.0, fy_;
|
||||||
|
|
||||||
|
// Derivative for calibration parameters (2 by 9)
|
||||||
|
if (H1)
|
||||||
|
*H1 = D2dcalibration(xd, yd, xi, yi, t * tt, t * t4, t * t6, t * t8, r, DK);
|
||||||
|
|
||||||
|
// Derivative for points in intrinsic coords (2 by 2)
|
||||||
|
if (H2)
|
||||||
|
*H2 =
|
||||||
|
D2dintrinsic(xi, yi, r, td, t, tt, t4, t6, t8, k1_, k2_, k3_, k4_, DK);
|
||||||
|
|
||||||
|
return uv;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
Point2 Cal3Fisheye::calibrate(const Point2& uv, const double tol) 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,
|
||||||
|
// throw exception if max iterations are reached
|
||||||
|
const int maxIterations = 10;
|
||||||
|
int iteration;
|
||||||
|
for (iteration = 0; iteration < maxIterations; ++iteration) {
|
||||||
|
Matrix2 jac;
|
||||||
|
|
||||||
|
// Calculate the current estimate (uv_hat) and the jacobian
|
||||||
|
const Point2 uv_hat = uncalibrate(pi, boost::none, jac);
|
||||||
|
|
||||||
|
// Test convergence
|
||||||
|
if ((uv_hat - uv).norm() < tol) break;
|
||||||
|
|
||||||
|
// Newton's method update step
|
||||||
|
pi = pi - jac.inverse() * (uv_hat - uv);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (iteration >= maxIterations)
|
||||||
|
throw std::runtime_error(
|
||||||
|
"Cal3Fisheye::calibrate fails to converge. need a better "
|
||||||
|
"initialization");
|
||||||
|
|
||||||
|
return pi;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
Matrix2 Cal3Fisheye::D2d_intrinsic(const Point2& p) const {
|
||||||
|
const double xi = p.x(), yi = p.y();
|
||||||
|
const double r = sqrt(xi * xi + yi * yi);
|
||||||
|
const double t = atan(r);
|
||||||
|
const double tt = t * t, t4 = tt * tt, t6 = t4 * tt, t8 = t4 * t4;
|
||||||
|
const double td = t * (1 + k1_ * tt + k2_ * t4 + k3_ * t6 + k4_ * t8);
|
||||||
|
|
||||||
|
Matrix2 DK;
|
||||||
|
DK << fx_, s_, 0.0, fy_;
|
||||||
|
|
||||||
|
return D2dintrinsic(xi, yi, r, td, t, tt, t4, t6, t8, k1_, k2_, k3_, k4_, DK);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
Matrix29 Cal3Fisheye::D2d_calibration(const Point2& p) const {
|
||||||
|
const double xi = p.x(), yi = p.y();
|
||||||
|
const double r = sqrt(xi * xi + yi * yi);
|
||||||
|
const double t = atan(r);
|
||||||
|
const double tt = t * t, t4 = tt * tt, t6 = tt * t4, t8 = t4 * t4;
|
||||||
|
const double td = t * (1 + k1_ * tt + k2_ * t4 + k3_ * t6 + k4_ * t8);
|
||||||
|
const double xd = td / r * xi, yd = td / r * yi;
|
||||||
|
|
||||||
|
Matrix2 DK;
|
||||||
|
DK << fx_, s_, 0.0, fy_;
|
||||||
|
return D2dcalibration(xd, yd, xi, yi, t * tt, t * t4, t * t6, t * t8, r, DK);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
void Cal3Fisheye::print(const std::string& s_) const {
|
||||||
|
gtsam::print((Matrix)K(), s_ + ".K");
|
||||||
|
gtsam::print(Vector(k()), s_ + ".k");
|
||||||
|
;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
bool Cal3Fisheye::equals(const Cal3Fisheye& K, double tol) const {
|
||||||
|
if (std::abs(fx_ - K.fx_) > tol || std::abs(fy_ - K.fy_) > tol ||
|
||||||
|
std::abs(s_ - K.s_) > tol || std::abs(u0_ - K.u0_) > tol ||
|
||||||
|
std::abs(v0_ - K.v0_) > tol || std::abs(k1_ - K.k1_) > tol ||
|
||||||
|
std::abs(k2_ - K.k2_) > tol || std::abs(k3_ - K.k3_) > tol ||
|
||||||
|
std::abs(k4_ - K.k4_) > tol)
|
||||||
|
return false;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
Cal3Fisheye Cal3Fisheye::retract(const Vector& d) const {
|
||||||
|
return Cal3Fisheye(vector() + d);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
Vector Cal3Fisheye::localCoordinates(const Cal3Fisheye& T2) const {
|
||||||
|
return T2.vector() - vector();
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace gtsam
|
||||||
|
/* ************************************************************************* */
|
||||||
|
|
@ -0,0 +1,211 @@
|
||||||
|
/* ----------------------------------------------------------------------------
|
||||||
|
|
||||||
|
* 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 Cal3Fisheye.h
|
||||||
|
* @brief Calibration of a fisheye camera
|
||||||
|
* @date Apr 8, 2020
|
||||||
|
* @author ghaggin
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <gtsam/geometry/Point2.h>
|
||||||
|
|
||||||
|
namespace gtsam {
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief Calibration of a fisheye camera
|
||||||
|
* @addtogroup geometry
|
||||||
|
* \nosubgrouping
|
||||||
|
*
|
||||||
|
* Uses same distortionmodel as OpenCV, with
|
||||||
|
* https://docs.opencv.org/master/db/d58/group__calib3d__fisheye.html
|
||||||
|
* 3D point in camera frame
|
||||||
|
* p = (x, y, z)
|
||||||
|
* Intrinsic coordinates:
|
||||||
|
* [x_i;y_i] = [x/z; y/z]
|
||||||
|
* Distorted coordinates:
|
||||||
|
* r^2 = (x_i)^2 + (y_i)^2
|
||||||
|
* th = atan(r)
|
||||||
|
* th_d = th(1 + k1*th^2 + k2*th^4 + k3*th^6 + k4*th^8)
|
||||||
|
* [x_d; y_d] = (th_d / r)*[x_i; y_i]
|
||||||
|
* Pixel coordinates:
|
||||||
|
* K = [fx s u0; 0 fy v0 ;0 0 1]
|
||||||
|
* [u; v; 1] = K*[x_d; y_d; 1]
|
||||||
|
*/
|
||||||
|
class GTSAM_EXPORT Cal3Fisheye {
|
||||||
|
protected:
|
||||||
|
double fx_, fy_, s_, u0_, v0_; // focal length, skew and principal point
|
||||||
|
double k1_, k2_, k3_, k4_; // fisheye distortion coefficients
|
||||||
|
|
||||||
|
public:
|
||||||
|
enum { dimension = 9 };
|
||||||
|
typedef boost::shared_ptr<Cal3Fisheye>
|
||||||
|
shared_ptr; ///< shared pointer to fisheye calibration object
|
||||||
|
|
||||||
|
/// @name Standard Constructors
|
||||||
|
/// @{
|
||||||
|
|
||||||
|
/// 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) {}
|
||||||
|
|
||||||
|
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)
|
||||||
|
: fx_(fx),
|
||||||
|
fy_(fy),
|
||||||
|
s_(s),
|
||||||
|
u0_(u0),
|
||||||
|
v0_(v0),
|
||||||
|
k1_(k1),
|
||||||
|
k2_(k2),
|
||||||
|
k3_(k3),
|
||||||
|
k4_(k4) {}
|
||||||
|
|
||||||
|
virtual ~Cal3Fisheye() {}
|
||||||
|
|
||||||
|
/// @}
|
||||||
|
/// @name Advanced Constructors
|
||||||
|
/// @{
|
||||||
|
|
||||||
|
Cal3Fisheye(const Vector& v);
|
||||||
|
|
||||||
|
/// @}
|
||||||
|
/// @name Standard Interface
|
||||||
|
/// @{
|
||||||
|
|
||||||
|
/// focal length x
|
||||||
|
inline double fx() const { return fx_; }
|
||||||
|
|
||||||
|
/// focal length x
|
||||||
|
inline double fy() const { return fy_; }
|
||||||
|
|
||||||
|
/// skew
|
||||||
|
inline double skew() const { return s_; }
|
||||||
|
|
||||||
|
/// image center in x
|
||||||
|
inline double px() const { return u0_; }
|
||||||
|
|
||||||
|
/// image center in y
|
||||||
|
inline double py() const { return v0_; }
|
||||||
|
|
||||||
|
/// First distortion coefficient
|
||||||
|
inline double k1() const { return k1_; }
|
||||||
|
|
||||||
|
/// Second distortion coefficient
|
||||||
|
inline double k2() const { return k2_; }
|
||||||
|
|
||||||
|
/// First tangential distortion coefficient
|
||||||
|
inline double k3() const { return k3_; }
|
||||||
|
|
||||||
|
/// Second tangential distortion coefficient
|
||||||
|
inline double k4() const { return k4_; }
|
||||||
|
|
||||||
|
/// return calibration matrix
|
||||||
|
Matrix3 K() const;
|
||||||
|
|
||||||
|
/// return distortion parameter vector
|
||||||
|
Vector4 k() const { return Vector4(k1_, k2_, k3_, k4_); }
|
||||||
|
|
||||||
|
/// Return all parameters as a vector
|
||||||
|
Vector9 vector() const;
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief convert intrinsic coordinates [x_i; y_i] to (distorted) image
|
||||||
|
* coordinates [u; v]
|
||||||
|
* @param p point in intrinsic coordinates
|
||||||
|
* @param Dcal optional 2*9 Jacobian wrpt intrinsic parameters (fx, fy, ...,
|
||||||
|
* k4)
|
||||||
|
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates (xi, yi)
|
||||||
|
* @return point in (distorted) image coordinates
|
||||||
|
*/
|
||||||
|
Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none,
|
||||||
|
OptionalJacobian<2, 2> Dp = boost::none) const;
|
||||||
|
|
||||||
|
/// Convert (distorted) image coordinates [u;v] to intrinsic coordinates [x_i,
|
||||||
|
/// y_i]
|
||||||
|
Point2 calibrate(const Point2& p, const double tol = 1e-5) const;
|
||||||
|
|
||||||
|
/// Derivative of uncalibrate wrpt intrinsic coordinates [x_i; y_i]
|
||||||
|
Matrix2 D2d_intrinsic(const Point2& p) const;
|
||||||
|
|
||||||
|
/// Derivative of uncalibrate wrpt the calibration parameters
|
||||||
|
/// [fx, fy, s, u0, v0, k1, k2, k3, k4]
|
||||||
|
Matrix29 D2d_calibration(const Point2& p) const;
|
||||||
|
|
||||||
|
/// @}
|
||||||
|
/// @name Testable
|
||||||
|
/// @{
|
||||||
|
|
||||||
|
/// print with optional string
|
||||||
|
virtual void print(const std::string& s = "") const;
|
||||||
|
|
||||||
|
/// assert equality up to a tolerance
|
||||||
|
bool equals(const Cal3Fisheye& K, double tol = 10e-9) const;
|
||||||
|
|
||||||
|
/// @}
|
||||||
|
/// @name Manifold
|
||||||
|
/// @{
|
||||||
|
|
||||||
|
/// Given delta vector, update calibration
|
||||||
|
Cal3Fisheye retract(const Vector& d) const;
|
||||||
|
|
||||||
|
/// Given a different calibration, calculate update to obtain it
|
||||||
|
Vector localCoordinates(const Cal3Fisheye& T2) const;
|
||||||
|
|
||||||
|
/// Return dimensions of calibration manifold object
|
||||||
|
virtual size_t dim() const { return 9; }
|
||||||
|
|
||||||
|
/// Return dimensions of calibration manifold object
|
||||||
|
static size_t Dim() { return 9; }
|
||||||
|
|
||||||
|
/// @}
|
||||||
|
/// @name Clone
|
||||||
|
/// @{
|
||||||
|
|
||||||
|
/// @return a deep copy of this object
|
||||||
|
virtual boost::shared_ptr<Cal3Fisheye> clone() const {
|
||||||
|
return boost::shared_ptr<Cal3Fisheye>(new Cal3Fisheye(*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_NVP(fx_);
|
||||||
|
ar& BOOST_SERIALIZATION_NVP(fy_);
|
||||||
|
ar& BOOST_SERIALIZATION_NVP(s_);
|
||||||
|
ar& BOOST_SERIALIZATION_NVP(u0_);
|
||||||
|
ar& BOOST_SERIALIZATION_NVP(v0_);
|
||||||
|
ar& BOOST_SERIALIZATION_NVP(k1_);
|
||||||
|
ar& BOOST_SERIALIZATION_NVP(k2_);
|
||||||
|
ar& BOOST_SERIALIZATION_NVP(k3_);
|
||||||
|
ar& BOOST_SERIALIZATION_NVP(k4_);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// @}
|
||||||
|
};
|
||||||
|
|
||||||
|
template <>
|
||||||
|
struct traits<Cal3Fisheye> : public internal::Manifold<Cal3Fisheye> {};
|
||||||
|
|
||||||
|
template <>
|
||||||
|
struct traits<const Cal3Fisheye> : public internal::Manifold<Cal3Fisheye> {};
|
||||||
|
|
||||||
|
} // namespace gtsam
|
||||||
|
|
@ -0,0 +1,206 @@
|
||||||
|
/* ----------------------------------------------------------------------------
|
||||||
|
|
||||||
|
* 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 testCal3Fisheye.cpp
|
||||||
|
* @brief Unit tests for fisheye calibration class
|
||||||
|
* @author ghaggin
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <CppUnitLite/TestHarness.h>
|
||||||
|
#include <gtsam/base/Testable.h>
|
||||||
|
#include <gtsam/base/numericalDerivative.h>
|
||||||
|
#include <gtsam/geometry/Cal3Fisheye.h>
|
||||||
|
#include <gtsam/geometry/Point3.h>
|
||||||
|
|
||||||
|
using namespace gtsam;
|
||||||
|
|
||||||
|
GTSAM_CONCEPT_TESTABLE_INST(Cal3Fisheye)
|
||||||
|
GTSAM_CONCEPT_MANIFOLD_INST(Cal3Fisheye)
|
||||||
|
|
||||||
|
static const double fx = 250, fy = 260, s = 0.1, u0 = 320, v0 = 240;
|
||||||
|
static Cal3Fisheye K(fx, fy, s, u0, v0, -0.013721808247486035,
|
||||||
|
0.020727425669427896, -0.012786476702685545,
|
||||||
|
0.0025242267320687625);
|
||||||
|
static Point2 p(2, 3);
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST(Cal3Fisheye, uncalibrate1) {
|
||||||
|
// Calculate the solution
|
||||||
|
const double xi = p.x(), yi = p.y();
|
||||||
|
const double r = sqrt(xi * xi + yi * yi);
|
||||||
|
const double t = atan(r);
|
||||||
|
const double tt = t * t, t4 = tt * tt, t6 = tt * t4, t8 = t4 * t4;
|
||||||
|
const double td =
|
||||||
|
t * (1 + K.k1() * tt + K.k2() * t4 + K.k3() * t6 + K.k4() * t8);
|
||||||
|
Vector3 pd(td / r * xi, td / r * yi, 1);
|
||||||
|
Vector3 v = K.K() * pd;
|
||||||
|
|
||||||
|
Point2 uv_sol(v[0] / v[2], v[1] / v[2]);
|
||||||
|
|
||||||
|
Point2 uv = K.uncalibrate(p);
|
||||||
|
CHECK(assert_equal(uv, uv_sol));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
/**
|
||||||
|
* Check that a point at (0,0) projects to the
|
||||||
|
* image center.
|
||||||
|
*/
|
||||||
|
TEST(Cal3Fisheye, uncalibrate2) {
|
||||||
|
Point2 pz(0, 0);
|
||||||
|
auto uv = K.uncalibrate(pz);
|
||||||
|
CHECK(assert_equal(uv, Point2(u0, v0)));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
/**
|
||||||
|
* This test uses cv2::fisheye::projectPoints to test that uncalibrate
|
||||||
|
* properly projects a point into the image plane. One notable difference
|
||||||
|
* between opencv and the Cal3Fisheye::uncalibrate function is the skew
|
||||||
|
* parameter. The equivalence is alpha = s/fx.
|
||||||
|
*
|
||||||
|
*
|
||||||
|
* Python script to project points with fisheye model in OpenCv
|
||||||
|
* (script run with OpenCv version 4.2.0 and Numpy version 1.18.2)
|
||||||
|
*/
|
||||||
|
// clang-format off
|
||||||
|
/*
|
||||||
|
===========================================================
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
import cv2
|
||||||
|
|
||||||
|
objpts = np.float64([[23,27,31]]).reshape(1,-1,3)
|
||||||
|
|
||||||
|
cameraMatrix = np.float64([
|
||||||
|
[250, 0, 320],
|
||||||
|
[0, 260, 240],
|
||||||
|
[0,0,1]
|
||||||
|
])
|
||||||
|
alpha = 0.1/250
|
||||||
|
distCoeffs = np.float64([-0.013721808247486035, 0.020727425669427896,-0.012786476702685545, 0.0025242267320687625])
|
||||||
|
|
||||||
|
rvec = np.float64([[0.,0.,0.]])
|
||||||
|
tvec = np.float64([[0.,0.,0.]]);
|
||||||
|
imagePoints, jacobian = cv2.fisheye.projectPoints(objpts, rvec, tvec, cameraMatrix, distCoeffs, alpha=alpha)
|
||||||
|
np.set_printoptions(precision=14)
|
||||||
|
print(imagePoints)
|
||||||
|
===========================================================
|
||||||
|
* Script output: [[[457.82638130304935 408.18905848512986]]]
|
||||||
|
*/
|
||||||
|
// clang-format on
|
||||||
|
TEST(Cal3Fisheye, uncalibrate3) {
|
||||||
|
Point3 p3(23, 27, 31);
|
||||||
|
Point2 xi(p3.x() / p3.z(), p3.y() / p3.z());
|
||||||
|
auto uv = K.uncalibrate(xi);
|
||||||
|
CHECK(assert_equal(uv, Point2(457.82638130304935, 408.18905848512986)));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST(Cal3Fisheye, calibrate1) {
|
||||||
|
Point2 pi;
|
||||||
|
Point2 uv;
|
||||||
|
Point2 pi_hat;
|
||||||
|
|
||||||
|
pi = Point2(0.5, 0.5); // point in intrinsic coordinates
|
||||||
|
uv = K.uncalibrate(pi); // map intrinsic coord to image plane (pi)
|
||||||
|
pi_hat = K.calibrate(uv); // map image coords (pi) back to intrinsic coords
|
||||||
|
CHECK(traits<Point2>::Equals(pi, pi_hat,
|
||||||
|
1e-5)); // check that the inv mapping works
|
||||||
|
|
||||||
|
pi = Point2(-0.7, -1.2);
|
||||||
|
uv = K.uncalibrate(pi);
|
||||||
|
pi_hat = K.calibrate(uv);
|
||||||
|
CHECK(traits<Point2>::Equals(pi, pi_hat, 1e-5));
|
||||||
|
|
||||||
|
pi = Point2(-3, 5);
|
||||||
|
uv = K.uncalibrate(pi);
|
||||||
|
pi_hat = K.calibrate(uv);
|
||||||
|
CHECK(traits<Point2>::Equals(pi, pi_hat, 1e-5));
|
||||||
|
|
||||||
|
pi = Point2(7, -12);
|
||||||
|
uv = K.uncalibrate(pi);
|
||||||
|
pi_hat = K.calibrate(uv);
|
||||||
|
CHECK(traits<Point2>::Equals(pi, pi_hat, 1e-5));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
/**
|
||||||
|
* Check that calibrate returns (0,0) for the image center
|
||||||
|
*/
|
||||||
|
TEST(Cal3Fisheye, calibrate2) {
|
||||||
|
Point2 uv(u0, v0);
|
||||||
|
auto xi_hat = K.calibrate(uv);
|
||||||
|
CHECK(assert_equal(xi_hat, Point2(0, 0)))
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Run calibrate on OpenCv test from uncalibrate3
|
||||||
|
* (script shown above)
|
||||||
|
* 3d point: (23, 27, 31)
|
||||||
|
* 2d point in image plane: (457.82638130304935, 408.18905848512986)
|
||||||
|
*/
|
||||||
|
TEST(Cal3Fisheye, calibrate3) {
|
||||||
|
Point3 p3(23, 27, 31);
|
||||||
|
Point2 xi(p3.x() / p3.z(), p3.y() / p3.z());
|
||||||
|
Point2 uv(457.82638130304935, 408.18905848512986);
|
||||||
|
auto xi_hat = K.calibrate(uv);
|
||||||
|
CHECK(assert_equal(xi_hat, xi));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
// For numerical derivatives
|
||||||
|
Point2 uncalibrate_(const Cal3Fisheye& k, const Point2& pt) {
|
||||||
|
return k.uncalibrate(pt);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST(Cal3Fisheye, Duncalibrate1) {
|
||||||
|
Matrix computed;
|
||||||
|
K.uncalibrate(p, computed, boost::none);
|
||||||
|
Matrix numerical = numericalDerivative21(uncalibrate_, K, p, 1e-7);
|
||||||
|
CHECK(assert_equal(numerical, computed, 1e-5));
|
||||||
|
Matrix separate = K.D2d_calibration(p);
|
||||||
|
CHECK(assert_equal(numerical, separate, 1e-5));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST(Cal3Fisheye, Duncalibrate2) {
|
||||||
|
Matrix computed;
|
||||||
|
K.uncalibrate(p, boost::none, computed);
|
||||||
|
Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7);
|
||||||
|
CHECK(assert_equal(numerical, computed, 1e-5));
|
||||||
|
Matrix separate = K.D2d_intrinsic(p);
|
||||||
|
CHECK(assert_equal(numerical, separate, 1e-5));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST(Cal3Fisheye, assert_equal) { CHECK(assert_equal(K, K, 1e-5)); }
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST(Cal3Fisheye, retract) {
|
||||||
|
Cal3Fisheye expected(K.fx() + 1, K.fy() + 2, K.skew() + 3, K.px() + 4,
|
||||||
|
K.py() + 5, K.k1() + 6, K.k2() + 7, K.k3() + 8,
|
||||||
|
K.k4() + 9);
|
||||||
|
Vector d(9);
|
||||||
|
d << 1, 2, 3, 4, 5, 6, 7, 8, 9;
|
||||||
|
Cal3Fisheye actual = K.retract(d);
|
||||||
|
CHECK(assert_equal(expected, actual, 1e-7));
|
||||||
|
CHECK(assert_equal(d, K.localCoordinates(actual), 1e-7));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
int main() {
|
||||||
|
TestResult tr;
|
||||||
|
return TestRegistry::runAllTests(tr);
|
||||||
|
}
|
||||||
|
/* ************************************************************************* */
|
||||||
Loading…
Reference in New Issue