12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

update interface files with latest develop

release/4.3a0
Varun Agrawal 2021-07-14 21:53:20 -04:00
parent 36c2aa1e56
commit 6919ad9277
10 changed files with 220 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/base/base.i
View File

@ -6,6 +6,8 @@ namespace gtsam {
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/Cal3DS2.h>
#include <gtsam/geometry/Cal3Fisheye.h>
#include <gtsam/geometry/Cal3Unified.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/geometry/EssentialMatrix.h>
@ -104,8 +106,8 @@ virtual class Value {
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}>
gtsam::Cal3Fisheye, gtsam::Cal3Unified, gtsam::EssentialMatrix,
gtsam::CalibratedCamera, gtsam::imuBias::ConstantBias}>
virtual class GenericValue : gtsam::Value {
void serializable() const;
};

2
gtsam/geometry/SimpleCamera.h
View File

@ -21,6 +21,7 @@
#include <gtsam/geometry/BearingRange.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/Cal3DS2.h>
#include <gtsam/geometry/Cal3Fisheye.h>
#include <gtsam/geometry/Cal3Unified.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/PinholeCamera.h>
@ -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
/**

58
gtsam/geometry/geometry.i
View File

@ -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 <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, 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<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 {
@ -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 <gtsam/geometry/BearingRange.h>
template <POSE, POINT, BEARING, RANGE>

10
gtsam/geometry/triangulation.h
View File

@ -18,14 +18,16 @@
#pragma once
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/Cal3Fisheye.h>
#include <gtsam/geometry/Cal3Unified.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/CameraSet.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/slam/TriangulationFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/slam/TriangulationFactor.h>
namespace gtsam {
@ -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

81
gtsam/linear/Coreset.h Normal file
View File

@ -0,0 +1,81 @@
#include <gtsam/linear/LossFunctions.h>
#include <Eigen/unsupported/Eigen/CX11/Tensor>
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

57
gtsam/nonlinear/nonlinear.i
View File

@ -6,6 +6,8 @@ namespace gtsam {
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/Cal3DS2.h>
#include <gtsam/geometry/Cal3Fisheye.h>
#include <gtsam/geometry/Cal3Unified.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/geometry/EssentialMatrix.h>
@ -151,11 +153,25 @@ class NonlinearFactorGraph {
gtsam::KeySet keys() const;
gtsam::KeyVector keyVector() const;
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::CalibratedCamera, gtsam::PinholeCamera<gtsam::Cal3_S2>,
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::PinholeCamera<gtsam::Cal3_S2>,
gtsam::PinholeCamera<gtsam::Cal3Bundler>,
gtsam::PinholeCamera<gtsam::Cal3Fisheye>,
gtsam::PinholeCamera<gtsam::Cal3Unified>,
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<gtsam::Cal3_S2>& simple_camera);
void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3_S2>& camera);
void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Fisheye>& camera);
void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Unified>& 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<gtsam::Cal3_S2>& simple_camera);
void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3_S2>& camera);
void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Fisheye>& camera);
void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Unified>& 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::Cal3_S2>,
gtsam::PinholeCamera<gtsam::Cal3Bundler>,
gtsam::PinholeCamera<gtsam::Cal3Fisheye>,
gtsam::PinholeCamera<gtsam::Cal3Unified>,
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::Cal3_S2>,
gtsam::PinholeCamera<gtsam::Cal3Bundler>, Vector, Matrix}>
gtsam::PinholeCamera<gtsam::Cal3Bundler>,
gtsam::PinholeCamera<gtsam::Cal3Fisheye>,
gtsam::PinholeCamera<gtsam::Cal3Unified>, Vector, Matrix}>
VALUE calculateEstimate(size_t key) const;
gtsam::Values calculateBestEstimate() const;
Matrix marginalCovariance(size_t key) const;
@ -734,10 +762,14 @@ template <T = {double,
gtsam::Cal3_S2,
gtsam::Cal3DS2,
gtsam::Cal3Bundler,
gtsam::Cal3Fisheye,
gtsam::Cal3Unified,
gtsam::CalibratedCamera,
gtsam::PinholeCamera<gtsam::Cal3_S2>,
gtsam::imuBias::ConstantBias,
gtsam::PinholeCamera<gtsam::Cal3Bundler>}>
gtsam::PinholeCamera<gtsam::Cal3Bundler>,
gtsam::PinholeCamera<gtsam::Cal3Fisheye>,
gtsam::PinholeCamera<gtsam::Cal3Unified>,
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 <T = {gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2,
gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose2,
gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera,
gtsam::PinholeCamera<gtsam::Cal3_S2>,
gtsam::PinholeCamera<gtsam::Cal3Bundler>,
gtsam::PinholeCamera<gtsam::Cal3Fisheye>,
gtsam::PinholeCamera<gtsam::Cal3Unified>,
gtsam::imuBias::ConstantBias}>
virtual class NonlinearEquality : gtsam::NoiseModelFactor {
// Constructor - forces exact evaluation

2
gtsam/sfm/sfm.i
View File

@ -87,6 +87,8 @@ class ShonanAveraging2 {
ShonanAveraging2(string g2oFile);
ShonanAveraging2(string g2oFile,
const gtsam::ShonanAveragingParameters2& parameters);
ShonanAveraging2(const gtsam::BetweenFactorPose2s &factors,
const gtsam::ShonanAveragingParameters2 &parameters);
// Query properties
size_t nrUnknowns() const;

15
gtsam/slam/slam.i
View File

@ -62,6 +62,12 @@ typedef gtsam::GenericProjectionFactor<gtsam::Pose3, gtsam::Point3,
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>
template <CAMERA, LANDMARK>
@ -80,8 +86,15 @@ typedef gtsam::GeneralSFMFactor<gtsam::PinholeCamera<gtsam::Cal3DS2>,
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCamera<gtsam::Cal3Bundler>,
gtsam::Point3>
GeneralSFMFactorCal3Bundler;
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCamera<gtsam::Cal3Fisheye>,
gtsam::Point3>
GeneralSFMFactorCal3Fisheye;
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCamera<gtsam::Cal3Unified>,
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,

2
python/CMakeLists.txt
View File

@ -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

4
python/gtsam/specializations/geometry.h
View File

@ -21,3 +21,7 @@ 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");