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

Fixed python wrapper

release/4.3a0
Frank Dellaert 2023-02-20 08:20:16 -08:00
parent 6df2f3eeda
commit 2ce0b57f05
5 changed files with 68 additions and 66 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

61
gtsam/nonlinear/nonlinear.i
View File

@ -650,4 +650,65 @@ virtual class NonlinearEquality2 : gtsam::NoiseModelFactor {
gtsam::Vector evaluateError(const T& x1, const T& x2);
};
#include <gtsam/nonlinear/FixedLagSmoother.h>
class FixedLagSmootherKeyTimestampMapValue {
FixedLagSmootherKeyTimestampMapValue(size_t key, double timestamp);
FixedLagSmootherKeyTimestampMapValue(const gtsam::FixedLagSmootherKeyTimestampMapValue& other);
};
class FixedLagSmootherKeyTimestampMap {
FixedLagSmootherKeyTimestampMap();
FixedLagSmootherKeyTimestampMap(const gtsam::FixedLagSmootherKeyTimestampMap& other);
// Note: no print function
// common STL methods
size_t size() const;
bool empty() const;
void clear();
double at(const size_t key) const;
void insert(const gtsam::FixedLagSmootherKeyTimestampMapValue& value);
};
class FixedLagSmootherResult {
size_t getIterations() const;
size_t getNonlinearVariables() const;
size_t getLinearVariables() const;
double getError() const;
};
virtual class FixedLagSmoother {
void print(string s) const;
bool equals(const gtsam::FixedLagSmoother& rhs, double tol) const;
gtsam::FixedLagSmootherKeyTimestampMap timestamps() const;
double smootherLag() const;
gtsam::FixedLagSmootherResult update(const gtsam::NonlinearFactorGraph &newFactors,
const gtsam::Values &newTheta,
const gtsam::FixedLagSmootherKeyTimestampMap &timestamps);
gtsam::FixedLagSmootherResult update(const gtsam::NonlinearFactorGraph &newFactors,
const gtsam::Values &newTheta,
const gtsam::FixedLagSmootherKeyTimestampMap &timestamps,
const gtsam::FactorIndices &factorsToRemove);
gtsam::Values calculateEstimate() const;
};
#include <gtsam/nonlinear/BatchFixedLagSmoother.h>
virtual class BatchFixedLagSmoother : gtsam::FixedLagSmoother {
BatchFixedLagSmoother();
BatchFixedLagSmoother(double smootherLag);
BatchFixedLagSmoother(double smootherLag, const gtsam::LevenbergMarquardtParams& params);
void print(string s = "BatchFixedLagSmoother:\n") const;
gtsam::LevenbergMarquardtParams params() const;
template <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
Vector, Matrix}>
VALUE calculateEstimate(size_t key) const;
};
} // namespace gtsam

63
gtsam_unstable/gtsam_unstable.i
View File

