Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Python fisheye interface #811

Merged
merged 26 commits into from
Jul 14, 2021
Merged

Python fisheye interface #811

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
26 commits
Select commit Hold shift + click to select a range
9967c59
Forward declaration for Set of Fisheye Cameras
roderick-koehle Jul 8, 2021
dfd50f9
Extend python wrapper to include fisheye models.
roderick-koehle Jul 8, 2021
03e2744
Merge pull request #1 from roderick-koehle/patch-3
roderick-koehle Jul 8, 2021
73d40a5
Merge pull request #2 from roderick-koehle/patch-2
roderick-koehle Jul 8, 2021
55c1274
Forward declaration of fisheye camera.
roderick-koehle Jul 8, 2021
c8fc3cd
Unit test for equidistant fisheye
roderick-koehle Jul 8, 2021
19e8cde
Extend unit testing of omnidirectional projection
roderick-koehle Jul 8, 2021
bdeb606
Introduce setUpClass, python snake_case variables
roderick-koehle Jul 9, 2021
6205057
Use of common setUpClass method
roderick-koehle Jul 9, 2021
a411b66
Correct tab to spaces to fix formatting
roderick-koehle Jul 9, 2021
66af007
Improved accuracy for analytic undistortion
roderick-koehle Jul 9, 2021
0304992
Add comment about initial guess in undistortion
roderick-koehle Jul 10, 2021
d54e234
Add ambiguous calibrate/uncalibrate declarations.
roderick-koehle Jul 10, 2021
3118fde
Missing CameraSet binding specialisations
roderick-koehle Jul 10, 2021
0a73961
Update ignore list in CMakeFile
roderick-koehle Jul 10, 2021
941594c
Testing CameraSet and triangulatePoint3
roderick-koehle Jul 11, 2021
c595767
Unittest, triangulation for Cal3Unified
roderick-koehle Jul 11, 2021
f53f5db
Merge branch 'borglab:develop' into python-fisheye-interface
roderick-koehle Jul 13, 2021
3402e46
Shared data for triangulation unit tests
roderick-koehle Jul 13, 2021
17c37de
Shared setup triangulation unit test
roderick-koehle Jul 13, 2021
3e41ece
Minor fix test_Cal3Unified
roderick-koehle Jul 13, 2021
d130387
Minor fix test_Cal3Fisheye
roderick-koehle Jul 13, 2021
305521e
Merge branch 'borglab:develop' into python-fisheye-interface
roderick-koehle Jul 14, 2021
16cfc7f
Remove commented out line
roderick-koehle Jul 14, 2021
c2bbe78
Remove comment
roderick-koehle Jul 14, 2021
a115788
Remove spaces in empty line
roderick-koehle Jul 14, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
14 changes: 12 additions & 2 deletions gtsam/geometry/Cal3Fisheye.cpp
View file
Open in desktop
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
Open in desktop
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>;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Hmmm. why do we even have these typedefs and includes here? Seems like it exacerbates header bloat/compile times.

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

My guess is that it is an artifact of before we understood the wrapper's behavior really well. The wrapper has seen some nice improvements in the past year so we may as well kill this, while moving the typedef for Cal3_S2 inside the deprecation block.


#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
/**
Expand Down
4 changes: 4 additions & 0 deletions gtsam/geometry/triangulation.h
View file
Open in desktop
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>>;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Similar question. Maybe we should pare down and only list here what's absolutely required in triangulation. Caller can include.


} // \namespace gtsam

104 changes: 94 additions & 10 deletions gtsam/gtsam.i
View file
Open in desktop
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 {
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

should this be a virtual class inheriting from a base class?

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yeah this should technically inherit from Cal3 (similar case for the other camera classes). This is my bad when I refactored the camera classes. 🙁

// 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
Open in desktop
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
Open in desktop
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