Skip to content

Commit

Permalink
Merge pull request #811 from roderick-koehle/python-fisheye-interface
Browse files Browse the repository at this point in the history 
Python fisheye interface
  • Loading branch information
@ProfFan
ProfFan authored Jul 14, 2021
2 parents fa42d96 + a115788 commit 740c9c6
Show file tree
Hide file tree
Showing 8 changed files with 356 additions and 12 deletions.
14 changes: 12 additions & 2 deletions gtsam/geometry/Cal3Fisheye.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -106,11 +106,21 @@ Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1,
/* ************************************************************************* */
Point2 Cal3Fisheye::calibrate(const Point2& uv, OptionalJacobian<2, 9> Dcal,
OptionalJacobian<2, 2> Dp) const {
// initial gues just inverts the pinhole model
// Apply inverse camera matrix to map the pixel coordinate (u, v)
// of the equidistant fisheye image to angular coordinate space (xd, yd)
// with radius theta given in radians.
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);
const double theta = sqrt(xd * xd + yd * yd);

// Provide initial guess for the Gauss-Newton search.
// The angular coordinates given by (xd, yd) are mapped back to
// the focal plane of the perspective undistorted projection pi.
// See Cal3Unified.calibrate() using the same pattern for the
// undistortion of omnidirectional fisheye projection.
const double scale = (theta > 0) ? tan(theta) / theta : 1.0;
Point2 pi(scale * xd, scale * yd);

// Perform newtons method, break when solution converges past tol_,
// throw exception if max iterations are reached
Expand Down
2 changes: 2 additions & 0 deletions gtsam/geometry/SimpleCamera.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,7 @@
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/Cal3DS2.h>
#include <gtsam/geometry/Cal3Unified.h>
#include <gtsam/geometry/Cal3Fisheye.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/PinholeCamera.h>