@ -40,6 +40,7 @@ class gtsam::LevenbergMarquardtParams;
class gtsam::ISAM2Params;
class gtsam::GaussianDensity;
class gtsam::LevenbergMarquardtOptimizer;
class gtsam::FixedLagSmoother;
namespace gtsam {
@ -530,67 +531,7 @@ virtual class DiscreteEulerPoincareHelicopter : gtsam::NoiseModelFactor {
//*************************************************************************
// nonlinear
//*************************************************************************
#include <gtsam_unstable/nonlinear/FixedLagSmoother.h>
class FixedLagSmootherKeyTimestampMapValue {
FixedLagSmootherKeyTimestampMapValue(size_t key, double timestamp);
FixedLagSmootherKeyTimestampMapValue(const gtsam::FixedLagSmootherKeyTimestampMapValue& other);
};
class FixedLagSmootherKeyTimestampMap {
FixedLagSmootherKeyTimestampMap();
FixedLagSmootherKeyTimestampMap(const gtsam::FixedLagSmootherKeyTimestampMap& other);
// Note: no print function
// common STL methods
size_t size() const;
bool empty() const;
void clear();
double at(const size_t key) const;
void insert(const gtsam::FixedLagSmootherKeyTimestampMapValue& value);
};
class FixedLagSmootherResult {
size_t getIterations() const;
size_t getNonlinearVariables() const;
size_t getLinearVariables() const;
double getError() const;
};
#include <gtsam_unstable/nonlinear/FixedLagSmoother.h>
virtual class FixedLagSmoother {
void print(string s) const;
bool equals(const gtsam::FixedLagSmoother& rhs, double tol) const;
gtsam::FixedLagSmootherKeyTimestampMap timestamps() const;
double smootherLag() const;
gtsam::FixedLagSmootherResult update(const gtsam::NonlinearFactorGraph &newFactors,
const gtsam::Values &newTheta,
const gtsam::FixedLagSmootherKeyTimestampMap &timestamps);
gtsam::FixedLagSmootherResult update(const gtsam::NonlinearFactorGraph &newFactors,
const gtsam::Values &newTheta,
const gtsam::FixedLagSmootherKeyTimestampMap &timestamps,
const gtsam::FactorIndices &factorsToRemove);
gtsam::Values calculateEstimate() const;
};
#include <gtsam_unstable/nonlinear/BatchFixedLagSmoother.h>
virtual class BatchFixedLagSmoother : gtsam::FixedLagSmoother {
BatchFixedLagSmoother();
BatchFixedLagSmoother(double smootherLag);
BatchFixedLagSmoother(double smootherLag, const gtsam::LevenbergMarquardtParams& params);
void print(string s = "BatchFixedLagSmoother:\n") const;
gtsam::LevenbergMarquardtParams params() const;
template <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
Vector, Matrix}>
VALUE calculateEstimate(size_t key) const;
};
#include <gtsam/nonlinear/FixedLagSmoother.h>
#include <gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h>
virtual class IncrementalFixedLagSmoother : gtsam::FixedLagSmoother {
IncrementalFixedLagSmoother();

2
python/CMakeLists.txt
View File

@ -40,6 +40,7 @@ set(ignore
gtsam::IndexPairVector
gtsam::BetweenFactorPose2s
gtsam::BetweenFactorPose3s
gtsam::FixedLagSmootherKeyTimestampMapValue
gtsam::Point2Vector
gtsam::Point2Pairs
gtsam::Point3Pairs
@ -144,7 +145,6 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON)
gtsam::Point2Vector
gtsam::Pose3Vector
gtsam::KeyVector
gtsam::FixedLagSmootherKeyTimestampMapValue
gtsam::BinaryMeasurementsPoint3
gtsam::BinaryMeasurementsUnit3
gtsam::BinaryMeasurementsRot3

4
python/gtsam_unstable/examples/FixedLagSmootherExample.py → python/gtsam/examples/FixedLagSmootherExample.py
View File

@ -25,13 +25,13 @@ def BatchFixedLagSmootherExample():
# Define a batch fixed lag smoother, which uses
# Levenberg-Marquardt to perform the nonlinear optimization
lag = 2.0
smoother_batch = gtsam_unstable.BatchFixedLagSmoother(lag)
smoother_batch = gtsam.BatchFixedLagSmoother(lag)
# Create containers to store the factors and linearization points
# that will be sent to the smoothers
new_factors = gtsam.NonlinearFactorGraph()
new_values = gtsam.Values()
new_timestamps = gtsam_unstable.FixedLagSmootherKeyTimestampMap()
new_timestamps = gtsam.FixedLagSmootherKeyTimestampMap()
# Create a prior on the first pose, placing it at the origin
prior_mean = gtsam.Pose2(0, 0, 0)

4
python/gtsam_unstable/tests/test_FixedLagSmootherExample.py → python/gtsam/tests/test_FixedLagSmootherExample.py
View File

@ -30,13 +30,13 @@ class TestFixedLagSmootherExample(GtsamTestCase):
# Define a batch fixed lag smoother, which uses
# Levenberg-Marquardt to perform the nonlinear optimization
lag = 2.0
smoother_batch = gtsam_unstable.BatchFixedLagSmoother(lag)
smoother_batch = gtsam.BatchFixedLagSmoother(lag)
# Create containers to store the factors and linearization points
# that will be sent to the smoothers
new_factors = gtsam.NonlinearFactorGraph()
new_values = gtsam.Values()
new_timestamps = gtsam_unstable.FixedLagSmootherKeyTimestampMap()
new_timestamps = gtsam.FixedLagSmootherKeyTimestampMap()
# Create a prior on the first pose, placing it at the origin
prior_mean = gtsam.Pose2(0, 0, 0)