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

Merge branch 'develop' into wrapper/update

release/4.3a0
Frank Dellaert 2023-02-23 08:07:33 -08:00
parent a9e18d072a d392a3e117
commit 1bf6954372
27 changed files with 433 additions and 396 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.github/workflows/build-linux.yml vendored
View File

@ -26,7 +26,7 @@ jobs:
ubuntu-22.04-clang-14,
]
build_type: [Debug, Release]
build_type: [Release]
build_unstable: [ON]
include:
- name: ubuntu-20.04-gcc-9

1
cmake/GtsamBuildTypes.cmake
View File

@ -129,7 +129,6 @@ else()
-fPIC # ensure proper code generation for shared libraries
$<$<CXX_COMPILER_ID:GNU>:-Wreturn-local-addr -Werror=return-local-addr> # Error: return local address
$<$<CXX_COMPILER_ID:Clang>:-Wreturn-stack-address -Werror=return-stack-address> # Error: return local address
$<$<CXX_COMPILER_ID:Clang>:-Wno-misleading-indentation> # Eigen triggers a ton!
$<$<CXX_COMPILER_ID:Clang>:-Wno-weak-template-vtables> # TODO(dellaert): don't know how to resolve
$<$<CXX_COMPILER_ID:Clang>:-Wno-weak-vtables> # TODO(dellaert): don't know how to resolve
-Wreturn-type -Werror=return-type # Error on missing return()

2
gtsam_unstable/examples/FixedLagSmootherExample.cpp → examples/FixedLagSmootherExample.cpp
View File

@ -23,7 +23,7 @@
*/
// This example demonstrates the use of the Fixed-Lag Smoothers in GTSAM unstable
#include <gtsam_unstable/nonlinear/BatchFixedLagSmoother.h>
#include <gtsam/nonlinear/BatchFixedLagSmoother.h>
#include <gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h>
// In GTSAM, measurement functions are represented as 'factors'. Several common factors

2
gtsam/base/VectorSpace.h
View File

