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

Merge pull request #1546 from borglab/wrap/pybind-stl

release/4.3a0
Varun Agrawal 2023-06-16 12:04:49 -04:00 committed by GitHub
parent 8bd690cd26 80a052d048
commit b3635cc6ce
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
49 changed files with 208 additions and 303 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
gtsam/base/base.i
View File

@ -41,6 +41,7 @@ class DSFMap {
std::map<KEY, This::Set> sets(); std::map<KEY, This::Set> sets();
}; };
// Used in Matlab wrapper
class IndexPairSet { class IndexPairSet {
IndexPairSet(); IndexPairSet();
// common STL methods // common STL methods
@ -54,6 +55,7 @@ class IndexPairSet {
bool count(gtsam::IndexPair key) const; // returns true if value exists bool count(gtsam::IndexPair key) const; // returns true if value exists
}; };
// Used in Matlab wrapper
class IndexPairVector { class IndexPairVector {
IndexPairVector(); IndexPairVector();
IndexPairVector(const gtsam::IndexPairVector& other); IndexPairVector(const gtsam::IndexPairVector& other);
@ -70,6 +72,7 @@ class IndexPairVector {
gtsam::IndexPairVector IndexPairSetAsArray(gtsam::IndexPairSet& set); gtsam::IndexPairVector IndexPairSetAsArray(gtsam::IndexPairSet& set);
// Used in Matlab wrapper
class IndexPairSetMap { class IndexPairSetMap {
IndexPairSetMap(); IndexPairSetMap();
// common STL methods // common STL methods

4
gtsam/geometry/Rot3.h
View File

@ -560,8 +560,8 @@ class GTSAM_EXPORT Rot3 : public LieGroup<Rot3, 3> {
#endif #endif
}; };
/// std::vector of Rot3s, mainly for wrapper /// std::vector of Rot3s, used in Matlab wrapper
using Rot3Vector = std::vector<Rot3, Eigen::aligned_allocator<Rot3> >; using Rot3Vector = std::vector<Rot3, Eigen::aligned_allocator<Rot3>>;
/** /**
* [RQ] receives a 3 by 3 matrix and returns an upper triangular matrix R * [RQ] receives a 3 by 3 matrix and returns an upper triangular matrix R

9
gtsam/geometry/geometry.i
View File

@ -29,6 +29,7 @@ class Point2 {
void serialize() const; void serialize() const;
}; };
// Used in Matlab wrapper
class Point2Pairs { class Point2Pairs {
Point2Pairs(); Point2Pairs();
size_t size() const; size_t size() const;
@ -38,6 +39,7 @@ class Point2Pairs {
}; };
// std::vector<gtsam::Point2> // std::vector<gtsam::Point2>
// Used in Matlab wrapper
class Point2Vector { class Point2Vector {
// Constructors // Constructors
Point2Vector(); Point2Vector();
@ -131,6 +133,7 @@ class Point3 {
gtsam::Point3 normalize(const gtsam::Point3 &p, Eigen::Ref<Eigen::MatrixXd> H) const; gtsam::Point3 normalize(const gtsam::Point3 &p, Eigen::Ref<Eigen::MatrixXd> H) const;
}; };
// Used in Matlab wrapper
class Point3Pairs { class Point3Pairs {
Point3Pairs(); Point3Pairs();
size_t size() const; size_t size() const;
@ -544,6 +547,7 @@ class Pose3 {
void serialize() const; void serialize() const;
}; };
// Used in Matlab wrapper
class Pose3Pairs { class Pose3Pairs {
Pose3Pairs(); Pose3Pairs();
size_t size() const; size_t size() const;
@ -552,6 +556,7 @@ class Pose3Pairs {
void push_back(const gtsam::Pose3Pair& pose_pair); void push_back(const gtsam::Pose3Pair& pose_pair);
}; };
// Used in Matlab wrapper
class Pose3Vector { class Pose3Vector {
Pose3Vector(); Pose3Vector();
size_t size() const; size_t size() const;
@ -1176,8 +1181,8 @@ class TriangulationParameters {
const gtsam::SharedNoiseModel& noiseModel = nullptr); const gtsam::SharedNoiseModel& noiseModel = nullptr);
}; };
// Templates appear not yet supported for free functions - issue raised at // Can be templated but overloaded for convenience.
// borglab/wrap#14 to add support // We can now call `triangulatePoint3` with any template type.
// Cal3_S2 versions // Cal3_S2 versions
gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses, gtsam::Point3 triangulatePoint3(const gtsam::Pose3Vector& poses,

2
gtsam/gtsam.i
View File

@ -105,6 +105,7 @@ class KeyGroupMap {
}; };
// Actually a FastSet<FactorIndex> // Actually a FastSet<FactorIndex>
// Used in Matlab wrapper
class FactorIndexSet { class FactorIndexSet {
FactorIndexSet(); FactorIndexSet();
FactorIndexSet(const gtsam::FactorIndexSet& set); FactorIndexSet(const gtsam::FactorIndexSet& set);
@ -121,6 +122,7 @@ class FactorIndexSet {
}; };
// Actually a vector<FactorIndex> // Actually a vector<FactorIndex>
// Used in Matlab wrapper
class FactorIndices { class FactorIndices {
FactorIndices(); FactorIndices();
FactorIndices(const gtsam::FactorIndices& other); FactorIndices(const gtsam::FactorIndices& other);

10
gtsam/inference/inference.i
View File

@ -176,13 +176,9 @@ class DotWriter {
class VariableIndex { class VariableIndex {
// Standard Constructors and Named Constructors // Standard Constructors and Named Constructors
VariableIndex(); VariableIndex();
// TODO: Templetize constructor when wrap supports it template <T = {gtsam::SymbolicFactorGraph, gtsam::GaussianFactorGraph,
// template<T = {gtsam::FactorGraph}> gtsam::NonlinearFactorGraph}>
// VariableIndex(const T& factorGraph, size_t nVariables); VariableIndex(const T& factorGraph);
// VariableIndex(const T& factorGraph);
VariableIndex(const gtsam::SymbolicFactorGraph& sfg);
VariableIndex(const gtsam::GaussianFactorGraph& gfg);
VariableIndex(const gtsam::NonlinearFactorGraph& fg);
VariableIndex(const gtsam::VariableIndex& other); VariableIndex(const gtsam::VariableIndex& other);
// Testable // Testable

2
gtsam/nonlinear/CustomFactor.cpp
View File

@ -28,7 +28,7 @@ Vector CustomFactor::unwhitenedError(const Values& x, OptionalMatrixVecType H) c
if(H) { if(H) {
/* /*
* In this case, we pass the raw pointer to the `std::vector<Matrix>` object directly to pybind. * In this case, we pass the raw pointer to the `std::vector<Matrix>` object directly to pybind.
* As the type `std::vector<Matrix>` has been marked as opaque in `preamble.h`, any changes in * As the type `std::vector<Matrix>` has been marked as opaque in `preamble/custom.h`, any changes in
* Python will be immediately reflected on the C++ side. * Python will be immediately reflected on the C++ side.
* *
* Example: * Example:

3
gtsam/nonlinear/GncParams.h
View File

@ -73,8 +73,7 @@ class GncParams {
double weightsTol = 1e-4; ///< If the weights are within weightsTol from being binary, stop iterating (only for TLS) double weightsTol = 1e-4; ///< If the weights are within weightsTol from being binary, stop iterating (only for TLS)
Verbosity verbosity = SILENT; ///< Verbosity level Verbosity verbosity = SILENT; ///< Verbosity level
//TODO(Varun) replace IndexVector with vector<size_t> once pybind11/stl.h is globally enabled. /// Use IndexVector for inliers and outliers since it is fast
/// Use IndexVector for inliers and outliers since it is fast + wrapping
using IndexVector = FastVector<uint64_t>; using IndexVector = FastVector<uint64_t>;
///< Slots in the factor graph corresponding to measurements that we know are inliers ///< Slots in the factor graph corresponding to measurements that we know are inliers
IndexVector knownInliers = IndexVector(); IndexVector knownInliers = IndexVector();

8
gtsam/nonlinear/nonlinear.i
View File

@ -305,8 +305,8 @@ virtual class GncParams {
double relativeCostTol; double relativeCostTol;
double weightsTol; double weightsTol;
gtsam::This::Verbosity verbosity; gtsam::This::Verbosity verbosity;
gtsam::KeyVector knownInliers; gtsam::This::IndexVector knownInliers;
gtsam::KeyVector knownOutliers; gtsam::This::IndexVector knownOutliers;
void setLossType(const gtsam::GncLossType type); void setLossType(const gtsam::GncLossType type);
void setMaxIterations(const size_t maxIter); void setMaxIterations(const size_t maxIter);
@ -314,8 +314,8 @@ virtual class GncParams {
void setRelativeCostTol(double value); void setRelativeCostTol(double value);
void setWeightsTol(double value); void setWeightsTol(double value);
void setVerbosityGNC(const gtsam::This::Verbosity value); void setVerbosityGNC(const gtsam::This::Verbosity value);
void setKnownInliers(const gtsam::KeyVector& knownIn); void setKnownInliers(const gtsam::This::IndexVector& knownIn);
void setKnownOutliers(const gtsam::KeyVector& knownOut); void setKnownOutliers(const gtsam::This::IndexVector& knownOut);
void print(const string& str = "GncParams: ") const; void print(const string& str = "GncParams: ") const;
enum Verbosity { enum Verbosity {

65
gtsam/sfm/sfm.i
View File

@ -6,15 +6,14 @@ namespace gtsam {
#include <gtsam/sfm/SfmTrack.h> #include <gtsam/sfm/SfmTrack.h>
class SfmTrack2d { class SfmTrack2d {
std::vector<pair<size_t, gtsam::Point2>> measurements; std::vector<gtsam::SfmMeasurement> measurements;
SfmTrack2d(); SfmTrack2d();
SfmTrack2d(const std::vector<gtsam::SfmMeasurement>& measurements); SfmTrack2d(const std::vector<gtsam::SfmMeasurement>& measurements);
size_t numberMeasurements() const; size_t numberMeasurements() const;
pair<size_t, gtsam::Point2> measurement(size_t idx) const; gtsam::SfmMeasurement measurement(size_t idx) const;
pair<size_t, size_t> siftIndex(size_t idx) const; pair<size_t, size_t> siftIndex(size_t idx) const;
void addMeasurement(size_t idx, const gtsam::Point2& m); void addMeasurement(size_t idx, const gtsam::Point2& m);
gtsam::SfmMeasurement measurement(size_t idx) const;
bool hasUniqueCameras() const; bool hasUniqueCameras() const;
Eigen::MatrixX2d measurementMatrix() const; Eigen::MatrixX2d measurementMatrix() const;
Eigen::VectorXi indexVector() const; Eigen::VectorXi indexVector() const;
@ -100,6 +99,7 @@ typedef gtsam::BinaryMeasurement<gtsam::Unit3> BinaryMeasurementUnit3;
typedef gtsam::BinaryMeasurement<gtsam::Rot3> BinaryMeasurementRot3; typedef gtsam::BinaryMeasurement<gtsam::Rot3> BinaryMeasurementRot3;
typedef gtsam::BinaryMeasurement<gtsam::Point3> BinaryMeasurementPoint3; typedef gtsam::BinaryMeasurement<gtsam::Point3> BinaryMeasurementPoint3;
// Used in Matlab wrapper
class BinaryMeasurementsUnit3 { class BinaryMeasurementsUnit3 {
BinaryMeasurementsUnit3(); BinaryMeasurementsUnit3();
size_t size() const; size_t size() const;
@ -107,6 +107,7 @@ class BinaryMeasurementsUnit3 {
void push_back(const gtsam::BinaryMeasurement<gtsam::Unit3>& measurement); void push_back(const gtsam::BinaryMeasurement<gtsam::Unit3>& measurement);
}; };
// Used in Matlab wrapper
class BinaryMeasurementsPoint3 { class BinaryMeasurementsPoint3 {
BinaryMeasurementsPoint3(); BinaryMeasurementsPoint3();
size_t size() const; size_t size() const;
@ -114,6 +115,7 @@ class BinaryMeasurementsPoint3 {
void push_back(const gtsam::BinaryMeasurement<gtsam::Point3>& measurement); void push_back(const gtsam::BinaryMeasurement<gtsam::Point3>& measurement);
}; };
// Used in Matlab wrapper
class BinaryMeasurementsRot3 { class BinaryMeasurementsRot3 {
BinaryMeasurementsRot3(); BinaryMeasurementsRot3();
size_t size() const; size_t size() const;
@ -121,41 +123,19 @@ class BinaryMeasurementsRot3 {
void push_back(const gtsam::BinaryMeasurement<gtsam::Rot3>& measurement); void push_back(const gtsam::BinaryMeasurement<gtsam::Rot3>& measurement);
}; };
#include <gtsam/slam/dataset.h>
#include <gtsam/sfm/ShonanAveraging.h> #include <gtsam/sfm/ShonanAveraging.h>
// TODO(frank): copy/pasta below until we have integer template parameters in template <d={2, 3}>
// wrap! class ShonanAveragingParameters {
ShonanAveragingParameters(const gtsam::LevenbergMarquardtParams& lm);
class ShonanAveragingParameters2 { ShonanAveragingParameters(const gtsam::LevenbergMarquardtParams& lm,
ShonanAveragingParameters2(const gtsam::LevenbergMarquardtParams& lm);
ShonanAveragingParameters2(const gtsam::LevenbergMarquardtParams& lm,
string method); string method);
gtsam::LevenbergMarquardtParams getLMParams() const; gtsam::LevenbergMarquardtParams getLMParams() const;
void setOptimalityThreshold(double value); void setOptimalityThreshold(double value);
double getOptimalityThreshold() const; double getOptimalityThreshold() const;
void setAnchor(size_t index, const gtsam::Rot2& value); void setAnchor(size_t index, const gtsam::This::Rot& value);
pair<size_t, gtsam::Rot2> getAnchor(); pair<size_t, gtsam::This::Rot> getAnchor();
void setAnchorWeight(double value);
double getAnchorWeight() const;
void setKarcherWeight(double value);
double getKarcherWeight() const;
void setGaugesWeight(double value);
double getGaugesWeight() const;
void setUseHuber(bool value);
bool getUseHuber() const;
void setCertifyOptimality(bool value);
bool getCertifyOptimality() const;
};
class ShonanAveragingParameters3 {
ShonanAveragingParameters3(const gtsam::LevenbergMarquardtParams& lm);
ShonanAveragingParameters3(const gtsam::LevenbergMarquardtParams& lm,
string method);
gtsam::LevenbergMarquardtParams getLMParams() const;
void setOptimalityThreshold(double value);
double getOptimalityThreshold() const;
void setAnchor(size_t index, const gtsam::Rot3& value);
pair<size_t, gtsam::Rot3> getAnchor();
void setAnchorWeight(double value); void setAnchorWeight(double value);
double getAnchorWeight() const; double getAnchorWeight() const;
void setKarcherWeight(double value); void setKarcherWeight(double value);
@ -168,6 +148,7 @@ class ShonanAveragingParameters3 {
bool getCertifyOptimality() const; bool getCertifyOptimality() const;
}; };
// NOTE(Varun): Not templated because each class has specializations defined.
class ShonanAveraging2 { class ShonanAveraging2 {
ShonanAveraging2(string g2oFile); ShonanAveraging2(string g2oFile);
ShonanAveraging2(string g2oFile, ShonanAveraging2(string g2oFile,
@ -214,18 +195,16 @@ class ShonanAveraging2 {
}; };
class ShonanAveraging3 { class ShonanAveraging3 {
ShonanAveraging3( ShonanAveraging3(const gtsam::This::Measurements& measurements,
const std::vector<gtsam::BinaryMeasurement<gtsam::Rot3>>& measurements, const gtsam::ShonanAveragingParameters3& parameters =
const gtsam::ShonanAveragingParameters3& parameters = gtsam::ShonanAveragingParameters3());
gtsam::ShonanAveragingParameters3());
ShonanAveraging3(string g2oFile); ShonanAveraging3(string g2oFile);
ShonanAveraging3(string g2oFile, ShonanAveraging3(string g2oFile,
const gtsam::ShonanAveragingParameters3& parameters); const gtsam::ShonanAveragingParameters3& parameters);
// TODO(frank): deprecate once we land pybind wrapper
ShonanAveraging3(const gtsam::BetweenFactorPose3s& factors);
ShonanAveraging3(const gtsam::BetweenFactorPose3s& factors, ShonanAveraging3(const gtsam::BetweenFactorPose3s& factors,
const gtsam::ShonanAveragingParameters3& parameters); const gtsam::ShonanAveragingParameters3& parameters =
gtsam::ShonanAveragingParameters3());
// Query properties // Query properties
size_t nrUnknowns() const; size_t nrUnknowns() const;
@ -267,6 +246,7 @@ class ShonanAveraging3 {
#include <gtsam/sfm/MFAS.h> #include <gtsam/sfm/MFAS.h>
// Used in Matlab wrapper
class KeyPairDoubleMap { class KeyPairDoubleMap {
KeyPairDoubleMap(); KeyPairDoubleMap();
KeyPairDoubleMap(const gtsam::KeyPairDoubleMap& other); KeyPairDoubleMap(const gtsam::KeyPairDoubleMap& other);
@ -310,12 +290,9 @@ class TranslationRecovery {
const gtsam::BinaryMeasurementsUnit3& relativeTranslations, const gtsam::BinaryMeasurementsUnit3& relativeTranslations,
const double scale, const double scale,
const gtsam::BinaryMeasurementsPoint3& betweenTranslations) const; const gtsam::BinaryMeasurementsPoint3& betweenTranslations) const;
// default empty betweenTranslations
gtsam::Values run(const gtsam::BinaryMeasurementsUnit3& relativeTranslations,
const double scale) const;
// default scale = 1.0, empty betweenTranslations // default scale = 1.0, empty betweenTranslations
gtsam::Values run( gtsam::Values run(const gtsam::BinaryMeasurementsUnit3& relativeTranslations,
const gtsam::BinaryMeasurementsUnit3& relativeTranslations) const; const double scale = 1.0) const;
}; };
namespace gtsfm { namespace gtsfm {

5
gtsam/slam/slam.i
View File

@ -251,7 +251,7 @@ void save2D(const gtsam::NonlinearFactorGraph& graph,
string filename); string filename);
// std::vector<gtsam::BetweenFactor<Pose2>::shared_ptr> // std::vector<gtsam::BetweenFactor<Pose2>::shared_ptr>
// Ignored by pybind -> will be List[BetweenFactorPose2] // Used in Matlab wrapper
class BetweenFactorPose2s { class BetweenFactorPose2s {
BetweenFactorPose2s(); BetweenFactorPose2s();
size_t size() const; size_t size() const;
@ -261,13 +261,14 @@ class BetweenFactorPose2s {
gtsam::BetweenFactorPose2s parse2DFactors(string filename); gtsam::BetweenFactorPose2s parse2DFactors(string filename);
// std::vector<gtsam::BetweenFactor<Pose3>::shared_ptr> // std::vector<gtsam::BetweenFactor<Pose3>::shared_ptr>
// Ignored by pybind -> will be List[BetweenFactorPose3] // Used in Matlab wrapper
class BetweenFactorPose3s { class BetweenFactorPose3s {
BetweenFactorPose3s(); BetweenFactorPose3s();
size_t size() const; size_t size() const;
gtsam::BetweenFactor<gtsam::Pose3>* at(size_t i) const; gtsam::BetweenFactor<gtsam::Pose3>* at(size_t i) const;
void push_back(const gtsam::BetweenFactor<gtsam::Pose3>* factor); void push_back(const gtsam::BetweenFactor<gtsam::Pose3>* factor);
}; };
gtsam::BetweenFactorPose3s parse3DFactors(string filename); gtsam::BetweenFactorPose3s parse3DFactors(string filename);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load3D(string filename); pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load3D(string filename);

2
python/CMakeLists.txt
View File

@ -48,10 +48,12 @@ set(ignore
gtsam::ISAM2ThresholdMapValue gtsam::ISAM2ThresholdMapValue
gtsam::FactorIndices gtsam::FactorIndices
gtsam::FactorIndexSet gtsam::FactorIndexSet
gtsam::IndexPairSet
gtsam::IndexPairSetMap gtsam::IndexPairSetMap
gtsam::IndexPairVector gtsam::IndexPairVector
gtsam::BetweenFactorPose2s gtsam::BetweenFactorPose2s
gtsam::BetweenFactorPose3s gtsam::BetweenFactorPose3s
gtsam::FixedLagSmootherKeyTimestampMap
gtsam::FixedLagSmootherKeyTimestampMapValue gtsam::FixedLagSmootherKeyTimestampMapValue
gtsam::Point2Vector gtsam::Point2Vector
gtsam::Point2Pairs gtsam::Point2Pairs

7
python/gtsam/examples/FixedLagSmootherExample.py
View File

@ -16,6 +16,7 @@ import numpy as np
import gtsam import gtsam
import gtsam_unstable import gtsam_unstable
def BatchFixedLagSmootherExample(): def BatchFixedLagSmootherExample():
""" """
Runs a batch fixed smoother on an agent with two odometry Runs a batch fixed smoother on an agent with two odometry
@ -31,7 +32,7 @@ def BatchFixedLagSmootherExample():
# that will be sent to the smoothers # that will be sent to the smoothers
new_factors = gtsam.NonlinearFactorGraph() new_factors = gtsam.NonlinearFactorGraph()
new_values = gtsam.Values() new_values = gtsam.Values()
new_timestamps = gtsam.FixedLagSmootherKeyTimestampMap() new_timestamps = {}
# Create a prior on the first pose, placing it at the origin # Create a prior on the first pose, placing it at the origin
prior_mean = gtsam.Pose2(0, 0, 0) prior_mean = gtsam.Pose2(0, 0, 0)
@ -39,7 +40,7 @@ def BatchFixedLagSmootherExample():
X1 = 0 X1 = 0
new_factors.push_back(gtsam.PriorFactorPose2(X1, prior_mean, prior_noise)) new_factors.push_back(gtsam.PriorFactorPose2(X1, prior_mean, prior_noise))
new_values.insert(X1, prior_mean) new_values.insert(X1, prior_mean)
new_timestamps.insert((X1, 0.0)) new_timestamps[X1] = 0.0
delta_time = 0.25 delta_time = 0.25
time = 0.25 time = 0.25
@ -49,7 +50,7 @@ def BatchFixedLagSmootherExample():
current_key = int(1000 * time) current_key = int(1000 * time)
# assign current key to the current timestamp # assign current key to the current timestamp
new_timestamps.insert((current_key, time)) new_timestamps[current_key] = time
# Add a guess for this pose to the new values # Add a guess for this pose to the new values
# Assume that the robot moves at 2 m/s. Position is time[s] * 2[m/s] # Assume that the robot moves at 2 m/s. Position is time[s] * 2[m/s]

11
python/gtsam/examples/TranslationAveragingExample.py
View File

@ -17,9 +17,8 @@ Date: September 2020
from collections import defaultdict from collections import defaultdict
from typing import List, Tuple from typing import List, Tuple
import numpy as np
import gtsam import gtsam
import numpy as np
from gtsam.examples import SFMdata from gtsam.examples import SFMdata
# Hyperparameters for 1dsfm, values used from Kyle Wilson's code. # Hyperparameters for 1dsfm, values used from Kyle Wilson's code.
@ -59,7 +58,7 @@ def get_data() -> Tuple[gtsam.Values, List[gtsam.BinaryMeasurementUnit3]]:
return wRc_values, i_iZj_list return wRc_values, i_iZj_list
def filter_outliers(w_iZj_list: gtsam.BinaryMeasurementsUnit3) -> gtsam.BinaryMeasurementsUnit3: def filter_outliers(w_iZj_list: List[gtsam.BinaryMeasurementUnit3]) -> List[gtsam.BinaryMeasurementUnit3]:
"""Removes outliers from a list of Unit3 measurements that are the """Removes outliers from a list of Unit3 measurements that are the
translation directions from camera i to camera j in the world frame.""" translation directions from camera i to camera j in the world frame."""
@ -89,14 +88,14 @@ def filter_outliers(w_iZj_list: gtsam.BinaryMeasurementsUnit3) -> gtsam.BinaryMe
avg_outlier_weights[keypair] += weight / len(outlier_weights) avg_outlier_weights[keypair] += weight / len(outlier_weights)
# Remove w_iZj that have weight greater than threshold, these are outliers. # Remove w_iZj that have weight greater than threshold, these are outliers.
w_iZj_inliers = gtsam.BinaryMeasurementsUnit3() w_iZj_inliers = []
[w_iZj_inliers.append(w_iZj) for w_iZj in w_iZj_list if avg_outlier_weights[( [w_iZj_inliers.append(w_iZj) for w_iZj in w_iZj_list if avg_outlier_weights[(
w_iZj.key1(), w_iZj.key2())] < OUTLIER_WEIGHT_THRESHOLD] w_iZj.key1(), w_iZj.key2())] < OUTLIER_WEIGHT_THRESHOLD]
return w_iZj_inliers return w_iZj_inliers
def estimate_poses(i_iZj_list: gtsam.BinaryMeasurementsUnit3, def estimate_poses(i_iZj_list: List[gtsam.BinaryMeasurementUnit3],
wRc_values: gtsam.Values) -> gtsam.Values: wRc_values: gtsam.Values) -> gtsam.Values:
"""Estimate poses given rotations and normalized translation directions between cameras. """Estimate poses given rotations and normalized translation directions between cameras.
@ -112,7 +111,7 @@ def estimate_poses(i_iZj_list: gtsam.BinaryMeasurementsUnit3,
""" """
# Convert the translation direction measurements to world frame using the rotations. # Convert the translation direction measurements to world frame using the rotations.
w_iZj_list = gtsam.BinaryMeasurementsUnit3() w_iZj_list = []
for i_iZj in i_iZj_list: for i_iZj in i_iZj_list:
w_iZj = gtsam.Unit3(wRc_values.atRot3(i_iZj.key1()) w_iZj = gtsam.Unit3(wRc_values.atRot3(i_iZj.key1())
.rotate(i_iZj.measured().point3())) .rotate(i_iZj.measured().point3()))

1
python/gtsam/gtsam.tpl
View File

@ -11,6 +11,7 @@
#include <pybind11/eigen.h> #include <pybind11/eigen.h>
#include <pybind11/stl_bind.h> #include <pybind11/stl_bind.h>
#include <pybind11/stl.h>
#include <pybind11/pybind11.h> #include <pybind11/pybind11.h>
#include <pybind11/operators.h> #include <pybind11/operators.h>
#include <pybind11/functional.h> #include <pybind11/functional.h>

6
python/gtsam/preamble/base.h
View File

@ -10,9 +10,3 @@
* Without this they will be automatically converted to a Python object, and all * Without this they will be automatically converted to a Python object, and all
* mutations on Python side will not be reflected on C++. * mutations on Python side will not be reflected on C++.
*/ */
PYBIND11_MAKE_OPAQUE(gtsam::IndexPairVector);
PYBIND11_MAKE_OPAQUE(gtsam::IndexPairSet);
PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Matrix>); // JacobianVector

2
python/gtsam/preamble/basis.h
View File

@ -10,5 +10,3 @@
* Without this they will be automatically converted to a Python object, and all * Without this they will be automatically converted to a Python object, and all
* mutations on Python side will not be reflected on C++. * mutations on Python side will not be reflected on C++.
*/ */
#include <pybind11/stl.h>

3
python/gtsam/preamble/custom.h
View File

@ -10,3 +10,6 @@
* Without this they will be automatically converted to a Python object, and all * Without this they will be automatically converted to a Python object, and all
* mutations on Python side will not be reflected on C++. * mutations on Python side will not be reflected on C++.
*/ */
// Added so that CustomFactor can pass the JacobianVector to the C++ side
PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Matrix>); // JacobianVector

3
python/gtsam/preamble/discrete.h
View File

@ -11,5 +11,4 @@
* mutations on Python side will not be reflected on C++. * mutations on Python side will not be reflected on C++.
*/ */
#include <pybind11/stl.h> PYBIND11_MAKE_OPAQUE(gtsam::DiscreteKeys);

12
python/gtsam/preamble/geometry.h
View File

@ -10,14 +10,8 @@
* Without this they will be automatically converted to a Python object, and all * Without this they will be automatically converted to a Python object, and all
* mutations on Python side will not be reflected on C++. * mutations on Python side will not be reflected on C++.
*/ */
#include <pybind11/stl.h>
PYBIND11_MAKE_OPAQUE(
std::vector<gtsam::Point2, Eigen::aligned_allocator<gtsam::Point2>>);
PYBIND11_MAKE_OPAQUE(gtsam::Point2Pairs);
PYBIND11_MAKE_OPAQUE(gtsam::Point3Pairs);
PYBIND11_MAKE_OPAQUE(gtsam::Pose2Pairs);
PYBIND11_MAKE_OPAQUE(gtsam::Pose3Pairs);
PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Pose3>);
PYBIND11_MAKE_OPAQUE(gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Bundler>>);
PYBIND11_MAKE_OPAQUE(gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3_S2>>); PYBIND11_MAKE_OPAQUE(gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3_S2>>);
PYBIND11_MAKE_OPAQUE(gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Bundler>>);
PYBIND11_MAKE_OPAQUE(gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Unified>>);
PYBIND11_MAKE_OPAQUE(gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Fisheye>>);

5
python/gtsam/preamble/gtsam.h
View File

@ -10,8 +10,3 @@
* Without this they will be automatically converted to a Python object, and all * Without this they will be automatically converted to a Python object, and all
* mutations on Python side will not be reflected on C++. * mutations on Python side will not be reflected on C++.
*/ */
#ifdef GTSAM_ALLOCATOR_TBB
PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Key, tbb::tbb_allocator<gtsam::Key>>);
#else
PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Key>);
#endif

