Merged in jeraguilon/gtsam/fixed-lag-smoother-wrapper (pull request #384)
Added the fixed lag smoother wrapperrelease/4.3a0
Jeremy Aguilon
Frank Dellaert
commit
d7b7c07bb7
1
THANKS
1
THANKS
|
|
@ -1,5 +1,6 @@
|
|||
GTSAM was made possible by the efforts of many collaborators at Georgia Tech, listed below with their current afffiliation, if they left Tech:
|
||||
|
||||
* Jeremy Aguilon, Facebook
|
||||
* Sungtae An
|
||||
* Doru Balcan, Bank of America
|
||||
* Chris Beall
|
||||
|
|
|
|||
|
|
@ -0,0 +1,102 @@
|
|||
"""
|
||||
GTSAM Copyright 2010-2018, Georgia Tech Research Corporation,
|
||||
Atlanta, Georgia 30332-0415
|
||||
All Rights Reserved
|
||||
Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
See LICENSE for the license information
|
||||
|
||||
Demonstration of the fixed-lag smoothers using a planar robot example
|
||||
and multiple odometry-like sensors
|
||||
Author: Frank Dellaert (C++), Jeremy Aguilon (Python)
|
||||
"""
|
||||
|
||||
import numpy as np
|
||||
|
||||
import gtsam
|
||||
import gtsam_unstable
|
||||
|
||||
|
||||
def _timestamp_key_value(key, value):
|
||||
"""
|
||||
|
||||
"""
|
||||
return gtsam_unstable.FixedLagSmootherKeyTimestampMapValue(
|
||||
key, value
|
||||
)
|
||||
|
||||
|
||||
def BatchFixedLagSmootherExample():
|
||||
"""
|
||||
Runs a batch fixed smoother on an agent with two odometry
|
||||
sensors that is simply moving to the
|
||||
"""
|
||||
|
||||
# Define a batch fixed lag smoother, which uses
|
||||
# Levenberg-Marquardt to perform the nonlinear optimization
|
||||
lag = 2.0
|
||||
smoother_batch = gtsam_unstable.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()
|
||||
|
||||
# Create a prior on the first pose, placing it at the origin
|
||||
prior_mean = gtsam.Pose2(0, 0, 0)
|
||||
prior_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
|
||||
X1 = 0
|
||||
new_factors.push_back(gtsam.PriorFactorPose2(X1, prior_mean, prior_noise))
|
||||
new_values.insert(X1, prior_mean)
|
||||
new_timestamps.insert(_timestamp_key_value(X1, 0.0))
|
||||
|
||||
delta_time = 0.25
|
||||
time = 0.25
|
||||
|
||||
while time <= 3.0:
|
||||
previous_key = 1000 * (time - delta_time)
|
||||
current_key = 1000 * time
|
||||
|
||||
# assign current key to the current timestamp
|
||||
new_timestamps.insert(_timestamp_key_value(current_key, time))
|
||||
|
||||
# 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]
|
||||
current_pose = gtsam.Pose2(time * 2, 0, 0)
|
||||
new_values.insert(current_key, current_pose)
|
||||
|
||||
# Add odometry factors from two different sources with different error
|
||||
# stats
|
||||
odometry_measurement_1 = gtsam.Pose2(0.61, -0.08, 0.02)
|
||||
odometry_noise_1 = gtsam.noiseModel_Diagonal.Sigmas(
|
||||
np.array([0.1, 0.1, 0.05]))
|
||||
new_factors.push_back(gtsam.BetweenFactorPose2(
|
||||
previous_key, current_key, odometry_measurement_1, odometry_noise_1
|
||||
))
|
||||
|
||||
odometry_measurement_2 = gtsam.Pose2(0.47, 0.03, 0.01)
|
||||
odometry_noise_2 = gtsam.noiseModel_Diagonal.Sigmas(
|
||||
np.array([0.05, 0.05, 0.05]))
|
||||
new_factors.push_back(gtsam.BetweenFactorPose2(
|
||||
previous_key, current_key, odometry_measurement_2, odometry_noise_2
|
||||
))
|
||||
|
||||
# Update the smoothers with the new factors. In this case,
|
||||
# one iteration must pass for Levenberg-Marquardt to accurately
|
||||
# estimate
|
||||
if time >= 0.50:
|
||||
smoother_batch.update(new_factors, new_values, new_timestamps)
|
||||
print("Timestamp = " + str(time) + ", Key = " + str(current_key))
|
||||
print(smoother_batch.calculateEstimatePose2(current_key))
|
||||
|
||||
new_timestamps.clear()
|
||||
new_values.clear()
|
||||
new_factors.resize(0)
|
||||
|
||||
time += delta_time
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
BatchFixedLagSmootherExample()
|
||||
print("Example complete")
|
||||
|
|
@ -0,0 +1,122 @@
|
|||
import unittest
|
||||
import gtsam
|
||||
import gtsam_unstable
|
||||
import numpy as np
|
||||
|
||||
|
||||
def _timestamp_key_value(key, value):
|
||||
return gtsam_unstable.FixedLagSmootherKeyTimestampMapValue(
|
||||
key, value
|
||||
)
|
||||
|
||||
|
||||
class TestFixedLagSmootherExample(unittest.TestCase):
|
||||
'''
|
||||
Tests the fixed lag smoother wrapper
|
||||
'''
|
||||
|
||||
def test_FixedLagSmootherExample(self):
|
||||
'''
|
||||
Simple test that checks for equality between C++ example
|
||||
file and the Python implementation. See
|
||||
gtsam_unstable/examples/FixedLagSmootherExample.cpp
|
||||
'''
|
||||
# Define a batch fixed lag smoother, which uses
|
||||
# Levenberg-Marquardt to perform the nonlinear optimization
|
||||
lag = 2.0
|
||||
smoother_batch = gtsam_unstable.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()
|
||||
|
||||
# Create a prior on the first pose, placing it at the origin
|
||||
prior_mean = gtsam.Pose2(0, 0, 0)
|
||||
prior_noise = gtsam.noiseModel_Diagonal.Sigmas(
|
||||
np.array([0.3, 0.3, 0.1]))
|
||||
X1 = 0
|
||||
new_factors.push_back(
|
||||
gtsam.PriorFactorPose2(
|
||||
X1, prior_mean, prior_noise))
|
||||
new_values.insert(X1, prior_mean)
|
||||
new_timestamps.insert(_timestamp_key_value(X1, 0.0))
|
||||
|
||||
delta_time = 0.25
|
||||
time = 0.25
|
||||
|
||||
i = 0
|
||||
|
||||
ground_truth = [
|
||||
gtsam.Pose2(0.995821, 0.0231012, 0.0300001),
|
||||
gtsam.Pose2(1.49284, 0.0457247, 0.045),
|
||||
gtsam.Pose2(1.98981, 0.0758879, 0.06),
|
||||
gtsam.Pose2(2.48627, 0.113502, 0.075),
|
||||
gtsam.Pose2(2.98211, 0.158558, 0.09),
|
||||
gtsam.Pose2(3.47722, 0.211047, 0.105),
|
||||
gtsam.Pose2(3.97149, 0.270956, 0.12),
|
||||
gtsam.Pose2(4.4648, 0.338272, 0.135),
|
||||
gtsam.Pose2(4.95705, 0.41298, 0.15),
|
||||
gtsam.Pose2(5.44812, 0.495063, 0.165),
|
||||
gtsam.Pose2(5.9379, 0.584503, 0.18),
|
||||
]
|
||||
|
||||
# Iterates from 0.25s to 3.0s, adding 0.25s each loop
|
||||
# In each iteration, the agent moves at a constant speed
|
||||
# and its two odometers measure the change. The smoothed
|
||||
# result is then compared to the ground truth
|
||||
while time <= 3.0:
|
||||
previous_key = 1000 * (time - delta_time)
|
||||
current_key = 1000 * time
|
||||
|
||||
# assign current key to the current timestamp
|
||||
new_timestamps.insert(_timestamp_key_value(current_key, time))
|
||||
|
||||
# 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]
|
||||
current_pose = gtsam.Pose2(time * 2, 0, 0)
|
||||
new_values.insert(current_key, current_pose)
|
||||
|
||||
# Add odometry factors from two different sources with different
|
||||
# error stats
|
||||
odometry_measurement_1 = gtsam.Pose2(0.61, -0.08, 0.02)
|
||||
odometry_noise_1 = gtsam.noiseModel_Diagonal.Sigmas(
|
||||
np.array([0.1, 0.1, 0.05]))
|
||||
new_factors.push_back(
|
||||
gtsam.BetweenFactorPose2(
|
||||
previous_key,
|
||||
current_key,
|
||||
odometry_measurement_1,
|
||||
odometry_noise_1))
|
||||
|
||||
odometry_measurement_2 = gtsam.Pose2(0.47, 0.03, 0.01)
|
||||
odometry_noise_2 = gtsam.noiseModel_Diagonal.Sigmas(
|
||||
np.array([0.05, 0.05, 0.05]))
|
||||
new_factors.push_back(
|
||||
gtsam.BetweenFactorPose2(
|
||||
previous_key,
|
||||
current_key,
|
||||
odometry_measurement_2,
|
||||
odometry_noise_2))
|
||||
|
||||
# Update the smoothers with the new factors. In this case,
|
||||
# one iteration must pass for Levenberg-Marquardt to accurately
|
||||
# estimate
|
||||
if time >= 0.50:
|
||||
smoother_batch.update(new_factors, new_values, new_timestamps)
|
||||
|
||||
estimate = smoother_batch.calculateEstimatePose2(current_key)
|
||||
self.assertTrue(estimate.equals(ground_truth[i], 1e-4))
|
||||
i += 1
|
||||
|
||||
new_timestamps.clear()
|
||||
new_values.clear()
|
||||
new_factors.resize(0)
|
||||
|
||||
time += delta_time
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
|
|
@ -111,23 +111,27 @@ int main(int argc, char** argv) {
|
|||
noiseModel::Diagonal::shared_ptr odometryNoise2 = noiseModel::Diagonal::Sigmas(Vector3(0.05, 0.05, 0.05));
|
||||
newFactors.push_back(BetweenFactor<Pose2>(previousKey, currentKey, odometryMeasurement2, odometryNoise2));
|
||||
|
||||
// Update the smoothers with the new factors
|
||||
smootherBatch.update(newFactors, newValues, newTimestamps);
|
||||
smootherISAM2.update(newFactors, newValues, newTimestamps);
|
||||
for(size_t i = 1; i < 2; ++i) { // Optionally perform multiple iSAM2 iterations
|
||||
smootherISAM2.update();
|
||||
// Update the smoothers with the new factors. In this example, batch smoother needs one iteration
|
||||
// to accurately converge. The ISAM smoother doesn't, but we only start getting estiates when
|
||||
// both are ready for simplicity.
|
||||
if (time >= 0.50) {
|
||||
smootherBatch.update(newFactors, newValues, newTimestamps);
|
||||
smootherISAM2.update(newFactors, newValues, newTimestamps);
|
||||
for(size_t i = 1; i < 2; ++i) { // Optionally perform multiple iSAM2 iterations
|
||||
smootherISAM2.update();
|
||||
}
|
||||
|
||||
// Print the optimized current pose
|
||||
cout << setprecision(5) << "Timestamp = " << time << endl;
|
||||
smootherBatch.calculateEstimate<Pose2>(currentKey).print("Batch Estimate:");
|
||||
smootherISAM2.calculateEstimate<Pose2>(currentKey).print("iSAM2 Estimate:");
|
||||
cout << endl;
|
||||
|
||||
// Clear contains for the next iteration
|
||||
newTimestamps.clear();
|
||||
newValues.clear();
|
||||
newFactors.resize(0);
|
||||
}
|
||||
|
||||
// Print the optimized current pose
|
||||
cout << setprecision(5) << "Timestamp = " << time << endl;
|
||||
smootherBatch.calculateEstimate<Pose2>(currentKey).print("Batch Estimate:");
|
||||
smootherISAM2.calculateEstimate<Pose2>(currentKey).print("iSAM2 Estimate:");
|
||||
cout << endl;
|
||||
|
||||
// Clear contains for the next iteration
|
||||
newTimestamps.clear();
|
||||
newValues.clear();
|
||||
newFactors.resize(0);
|
||||
}
|
||||
|
||||
// And to demonstrate the fixed-lag aspect, print the keys contained in each smoother after 3.0 seconds
|
||||
|
|
|
|||
|
|
@ -505,132 +505,130 @@ virtual class DiscreteEulerPoincareHelicopter : gtsam::NoiseModelFactor {
|
|||
//*************************************************************************
|
||||
// nonlinear
|
||||
//*************************************************************************
|
||||
// #include <gtsam_unstable/nonlinear/sequentialSummarization.h>
|
||||
// gtsam::GaussianFactorGraph* summarizeGraphSequential(
|
||||
// const gtsam::GaussianFactorGraph& full_graph, const gtsam::KeyVector& indices);
|
||||
// gtsam::GaussianFactorGraph* summarizeGraphSequential(
|
||||
// const gtsam::GaussianFactorGraph& full_graph, const gtsam::Ordering& ordering, const gtsam::KeySet& saved_keys);
|
||||
#include <gtsam_unstable/nonlinear/FixedLagSmoother.h>
|
||||
class FixedLagSmootherKeyTimestampMapValue {
|
||||
FixedLagSmootherKeyTimestampMapValue(size_t key, double timestamp);
|
||||
FixedLagSmootherKeyTimestampMapValue(const gtsam::FixedLagSmootherKeyTimestampMapValue& other);
|
||||
};
|
||||
|
||||
// #include <gtsam_unstable/nonlinear/FixedLagSmoother.h>
|
||||
// class FixedLagSmootherKeyTimestampMapValue {
|
||||
// FixedLagSmootherKeyTimestampMapValue(const gtsam::Key& 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 gtsam::Key& 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::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);
|
||||
//
|
||||
// gtsam::LevenbergMarquardtParams params() const;
|
||||
// };
|
||||
//
|
||||
// #include <gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h>
|
||||
// virtual class IncrementalFixedLagSmoother : gtsam::FixedLagSmoother {
|
||||
// IncrementalFixedLagSmoother();
|
||||
// IncrementalFixedLagSmoother(double smootherLag);
|
||||
// IncrementalFixedLagSmoother(double smootherLag, const gtsam::ISAM2Params& params);
|
||||
//
|
||||
// gtsam::ISAM2Params params() const;
|
||||
// };
|
||||
//
|
||||
// #include <gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h>
|
||||
// virtual class ConcurrentFilter {
|
||||
// void print(string s) const;
|
||||
// bool equals(const gtsam::ConcurrentFilter& rhs, double tol) const;
|
||||
// };
|
||||
//
|
||||
// virtual class ConcurrentSmoother {
|
||||
// void print(string s) const;
|
||||
// bool equals(const gtsam::ConcurrentSmoother& rhs, double tol) const;
|
||||
// };
|
||||
//
|
||||
// // Synchronize function
|
||||
// void synchronize(gtsam::ConcurrentFilter& filter, gtsam::ConcurrentSmoother& smoother);
|
||||
//
|
||||
// #include <gtsam_unstable/nonlinear/ConcurrentBatchFilter.h>
|
||||
// class ConcurrentBatchFilterResult {
|
||||
// size_t getIterations() const;
|
||||
// size_t getLambdas() const;
|
||||
// size_t getNonlinearVariables() const;
|
||||
// size_t getLinearVariables() const;
|
||||
// double getError() const;
|
||||
// };
|
||||
//
|
||||
// virtual class ConcurrentBatchFilter : gtsam::ConcurrentFilter {
|
||||
// ConcurrentBatchFilter();
|
||||
// ConcurrentBatchFilter(const gtsam::LevenbergMarquardtParams& parameters);
|
||||
//
|
||||
// gtsam::NonlinearFactorGraph getFactors() const;
|
||||
// gtsam::Values getLinearizationPoint() const;
|
||||
// gtsam::Ordering getOrdering() const;
|
||||
// gtsam::VectorValues getDelta() const;
|
||||
//
|
||||
// gtsam::ConcurrentBatchFilterResult update();
|
||||
// gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors);
|
||||
// gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta);
|
||||
// gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyList& keysToMove);
|
||||
// gtsam::Values calculateEstimate() const;
|
||||
// };
|
||||
//
|
||||
// #include <gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h>
|
||||
// class ConcurrentBatchSmootherResult {
|
||||
// size_t getIterations() const;
|
||||
// size_t getLambdas() const;
|
||||
// size_t getNonlinearVariables() const;
|
||||
// size_t getLinearVariables() const;
|
||||
// double getError() const;
|
||||
// };
|
||||
//
|
||||
// virtual class ConcurrentBatchSmoother : gtsam::ConcurrentSmoother {
|
||||
// ConcurrentBatchSmoother();
|
||||
// ConcurrentBatchSmoother(const gtsam::LevenbergMarquardtParams& parameters);
|
||||
//
|
||||
// gtsam::NonlinearFactorGraph getFactors() const;
|
||||
// gtsam::Values getLinearizationPoint() const;
|
||||
// gtsam::Ordering getOrdering() const;
|
||||
// gtsam::VectorValues getDelta() const;
|
||||
//
|
||||
// gtsam::ConcurrentBatchSmootherResult update();
|
||||
// gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors);
|
||||
// gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta);
|
||||
// gtsam::Values calculateEstimate() const;
|
||||
// };
|
||||
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::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);
|
||||
|
||||
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_unstable/nonlinear/IncrementalFixedLagSmoother.h>
|
||||
virtual class IncrementalFixedLagSmoother : gtsam::FixedLagSmoother {
|
||||
IncrementalFixedLagSmoother();
|
||||
IncrementalFixedLagSmoother(double smootherLag);
|
||||
IncrementalFixedLagSmoother(double smootherLag, const gtsam::ISAM2Params& params);
|
||||
|
||||
gtsam::ISAM2Params params() const;
|
||||
};
|
||||
|
||||
#include <gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h>
|
||||
virtual class ConcurrentFilter {
|
||||
void print(string s) const;
|
||||
bool equals(const gtsam::ConcurrentFilter& rhs, double tol) const;
|
||||
};
|
||||
|
||||
virtual class ConcurrentSmoother {
|
||||
void print(string s) const;
|
||||
bool equals(const gtsam::ConcurrentSmoother& rhs, double tol) const;
|
||||
};
|
||||
|
||||
// Synchronize function
|
||||
void synchronize(gtsam::ConcurrentFilter& filter, gtsam::ConcurrentSmoother& smoother);
|
||||
|
||||
#include <gtsam_unstable/nonlinear/ConcurrentBatchFilter.h>
|
||||
class ConcurrentBatchFilterResult {
|
||||
size_t getIterations() const;
|
||||
size_t getLambdas() const;
|
||||
size_t getNonlinearVariables() const;
|
||||
size_t getLinearVariables() const;
|
||||
double getError() const;
|
||||
};
|
||||
|
||||
virtual class ConcurrentBatchFilter : gtsam::ConcurrentFilter {
|
||||
ConcurrentBatchFilter();
|
||||
ConcurrentBatchFilter(const gtsam::LevenbergMarquardtParams& parameters);
|
||||
|
||||
gtsam::NonlinearFactorGraph getFactors() const;
|
||||
gtsam::Values getLinearizationPoint() const;
|
||||
gtsam::Ordering getOrdering() const;
|
||||
gtsam::VectorValues getDelta() const;
|
||||
|
||||
gtsam::ConcurrentBatchFilterResult update();
|
||||
gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors);
|
||||
gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta);
|
||||
gtsam::ConcurrentBatchFilterResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta, const gtsam::KeyList& keysToMove);
|
||||
gtsam::Values calculateEstimate() const;
|
||||
};
|
||||
|
||||
#include <gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h>
|
||||
class ConcurrentBatchSmootherResult {
|
||||
size_t getIterations() const;
|
||||
size_t getLambdas() const;
|
||||
size_t getNonlinearVariables() const;
|
||||
size_t getLinearVariables() const;
|
||||
double getError() const;
|
||||
};
|
||||
|
||||
virtual class ConcurrentBatchSmoother : gtsam::ConcurrentSmoother {
|
||||
ConcurrentBatchSmoother();
|
||||
ConcurrentBatchSmoother(const gtsam::LevenbergMarquardtParams& parameters);
|
||||
|
||||
gtsam::NonlinearFactorGraph getFactors() const;
|
||||
gtsam::Values getLinearizationPoint() const;
|
||||
gtsam::Ordering getOrdering() const;
|
||||
gtsam::VectorValues getDelta() const;
|
||||
|
||||
gtsam::ConcurrentBatchSmootherResult update();
|
||||
gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors);
|
||||
gtsam::ConcurrentBatchSmootherResult update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta);
|
||||
gtsam::Values calculateEstimate() const;
|
||||
};
|
||||
|
||||
//*************************************************************************
|
||||
// slam
|
||||
|
|
|
|||
Loading…
Reference in New Issue