@ -185,7 +185,7 @@ template<class Class>
struct VectorSpaceTraits: VectorSpaceImpl<Class, Class::dimension> {
// Check that Class has the necessary machinery
GTSAM_CONCEPT_ASSERT(HasVectorSpacePrereqs<Class>);
GTSAM_CONCEPT_ASSERT(HasVectorSpacePrereqs<Class>);
typedef vector_space_tag structure_category;

2
gtsam/base/concepts.h
View File

@ -19,7 +19,7 @@
// This does something sensible:
#define BOOST_CONCEPT_USAGE(concept) void check##concept()
// These just ignore the concept checking for now:
#define GTSAM_CONCEPT_ASSERT(concept)
#define GTSAM_CONCEPT_ASSERT(concept) void*(concept)
#define GTSAM_CONCEPT_REQUIRES(concept, return_type) return_type
#endif

2
gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp → gtsam/nonlinear/BatchFixedLagSmoother.cpp
View File

@ -17,7 +17,7 @@
* @date Oct 14, 2012
*/
#include <gtsam_unstable/nonlinear/BatchFixedLagSmoother.h>
#include <gtsam/nonlinear/BatchFixedLagSmoother.h>
#include <gtsam/nonlinear/LinearContainerFactor.h>
#include <gtsam/linear/GaussianJunctionTree.h>
#include <gtsam/linear/GaussianFactorGraph.h>

182
gtsam/nonlinear/BatchFixedLagSmoother.h Normal file
View File

@ -0,0 +1,182 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, 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
* -------------------------------------------------------------------------- */
/**
* @file BatchFixedLagSmoother.h
* @brief An LM-based fixed-lag smoother.
*
* @author Michael Kaess, Stephen Williams
* @date Oct 14, 2012
*/
// \callgraph
#pragma once
#include <gtsam/nonlinear/FixedLagSmoother.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <queue>
namespace gtsam {
class GTSAM_EXPORT BatchFixedLagSmoother : public FixedLagSmoother {
public:
/// Typedef for a shared pointer to an Incremental Fixed-Lag Smoother
typedef std::shared_ptr<BatchFixedLagSmoother> shared_ptr;
/** default constructor */
BatchFixedLagSmoother(double smootherLag = 0.0, const LevenbergMarquardtParams& parameters = LevenbergMarquardtParams(), bool enforceConsistency = true) :
FixedLagSmoother(smootherLag), parameters_(parameters), enforceConsistency_(enforceConsistency) { }
/** destructor */
~BatchFixedLagSmoother() override {}
/** Print the factor for debugging and testing (implementing Testable) */
void print(const std::string& s = "BatchFixedLagSmoother:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
/** Check if two IncrementalFixedLagSmoother Objects are equal */
bool equals(const FixedLagSmoother& rhs, double tol = 1e-9) const override;
/** Add new factors, updating the solution and relinearizing as needed. */
Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(),
const Values& newTheta = Values(),
const KeyTimestampMap& timestamps = KeyTimestampMap(),
const FactorIndices& factorsToRemove = FactorIndices()) override;
/** Compute an estimate from the incomplete linear delta computed during the last update.
* This delta is incomplete because it was not updated below wildfire_threshold. If only
* a single variable is needed, it is faster to call calculateEstimate(const KEY&).
*/
Values calculateEstimate() const override {
return theta_.retract(delta_);
}
/** Compute an estimate for a single variable using its incomplete linear delta computed
* during the last update. This is faster than calling the no-argument version of
* calculateEstimate, which operates on all variables.
* @param key
* @return
*/
template<class VALUE>
VALUE calculateEstimate(Key key) const {
const Vector delta = delta_.at(key);
return traits<VALUE>::Retract(theta_.at<VALUE>(key), delta);
}
/** read the current set of optimizer parameters */
const LevenbergMarquardtParams& params() const {
return parameters_;
}
/** update the current set of optimizer parameters */
LevenbergMarquardtParams& params() {
return parameters_;
}
/** Access the current set of factors */
const NonlinearFactorGraph& getFactors() const {
return factors_;
}
/** Access the current linearization point */
const Values& getLinearizationPoint() const {
return theta_;
}
/** Access the current ordering */
const Ordering& getOrdering() const {
return ordering_;
}
/** Access the current set of deltas to the linearization point */
const VectorValues& getDelta() const {
return delta_;
}
/// Calculate marginal covariance on given variable
Matrix marginalCovariance(Key key) const;
/// Marginalize specific keys from a linear graph.
/// Does not check whether keys actually exist in graph.
/// In that case will fail somewhere deep within elimination
static GaussianFactorGraph CalculateMarginalFactors(
const GaussianFactorGraph& graph, const KeyVector& keys,
const GaussianFactorGraph::Eliminate& eliminateFunction = EliminatePreferCholesky);
/// Marginalize specific keys from a nonlinear graph, wrap in LinearContainers
static NonlinearFactorGraph CalculateMarginalFactors(
const NonlinearFactorGraph& graph, const Values& theta, const KeyVector& keys,
const GaussianFactorGraph::Eliminate& eliminateFunction = EliminatePreferCholesky);
protected:
/** A typedef defining an Key-Factor mapping **/
typedef std::map<Key, std::set<Key> > FactorIndex;
/** The L-M optimization parameters **/
LevenbergMarquardtParams parameters_;
/** A flag indicating if the optimizer should enforce probabilistic consistency by maintaining the
* linearization point of all variables involved in linearized/marginal factors at the edge of the
* smoothing window. This idea is from ??? TODO: Look up paper reference **/
bool enforceConsistency_;
/** The nonlinear factors **/
NonlinearFactorGraph factors_;
/** The current linearization point **/
Values theta_;
/** The set of values involved in current linear factors. **/
Values linearValues_;
/** The current ordering */
Ordering ordering_;
/** The current set of linear deltas */
VectorValues delta_;
/** The set of available factor graph slots. These occur because we are constantly deleting factors, leaving holes. **/
std::queue<size_t> availableSlots_;
/** A cross-reference structure to allow efficient factor lookups by key **/
FactorIndex factorIndex_;
/** Augment the list of factors with a set of new factors */
void insertFactors(const NonlinearFactorGraph& newFactors);
/** Remove factors from the list of factors by slot index */
void removeFactors(const std::set<size_t>& deleteFactors);
/** Erase any keys associated with timestamps before the provided time */
void eraseKeys(const KeyVector& keys);
/** Use colamd to update into an efficient ordering */
void reorder(const KeyVector& marginalizeKeys = KeyVector());
/** Optimize the current graph using a modified version of L-M */
Result optimize();
/** Marginalize out selected variables */
void marginalize(const KeyVector& marginalizableKeys);
private:
/** Private methods for printing debug information */
static void PrintKeySet(const std::set<Key>& keys, const std::string& label);
static void PrintKeySet(const gtsam::KeySet& keys, const std::string& label);
static void PrintSymbolicFactor(const NonlinearFactor::shared_ptr& factor);
static void PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor);
static void PrintSymbolicGraph(const NonlinearFactorGraph& graph, const std::string& label);
static void PrintSymbolicGraph(const GaussianFactorGraph& graph, const std::string& label);
}; // BatchFixedLagSmoother
} /// namespace gtsam