8
python/gtsam/preamble/hybrid.h
View File

@ -10,11 +10,3 @@
* Without this they will be automatically converted to a Python object, and all * Without this they will be automatically converted to a Python object, and all
* mutations on Python side will not be reflected on C++. * mutations on Python side will not be reflected on C++.
*/ */
#include <pybind11/stl.h>
// NOTE: Needed since we are including pybind11/stl.h.
#ifdef GTSAM_ALLOCATOR_TBB
PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Key, tbb::tbb_allocator<gtsam::Key>>);
#else
PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Key>);
#endif

14
python/gtsam/preamble/sfm.h
View File

@ -11,17 +11,3 @@
* mutations on Python side will not be reflected on C++. * mutations on Python side will not be reflected on C++.
*/ */
// Including <stl.h> can cause some serious hard-to-debug bugs!!!
// #include <pybind11/stl.h>
#include <pybind11/stl_bind.h>
PYBIND11_MAKE_OPAQUE(std::vector<gtsam::SfmMeasurement>);
PYBIND11_MAKE_OPAQUE(std::vector<gtsam::SfmTrack>);
PYBIND11_MAKE_OPAQUE(std::vector<gtsam::SfmCamera>);
PYBIND11_MAKE_OPAQUE(
std::vector<gtsam::BinaryMeasurement<gtsam::Unit3>>);
PYBIND11_MAKE_OPAQUE(
std::vector<gtsam::BinaryMeasurement<gtsam::Rot3>>);
PYBIND11_MAKE_OPAQUE(
std::vector<gtsam::gtsfm::Keypoints>);
PYBIND11_MAKE_OPAQUE(gtsam::gtsfm::MatchIndicesMap);