Expand All @@ -33,6 +34,7 @@ namespace gtsam {
using PinholeCameraCal3Bundler = gtsam::PinholeCamera<gtsam::Cal3Bundler>;
using PinholeCameraCal3DS2 = gtsam::PinholeCamera<gtsam::Cal3DS2>;
using PinholeCameraCal3Unified = gtsam::PinholeCamera<gtsam::Cal3Unified>;
using PinholeCameraCal3Fisheye = gtsam::PinholeCamera<gtsam::Cal3Fisheye>;

#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
/**
Expand Down
4 changes: 4 additions & 0 deletions gtsam/geometry/triangulation.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,8 @@

#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/Cal3Fisheye.h>
#include <gtsam/geometry/Cal3Unified.h>
#include <gtsam/geometry/CameraSet.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Pose2.h>
Expand Down Expand Up @@ -499,6 +501,8 @@ TriangulationResult triangulateSafe(const CameraSet<CAMERA>& cameras,
// Vector of Cameras - used by the Python/MATLAB wrapper
using CameraSetCal3Bundler = CameraSet<PinholeCamera<Cal3Bundler>>;
using CameraSetCal3_S2 = CameraSet<PinholeCamera<Cal3_S2>>;
using CameraSetCal3Fisheye = CameraSet<PinholeCamera<Cal3Fisheye>>;
using CameraSetCal3Unified = CameraSet<PinholeCamera<Cal3Unified>>;

} // \namespace gtsam

104 changes: 94 additions & 10 deletions gtsam/gtsam.i
View file
Original file line number Diff line number Diff line change
Expand Up @@ -231,7 +231,7 @@ virtual class Value {
};

#include <gtsam/base/GenericValue.h>
template<T = {Vector, Matrix, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::StereoPoint2, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::CalibratedCamera, gtsam::imuBias::ConstantBias}>
template<T = {Vector, Matrix, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::StereoPoint2, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::Cal3Fisheye, gtsam::Cal3Unified, gtsam::EssentialMatrix, gtsam::CalibratedCamera, gtsam::imuBias::ConstantBias}>
virtual class GenericValue : gtsam::Value {
void serializable() const;
};
Expand Down Expand Up @@ -911,6 +911,12 @@ virtual class Cal3Unified : gtsam::Cal3DS2_Base {
gtsam::Cal3Unified retract(Vector v) const;
Vector localCoordinates(const gtsam::Cal3Unified& c) const;

// Action on Point2
// Note: the signature of this functions differ from the functions
// with equal name in the base class.
gtsam::Point2 calibrate(const gtsam::Point2& p) const;
gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;

// enabling serialization functionality
void serialize() const;

Expand Down Expand Up @@ -978,6 +984,52 @@ class Cal3Bundler {
void pickle() const;
};

#include <gtsam/geometry/Cal3Fisheye.h>
class Cal3Fisheye {
// Standard Constructors
Cal3Fisheye();
Cal3Fisheye(double fx, double fy, double s, double u0, double v0, double k1, double k2, double k3, double k4);
Cal3Fisheye(double fx, double fy, double s, double u0, double v0, double k1, double k2, double k3, double k4, double tol);
Cal3Fisheye(Vector v);

// Testable
void print(string s = "Cal3Fisheye") const;
bool equals(const gtsam::Cal3Fisheye& rhs, double tol) const;

// Manifold
static size_t Dim();
size_t dim() const;
gtsam::Cal3Fisheye retract(Vector v) const;
Vector localCoordinates(const gtsam::Cal3Fisheye& c) const;

// Action on Point2
gtsam::Point2 calibrate(const gtsam::Point2& p) const;
gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;

// Standard Interface
double fx() const;
double fy() const;
double skew() const;
double k1() const;
double k2() const;
double k3() const;
double k4() const;
double px() const;
double py() const;
gtsam::Point2 principalPoint() const;
Vector vector() const;
Vector k() const;
Matrix K() const;
Matrix inverse() const;

// enabling serialization functionality
void serialize() const;

// enable pickling in python
void pickle() const;
};


#include <gtsam/geometry/CalibratedCamera.h>
class CalibratedCamera {
// Standard Constructors and Named Constructors
Expand Down Expand Up @@ -1086,6 +1138,7 @@ typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2;
typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
typedef gtsam::PinholeCamera<gtsam::Cal3Fisheye> PinholeCameraCal3Fisheye;

template<T>
class CameraSet {
Expand Down Expand Up @@ -1146,7 +1199,13 @@ gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3_S2& cameras,
gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Bundler& cameras,
const gtsam::Point2Vector& measurements, double rank_tol,
bool optimize);

gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Fisheye& cameras,
const gtsam::Point2Vector& measurements, double rank_tol,
bool optimize);
gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Unified& cameras,
const gtsam::Point2Vector& measurements, double rank_tol,
bool optimize);

//*************************************************************************
// Symbolic
//*************************************************************************
Expand Down Expand Up @@ -2119,8 +2178,11 @@ class NonlinearFactorGraph {
template <T = {double, Vector, gtsam::Point2, gtsam::StereoPoint2,
gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4,
gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,
gtsam::Cal3Fisheye, gtsam::Cal3Unified,
gtsam::CalibratedCamera, gtsam::PinholeCameraCal3_S2,
gtsam::PinholeCamera<gtsam::Cal3Bundler>,
gtsam::PinholeCameraCal3Bundler,
gtsam::PinholeCameraCal3Fisheye,
gtsam::PinholeCameraCal3Unified,
gtsam::imuBias::ConstantBias}>
void addPrior(size_t key, const T& prior,
const gtsam::noiseModel::Base* noiseModel);
Expand Down Expand Up @@ -2253,9 +2315,13 @@ class Values {
void insert(size_t j, const gtsam::Cal3_S2& cal3_s2);
void insert(size_t j, const gtsam::Cal3DS2& cal3ds2);
void insert(size_t j, const gtsam::Cal3Bundler& cal3bundler);
void insert(size_t j, const gtsam::Cal3Fisheye& cal3fisheye);
void insert(size_t j, const gtsam::Cal3Unified& cal3unified);
void insert(size_t j, const gtsam::EssentialMatrix& essential_matrix);
void insert(size_t j, const gtsam::PinholeCameraCal3_S2& simple_camera);
void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
void insert(size_t j, const gtsam::PinholeCameraCal3Bundler& camera);
void insert(size_t j, const gtsam::PinholeCameraCal3Fisheye& camera);
void insert(size_t j, const gtsam::PinholeCameraCal3Unified& camera);
void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
void insert(size_t j, const gtsam::NavState& nav_state);
void insert(size_t j, double c);
Expand All @@ -2273,9 +2339,13 @@ class Values {
void update(size_t j, const gtsam::Cal3_S2& cal3_s2);
void update(size_t j, const gtsam::Cal3DS2& cal3ds2);
void update(size_t j, const gtsam::Cal3Bundler& cal3bundler);
void update(size_t j, const gtsam::Cal3Fisheye& cal3fisheye);
void update(size_t j, const gtsam::Cal3Unified& cal3unified);
void update(size_t j, const gtsam::EssentialMatrix& essential_matrix);
void update(size_t j, const gtsam::PinholeCameraCal3_S2& simple_camera);
void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
void update(size_t j, const gtsam::PinholeCameraCal3Bundler& camera);
void update(size_t j, const gtsam::PinholeCameraCal3Fisheye& camera);
void update(size_t j, const gtsam::PinholeCameraCal3Unified& camera);
void update(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
void update(size_t j, const gtsam::NavState& nav_state);
void update(size_t j, Vector vector);
Expand All @@ -2295,9 +2365,13 @@ class Values {
gtsam::Cal3_S2,
gtsam::Cal3DS2,
gtsam::Cal3Bundler,
gtsam::Cal3Fisheye,
gtsam::Cal3Unified,
gtsam::EssentialMatrix,
gtsam::PinholeCameraCal3_S2,
gtsam::PinholeCamera<gtsam::Cal3Bundler>,
gtsam::PinholeCameraCal3Bundler,
gtsam::PinholeCameraCal3Fisheye,
gtsam::PinholeCameraCal3Unified,
gtsam::imuBias::ConstantBias,
gtsam::NavState,
Vector,
Expand Down Expand Up @@ -2604,7 +2678,9 @@ class ISAM2 {
template <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
gtsam::Cal3Bundler, gtsam::EssentialMatrix,
gtsam::PinholeCameraCal3_S2, gtsam::PinholeCamera<gtsam::Cal3Bundler>,
gtsam::PinholeCameraCal3_S2, gtsam::PinholeCameraCal3Bundler,
gtsam::Cal3Fisheye, gtsam::Cal3Unified,
gtsam::PinholeCameraCal3Fisheye, gtsam::PinholeCameraCal3Unified,
Vector, Matrix}>
VALUE calculateEstimate(size_t key) const;
gtsam::Values calculateBestEstimate() const;
Expand Down Expand Up @@ -2656,10 +2732,14 @@ template <T = {double,
gtsam::Cal3_S2,
gtsam::Cal3DS2,
gtsam::Cal3Bundler,
gtsam::Cal3Fisheye,
gtsam::Cal3Unified,
gtsam::CalibratedCamera,
gtsam::PinholeCameraCal3_S2,
gtsam::imuBias::ConstantBias,
gtsam::PinholeCamera<gtsam::Cal3Bundler>}>
gtsam::PinholeCameraCal3Bundler,
gtsam::PinholeCameraCal3Fisheye,
gtsam::PinholeCameraCal3Unified}>
virtual class PriorFactor : gtsam::NoiseModelFactor {
PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
T prior() const;
Expand Down Expand Up @@ -2801,6 +2881,8 @@ virtual class GenericProjectionFactor : gtsam::NoiseModelFactor {
};
typedef gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3_S2> GenericProjectionFactorCal3_S2;
typedef gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3DS2> GenericProjectionFactorCal3DS2;
typedef gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3Fisheye> GenericProjectionFactorCal3Fisheye;
typedef gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3Unified> GenericProjectionFactorCal3Unified;


#include <gtsam/slam/GeneralSFMFactor.h>
Expand All @@ -2811,9 +2893,11 @@ virtual class GeneralSFMFactor : gtsam::NoiseModelFactor {
};
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3_S2, gtsam::Point3> GeneralSFMFactorCal3_S2;
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCamera<gtsam::Cal3Bundler>, gtsam::Point3> GeneralSFMFactorCal3Bundler;
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3Bundler, gtsam::Point3> GeneralSFMFactorCal3Bundler;
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3Fisheye, gtsam::Point3> GeneralSFMFactorCal3Fisheye;
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3Unified, gtsam::Point3> GeneralSFMFactorCal3Unified;

template<CALIBRATION = {gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler}>
template<CALIBRATION = {gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::Cal3Fisheye, gtsam::Cal3Unified}>
virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor {
GeneralSFMFactor2(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t poseKey, size_t landmarkKey, size_t calibKey);
gtsam::Point2 measured() const;
Expand Down
2 changes: 2 additions & 0 deletions python/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -102,6 +102,8 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON)
gtsam::BinaryMeasurementsUnit3
gtsam::CameraSetCal3_S2
gtsam::CameraSetCal3Bundler
gtsam::CameraSetCal3Unified
gtsam::CameraSetCal3Fisheye
gtsam::KeyPairDoubleMap)

pybind_wrap(gtsam_unstable_py # target
Expand Down
2 changes: 2 additions & 0 deletions python/gtsam/specializations.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,4 +32,6 @@ py::bind_vector<gtsam::IndexPairVector>(m_, "IndexPairVector");
py::bind_map<gtsam::KeyPairDoubleMap>(m_, "KeyPairDoubleMap");
py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3_S2> > >(m_, "CameraSetCal3_S2");
py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Bundler> > >(m_, "CameraSetCal3Bundler");
py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Unified> > >(m_, "CameraSetCal3Unified");
py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Fisheye> > >(m_, "CameraSetCal3Fisheye");
py::bind_vector<std::vector<gtsam::Matrix> >(m_, "JacobianVector");
Loading

0 comments on commit 740c9c6

Please sign in to comment.