4
gtsam/nonlinear/Expression.h
View File

@ -218,7 +218,7 @@ protected:
template <typename T>
class ScalarMultiplyExpression : public Expression<T> {
// Check that T is a vector space
GTSAM_CONCEPT_ASSERT(gtsam::IsVectorSpace<T>);
GTSAM_CONCEPT_ASSERT(IsVectorSpace<T>);
public:
explicit ScalarMultiplyExpression(double s, const Expression<T>& e);
@ -231,7 +231,7 @@ class ScalarMultiplyExpression : public Expression<T> {
template <typename T>
class BinarySumExpression : public Expression<T> {
// Check that T is a vector space
GTSAM_CONCEPT_ASSERT(gtsam::IsVectorSpace<T>);
GTSAM_CONCEPT_ASSERT(IsVectorSpace<T>);
public:
explicit BinarySumExpression(const Expression<T>& e1, const Expression<T>& e2);

2
gtsam_unstable/nonlinear/FixedLagSmoother.cpp → gtsam/nonlinear/FixedLagSmoother.cpp
View File

@ -17,7 +17,7 @@
* @date Feb 27, 2013
*/
#include <gtsam_unstable/nonlinear/FixedLagSmoother.h>
#include <gtsam/nonlinear/FixedLagSmoother.h>
namespace gtsam {

138
gtsam/nonlinear/FixedLagSmoother.h Normal file
View File

@ -0,0 +1,138 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, 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
* -------------------------------------------------------------------------- */
/**
* @file FixedLagSmoother.h
* @brief Base class for a fixed-lag smoother. This mimics the basic interface to iSAM2.
*
* @author Stephen Williams
* @date Feb 27, 2013
*/
// \callgraph
#pragma once
#include <gtsam_unstable/dllexport.h>
#include <gtsam/inference/Key.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <map>
#include <vector>
namespace gtsam {
class GTSAM_EXPORT FixedLagSmoother {
public:
/// Typedef for a shared pointer to an Incremental Fixed-Lag Smoother
typedef std::shared_ptr<FixedLagSmoother> shared_ptr;
/// Typedef for a Key-Timestamp map/database
typedef std::map<Key, double> KeyTimestampMap;
typedef std::multimap<double, Key> TimestampKeyMap;
/**
* Meta information returned about the update
*/
// TODO: Think of some more things to put here
struct Result {
size_t iterations; ///< The number of optimizer iterations performed
size_t intermediateSteps; ///< The number of intermediate steps performed within the optimization. For L-M, this is the number of lambdas tried.
size_t nonlinearVariables; ///< The number of variables that can be relinearized
size_t linearVariables; ///< The number of variables that must keep a constant linearization point
double error; ///< The final factor graph error
Result() : iterations(0), intermediateSteps(0), nonlinearVariables(0), linearVariables(0), error(0) {}
/// Getter methods
size_t getIterations() const { return iterations; }
size_t getIntermediateSteps() const { return intermediateSteps; }
size_t getNonlinearVariables() const { return nonlinearVariables; }
size_t getLinearVariables() const { return linearVariables; }
double getError() const { return error; }
void print() const;
};
/** default constructor */
FixedLagSmoother(double smootherLag = 0.0) : smootherLag_(smootherLag) { }
/** destructor */
virtual ~FixedLagSmoother() { }
/** Print the factor for debugging and testing (implementing Testable) */
virtual void print(
const std::string& s = "FixedLagSmoother:\n",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/** Check if two IncrementalFixedLagSmoother Objects are equal */
virtual bool equals(const FixedLagSmoother& rhs, double tol = 1e-9) const;
/** read the current smoother lag */
double smootherLag() const {
return smootherLag_;
}
/** write to the current smoother lag */
double& smootherLag() {
return smootherLag_;
}
/** Access the current set of timestamps associated with each variable */
const KeyTimestampMap& timestamps() const {
return keyTimestampMap_;
}
/** Add new factors, updating the solution and relinearizing as needed. */
virtual Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(),
const Values& newTheta = Values(),
const KeyTimestampMap& timestamps = KeyTimestampMap(),
const FactorIndices& factorsToRemove = FactorIndices()) = 0;
/** Compute an estimate from the incomplete linear delta computed during the last update.
* This delta is incomplete because it was not updated below wildfire_threshold. If only
* a single variable is needed, it is faster to call calculateEstimate(const KEY&).
*/
virtual Values calculateEstimate() const = 0;
protected:
/** The length of the smoother lag. Any variable older than this amount will be marginalized out. */
double smootherLag_;
/** The current timestamp associated with each tracked key */
TimestampKeyMap timestampKeyMap_;
KeyTimestampMap keyTimestampMap_;
/** Update the Timestamps associated with the keys */
void updateKeyTimestampMap(const KeyTimestampMap& newTimestamps);
/** Erase keys from the Key-Timestamps database */
void eraseKeyTimestampMap(const KeyVector& keys);
/** Find the most recent timestamp of the system */
double getCurrentTimestamp() const;
/** Find all of the keys associated with timestamps before the provided time */
KeyVector findKeysBefore(double timestamp) const;
/** Find all of the keys associated with timestamps before the provided time */
KeyVector findKeysAfter(double timestamp) const;
}; // FixedLagSmoother
/// Typedef for matlab wrapping
typedef FixedLagSmoother::KeyTimestampMap FixedLagSmootherKeyTimestampMap;
typedef FixedLagSmootherKeyTimestampMap::value_type FixedLagSmootherKeyTimestampMapValue;
typedef FixedLagSmoother::Result FixedLagSmootherResult;
} /// namespace gtsam

2
gtsam/nonlinear/internal/ExpressionNode.h
View File

@ -561,7 +561,7 @@ public:
template <class T>
class ScalarMultiplyNode : public ExpressionNode<T> {
// Check that T is a vector space
GTSAM_CONCEPT_ASSERT(gtsam::IsVectorSpace<T>);
GTSAM_CONCEPT_ASSERT(IsVectorSpace<T>);
double scalar_;
std::shared_ptr<ExpressionNode<T> > expression_;

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

2
gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp → gtsam/nonlinear/tests/testBatchFixedLagSmoother.cpp
View File

@ -17,7 +17,7 @@
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam_unstable/nonlinear/BatchFixedLagSmoother.h>
#include <gtsam/nonlinear/BatchFixedLagSmoother.h>
#include <gtsam/base/debug.h>
#include <gtsam/inference/Key.h>
#include <gtsam/inference/Ordering.h>

4
gtsam/nonlinear/utilities.h
View File

@ -328,12 +328,12 @@ Values localToWorld(const Values& local, const Pose2& base,
// if value is a Pose2, compose it with base pose
Pose2 pose = local.at<Pose2>(key);
world.insert(key, base.compose(pose));
} catch (const std::exception& e1) {
} catch ([[maybe_unused]] const std::exception& e1) {
try {
// if value is a Point2, transform it from base pose
Point2 point = local.at<Point2>(key);
world.insert(key, base.transformFrom(point));
} catch (const std::exception& e2) {
} catch ([[maybe_unused]] const std::exception& e2) {
// if not Pose2 or Point2, do nothing
#ifndef NDEBUG
std::cerr << "Values[key] is neither Pose2 nor Point2, so skip" << std::endl;

2
gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp
View File

@ -28,7 +28,7 @@
#include <gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h>
// We will compare the results to a similar Fixed-Lag Smoother
#include <gtsam_unstable/nonlinear/BatchFixedLagSmoother.h>
#include <gtsam/nonlinear/BatchFixedLagSmoother.h>
// In GTSAM, measurement functions are represented as 'factors'. Several common factors
// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.

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();

7
gtsam_unstable/linear/InfeasibleInitialValues.h
View File

@ -33,13 +33,10 @@ public:
}
const char *what() const noexcept override {
if (description_.empty())
if (description_->empty())
description_ =
"An infeasible initial value was provided for the solver.\n";
return description_.c_str();
return description_->c_str();
}
private:
mutable std::string description_;
};
}

7
gtsam_unstable/linear/InfeasibleOrUnboundedProblem.h
View File

@ -29,12 +29,9 @@ public:
}
const char* what() const noexcept override {
if (description_.empty())
if (description_->empty())
description_ = "The problem is either infeasible or unbounded.\n";
return description_.c_str();
return description_->c_str();
}
private:
mutable std::string description_;
};
}

173
gtsam_unstable/nonlinear/BatchFixedLagSmoother.h
View File

@ -9,174 +9,13 @@
* -------------------------------------------------------------------------- */
/**
* @file BatchFixedLagSmoother.h
* @brief An LM-based fixed-lag smoother.
*
* @author Michael Kaess, Stephen Williams
* @date Oct 14, 2012
*/
// \callgraph
#pragma once
#include <gtsam_unstable/nonlinear/FixedLagSmoother.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <queue>
#ifdef _MSC_VER
#pragma message("BatchFixedLagSmoother was moved to the gtsam/nonlinear directory")
#else
#warning "BatchFixedLagSmoother was moved to the gtsam/nonlinear directory"
#endif
namespace gtsam {
class GTSAM_UNSTABLE_EXPORT BatchFixedLagSmoother : public FixedLagSmoother {
public:
/// Typedef for a shared pointer to an Incremental Fixed-Lag Smoother
typedef std::shared_ptr<BatchFixedLagSmoother> shared_ptr;
/** default constructor */
BatchFixedLagSmoother(double smootherLag = 0.0, const LevenbergMarquardtParams& parameters = LevenbergMarquardtParams(), bool enforceConsistency = true) :
FixedLagSmoother(smootherLag), parameters_(parameters), enforceConsistency_(enforceConsistency) { };
/** destructor */
~BatchFixedLagSmoother() override { };
/** Print the factor for debugging and testing (implementing Testable) */
void print(const std::string& s = "BatchFixedLagSmoother:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;
/** Check if two IncrementalFixedLagSmoother Objects are equal */
bool equals(const FixedLagSmoother& rhs, double tol = 1e-9) const override;
/** Add new factors, updating the solution and relinearizing as needed. */
Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(),
const Values& newTheta = Values(),
const KeyTimestampMap& timestamps = KeyTimestampMap(),
const FactorIndices& factorsToRemove = FactorIndices()) override;
/** Compute an estimate from the incomplete linear delta computed during the last update.
* This delta is incomplete because it was not updated below wildfire_threshold. If only
* a single variable is needed, it is faster to call calculateEstimate(const KEY&).
*/
Values calculateEstimate() const override {
return theta_.retract(delta_);
}
/** Compute an estimate for a single variable using its incomplete linear delta computed
* during the last update. This is faster than calling the no-argument version of
* calculateEstimate, which operates on all variables.
* @param key
* @return
*/
template<class VALUE>
VALUE calculateEstimate(Key key) const {
const Vector delta = delta_.at(key);
return traits<VALUE>::Retract(theta_.at<VALUE>(key), delta);
}
/** read the current set of optimizer parameters */
const LevenbergMarquardtParams& params() const {
return parameters_;
}
/** update the current set of optimizer parameters */
LevenbergMarquardtParams& params() {
return parameters_;
}
/** Access the current set of factors */
const NonlinearFactorGraph& getFactors() const {
return factors_;
}
/** Access the current linearization point */
const Values& getLinearizationPoint() const {
return theta_;
}
/** Access the current ordering */
const Ordering& getOrdering() const {
return ordering_;
}
/** Access the current set of deltas to the linearization point */
const VectorValues& getDelta() const {
return delta_;
}
/// Calculate marginal covariance on given variable
Matrix marginalCovariance(Key key) const;
/// Marginalize specific keys from a linear graph.
/// Does not check whether keys actually exist in graph.
/// In that case will fail somewhere deep within elimination
static GaussianFactorGraph CalculateMarginalFactors(
const GaussianFactorGraph& graph, const KeyVector& keys,
const GaussianFactorGraph::Eliminate& eliminateFunction = EliminatePreferCholesky);
/// Marginalize specific keys from a nonlinear graph, wrap in LinearContainers
static NonlinearFactorGraph CalculateMarginalFactors(
const NonlinearFactorGraph& graph, const Values& theta, const KeyVector& keys,
const GaussianFactorGraph::Eliminate& eliminateFunction = EliminatePreferCholesky);
protected:
/** A typedef defining an Key-Factor mapping **/
typedef std::map<Key, std::set<Key> > FactorIndex;
/** The L-M optimization parameters **/
LevenbergMarquardtParams parameters_;
/** A flag indicating if the optimizer should enforce probabilistic consistency by maintaining the
* linearization point of all variables involved in linearized/marginal factors at the edge of the
* smoothing window. This idea is from ??? TODO: Look up paper reference **/
bool enforceConsistency_;
/** The nonlinear factors **/
NonlinearFactorGraph factors_;
/** The current linearization point **/
Values theta_;
/** The set of values involved in current linear factors. **/
Values linearValues_;
/** The current ordering */
Ordering ordering_;
/** The current set of linear deltas */
VectorValues delta_;
/** The set of available factor graph slots. These occur because we are constantly deleting factors, leaving holes. **/
std::queue<size_t> availableSlots_;
/** A cross-reference structure to allow efficient factor lookups by key **/
FactorIndex factorIndex_;
/** Augment the list of factors with a set of new factors */
void insertFactors(const NonlinearFactorGraph& newFactors);
/** Remove factors from the list of factors by slot index */
void removeFactors(const std::set<size_t>& deleteFactors);
/** Erase any keys associated with timestamps before the provided time */
void eraseKeys(const KeyVector& keys);
/** Use colamd to update into an efficient ordering */
void reorder(const KeyVector& marginalizeKeys = KeyVector());
/** Optimize the current graph using a modified version of L-M */
Result optimize();
/** Marginalize out selected variables */
void marginalize(const KeyVector& marginalizableKeys);
private:
/** Private methods for printing debug information */
static void PrintKeySet(const std::set<Key>& keys, const std::string& label);
static void PrintKeySet(const gtsam::KeySet& keys, const std::string& label);
static void PrintSymbolicFactor(const NonlinearFactor::shared_ptr& factor);
static void PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor);
static void PrintSymbolicGraph(const NonlinearFactorGraph& graph, const std::string& label);
static void PrintSymbolicGraph(const GaussianFactorGraph& graph, const std::string& label);
}; // BatchFixedLagSmoother
} /// namespace gtsam
#include <gtsam/nonlinear/BatchFixedLagSmoother.h>