6
python/gtsam/preamble/slam.h
View File

@ -10,9 +10,3 @@
* Without this they will be automatically converted to a Python object, and all * Without this they will be automatically converted to a Python object, and all
* mutations on Python side will not be reflected on C++. * mutations on Python side will not be reflected on C++.
*/ */
PYBIND11_MAKE_OPAQUE(
std::vector<std::shared_ptr<gtsam::BetweenFactor<gtsam::Pose3> > >);
PYBIND11_MAKE_OPAQUE(
std::vector<std::shared_ptr<gtsam::BetweenFactor<gtsam::Pose2> > >);
PYBIND11_MAKE_OPAQUE(gtsam::Rot3Vector);

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

@ -11,7 +11,3 @@
* and saves one copy operation. * and saves one copy operation.
*/ */
py::bind_map<gtsam::IndexPairSetMap>(m_, "IndexPairSetMap");
py::bind_vector<gtsam::IndexPairVector>(m_, "IndexPairVector");
py::bind_vector<std::vector<gtsam::Matrix> >(m_, "JacobianVector");

3
python/gtsam/specializations/custom.h
View File

@ -10,3 +10,6 @@
* with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python, * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python,
* and saves one copy operation. * and saves one copy operation.
*/ */
// Added so that CustomFactor can pass the JacobianVector to the C++ side
py::bind_vector<std::vector<gtsam::Matrix> >(m_, "JacobianVector");

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

