Skip to content

Commit 740c9c6

Browse files
ProfFanProfFan
authored
Merge pull request #811 from roderick-koehle/python-fisheye-interface
Python fisheye interface
2 parents fa42d96 + a115788 commit 740c9c6

File tree

8 files changed

+356
-12
lines changed

8 files changed

+356
-12
lines changed

‎gtsam/geometry/Cal3Fisheye.cpp‎

Lines changed: 12 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -106,11 +106,21 @@ Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1,
106106
/* ************************************************************************* */
107107
Point2 Cal3Fisheye::calibrate(const Point2& uv, OptionalJacobian<2, 9> Dcal,
108108
OptionalJacobian<2, 2> Dp) const {
109-
// initial gues just inverts the pinhole model
109+
// Apply inverse camera matrix to map the pixel coordinate (u, v)
110+
// of the equidistant fisheye image to angular coordinate space (xd, yd)
111+
// with radius theta given in radians.
110112
const double u = uv.x(), v = uv.y();
111113
const double yd = (v - v0_) / fy_;
112114
const double xd = (u - s_ * yd - u0_) / fx_;
113-
Point2 pi(xd, yd);
115+
const double theta = sqrt(xd * xd + yd * yd);
116+
117+
// Provide initial guess for the Gauss-Newton search.
118+
// The angular coordinates given by (xd, yd) are mapped back to
119+
// the focal plane of the perspective undistorted projection pi.
120+
// See Cal3Unified.calibrate() using the same pattern for the
121+
// undistortion of omnidirectional fisheye projection.
122+
const double scale = (theta > 0) ? tan(theta) / theta : 1.0;
123+
Point2 pi(scale * xd, scale * yd);
114124

115125
// Perform newtons method, break when solution converges past tol_,
116126
// throw exception if max iterations are reached

‎gtsam/geometry/SimpleCamera.h‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -22,6 +22,7 @@
2222
#include <gtsam/geometry/Cal3Bundler.h>
2323
#include <gtsam/geometry/Cal3DS2.h>
2424
#include <gtsam/geometry/Cal3Unified.h>
25+
#include <gtsam/geometry/Cal3Fisheye.h>
2526
#include <gtsam/geometry/Cal3_S2.h>
2627
#include <gtsam/geometry/PinholeCamera.h>
2728

@@ -33,6 +34,7 @@ namespace gtsam {
3334
using PinholeCameraCal3Bundler = gtsam::PinholeCamera<gtsam::Cal3Bundler>;
3435
using PinholeCameraCal3DS2 = gtsam::PinholeCamera<gtsam::Cal3DS2>;
3536
using PinholeCameraCal3Unified = gtsam::PinholeCamera<gtsam::Cal3Unified>;
37+
using PinholeCameraCal3Fisheye = gtsam::PinholeCamera<gtsam::Cal3Fisheye>;
3638

3739
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
3840
/**

‎gtsam/geometry/triangulation.h‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -20,6 +20,8 @@
2020

2121
#include <gtsam/geometry/Cal3_S2.h>
2222
#include <gtsam/geometry/Cal3Bundler.h>
23+
#include <gtsam/geometry/Cal3Fisheye.h>
24+
#include <gtsam/geometry/Cal3Unified.h>
2325
#include <gtsam/geometry/CameraSet.h>
2426
#include <gtsam/geometry/PinholeCamera.h>
2527
#include <gtsam/geometry/Pose2.h>
@@ -499,6 +501,8 @@ TriangulationResult triangulateSafe(const CameraSet<CAMERA>& cameras,
499501
// Vector of Cameras - used by the Python/MATLAB wrapper
500502
using CameraSetCal3Bundler = CameraSet<PinholeCamera<Cal3Bundler>>;
501503
using CameraSetCal3_S2 = CameraSet<PinholeCamera<Cal3_S2>>;
504+
using CameraSetCal3Fisheye = CameraSet<PinholeCamera<Cal3Fisheye>>;
505+
using CameraSetCal3Unified = CameraSet<PinholeCamera<Cal3Unified>>;
502506

503507
} // \namespace gtsam
504508

‎gtsam/gtsam.i‎

Lines changed: 94 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -231,7 +231,7 @@ virtual class Value {
231231
};
232232

233233
#include <gtsam/base/GenericValue.h>
234-
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}>
234+
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}>
235235
virtual class GenericValue : gtsam::Value {
236236
void serializable() const;
237237
};
@@ -911,6 +911,12 @@ virtual class Cal3Unified : gtsam::Cal3DS2_Base {
911911
gtsam::Cal3Unified retract(Vector v) const;
912912
Vector localCoordinates(const gtsam::Cal3Unified& c) const;
913913

914+
// Action on Point2
915+
// Note: the signature of this functions differ from the functions
916+
// with equal name in the base class.
917+
gtsam::Point2 calibrate(const gtsam::Point2& p) const;
918+
gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
919+
914920
// enabling serialization functionality
915921
void serialize() const;
916922

@@ -978,6 +984,52 @@ class Cal3Bundler {
978984
void pickle() const;
979985
};
980986

987+
#include <gtsam/geometry/Cal3Fisheye.h>
988+
class Cal3Fisheye {
989+
// Standard Constructors
990+
Cal3Fisheye();
991+
Cal3Fisheye(double fx, double fy, double s, double u0, double v0, double k1, double k2, double k3, double k4);
992+
Cal3Fisheye(double fx, double fy, double s, double u0, double v0, double k1, double k2, double k3, double k4, double tol);
993+
Cal3Fisheye(Vector v);
994+
995+
// Testable
996+
void print(string s = "Cal3Fisheye") const;
997+
bool equals(const gtsam::Cal3Fisheye& rhs, double tol) const;
998+
999+
// Manifold
1000+
static size_t Dim();
1001+
size_t dim() const;
1002+
gtsam::Cal3Fisheye retract(Vector v) const;
1003+
Vector localCoordinates(const gtsam::Cal3Fisheye& c) const;
1004+
1005+
// Action on Point2
1006+
gtsam::Point2 calibrate(const gtsam::Point2& p) const;
1007+
gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
1008+
1009+
// Standard Interface
1010+
double fx() const;
1011+
double fy() const;
1012+
double skew() const;
1013+
double k1() const;
1014+
double k2() const;
1015+
double k3() const;
1016+
double k4() const;
1017+
double px() const;
1018+
double py() const;
1019+
gtsam::Point2 principalPoint() const;
1020+
Vector vector() const;
1021+
Vector k() const;
1022+
Matrix K() const;
1023+
Matrix inverse() const;
1024+
1025+
// enabling serialization functionality
1026+
void serialize() const;
1027+
1028+
// enable pickling in python
1029+
void pickle() const;
1030+
};
1031+
1032+
9811033
#include <gtsam/geometry/CalibratedCamera.h>
9821034
class CalibratedCamera {
9831035
// Standard Constructors and Named Constructors
@@ -1086,6 +1138,7 @@ typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
10861138
typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2;
10871139
typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
10881140
typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
1141+
typedef gtsam::PinholeCamera<gtsam::Cal3Fisheye> PinholeCameraCal3Fisheye;
10891142

10901143
template<T>
10911144
class CameraSet {
@@ -1146,7 +1199,13 @@ gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3_S2& cameras,
11461199
gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Bundler& cameras,
11471200
const gtsam::Point2Vector& measurements, double rank_tol,
11481201
bool optimize);
1149-
1202+
gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Fisheye& cameras,
1203+
const gtsam::Point2Vector& measurements, double rank_tol,
1204+
bool optimize);
1205+
gtsam::Point3 triangulatePoint3(const gtsam::CameraSetCal3Unified& cameras,
1206+
const gtsam::Point2Vector& measurements, double rank_tol,
1207+
bool optimize);
1208+
11501209
//*************************************************************************
11511210
// Symbolic
11521211
//*************************************************************************
@@ -2119,8 +2178,11 @@ class NonlinearFactorGraph {
21192178
template <T = {double, Vector, gtsam::Point2, gtsam::StereoPoint2,
21202179
gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4,
21212180
gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,
2181+
gtsam::Cal3Fisheye, gtsam::Cal3Unified,
21222182
gtsam::CalibratedCamera, gtsam::PinholeCameraCal3_S2,
2123-
gtsam::PinholeCamera<gtsam::Cal3Bundler>,
2183+
gtsam::PinholeCameraCal3Bundler,
2184+
gtsam::PinholeCameraCal3Fisheye,
2185+
gtsam::PinholeCameraCal3Unified,
21242186
gtsam::imuBias::ConstantBias}>
21252187
void addPrior(size_t key, const T& prior,
21262188
const gtsam::noiseModel::Base* noiseModel);
@@ -2253,9 +2315,13 @@ class Values {
22532315
void insert(size_t j, const gtsam::Cal3_S2& cal3_s2);
22542316
void insert(size_t j, const gtsam::Cal3DS2& cal3ds2);
22552317
void insert(size_t j, const gtsam::Cal3Bundler& cal3bundler);
2318+
void insert(size_t j, const gtsam::Cal3Fisheye& cal3fisheye);
2319+
void insert(size_t j, const gtsam::Cal3Unified& cal3unified);
22562320
void insert(size_t j, const gtsam::EssentialMatrix& essential_matrix);
22572321
void insert(size_t j, const gtsam::PinholeCameraCal3_S2& simple_camera);
2258-
void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
2322+
void insert(size_t j, const gtsam::PinholeCameraCal3Bundler& camera);
2323+
void insert(size_t j, const gtsam::PinholeCameraCal3Fisheye& camera);
2324+
void insert(size_t j, const gtsam::PinholeCameraCal3Unified& camera);
22592325
void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
22602326
void insert(size_t j, const gtsam::NavState& nav_state);
22612327
void insert(size_t j, double c);
@@ -2273,9 +2339,13 @@ class Values {
22732339
void update(size_t j, const gtsam::Cal3_S2& cal3_s2);
22742340
void update(size_t j, const gtsam::Cal3DS2& cal3ds2);
22752341
void update(size_t j, const gtsam::Cal3Bundler& cal3bundler);
2342+
void update(size_t j, const gtsam::Cal3Fisheye& cal3fisheye);
2343+
void update(size_t j, const gtsam::Cal3Unified& cal3unified);
22762344
void update(size_t j, const gtsam::EssentialMatrix& essential_matrix);
22772345
void update(size_t j, const gtsam::PinholeCameraCal3_S2& simple_camera);
2278-
void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
2346+
void update(size_t j, const gtsam::PinholeCameraCal3Bundler& camera);
2347+
void update(size_t j, const gtsam::PinholeCameraCal3Fisheye& camera);
2348+
void update(size_t j, const gtsam::PinholeCameraCal3Unified& camera);
22792349
void update(size_t j, const gtsam::imuBias::ConstantBias& constant_bias);
22802350
void update(size_t j, const gtsam::NavState& nav_state);
22812351
void update(size_t j, Vector vector);
@@ -2295,9 +2365,13 @@ class Values {
22952365
gtsam::Cal3_S2,
22962366
gtsam::Cal3DS2,
22972367
gtsam::Cal3Bundler,
2368+
gtsam::Cal3Fisheye,
2369+
gtsam::Cal3Unified,
22982370
gtsam::EssentialMatrix,
22992371
gtsam::PinholeCameraCal3_S2,
2300-
gtsam::PinholeCamera<gtsam::Cal3Bundler>,
2372+
gtsam::PinholeCameraCal3Bundler,
2373+
gtsam::PinholeCameraCal3Fisheye,
2374+
gtsam::PinholeCameraCal3Unified,
23012375
gtsam::imuBias::ConstantBias,
23022376
gtsam::NavState,
23032377
Vector,
@@ -2604,7 +2678,9 @@ class ISAM2 {
26042678
template <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
26052679
gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
26062680
gtsam::Cal3Bundler, gtsam::EssentialMatrix,
2607-
gtsam::PinholeCameraCal3_S2, gtsam::PinholeCamera<gtsam::Cal3Bundler>,
2681+
gtsam::PinholeCameraCal3_S2, gtsam::PinholeCameraCal3Bundler,
2682+
gtsam::Cal3Fisheye, gtsam::Cal3Unified,
2683+
gtsam::PinholeCameraCal3Fisheye, gtsam::PinholeCameraCal3Unified,
26082684
Vector, Matrix}>
26092685
VALUE calculateEstimate(size_t key) const;
26102686
gtsam::Values calculateBestEstimate() const;
@@ -2656,10 +2732,14 @@ template <T = {double,
26562732
gtsam::Cal3_S2,
26572733
gtsam::Cal3DS2,
26582734
gtsam::Cal3Bundler,
2735+
gtsam::Cal3Fisheye,
2736+
gtsam::Cal3Unified,
26592737
gtsam::CalibratedCamera,
26602738
gtsam::PinholeCameraCal3_S2,
26612739
gtsam::imuBias::ConstantBias,
2662-
gtsam::PinholeCamera<gtsam::Cal3Bundler>}>
2740+
gtsam::PinholeCameraCal3Bundler,
2741+
gtsam::PinholeCameraCal3Fisheye,
2742+
gtsam::PinholeCameraCal3Unified}>
26632743
virtual class PriorFactor : gtsam::NoiseModelFactor {
26642744
PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
26652745
T prior() const;
@@ -2801,6 +2881,8 @@ virtual class GenericProjectionFactor : gtsam::NoiseModelFactor {
28012881
};
28022882
typedef gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3_S2> GenericProjectionFactorCal3_S2;
28032883
typedef gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3DS2> GenericProjectionFactorCal3DS2;
2884+
typedef gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3Fisheye> GenericProjectionFactorCal3Fisheye;
2885+
typedef gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3Unified> GenericProjectionFactorCal3Unified;
28042886

28052887

28062888
#include <gtsam/slam/GeneralSFMFactor.h>
@@ -2811,9 +2893,11 @@ virtual class GeneralSFMFactor : gtsam::NoiseModelFactor {
28112893
};
28122894
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3_S2, gtsam::Point3> GeneralSFMFactorCal3_S2;
28132895
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
2814-
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCamera<gtsam::Cal3Bundler>, gtsam::Point3> GeneralSFMFactorCal3Bundler;
2896+
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3Bundler, gtsam::Point3> GeneralSFMFactorCal3Bundler;
2897+
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3Fisheye, gtsam::Point3> GeneralSFMFactorCal3Fisheye;
2898+
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3Unified, gtsam::Point3> GeneralSFMFactorCal3Unified;
28152899

2816-
template<CALIBRATION = {gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler}>
2900+
template<CALIBRATION = {gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::Cal3Fisheye, gtsam::Cal3Unified}>
28172901
virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor {
28182902
GeneralSFMFactor2(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t poseKey, size_t landmarkKey, size_t calibKey);
28192903
gtsam::Point2 measured() const;

‎python/CMakeLists.txt‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -102,6 +102,8 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON)
102102
gtsam::BinaryMeasurementsUnit3
103103
gtsam::CameraSetCal3_S2
104104
gtsam::CameraSetCal3Bundler
105+
gtsam::CameraSetCal3Unified
106+
gtsam::CameraSetCal3Fisheye
105107
gtsam::KeyPairDoubleMap)
106108

107109
pybind_wrap(gtsam_unstable_py # target

‎python/gtsam/specializations.h‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -32,4 +32,6 @@ py::bind_vector<gtsam::IndexPairVector>(m_, "IndexPairVector");
3232
py::bind_map<gtsam::KeyPairDoubleMap>(m_, "KeyPairDoubleMap");
3333
py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3_S2> > >(m_, "CameraSetCal3_S2");
3434
py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Bundler> > >(m_, "CameraSetCal3Bundler");
35+
py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Unified> > >(m_, "CameraSetCal3Unified");
36+
py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Fisheye> > >(m_, "CameraSetCal3Fisheye");
3537
py::bind_vector<std::vector<gtsam::Matrix> >(m_, "JacobianVector");

0 commit comments

Comments
 (0)