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

Wrapper updates 

- New readBal function
- Extra utility functions for reading Values from SfmData
- Quaternion class
- Fixed issue with Rot3::toQuaternion()
- Pose3 constructor with Pose2 as arg
- K() function in Cal3Bundler
- Added better support for PinholeCameraCal3_S2 in Values
- Updated TODOs
release/4.3a0
Varun Agrawal 2020-09-03 00:44:08 -04:00
parent e95840b1b5
commit 13486166c6
1 changed files with 25 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
gtsam/gtsam.i
View File

@ -623,6 +623,15 @@ class SOn {
void serialize() const; void serialize() const;
}; };
#include <gtsam/geometry/Quaternion.h>
class Quaternion {
double w() const;
double x() const;
double y() const;
double z() const;
Vector coeffs() const;
};
#include <gtsam/geometry/Rot3.h> #include <gtsam/geometry/Rot3.h>
class Rot3 { class Rot3 {
// Standard Constructors and Named Constructors // Standard Constructors and Named Constructors
@ -659,7 +668,7 @@ class Rot3 {
gtsam::Rot3 between(const gtsam::Rot3& p2) const; gtsam::Rot3 between(const gtsam::Rot3& p2) const;
// Manifold // Manifold
//gtsam::Rot3 retractCayley(Vector v) const; // FIXME, does not exist in both Matrix and Quaternion options //gtsam::Rot3 retractCayley(Vector v) const; // TODO, does not exist in both Matrix and Quaternion options
gtsam::Rot3 retract(Vector v) const; gtsam::Rot3 retract(Vector v) const;
Vector localCoordinates(const gtsam::Rot3& p) const; Vector localCoordinates(const gtsam::Rot3& p) const;
@ -680,7 +689,7 @@ class Rot3 {
double pitch() const; double pitch() const;
double yaw() const; double yaw() const;
pair<gtsam::Unit3, double> axisAngle() const; pair<gtsam::Unit3, double> axisAngle() const;
// Vector toQuaternion() const; // FIXME: Can't cast to Vector properly gtsam::Quaternion toQuaternion() const;
Vector quaternion() const; Vector quaternion() const;
gtsam::Rot3 slerp(double t, const gtsam::Rot3& other) const; gtsam::Rot3 slerp(double t, const gtsam::Rot3& other) const;
@ -748,7 +757,7 @@ class Pose3 {
Pose3(); Pose3();
Pose3(const gtsam::Pose3& other); Pose3(const gtsam::Pose3& other);
Pose3(const gtsam::Rot3& r, const gtsam::Point3& t); Pose3(const gtsam::Rot3& r, const gtsam::Point3& t);
Pose3(const gtsam::Pose2& pose2); // FIXME: shadows Pose3(Pose3 pose) Pose3(const gtsam::Pose2& pose2);
Pose3(Matrix mat); Pose3(Matrix mat);
// Testable // Testable
@ -1015,7 +1024,7 @@ class Cal3Bundler {
double v0() const; double v0() const;
Vector vector() const; Vector vector() const;
Vector k() const; Vector k() const;
//Matrix K() const; //FIXME: Uppercase Matrix K() const;
// enabling serialization functionality // enabling serialization functionality
void serialize() const; void serialize() const;
@ -1134,7 +1143,7 @@ gtsam::SimpleCamera simpleCamera(const Matrix& P);
// Some typedefs for common camera types // Some typedefs for common camera types
// PinholeCameraCal3_S2 is the same as SimpleCamera above // PinholeCameraCal3_S2 is the same as SimpleCamera above
typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2; typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
// due to lack of jacobians of Cal3DS2_Base::calibrate, PinholeCamera does not apply to Cal3DS2/Unified //TODO (Issue 237) due to lack of jacobians of Cal3DS2_Base::calibrate, PinholeCamera does not apply to Cal3DS2/Unified
//typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2; //typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2;
//typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified; //typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
//typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler; //typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
@ -1330,7 +1339,7 @@ class SymbolicBayesTree {
// // const std::list<derived_ptr>& children() const { return children_; } // // const std::list<derived_ptr>& children() const { return children_; }
// // derived_ptr parent() const { return parent_.lock(); } // // derived_ptr parent() const { return parent_.lock(); }
// //
// // FIXME: need wrapped versions graphs, BayesNet // // TODO: need wrapped versions graphs, BayesNet
// // BayesNet<ConditionalType> shortcut(derived_ptr root, Eliminate function) const; // // BayesNet<ConditionalType> shortcut(derived_ptr root, Eliminate function) const;
// // FactorGraph<FactorType> marginal(derived_ptr root, Eliminate function) const; // // FactorGraph<FactorType> marginal(derived_ptr root, Eliminate function) const;
// // FactorGraph<FactorType> joint(derived_ptr C2, derived_ptr root, Eliminate function) const; // // FactorGraph<FactorType> joint(derived_ptr C2, derived_ptr root, Eliminate function) const;
@ -2129,7 +2138,7 @@ virtual class NonlinearFactor {
bool active(const gtsam::Values& c) const; bool active(const gtsam::Values& c) const;
gtsam::GaussianFactor* linearize(const gtsam::Values& c) const; gtsam::GaussianFactor* linearize(const gtsam::Values& c) const;
gtsam::NonlinearFactor* clone() const; gtsam::NonlinearFactor* clone() const;
// gtsam::NonlinearFactor* rekey(const gtsam::KeyVector& newKeys) const; //FIXME: Conversion from KeyVector to std::vector does not happen // gtsam::NonlinearFactor* rekey(const gtsam::KeyVector& newKeys) const; //TODO: Conversion from KeyVector to std::vector does not happen
}; };
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
@ -2193,6 +2202,7 @@ class Values {
void insert(size_t j, const gtsam::Cal3Bundler& cal3bundler); void insert(size_t j, const gtsam::Cal3Bundler& cal3bundler);
void insert(size_t j, const gtsam::EssentialMatrix& essential_matrix); 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::PinholeCameraCal3_S2& simple_camera);
// void insert(size_t j, const gtsam::PinholeCameraCal3Bundler& camera);
void insert(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); 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, const gtsam::NavState& nav_state);
@ -2209,12 +2219,14 @@ class Values {
void update(size_t j, const gtsam::Cal3DS2& cal3ds2); 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::Cal3Bundler& cal3bundler);
void update(size_t j, const gtsam::EssentialMatrix& essential_matrix); 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::PinholeCameraCal3Bundler& camera);
void update(size_t j, const gtsam::imuBias::ConstantBias& constant_bias); 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, const gtsam::NavState& nav_state);
void update(size_t j, Vector vector); void update(size_t j, Vector vector);
void update(size_t j, Matrix matrix); void update(size_t j, Matrix matrix);
template<T = {gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Pose2, gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::imuBias::ConstantBias, gtsam::NavState, Vector, Matrix}> template<T = {gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Pose2, gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::PinholeCameraCal3_S2, gtsam::imuBias::ConstantBias, gtsam::NavState, Vector, Matrix}>
T at(size_t j); T at(size_t j);
/// version for double /// version for double
@ -2698,7 +2710,7 @@ virtual class GeneralSFMFactor : gtsam::NoiseModelFactor {
gtsam::Point2 measured() const; gtsam::Point2 measured() const;
}; };
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3_S2, gtsam::Point3> GeneralSFMFactorCal3_S2; typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3_S2, gtsam::Point3> GeneralSFMFactorCal3_S2;
// due to lack of jacobians of Cal3DS2_Base::calibrate, GeneralSFMFactor does not apply to Cal3DS2 //TODO (Issue 237) due to lack of jacobians of Cal3DS2_Base::calibrate, GeneralSFMFactor does not apply to Cal3DS2
//typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2; //typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
template<CALIBRATION = {gtsam::Cal3_S2}> template<CALIBRATION = {gtsam::Cal3_S2}>
@ -2800,6 +2812,10 @@ class SfmData {
gtsam::SfmTrack track(size_t idx) const; gtsam::SfmTrack track(size_t idx) const;
}; };
gtsam::SfmData readBal(string filename);
gtsam::Values initialCamerasEstimate(const gtsam::SfmData& db);
gtsam::Values initialCamerasAndPointsEstimate(const gtsam::SfmData& db);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename, pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
gtsam::noiseModel::Diagonal* model, int maxIndex, bool addNoise, bool smart); gtsam::noiseModel::Diagonal* model, int maxIndex, bool addNoise, bool smart);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename, pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,