@ -11,14 +11,6 @@
* and saves one copy operation. * and saves one copy operation.
*/ */
py::bind_vector<
std::vector<gtsam::Point2, Eigen::aligned_allocator<gtsam::Point2>>>(
m_, "Point2Vector");
py::bind_vector<std::vector<gtsam::Point2Pair>>(m_, "Point2Pairs");
py::bind_vector<std::vector<gtsam::Point3Pair>>(m_, "Point3Pairs");
py::bind_vector<std::vector<gtsam::Pose2Pair>>(m_, "Pose2Pairs");
py::bind_vector<std::vector<gtsam::Pose3Pair>>(m_, "Pose3Pairs");
py::bind_vector<std::vector<gtsam::Pose3>>(m_, "Pose3Vector");
py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3_S2>>>( py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3_S2>>>(
m_, "CameraSetCal3_S2"); m_, "CameraSetCal3_S2");
py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3DS2>>>( py::bind_vector<gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3DS2>>>(

8
python/gtsam/specializations/gtsam.h
View File

@ -10,11 +10,3 @@
* with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python, * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python,
* and saves one copy operation. * and saves one copy operation.
*/ */
#ifdef GTSAM_ALLOCATOR_TBB
py::bind_vector<std::vector<gtsam::Key, tbb::tbb_allocator<gtsam::Key> > >(m_, "KeyVector");
py::implicitly_convertible<py::list, std::vector<gtsam::Key, tbb::tbb_allocator<gtsam::Key> > >();
#else
py::bind_vector<std::vector<gtsam::Key> >(m_, "KeyVector");
py::implicitly_convertible<py::list, std::vector<gtsam::Key> >();
#endif

2
python/gtsam/specializations/inference.h
View File

@ -10,5 +10,3 @@
* with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python, * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python,
* and saves one copy operation. * and saves one copy operation.
*/ */
py::bind_map<std::map<char, double>>(m_, "__MapCharDouble");

15
python/gtsam/specializations/sfm.h
View File

@ -11,18 +11,3 @@
* and saves one copy operation. * and saves one copy operation.
*/ */
py::bind_vector<std::vector<gtsam::BinaryMeasurement<gtsam::Point3> > >(
m_, "BinaryMeasurementsPoint3");
py::bind_vector<std::vector<gtsam::BinaryMeasurement<gtsam::Unit3> > >(
m_, "BinaryMeasurementsUnit3");
py::bind_vector<std::vector<gtsam::BinaryMeasurement<gtsam::Rot3> > >(
m_, "BinaryMeasurementsRot3");
py::bind_map<gtsam::KeyPairDoubleMap>(m_, "KeyPairDoubleMap");
py::bind_vector<std::vector<gtsam::SfmTrack2d>>(m_, "SfmTrack2dVector");
py::bind_vector<std::vector<gtsam::SfmTrack>>(m_, "SfmTracks");
py::bind_vector<std::vector<gtsam::SfmCamera>>(m_, "SfmCameras");
py::bind_vector<std::vector<std::pair<size_t, gtsam::Point2>>>(
m_, "SfmMeasurementVector");
py::bind_map<gtsam::gtsfm::MatchIndicesMap>(m_, "MatchIndicesMap");
py::bind_vector<std::vector<gtsam::gtsfm::Keypoints>>(m_, "KeypointsVector");

8
python/gtsam/specializations/slam.h
View File

@ -10,11 +10,3 @@
* with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python, * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python,
* and saves one copy operation. * and saves one copy operation.
*/ */
py::bind_vector<
std::vector<std::shared_ptr<gtsam::BetweenFactor<gtsam::Pose3>>>>(
m_, "BetweenFactorPose3s");
py::bind_vector<
std::vector<std::shared_ptr<gtsam::BetweenFactor<gtsam::Pose2>>>>(
m_, "BetweenFactorPose2s");
py::bind_vector<gtsam::Rot3Vector>(m_, "Rot3Vector");

13
python/gtsam/tests/test_Cal3Fisheye.py
View File

@ -11,11 +11,10 @@ Refactored: Roderick Koehle
""" """
import unittest import unittest
import numpy as np
import gtsam import gtsam
from gtsam.utils.test_case import GtsamTestCase import numpy as np
from gtsam.symbol_shorthand import K, L, P from gtsam.symbol_shorthand import K, L, P
from gtsam.utils.test_case import GtsamTestCase
def ulp(ftype=np.float64): def ulp(ftype=np.float64):
@ -51,9 +50,9 @@ class TestCal3Fisheye(GtsamTestCase):
camera1 = gtsam.PinholeCameraCal3Fisheye(pose1) camera1 = gtsam.PinholeCameraCal3Fisheye(pose1)
camera2 = gtsam.PinholeCameraCal3Fisheye(pose2) camera2 = gtsam.PinholeCameraCal3Fisheye(pose2)
cls.origin = np.array([0.0, 0.0, 0.0]) cls.origin = np.array([0.0, 0.0, 0.0])
cls.poses = gtsam.Pose3Vector([pose1, pose2]) cls.poses = [pose1, pose2]
cls.cameras = gtsam.CameraSetCal3Fisheye([camera1, camera2]) cls.cameras = [camera1, camera2]
cls.measurements = gtsam.Point2Vector([k.project(cls.origin) for k in cls.cameras]) cls.measurements = [k.project(cls.origin) for k in cls.cameras]
def test_Cal3Fisheye(self): def test_Cal3Fisheye(self):
K = gtsam.Cal3Fisheye() K = gtsam.Cal3Fisheye()
@ -187,7 +186,7 @@ class TestCal3Fisheye(GtsamTestCase):
def test_triangulation_rectify(self): def test_triangulation_rectify(self):
"""Estimate spatial point from image measurements using rectification""" """Estimate spatial point from image measurements using rectification"""
rectified = gtsam.Point2Vector([k.calibration().calibrate(pt) for k, pt in zip(self.cameras, self.measurements)]) rectified = [k.calibration().calibrate(pt) for k, pt in zip(self.cameras, self.measurements)]
shared_cal = gtsam.Cal3_S2() shared_cal = gtsam.Cal3_S2()
triangulated = gtsam.triangulatePoint3(self.poses, shared_cal, rectified, rank_tol=1e-9, optimize=False) triangulated = gtsam.triangulatePoint3(self.poses, shared_cal, rectified, rank_tol=1e-9, optimize=False)
self.gtsamAssertEquals(triangulated, self.origin) self.gtsamAssertEquals(triangulated, self.origin)

13
python/gtsam/tests/test_Cal3Unified.py
View File

@ -10,11 +10,10 @@ Author: Frank Dellaert & Duy Nguyen Ta (Python)
""" """
import unittest import unittest
import numpy as np
import gtsam import gtsam
from gtsam.utils.test_case import GtsamTestCase import numpy as np
from gtsam.symbol_shorthand import K, L, P from gtsam.symbol_shorthand import K, L, P
from gtsam.utils.test_case import GtsamTestCase
class TestCal3Unified(GtsamTestCase): class TestCal3Unified(GtsamTestCase):
@ -48,9 +47,9 @@ class TestCal3Unified(GtsamTestCase):
camera1 = gtsam.PinholeCameraCal3Unified(pose1, cls.stereographic) camera1 = gtsam.PinholeCameraCal3Unified(pose1, cls.stereographic)
camera2 = gtsam.PinholeCameraCal3Unified(pose2, cls.stereographic) camera2 = gtsam.PinholeCameraCal3Unified(pose2, cls.stereographic)
cls.origin = np.array([0.0, 0.0, 0.0]) cls.origin = np.array([0.0, 0.0, 0.0])
cls.poses = gtsam.Pose3Vector([pose1, pose2]) cls.poses = [pose1, pose2]
cls.cameras = gtsam.CameraSetCal3Unified([camera1, camera2]) cls.cameras = [camera1, camera2]
cls.measurements = gtsam.Point2Vector([k.project(cls.origin) for k in cls.cameras]) cls.measurements = [k.project(cls.origin) for k in cls.cameras]
def test_Cal3Unified(self): def test_Cal3Unified(self):
K = gtsam.Cal3Unified() K = gtsam.Cal3Unified()
@ -158,7 +157,7 @@ class TestCal3Unified(GtsamTestCase):
def test_triangulation_rectify(self): def test_triangulation_rectify(self):
"""Estimate spatial point from image measurements using rectification""" """Estimate spatial point from image measurements using rectification"""
rectified = gtsam.Point2Vector([k.calibration().calibrate(pt) for k, pt in zip(self.cameras, self.measurements)]) rectified = [k.calibration().calibrate(pt) for k, pt in zip(self.cameras, self.measurements)]
shared_cal = gtsam.Cal3_S2() shared_cal = gtsam.Cal3_S2()
triangulated = gtsam.triangulatePoint3(self.poses, shared_cal, rectified, rank_tol=1e-9, optimize=False) triangulated = gtsam.triangulatePoint3(self.poses, shared_cal, rectified, rank_tol=1e-9, optimize=False)
self.gtsamAssertEquals(triangulated, self.origin) self.gtsamAssertEquals(triangulated, self.origin)

3
python/gtsam/tests/test_DecisionTreeFactor.py
View File

@ -13,7 +13,8 @@ Author: Frank Dellaert
import unittest import unittest
from gtsam import DecisionTreeFactor, DiscreteValues, DiscreteDistribution, Ordering from gtsam import (DecisionTreeFactor, DiscreteDistribution, DiscreteValues,
Ordering)
from gtsam.utils.test_case import GtsamTestCase from gtsam.utils.test_case import GtsamTestCase

12
python/gtsam/tests/test_DsfTrackGenerator.py
View File

@ -5,13 +5,13 @@ Authors: John Lambert
import unittest import unittest
import gtsam
import numpy as np import numpy as np
from gtsam import (IndexPair, KeypointsVector, MatchIndicesMap, Point2,
SfmMeasurementVector, SfmTrack2d)
from gtsam.gtsfm import Keypoints from gtsam.gtsfm import Keypoints
from gtsam.utils.test_case import GtsamTestCase from gtsam.utils.test_case import GtsamTestCase
import gtsam
from gtsam import IndexPair, Point2, SfmTrack2d
class TestDsfTrackGenerator(GtsamTestCase): class TestDsfTrackGenerator(GtsamTestCase):
"""Tests for DsfTrackGenerator.""" """Tests for DsfTrackGenerator."""
@ -23,14 +23,14 @@ class TestDsfTrackGenerator(GtsamTestCase):
kps_i1 = Keypoints(np.array([[50.0, 60], [70, 80], [90, 100]])) kps_i1 = Keypoints(np.array([[50.0, 60], [70, 80], [90, 100]]))
kps_i2 = Keypoints(np.array([[110.0, 120], [130, 140]])) kps_i2 = Keypoints(np.array([[110.0, 120], [130, 140]]))
keypoints_list = KeypointsVector() keypoints_list = []
keypoints_list.append(kps_i0) keypoints_list.append(kps_i0)
keypoints_list.append(kps_i1) keypoints_list.append(kps_i1)
keypoints_list.append(kps_i2) keypoints_list.append(kps_i2)
# For each image pair (i1,i2), we provide a (K,2) matrix # For each image pair (i1,i2), we provide a (K,2) matrix
# of corresponding image indices (k1,k2). # of corresponding image indices (k1,k2).
matches_dict = MatchIndicesMap() matches_dict = {}
matches_dict[IndexPair(0, 1)] = np.array([[0, 0], [1, 1]]) matches_dict[IndexPair(0, 1)] = np.array([[0, 0], [1, 1]])
matches_dict[IndexPair(1, 2)] = np.array([[2, 0], [1, 1]]) matches_dict[IndexPair(1, 2)] = np.array([[2, 0], [1, 1]])
@ -86,7 +86,7 @@ class TestSfmTrack2d(GtsamTestCase):
def test_sfm_track_2d_constructor(self) -> None: def test_sfm_track_2d_constructor(self) -> None:
"""Test construction of 2D SfM track.""" """Test construction of 2D SfM track."""
measurements = SfmMeasurementVector() measurements = []
measurements.append((0, Point2(10, 20))) measurements.append((0, Point2(10, 20)))
track = SfmTrack2d(measurements=measurements) track = SfmTrack2d(measurements=measurements)
track.measurement(0) track.measurement(0)

9
python/gtsam/tests/test_FixedLagSmootherExample.py
View File

@ -11,10 +11,11 @@ Author: Frank Dellaert & Duy Nguyen Ta (Python)
import unittest import unittest
import numpy as np import numpy as np
from gtsam.utils.test_case import GtsamTestCase
import gtsam import gtsam
import gtsam_unstable import gtsam_unstable
from gtsam.utils.test_case import GtsamTestCase
class TestFixedLagSmootherExample(GtsamTestCase): class TestFixedLagSmootherExample(GtsamTestCase):
''' '''
@ -36,7 +37,7 @@ class TestFixedLagSmootherExample(GtsamTestCase):
# that will be sent to the smoothers # that will be sent to the smoothers
new_factors = gtsam.NonlinearFactorGraph() new_factors = gtsam.NonlinearFactorGraph()
new_values = gtsam.Values() new_values = gtsam.Values()
new_timestamps = gtsam.FixedLagSmootherKeyTimestampMap() new_timestamps = {}
# Create a prior on the first pose, placing it at the origin # Create a prior on the first pose, placing it at the origin
prior_mean = gtsam.Pose2(0, 0, 0) prior_mean = gtsam.Pose2(0, 0, 0)
@ -47,7 +48,7 @@ class TestFixedLagSmootherExample(GtsamTestCase):
gtsam.PriorFactorPose2( gtsam.PriorFactorPose2(
X1, prior_mean, prior_noise)) X1, prior_mean, prior_noise))
new_values.insert(X1, prior_mean) new_values.insert(X1, prior_mean)
new_timestamps.insert((X1, 0.0)) new_timestamps[X1] = 0.0
delta_time = 0.25 delta_time = 0.25
time = 0.25 time = 0.25
@ -77,7 +78,7 @@ class TestFixedLagSmootherExample(GtsamTestCase):
current_key = int(1000 * time) current_key = int(1000 * time)
# assign current key to the current timestamp # assign current key to the current timestamp
new_timestamps.insert((current_key, time)) new_timestamps[current_key] = time
# Add a guess for this pose to the new values # Add a guess for this pose to the new values
# Assume that the robot moves at 2 m/s. Position is time[s] * # Assume that the robot moves at 2 m/s. Position is time[s] *