6
gtsam_unstable/nonlinear/ConcurrentBatchFilter.h
View File

@ -50,7 +50,7 @@ public:
double error; ///< The final factor graph error
/// Constructor
Result() : iterations(0), lambdas(0), nonlinearVariables(0), linearVariables(0), error(0) {};
Result() : iterations(0), lambdas(0), nonlinearVariables(0), linearVariables(0), error(0) {}
/// Getter methods
size_t getIterations() const { return iterations; }
@ -61,10 +61,10 @@ public:
};
/** Default constructor */
ConcurrentBatchFilter(const LevenbergMarquardtParams& parameters = LevenbergMarquardtParams()) : parameters_(parameters) {};
ConcurrentBatchFilter(const LevenbergMarquardtParams& parameters = LevenbergMarquardtParams()) : parameters_(parameters) {}
/** Default destructor */
~ConcurrentBatchFilter() override {};
~ConcurrentBatchFilter() override = default;
/** Implement a GTSAM standard 'print' function */
void print(const std::string& s = "Concurrent Batch Filter:\n", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override;

16
gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.h
View File

@ -41,10 +41,10 @@ public:
typedef std::shared_ptr<ConcurrentFilter> shared_ptr;
/** Default constructor */
ConcurrentFilter() {};
ConcurrentFilter() = default;
/** Default destructor */
virtual ~ConcurrentFilter() {};
virtual ~ConcurrentFilter() = default;
/** Implement a standard 'print' function */
virtual void print(
@ -58,7 +58,7 @@ public:
* Perform any required operations before the synchronization process starts.
* Called by 'synchronize'
*/
virtual void presync() {};
virtual void presync() {}
/**
* Populate the provided containers with factors that constitute the filter branch summarization
@ -91,7 +91,7 @@ public:
* Perform any required operations after the synchronization process finishes.
* Called by 'synchronize'
*/
virtual void postsync() {};
virtual void postsync() {}
}; // ConcurrentFilter
@ -103,10 +103,10 @@ public:
typedef std::shared_ptr<ConcurrentSmoother> shared_ptr;
/** Default constructor */
ConcurrentSmoother() {};
ConcurrentSmoother() {}
/** Default destructor */
virtual ~ConcurrentSmoother() {};
virtual ~ConcurrentSmoother() = default;
/** Implement a standard 'print' function */
virtual void print(
@ -120,7 +120,7 @@ public:
* Perform any required operations before the synchronization process starts.
* Called by 'synchronize'
*/
virtual void presync() {};
virtual void presync() {}
/**
* Populate the provided containers with factors that constitute the smoother branch summarization
@ -147,7 +147,7 @@ public:
* Perform any required operations after the synchronization process finishes.
* Called by 'synchronize'
*/
virtual void postsync() {};
virtual void postsync() {}
}; // ConcurrentSmoother

129
gtsam_unstable/nonlinear/FixedLagSmoother.h
View File

@ -9,130 +9,13 @@
* -------------------------------------------------------------------------- */
/**
* @file FixedLagSmoother.h
* @brief Base class for a fixed-lag smoother. This mimics the basic interface to iSAM2.
*
* @author Stephen Williams
* @date Feb 27, 2013
*/
// \callgraph
#pragma once
#include <gtsam_unstable/dllexport.h>
#include <gtsam/inference/Key.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <map>
#include <vector>
namespace gtsam {
class GTSAM_UNSTABLE_EXPORT FixedLagSmoother {
public:
/// Typedef for a shared pointer to an Incremental Fixed-Lag Smoother
typedef std::shared_ptr<FixedLagSmoother> shared_ptr;
/// Typedef for a Key-Timestamp map/database
typedef std::map<Key, double> KeyTimestampMap;
typedef std::multimap<double, Key> TimestampKeyMap;
/**
* Meta information returned about the update
*/
// TODO: Think of some more things to put here
struct Result {
size_t iterations; ///< The number of optimizer iterations performed
size_t intermediateSteps; ///< The number of intermediate steps performed within the optimization. For L-M, this is the number of lambdas tried.
size_t nonlinearVariables; ///< The number of variables that can be relinearized
size_t linearVariables; ///< The number of variables that must keep a constant linearization point
double error; ///< The final factor graph error
Result() : iterations(0), intermediateSteps(0), nonlinearVariables(0), linearVariables(0), error(0) {};
/// Getter methods
size_t getIterations() const { return iterations; }
size_t getIntermediateSteps() const { return intermediateSteps; }
size_t getNonlinearVariables() const { return nonlinearVariables; }
size_t getLinearVariables() const { return linearVariables; }
double getError() const { return error; }
void print() const;
};
/** default constructor */
FixedLagSmoother(double smootherLag = 0.0) : smootherLag_(smootherLag) { }
/** destructor */
virtual ~FixedLagSmoother() { }
/** Print the factor for debugging and testing (implementing Testable) */
virtual void print(
const std::string& s = "FixedLagSmoother:\n",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/** Check if two IncrementalFixedLagSmoother Objects are equal */
virtual bool equals(const FixedLagSmoother& rhs, double tol = 1e-9) const;
/** read the current smoother lag */
double smootherLag() const {
return smootherLag_;
}
/** write to the current smoother lag */
double& smootherLag() {
return smootherLag_;
}
/** Access the current set of timestamps associated with each variable */
const KeyTimestampMap& timestamps() const {
return keyTimestampMap_;
}
/** Add new factors, updating the solution and relinearizing as needed. */
virtual Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(),
const Values& newTheta = Values(),
const KeyTimestampMap& timestamps = KeyTimestampMap(),
const FactorIndices& factorsToRemove = FactorIndices()) = 0;
/** Compute an estimate from the incomplete linear delta computed during the last update.
* This delta is incomplete because it was not updated below wildfire_threshold. If only
* a single variable is needed, it is faster to call calculateEstimate(const KEY&).
*/
virtual Values calculateEstimate() const = 0;
#ifdef _MSC_VER
#pragma message("FixedLagSmoother was moved to the gtsam/nonlinear directory")
#else
#warning "FixedLagSmoother was moved to the gtsam/nonlinear directory"
#endif
protected:
/** The length of the smoother lag. Any variable older than this amount will be marginalized out. */
double smootherLag_;
/** The current timestamp associated with each tracked key */
TimestampKeyMap timestampKeyMap_;
KeyTimestampMap keyTimestampMap_;
/** Update the Timestamps associated with the keys */
void updateKeyTimestampMap(const KeyTimestampMap& newTimestamps);
/** Erase keys from the Key-Timestamps database */
void eraseKeyTimestampMap(const KeyVector& keys);
/** Find the most recent timestamp of the system */
double getCurrentTimestamp() const;
/** Find all of the keys associated with timestamps before the provided time */
KeyVector findKeysBefore(double timestamp) const;
/** Find all of the keys associated with timestamps before the provided time */
KeyVector findKeysAfter(double timestamp) const;
}; // FixedLagSmoother
/// Typedef for matlab wrapping
typedef FixedLagSmoother::KeyTimestampMap FixedLagSmootherKeyTimestampMap;
typedef FixedLagSmootherKeyTimestampMap::value_type FixedLagSmootherKeyTimestampMapValue;
typedef FixedLagSmoother::Result FixedLagSmootherResult;
} /// namespace gtsam
#include <gtsam/nonlinear/FixedLagSmoother.h>

2
gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h
View File

@ -20,7 +20,7 @@
// \callgraph
#pragma once
#include <gtsam_unstable/nonlinear/FixedLagSmoother.h>
#include <gtsam/nonlinear/FixedLagSmoother.h>
#include <gtsam/nonlinear/ISAM2.h>
namespace gtsam {

8
gtsam_unstable/nonlinear/LinearizedFactor.h
View File

@ -45,7 +45,7 @@ protected:
public:
/** default constructor for serialization */
LinearizedGaussianFactor() {};
LinearizedGaussianFactor() = default;
/**
* @param gaussian: A jacobian or hessian factor
@ -53,7 +53,7 @@ public:
*/
LinearizedGaussianFactor(const GaussianFactor::shared_ptr& gaussian, const Values& lin_points);
~LinearizedGaussianFactor() override {};
~LinearizedGaussianFactor() override = default;
// access functions
const Values& linearizationPoint() const { return lin_points_; }
@ -128,11 +128,11 @@ public:
// access functions
const constBVector b() const { return Ab_(size()).col(0); }
const constABlock A() const { return Ab_.range(0, size()); };
const constABlock A() const { return Ab_.range(0, size()); }
const constABlock A(Key key) const { return Ab_(std::find(begin(), end(), key) - begin()); }
/** get the dimension of the factor (number of rows on linearization) */
size_t dim() const override { return Ab_.rows(); };
size_t dim() const override { return Ab_.rows(); }
/** Calculate the error of the factor */
double error(const Values& c) const override;

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)