diff --git a/gtsam/base/base.i b/gtsam/base/base.i index 09278ff5b..c24b04088 100644 --- a/gtsam/base/base.i +++ b/gtsam/base/base.i @@ -6,6 +6,8 @@ namespace gtsam { #include #include +#include +#include #include #include #include @@ -104,8 +106,8 @@ virtual class Value { template + gtsam::Cal3Fisheye, gtsam::Cal3Unified, gtsam::EssentialMatrix, + gtsam::CalibratedCamera, gtsam::imuBias::ConstantBias}> virtual class GenericValue : gtsam::Value { void serializable() const; }; diff --git a/gtsam/geometry/SimpleCamera.h b/gtsam/geometry/SimpleCamera.h index aa00222c7..27b8bb549 100644 --- a/gtsam/geometry/SimpleCamera.h +++ b/gtsam/geometry/SimpleCamera.h @@ -21,6 +21,7 @@ #include #include #include +#include #include #include #include @@ -33,6 +34,7 @@ namespace gtsam { using PinholeCameraCal3Bundler = gtsam::PinholeCamera; using PinholeCameraCal3DS2 = gtsam::PinholeCamera; using PinholeCameraCal3Unified = gtsam::PinholeCamera; + using PinholeCameraCal3Fisheye = gtsam::PinholeCamera; #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 /** diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 6217d9e80..4e56347d3 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -684,6 +684,57 @@ 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; + + // enable pickling in python + void pickle() const; +}; + +#include +class Cal3Fisheye { + // Standard Constructors + Cal3Fisheye(); + Cal3Fisheye(double fx, double fy, double s, double u0, double v0, double k1, + double k2, double k3, double k4, double tol = 1e-5); + 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; @@ -860,6 +911,7 @@ typedef gtsam::PinholeCamera PinholeCameraCal3_S2; typedef gtsam::PinholeCamera PinholeCameraCal3DS2; typedef gtsam::PinholeCamera PinholeCameraCal3Unified; typedef gtsam::PinholeCamera PinholeCameraCal3Bundler; +typedef gtsam::PinholeCamera PinholeCameraCal3Fisheye; template class CameraSet { @@ -924,6 +976,12 @@ 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); #include template diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index 1df9efd22..6f6ade6f7 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -18,14 +18,16 @@ #pragma once -#include #include +#include +#include +#include #include #include #include -#include -#include #include +#include +#include namespace gtsam { @@ -499,6 +501,8 @@ TriangulationResult triangulateSafe(const CameraSet& cameras, // Vector of Cameras - used by the Python/MATLAB wrapper using CameraSetCal3Bundler = CameraSet>; using CameraSetCal3_S2 = CameraSet>; +using CameraSetCal3Fisheye = CameraSet>; +using CameraSetCal3Unified = CameraSet>; } // \namespace gtsam diff --git a/gtsam/linear/Coreset.h b/gtsam/linear/Coreset.h new file mode 100644 index 000000000..f622d9c81 --- /dev/null +++ b/gtsam/linear/Coreset.h @@ -0,0 +1,81 @@ +#include + +#include + +namespace gtsam { + +Vector FastCaratheodory(Matrix& P, Vector& weights, size_t coreset_size) { + size_t n = P.rows(), d = P.cols(); + size_t m = 2 * d + 2; + + if (n < d + 1) { + return weights; + } + + Vector weights = weights / weights.sum(); + + size_t chunk_size = ceil(n / m); + size_t current_m = ceil(n / chunk_size); + + size_t add_z = chunk_size - size_t(n % chunk_size); + Matrix u(weights.size(), 1); + u.col(0) = weights; + + if (add_z != chunk_size) { + Matrix zeros = Matrix::Zero(add_z, d); + Matrix P_new = Matrix(P.rows() + zeros.rows(), P.cols() + zeros.cols()); + P_new << P, zeros; + zeros = Matrix::Zero(add_z, u.cols()); + Matrix u_new(u.rows() + zeros.rows(), u.cols() + zeros.cols()); + u_new << u, zeros; + } + + Vector idxarray = Vector::LinSpaced(n, 0, n - 1); + Eigen::Tensor p_groups; + + // p_groups = P.reshape(current_m, chunk_size, P.shape[1]) + // u_groups = u.reshape(current_m, chunk_size) + // idx_group = idxarray.reshape(current_m, chunk_size) + // u_nonzero = np.count_nonzero(u) + + // if not coreset_size: + // coreset_size = d+1 + // while u_nonzero > coreset_size: + + // groups_means = np.einsum('ijk,ij->ik',p_groups, u_groups) + // group_weigts = np.ones(groups_means.shape[0], dtype = + // dtype)*1/current_m + + // Cara_u_idx = Caratheodory(groups_means , group_weigts,dtype = dtype ) + + // IDX = np.nonzero(Cara_u_idx) + + // new_P = p_groups[IDX].reshape(-1,d) + + // subset_u = (current_m * u_groups[IDX] * Cara_u_idx[IDX][:, + // np.newaxis]).reshape(-1, 1) new_idx_array = + // idx_group[IDX].reshape(-1,1) + // ##############################################################################3 + // u_nonzero = np.count_nonzero(subset_u) + // chunk_size = math.ceil(new_P.shape[0]/ m) + // current_m = math.ceil(new_P.shape[0]/ chunk_size) + + // add_z = chunk_size - int(new_P.shape[0] % chunk_size) + // if add_z != chunk_size: + // new_P = np.concatenate((new_P, np.zeros((add_z, new_P.shape[1]), + // dtype = dtype))) subset_u = np.concatenate((subset_u, + // np.zeros((add_z, subset_u.shape[1]),dtype = dtype))) + // new_idx_array = np.concatenate((new_idx_array, np.zeros((add_z, + // new_idx_array.shape[1]),dtype = dtype))) + // p_groups = new_P.reshape(current_m, chunk_size, new_P.shape[1]) + // u_groups = subset_u.reshape(current_m, chunk_size) + // idx_group = new_idx_array.reshape(current_m , chunk_size) + // ########################################################### + + // new_u = np.zeros(n) + // subset_u = subset_u[(new_idx_array < n)] + // new_idx_array = new_idx_array[(new_idx_array < + // n)].reshape(-1).astype(int) new_u[new_idx_array] = subset_u return u_sum + // * new_u +} +} // namespace gtsam \ No newline at end of file diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index e22ac5954..8071e8bc7 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -6,6 +6,8 @@ namespace gtsam { #include #include +#include +#include #include #include #include @@ -151,11 +153,25 @@ class NonlinearFactorGraph { gtsam::KeySet keys() const; gtsam::KeyVector keyVector() const; - template , + template , gtsam::PinholeCamera, + gtsam::PinholeCamera, + gtsam::PinholeCamera, gtsam::imuBias::ConstantBias}> void addPrior(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); @@ -291,10 +307,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::PinholeCamera& simple_camera); + void insert(size_t j, const gtsam::PinholeCamera& camera); void insert(size_t j, const gtsam::PinholeCamera& camera); + void insert(size_t j, const gtsam::PinholeCamera& camera); + void insert(size_t j, const gtsam::PinholeCamera& 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); @@ -312,10 +331,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::PinholeCamera& simple_camera); + void update(size_t j, const gtsam::PinholeCamera& camera); void update(size_t j, const gtsam::PinholeCamera& camera); + void update(size_t j, const gtsam::PinholeCamera& camera); + void update(size_t j, const gtsam::PinholeCamera& 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); @@ -335,9 +357,13 @@ class Values { gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, + gtsam::Cal3Fisheye, + gtsam::Cal3Unified, gtsam::EssentialMatrix, gtsam::PinholeCamera, gtsam::PinholeCamera, + gtsam::PinholeCamera, + gtsam::PinholeCamera, gtsam::imuBias::ConstantBias, gtsam::NavState, Vector, @@ -681,7 +707,9 @@ class ISAM2 { gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::PinholeCamera, - gtsam::PinholeCamera, Vector, Matrix}> + gtsam::PinholeCamera, + gtsam::PinholeCamera, + gtsam::PinholeCamera, Vector, Matrix}> VALUE calculateEstimate(size_t key) const; gtsam::Values calculateBestEstimate() const; Matrix marginalCovariance(size_t key) const; @@ -734,10 +762,14 @@ template , - gtsam::imuBias::ConstantBias, - gtsam::PinholeCamera}> + gtsam::PinholeCamera, + gtsam::PinholeCamera, + gtsam::PinholeCamera, + gtsam::imuBias::ConstantBias}> virtual class PriorFactor : gtsam::NoiseModelFactor { PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); @@ -755,6 +787,9 @@ template , + gtsam::PinholeCamera, + gtsam::PinholeCamera, + gtsam::PinholeCamera, gtsam::imuBias::ConstantBias}> virtual class NonlinearEquality : gtsam::NoiseModelFactor { // Constructor - forces exact evaluation diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i index c86de8d88..705892e60 100644 --- a/gtsam/sfm/sfm.i +++ b/gtsam/sfm/sfm.i @@ -87,6 +87,8 @@ class ShonanAveraging2 { ShonanAveraging2(string g2oFile); ShonanAveraging2(string g2oFile, const gtsam::ShonanAveragingParameters2& parameters); + ShonanAveraging2(const gtsam::BetweenFactorPose2s &factors, + const gtsam::ShonanAveragingParameters2 ¶meters); // Query properties size_t nrUnknowns() const; diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index 457e907b9..1c04fd14c 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -62,6 +62,12 @@ typedef gtsam::GenericProjectionFactor GenericProjectionFactorCal3DS2; +typedef gtsam::GenericProjectionFactor + GenericProjectionFactorCal3Fisheye; +typedef gtsam::GenericProjectionFactor + GenericProjectionFactorCal3Unified; #include template @@ -80,8 +86,15 @@ typedef gtsam::GeneralSFMFactor, typedef gtsam::GeneralSFMFactor, gtsam::Point3> GeneralSFMFactorCal3Bundler; +typedef gtsam::GeneralSFMFactor, + gtsam::Point3> + GeneralSFMFactorCal3Fisheye; +typedef gtsam::GeneralSFMFactor, + gtsam::Point3> + GeneralSFMFactorCal3Unified; -template +template virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor { GeneralSFMFactor2(const gtsam::Point2& measured, const gtsam::noiseModel::Base* model, size_t poseKey, diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 676479bd5..7b8347da5 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -116,6 +116,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 diff --git a/python/gtsam/specializations/geometry.h b/python/gtsam/specializations/geometry.h index 3d22581d9..e11d3cc4f 100644 --- a/python/gtsam/specializations/geometry.h +++ b/python/gtsam/specializations/geometry.h @@ -21,3 +21,7 @@ py::bind_vector>>( m_, "CameraSetCal3_S2"); py::bind_vector>>( m_, "CameraSetCal3Bundler"); +py::bind_vector>>( + m_, "CameraSetCal3Unified"); +py::bind_vector>>( + m_, "CameraSetCal3Fisheye");