2
python/gtsam/tests/test_GaussianFactorGraph.py
View File

@ -98,7 +98,7 @@ class TestGaussianFactorGraph(GtsamTestCase):
bn = gfg.eliminateSequential(ordering) bn = gfg.eliminateSequential(ordering)
self.assertEqual(bn.size(), 3) self.assertEqual(bn.size(), 3)
keyVector = gtsam.KeyVector() keyVector = []
keyVector.append(keys[2]) keyVector.append(keys[2])
#TODO(Varun) Below code causes segfault in Debug config #TODO(Varun) Below code causes segfault in Debug config
# ordering = gtsam.Ordering.ColamdConstrainedLastGaussianFactorGraph(gfg, keyVector) # ordering = gtsam.Ordering.ColamdConstrainedLastGaussianFactorGraph(gfg, keyVector)

17
python/gtsam/tests/test_HybridBayesNet.py
View File

@ -17,8 +17,9 @@ import numpy as np
from gtsam.symbol_shorthand import A, X from gtsam.symbol_shorthand import A, X
from gtsam.utils.test_case import GtsamTestCase from gtsam.utils.test_case import GtsamTestCase
from gtsam import (DiscreteConditional, DiscreteKeys, DiscreteValues, GaussianConditional, from gtsam import (DiscreteConditional, DiscreteKeys, DiscreteValues,
GaussianMixture, HybridBayesNet, HybridValues, noiseModel, VectorValues) GaussianConditional, GaussianMixture, HybridBayesNet,
HybridValues, VectorValues, noiseModel)
class TestHybridBayesNet(GtsamTestCase): class TestHybridBayesNet(GtsamTestCase):
@ -31,8 +32,8 @@ class TestHybridBayesNet(GtsamTestCase):
# Create the continuous conditional # Create the continuous conditional
I_1x1 = np.eye(1) I_1x1 = np.eye(1)
conditional = GaussianConditional.FromMeanAndStddev(X(0), 2 * I_1x1, X(1), [-4], conditional = GaussianConditional.FromMeanAndStddev(
5.0) X(0), 2 * I_1x1, X(1), [-4], 5.0)
# Create the noise models # Create the noise models
model0 = noiseModel.Diagonal.Sigmas([2.0]) model0 = noiseModel.Diagonal.Sigmas([2.0])
@ -47,8 +48,9 @@ class TestHybridBayesNet(GtsamTestCase):
# Create hybrid Bayes net. # Create hybrid Bayes net.
bayesNet = HybridBayesNet() bayesNet = HybridBayesNet()
bayesNet.push_back(conditional) bayesNet.push_back(conditional)
bayesNet.push_back(GaussianMixture( bayesNet.push_back(
[X(1)], [], discrete_keys, [conditional0, conditional1])) GaussianMixture([X(1)], [], discrete_keys,
[conditional0, conditional1]))
bayesNet.push_back(DiscreteConditional(Asia, "99/1")) bayesNet.push_back(DiscreteConditional(Asia, "99/1"))
# Create values at which to evaluate. # Create values at which to evaluate.
@ -89,7 +91,8 @@ class TestHybridBayesNet(GtsamTestCase):
self.assertTrue(probability >= 0.0) self.assertTrue(probability >= 0.0)
logProb = conditional.logProbability(values) logProb = conditional.logProbability(values)
self.assertAlmostEqual(probability, np.exp(logProb)) self.assertAlmostEqual(probability, np.exp(logProb))
expected = conditional.logNormalizationConstant() - conditional.error(values) expected = conditional.logNormalizationConstant() - \
conditional.error(values)
self.assertAlmostEqual(logProb, expected) self.assertAlmostEqual(logProb, expected)

11
python/gtsam/tests/test_KarcherMeanFactor.py
View File

