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