@ -31,7 +31,7 @@ class TestKarcherMean(GtsamTestCase):
def test_find(self): def test_find(self):
# Check that optimizing for Karcher mean (which minimizes Between distance) # Check that optimizing for Karcher mean (which minimizes Between distance)
# gets correct result. # gets correct result.
rotations = gtsam.Rot3Vector([R, R.inverse()]) rotations = [R, R.inverse()]
expected = Rot3() expected = Rot3()
actual = gtsam.FindKarcherMean(rotations) actual = gtsam.FindKarcherMean(rotations)
self.gtsamAssertEquals(expected, actual) self.gtsamAssertEquals(expected, actual)
@ -42,7 +42,7 @@ class TestKarcherMean(GtsamTestCase):
a2Rb2 = Rot3() a2Rb2 = Rot3()
a3Rb3 = Rot3() a3Rb3 = Rot3()
aRb_list = gtsam.Rot3Vector([a1Rb1, a2Rb2, a3Rb3]) aRb_list = [a1Rb1, a2Rb2, a3Rb3]
aRb_expected = Rot3() aRb_expected = Rot3()
aRb = gtsam.FindKarcherMean(aRb_list) aRb = gtsam.FindKarcherMean(aRb_list)
@ -58,9 +58,7 @@ class TestKarcherMean(GtsamTestCase):
graph = gtsam.NonlinearFactorGraph() graph = gtsam.NonlinearFactorGraph()
R12 = R.compose(R.compose(R)) R12 = R.compose(R.compose(R))
graph.add(gtsam.BetweenFactorRot3(1, 2, R12, MODEL)) graph.add(gtsam.BetweenFactorRot3(1, 2, R12, MODEL))
keys = gtsam.KeyVector() keys = [1, 2]
keys.append(1)
keys.append(2)
graph.add(gtsam.KarcherMeanFactorRot3(keys)) graph.add(gtsam.KarcherMeanFactorRot3(keys))
initial = gtsam.Values() initial = gtsam.Values()
@ -69,8 +67,7 @@ class TestKarcherMean(GtsamTestCase):
expected = Rot3() expected = Rot3()
result = gtsam.GaussNewtonOptimizer(graph, initial).optimize() result = gtsam.GaussNewtonOptimizer(graph, initial).optimize()
actual = gtsam.FindKarcherMean( actual = gtsam.FindKarcherMean([result.atRot3(1), result.atRot3(2)])
gtsam.Rot3Vector([result.atRot3(1), result.atRot3(2)]))
self.gtsamAssertEquals(expected, actual) self.gtsamAssertEquals(expected, actual)
self.gtsamAssertEquals( self.gtsamAssertEquals(
R12, result.atRot3(1).between(result.atRot3(2))) R12, result.atRot3(1).between(result.atRot3(2)))

5
python/gtsam/tests/test_Pose2.py
View File

@ -11,9 +11,8 @@ Author: Frank Dellaert & Duy Nguyen Ta & John Lambert
import math import math
import unittest import unittest
import gtsam
import numpy as np import numpy as np
from gtsam import Point2, Point2Pairs, Pose2 from gtsam import Point2, Pose2
from gtsam.utils.test_case import GtsamTestCase from gtsam.utils.test_case import GtsamTestCase
@ -83,7 +82,7 @@ class TestPose2(GtsamTestCase):
] ]
# fmt: on # fmt: on
ab_pairs = Point2Pairs(list(zip(pts_a, pts_b))) ab_pairs = list(zip(pts_a, pts_b))
aTb = Pose2.Align(ab_pairs) aTb = Pose2.Align(ab_pairs)
self.assertIsNotNone(aTb) self.assertIsNotNone(aTb)

7
python/gtsam/tests/test_Pose3.py
View File

@ -12,10 +12,9 @@ Author: Frank Dellaert, Duy Nguyen Ta
import math import math
import unittest import unittest
import numpy as np
import gtsam import gtsam
from gtsam import Point3, Pose3, Rot3, Point3Pairs import numpy as np
from gtsam import Point3, Pose3, Rot3
from gtsam.utils.test_case import GtsamTestCase from gtsam.utils.test_case import GtsamTestCase
@ -223,7 +222,7 @@ class TestPose3(GtsamTestCase):
sTt = Pose3(Rot3.Rodrigues(0, 0, -math.pi), Point3(2, 4, 0)) sTt = Pose3(Rot3.Rodrigues(0, 0, -math.pi), Point3(2, 4, 0))
transformed = sTt.transformTo(square) transformed = sTt.transformTo(square)
st_pairs = Point3Pairs() st_pairs = []
for j in range(4): for j in range(4):
st_pairs.append((square[:,j], transformed[:,j])) st_pairs.append((square[:,j], transformed[:,j]))

15
python/gtsam/tests/test_ShonanAveraging.py
View File

@ -14,16 +14,9 @@ import unittest
import gtsam import gtsam
import numpy as np import numpy as np
from gtsam import ( from gtsam import (BetweenFactorPose2, LevenbergMarquardtParams, Pose2, Rot2,
BetweenFactorPose2, ShonanAveraging2, ShonanAveraging3,
LevenbergMarquardtParams, ShonanAveragingParameters2, ShonanAveragingParameters3)
Rot2,
Pose2,
ShonanAveraging2,
ShonanAveragingParameters2,
ShonanAveraging3,
ShonanAveragingParameters3,
)
from gtsam.utils.test_case import GtsamTestCase from gtsam.utils.test_case import GtsamTestCase
DEFAULT_PARAMS = ShonanAveragingParameters3( DEFAULT_PARAMS = ShonanAveragingParameters3(
@ -183,7 +176,7 @@ class TestShonanAveraging(GtsamTestCase):
shonan_params.setCertifyOptimality(True) shonan_params.setCertifyOptimality(True)
noise_model = gtsam.noiseModel.Unit.Create(3) noise_model = gtsam.noiseModel.Unit.Create(3)
between_factors = gtsam.BetweenFactorPose2s() between_factors = []
for (i1, i2), i2Ri1 in i2Ri1_dict.items(): for (i1, i2), i2Ri1 in i2Ri1_dict.items():
i2Ti1 = Pose2(i2Ri1, np.zeros(2)) i2Ti1 = Pose2(i2Ri1, np.zeros(2))
between_factors.append( between_factors.append(

10
python/gtsam/tests/test_Sim2.py
View File

@ -12,7 +12,7 @@ Author: John Lambert
import unittest import unittest
import numpy as np import numpy as np
from gtsam import Pose2, Pose2Pairs, Rot2, Similarity2 from gtsam import Pose2, Rot2, Similarity2
from gtsam.utils.test_case import GtsamTestCase from gtsam.utils.test_case import GtsamTestCase
@ -20,7 +20,7 @@ class TestSim2(GtsamTestCase):
"""Test selected Sim2 methods.""" """Test selected Sim2 methods."""
def test_align_poses_along_straight_line(self) -> None: def test_align_poses_along_straight_line(self) -> None:
"""Test Align Pose2Pairs method. """Test Align of list of Pose2Pair.
Scenario: Scenario:
3 object poses 3 object poses
@ -46,7 +46,7 @@ class TestSim2(GtsamTestCase):
wToi_list = [wTo0, wTo1, wTo2] wToi_list = [wTo0, wTo1, wTo2]
we_pairs = Pose2Pairs(list(zip(wToi_list, eToi_list))) we_pairs = list(zip(wToi_list, eToi_list))
# Recover the transformation wSe (i.e. world_S_egovehicle) # Recover the transformation wSe (i.e. world_S_egovehicle)
wSe = Similarity2.Align(we_pairs) wSe = Similarity2.Align(we_pairs)
@ -81,7 +81,7 @@ class TestSim2(GtsamTestCase):
wToi_list = [wTo0, wTo1, wTo2] wToi_list = [wTo0, wTo1, wTo2]
we_pairs = Pose2Pairs(list(zip(wToi_list, eToi_list))) we_pairs = list(zip(wToi_list, eToi_list))
# Recover the transformation wSe (i.e. world_S_egovehicle) # Recover the transformation wSe (i.e. world_S_egovehicle)
wSe = Similarity2.Align(we_pairs) wSe = Similarity2.Align(we_pairs)
@ -119,7 +119,7 @@ class TestSim2(GtsamTestCase):
bTi_list = [bTi0, bTi1, bTi2, bTi3] bTi_list = [bTi0, bTi1, bTi2, bTi3]
ab_pairs = Pose2Pairs(list(zip(aTi_list, bTi_list))) ab_pairs = list(zip(aTi_list, bTi_list))
# Recover the transformation wSe (i.e. world_S_egovehicle) # Recover the transformation wSe (i.e. world_S_egovehicle)
aSb = Similarity2.Align(ab_pairs) aSb = Similarity2.Align(ab_pairs)

13
python/gtsam/tests/test_Sim3.py
View File

@ -12,10 +12,9 @@ Author: John Lambert
import unittest import unittest
from typing import List, Optional from typing import List, Optional
import numpy as np
import gtsam import gtsam
from gtsam import Point3, Pose3, Pose3Pairs, Rot3, Similarity3 import numpy as np
from gtsam import Point3, Pose3, Rot3, Similarity3
from gtsam.utils.test_case import GtsamTestCase from gtsam.utils.test_case import GtsamTestCase
@ -49,7 +48,7 @@ class TestSim3(GtsamTestCase):
wToi_list = [wTo0, wTo1, wTo2] wToi_list = [wTo0, wTo1, wTo2]
we_pairs = Pose3Pairs(list(zip(wToi_list, eToi_list))) we_pairs = list(zip(wToi_list, eToi_list))
# Recover the transformation wSe (i.e. world_S_egovehicle) # Recover the transformation wSe (i.e. world_S_egovehicle)
wSe = Similarity3.Align(we_pairs) wSe = Similarity3.Align(we_pairs)
@ -84,7 +83,7 @@ class TestSim3(GtsamTestCase):
wToi_list = [wTo0, wTo1, wTo2] wToi_list = [wTo0, wTo1, wTo2]
we_pairs = Pose3Pairs(list(zip(wToi_list, eToi_list))) we_pairs = list(zip(wToi_list, eToi_list))
# Recover the transformation wSe (i.e. world_S_egovehicle) # Recover the transformation wSe (i.e. world_S_egovehicle)
wSe = Similarity3.Align(we_pairs) wSe = Similarity3.Align(we_pairs)
@ -122,7 +121,7 @@ class TestSim3(GtsamTestCase):
bTi_list = [bTi0, bTi1, bTi2, bTi3] bTi_list = [bTi0, bTi1, bTi2, bTi3]
ab_pairs = Pose3Pairs(list(zip(aTi_list, bTi_list))) ab_pairs = list(zip(aTi_list, bTi_list))
# Recover the transformation wSe (i.e. world_S_egovehicle) # Recover the transformation wSe (i.e. world_S_egovehicle)
aSb = Similarity3.Align(ab_pairs) aSb = Similarity3.Align(ab_pairs)
@ -689,7 +688,7 @@ def align_poses_sim3(aTi_list: List[Pose3], bTi_list: List[Pose3]) -> Similarity
assert len(aTi_list) == len(bTi_list) assert len(aTi_list) == len(bTi_list)
assert n_to_align >= 2, "SIM(3) alignment uses at least 2 frames" assert n_to_align >= 2, "SIM(3) alignment uses at least 2 frames"
ab_pairs = Pose3Pairs(list(zip(aTi_list, bTi_list))) ab_pairs = list(zip(aTi_list, bTi_list))
aSb = Similarity3.Align(ab_pairs) aSb = Similarity3.Align(ab_pairs)

48
python/gtsam/tests/test_TranslationRecovery.py
View File

@ -1,25 +1,25 @@
from __future__ import print_function
import numpy as np
import unittest import unittest
import gtsam import gtsam
import numpy as np
""" Returns example pose values of 3 points A, B and C in the world frame """
def ExampleValues(): def ExampleValues():
""" Returns example pose values of 3 points A, B and C in the world frame """
T = [] T = []
T.append(gtsam.Point3(np.array([3.14, 1.59, 2.65]))) T.append(gtsam.Point3(np.array([3.14, 1.59, 2.65])))
T.append(gtsam.Point3(np.array([-1.0590e+00, -3.6017e-02, -1.5720e+00]))) T.append(gtsam.Point3(np.array([-1.0590e+00, -3.6017e-02, -1.5720e+00])))
T.append(gtsam.Point3(np.array([8.5034e+00, 6.7499e+00, -3.6383e+00]))) T.append(gtsam.Point3(np.array([8.5034e+00, 6.7499e+00, -3.6383e+00])))
data = gtsam.Values() data = gtsam.Values()
for i in range(len(T)): for i, t in enumerate(T):
data.insert(i, gtsam.Pose3(gtsam.Rot3(), T[i])) data.insert(i, gtsam.Pose3(gtsam.Rot3(), t))
return data return data
""" Returns binary measurements for the points in the given edges."""
def SimulateMeasurements(gt_poses, graph_edges): def SimulateMeasurements(gt_poses, graph_edges):
measurements = gtsam.BinaryMeasurementsUnit3() """ Returns binary measurements for the points in the given edges."""
measurements = []
for edge in graph_edges: for edge in graph_edges:
Ta = gt_poses.atPose3(edge[0]).translation() Ta = gt_poses.atPose3(edge[0]).translation()
Tb = gt_poses.atPose3(edge[1]).translation() Tb = gt_poses.atPose3(edge[1]).translation()
@ -28,18 +28,20 @@ def SimulateMeasurements(gt_poses, graph_edges):
gtsam.noiseModel.Isotropic.Sigma(3, 0.01))) gtsam.noiseModel.Isotropic.Sigma(3, 0.01)))
return measurements return measurements
""" Tests for the translation recovery class """
class TestTranslationRecovery(unittest.TestCase): class TestTranslationRecovery(unittest.TestCase):
"""Test selected Translation Recovery methods.""" """ Tests for the translation recovery class."""
def test_constructor(self): def test_constructor(self):
"""Construct from binary measurements.""" """Construct from binary measurements."""
algorithm = gtsam.TranslationRecovery() algorithm = gtsam.TranslationRecovery()
self.assertIsInstance(algorithm, gtsam.TranslationRecovery) self.assertIsInstance(algorithm, gtsam.TranslationRecovery)
algorithm_params = gtsam.TranslationRecovery(gtsam.LevenbergMarquardtParams()) algorithm_params = gtsam.TranslationRecovery(
gtsam.LevenbergMarquardtParams())
self.assertIsInstance(algorithm_params, gtsam.TranslationRecovery) self.assertIsInstance(algorithm_params, gtsam.TranslationRecovery)
def test_run(self): def test_run(self):
"""Test selected Translation Recovery methods."""
gt_poses = ExampleValues() gt_poses = ExampleValues()
measurements = SimulateMeasurements(gt_poses, [[0, 1], [0, 2], [1, 2]]) measurements = SimulateMeasurements(gt_poses, [[0, 1], [0, 2], [1, 2]])
@ -51,15 +53,23 @@ class TestTranslationRecovery(unittest.TestCase):
scale = 2.0 scale = 2.0
result = algorithm.run(measurements, scale) result = algorithm.run(measurements, scale)
w_aTc = gt_poses.atPose3(2).translation() - gt_poses.atPose3(0).translation() w_aTc = gt_poses.atPose3(2).translation() - gt_poses.atPose3(
w_aTb = gt_poses.atPose3(1).translation() - gt_poses.atPose3(0).translation() 0).translation()
w_aTc_expected = w_aTc*scale/np.linalg.norm(w_aTb) w_aTb = gt_poses.atPose3(1).translation() - gt_poses.atPose3(
w_aTb_expected = w_aTb*scale/np.linalg.norm(w_aTb) 0).translation()
w_aTc_expected = w_aTc * scale / np.linalg.norm(w_aTb)
w_aTb_expected = w_aTb * scale / np.linalg.norm(w_aTb)
np.testing.assert_array_almost_equal(result.atPoint3(0),
np.array([0, 0, 0]), 6,
"Origin result is incorrect.")
np.testing.assert_array_almost_equal(result.atPoint3(1),
w_aTb_expected, 6,
"Point B result is incorrect.")
np.testing.assert_array_almost_equal(result.atPoint3(2),
w_aTc_expected, 6,
"Point C result is incorrect.")
np.testing.assert_array_almost_equal(result.atPoint3(0), np.array([0,0,0]), 6, "Origin result is incorrect.")
np.testing.assert_array_almost_equal(result.atPoint3(1), w_aTb_expected, 6, "Point B result is incorrect.")
np.testing.assert_array_almost_equal(result.atPoint3(2), w_aTc_expected, 6, "Point C result is incorrect.")
if __name__ == "__main__": if __name__ == "__main__":
unittest.main() unittest.main()

37
python/gtsam/tests/test_Triangulation.py
View File

@ -11,14 +11,13 @@ Authors: Frank Dellaert & Fan Jiang (Python) & Sushmita Warrier & John Lambert
# pylint: disable=no-name-in-module, invalid-name, no-member # pylint: disable=no-name-in-module, invalid-name, no-member
import unittest import unittest
from typing import Iterable, List, Optional, Tuple, Union from typing import Iterable, List, Optional, Tuple, Union
import numpy as np
import gtsam import gtsam
import numpy as np
from gtsam import (Cal3_S2, Cal3Bundler, CameraSetCal3_S2, from gtsam import (Cal3_S2, Cal3Bundler, CameraSetCal3_S2,
CameraSetCal3Bundler, PinholeCameraCal3_S2, CameraSetCal3Bundler, PinholeCameraCal3_S2,
PinholeCameraCal3Bundler, Point2, Point2Vector, Point3, PinholeCameraCal3Bundler, Point2, Point3, Pose3, Rot3,
Pose3, Pose3Vector, Rot3, TriangulationParameters, TriangulationParameters, TriangulationResult)
TriangulationResult)
from gtsam.utils.test_case import GtsamTestCase from gtsam.utils.test_case import GtsamTestCase
UPRIGHT = Rot3.Ypr(-np.pi / 2, 0.0, -np.pi / 2) UPRIGHT = Rot3.Ypr(-np.pi / 2, 0.0, -np.pi / 2)
@ -35,9 +34,7 @@ class TestTriangulationExample(GtsamTestCase):
# create second camera 1 meter to the right of first camera # create second camera 1 meter to the right of first camera
pose2 = pose1.compose(Pose3(Rot3(), Point3(1, 0, 0))) pose2 = pose1.compose(Pose3(Rot3(), Point3(1, 0, 0)))
# twoPoses # twoPoses
self.poses = Pose3Vector() self.poses = [pose1, pose2]
self.poses.append(pose1)
self.poses.append(pose2)
# landmark ~5 meters infront of camera # landmark ~5 meters infront of camera
self.landmark = Point3(5, 0.5, 1.2) self.landmark = Point3(5, 0.5, 1.2)
@ -49,7 +46,7 @@ class TestTriangulationExample(GtsamTestCase):
cal_params: Iterable[Iterable[Union[int, float]]], cal_params: Iterable[Iterable[Union[int, float]]],
camera_set: Optional[Union[CameraSetCal3Bundler, camera_set: Optional[Union[CameraSetCal3Bundler,
CameraSetCal3_S2]] = None, CameraSetCal3_S2]] = None,
) -> Tuple[Point2Vector, Union[CameraSetCal3Bundler, CameraSetCal3_S2, ) -> Tuple[List[Point2], Union[CameraSetCal3Bundler, CameraSetCal3_S2,
List[Cal3Bundler], List[Cal3_S2]]]: List[Cal3Bundler], List[Cal3_S2]]]:
""" """
Generate vector of measurements for given calibration and camera model. Generate vector of measurements for given calibration and camera model.
@ -67,7 +64,7 @@ class TestTriangulationExample(GtsamTestCase):
cameras = camera_set() cameras = camera_set()
else: else:
cameras = [] cameras = []
measurements = Point2Vector() measurements = []
for k, pose in zip(cal_params, self.poses): for k, pose in zip(cal_params, self.poses):
K = calibration(*k) K = calibration(*k)
@ -96,7 +93,7 @@ class TestTriangulationExample(GtsamTestCase):
self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9) self.gtsamAssertEquals(self.landmark, triangulated_landmark, 1e-9)
# Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) # Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814)
measurements_noisy = Point2Vector() measurements_noisy = []
measurements_noisy.append(measurements[0] - np.array([0.1, 0.5])) measurements_noisy.append(measurements[0] - np.array([0.1, 0.5]))
measurements_noisy.append(measurements[1] - np.array([-0.2, 0.3])) measurements_noisy.append(measurements[1] - np.array([-0.2, 0.3]))
@ -163,8 +160,8 @@ class TestTriangulationExample(GtsamTestCase):
z2: Point2 = camera2.project(landmark) z2: Point2 = camera2.project(landmark)
z3: Point2 = camera3.project(landmark) z3: Point2 = camera3.project(landmark)
poses = gtsam.Pose3Vector([pose1, pose2, pose3]) poses = [pose1, pose2, pose3]
measurements = Point2Vector([z1, z2, z3]) measurements = [z1, z2, z3]
# noise free, so should give exactly the landmark # noise free, so should give exactly the landmark
actual = gtsam.triangulatePoint3(poses, actual = gtsam.triangulatePoint3(poses,
@ -226,10 +223,10 @@ class TestTriangulationExample(GtsamTestCase):
z2 = camera2.project(self.landmark) z2 = camera2.project(self.landmark)
cameras = CameraSetCal3_S2() cameras = CameraSetCal3_S2()
measurements = Point2Vector()
cameras.append(camera1) cameras.append(camera1)
cameras.append(camera2) cameras.append(camera2)
measurements = []
measurements.append(z1) measurements.append(z1)
measurements.append(z2) measurements.append(z2)
@ -242,8 +239,8 @@ class TestTriangulationExample(GtsamTestCase):
self.assertTrue(actual.valid()) self.assertTrue(actual.valid())
landmarkDistanceThreshold = 4 # landmark is farther than that landmarkDistanceThreshold = 4 # landmark is farther than that
params2 = TriangulationParameters( params2 = TriangulationParameters(1.0, False,
1.0, False, landmarkDistanceThreshold) landmarkDistanceThreshold)
actual = gtsam.triangulateSafe(cameras, measurements, params2) actual = gtsam.triangulateSafe(cameras, measurements, params2)
self.assertTrue(actual.farPoint()) self.assertTrue(actual.farPoint())
@ -258,15 +255,17 @@ class TestTriangulationExample(GtsamTestCase):
measurements.append(z3 + Point2(10, -10)) measurements.append(z3 + Point2(10, -10))
landmarkDistanceThreshold = 10 # landmark is closer than that landmarkDistanceThreshold = 10 # landmark is closer than that
outlierThreshold = 100 # loose, the outlier is going to pass outlierThreshold = 100 # loose, the outlier is going to pass
params3 = TriangulationParameters(1.0, False, landmarkDistanceThreshold, params3 = TriangulationParameters(1.0, False,
landmarkDistanceThreshold,
outlierThreshold) outlierThreshold)
actual = gtsam.triangulateSafe(cameras, measurements, params3) actual = gtsam.triangulateSafe(cameras, measurements, params3)
self.assertTrue(actual.valid()) self.assertTrue(actual.valid())
# now set stricter threshold for outlier rejection # now set stricter threshold for outlier rejection
outlierThreshold = 5 # tighter, the outlier is not going to pass outlierThreshold = 5 # tighter, the outlier is not going to pass
params4 = TriangulationParameters(1.0, False, landmarkDistanceThreshold, params4 = TriangulationParameters(1.0, False,
landmarkDistanceThreshold,
outlierThreshold) outlierThreshold)
actual = gtsam.triangulateSafe(cameras, measurements, params4) actual = gtsam.triangulateSafe(cameras, measurements, params4)
self.assertTrue(actual.outlier()) self.assertTrue(actual.outlier())

39
python/gtsam/tests/test_custom_factor.py
View File

@ -8,31 +8,34 @@ See LICENSE for the license information
CustomFactor unit tests. CustomFactor unit tests.
Author: Fan Jiang Author: Fan Jiang
""" """
from typing import List
import unittest import unittest
from gtsam import Values, Pose2, CustomFactor from typing import List
import numpy as np import numpy as np
from gtsam.utils.test_case import GtsamTestCase
import gtsam import gtsam
from gtsam.utils.test_case import GtsamTestCase from gtsam import CustomFactor, Pose2, Values
class TestCustomFactor(GtsamTestCase): class TestCustomFactor(GtsamTestCase):
def test_new(self): def test_new(self):
"""Test the creation of a new CustomFactor""" """Test the creation of a new CustomFactor"""
def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]): def error_func(this: CustomFactor, v: gtsam.Values,
H: List[np.ndarray]):
"""Minimal error function stub""" """Minimal error function stub"""
return np.array([1, 0, 0]) return np.array([1, 0, 0]), H
noise_model = gtsam.noiseModel.Unit.Create(3) noise_model = gtsam.noiseModel.Unit.Create(3)
cf = CustomFactor(noise_model, gtsam.KeyVector([0]), error_func) cf = CustomFactor(noise_model, [0], error_func)
def test_new_keylist(self): def test_new_keylist(self):
"""Test the creation of a new CustomFactor""" """Test the creation of a new CustomFactor"""
def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]): def error_func(this: CustomFactor, v: gtsam.Values,
H: List[np.ndarray]):
"""Minimal error function stub""" """Minimal error function stub"""
return np.array([1, 0, 0]) return np.array([1, 0, 0])
@ -43,7 +46,8 @@ class TestCustomFactor(GtsamTestCase):
"""Test if calling the factor works (only error)""" """Test if calling the factor works (only error)"""
expected_pose = Pose2(1, 1, 0) expected_pose = Pose2(1, 1, 0)
def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]) -> np.ndarray: def error_func(this: CustomFactor, v: gtsam.Values,
H: List[np.ndarray]) -> np.ndarray:
"""Minimal error function with no Jacobian""" """Minimal error function with no Jacobian"""
key0 = this.keys()[0] key0 = this.keys()[0]
error = -v.atPose2(key0).localCoordinates(expected_pose) error = -v.atPose2(key0).localCoordinates(expected_pose)
@ -65,7 +69,8 @@ class TestCustomFactor(GtsamTestCase):
expected = Pose2(2, 2, np.pi / 2) expected = Pose2(2, 2, np.pi / 2)
def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]): def error_func(this: CustomFactor, v: gtsam.Values,
H: List[np.ndarray]):
""" """
the custom error function. One can freely use variables captured the custom error function. One can freely use variables captured
from the outside scope. Or the variables can be acquired by indexing `v`. from the outside scope. Or the variables can be acquired by indexing `v`.
@ -84,7 +89,7 @@ class TestCustomFactor(GtsamTestCase):
return error return error
noise_model = gtsam.noiseModel.Unit.Create(3) noise_model = gtsam.noiseModel.Unit.Create(3)
cf = CustomFactor(noise_model, gtsam.KeyVector([0, 1]), error_func) cf = CustomFactor(noise_model, [0, 1], error_func)
v = Values() v = Values()
v.insert(0, gT1) v.insert(0, gT1)
v.insert(1, gT2) v.insert(1, gT2)
@ -104,7 +109,8 @@ class TestCustomFactor(GtsamTestCase):
gT1 = Pose2(1, 2, np.pi / 2) gT1 = Pose2(1, 2, np.pi / 2)
gT2 = Pose2(-1, 4, np.pi) gT2 = Pose2(-1, 4, np.pi)
def error_func(this: CustomFactor, v: gtsam.Values, _: List[np.ndarray]): def error_func(this: CustomFactor, v: gtsam.Values,
_: List[np.ndarray]):
"""Minimal error function stub""" """Minimal error function stub"""
return np.array([1, 0, 0]) return np.array([1, 0, 0])
@ -125,7 +131,8 @@ class TestCustomFactor(GtsamTestCase):
expected = Pose2(2, 2, np.pi / 2) expected = Pose2(2, 2, np.pi / 2)
def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]): def error_func(this: CustomFactor, v: gtsam.Values,
H: List[np.ndarray]):
""" """
Error function that mimics a BetweenFactor Error function that mimics a BetweenFactor
:param this: reference to the current CustomFactor being evaluated :param this: reference to the current CustomFactor being evaluated
@ -138,7 +145,8 @@ class TestCustomFactor(GtsamTestCase):
gT1, gT2 = v.atPose2(key0), v.atPose2(key1) gT1, gT2 = v.atPose2(key0), v.atPose2(key1)
error = expected.localCoordinates(gT1.between(gT2)) error = expected.localCoordinates(gT1.between(gT2))
self.assertTrue(H is None) # Should be true if we only request the error self.assertTrue(
H is None) # Should be true if we only request the error
if H is not None: if H is not None:
result = gT1.between(gT2) result = gT1.between(gT2)
@ -147,7 +155,7 @@ class TestCustomFactor(GtsamTestCase):
return error return error
noise_model = gtsam.noiseModel.Unit.Create(3) noise_model = gtsam.noiseModel.Unit.Create(3)
cf = CustomFactor(noise_model, gtsam.KeyVector([0, 1]), error_func) cf = CustomFactor(noise_model, [0, 1], error_func)
v = Values() v = Values()
v.insert(0, gT1) v.insert(0, gT1)
v.insert(1, gT2) v.insert(1, gT2)
@ -165,7 +173,8 @@ class TestCustomFactor(GtsamTestCase):
expected = Pose2(2, 2, np.pi / 2) expected = Pose2(2, 2, np.pi / 2)
def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]): def error_func(this: CustomFactor, v: gtsam.Values,
H: List[np.ndarray]):
""" """
Error function that mimics a BetweenFactor Error function that mimics a BetweenFactor
:param this: reference to the current CustomFactor being evaluated :param this: reference to the current CustomFactor being evaluated

3
python/gtsam/utils/plot.py
View File

@ -12,7 +12,6 @@ from mpl_toolkits.mplot3d import Axes3D # pylint: disable=unused-import
import gtsam import gtsam
from gtsam import Marginals, Point2, Point3, Pose2, Pose3, Values from gtsam import Marginals, Point2, Point3, Pose2, Pose3, Values
# For translation between a scaling of the uncertainty ellipse and the # For translation between a scaling of the uncertainty ellipse and the
# percentage of inliers see discussion in # percentage of inliers see discussion in
# [PR 1067](https://github.com/borglab/gtsam/pull/1067) # [PR 1067](https://github.com/borglab/gtsam/pull/1067)
@ -557,7 +556,7 @@ def plot_incremental_trajectory(fignum: int,
axes = fig.axes[0] axes = fig.axes[0]
poses = gtsam.utilities.allPose3s(values) poses = gtsam.utilities.allPose3s(values)
keys = gtsam.KeyVector(poses.keys()) keys = poses.keys()
for key in keys[start:]: for key in keys[start:]:
if values.exists(key): if values.exists(key):

1
timing/timeTest.cpp
View File

@ -21,7 +21,6 @@ using namespace gtsam;
int main(int argc, char *argv[]) { int main(int argc, char *argv[]) {
// FIXME: ticPush_ does not exist
{ {
gttic_(top1); gttic_(top1);
gttic_(sub1); gttic_(sub1);