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

GTSAM-style formatting

release/4.3a0
dellaert 2015-05-13 23:44:46 -07:00
parent 13a4da21b2
commit 79d20b6c44
5 changed files with 192 additions and 146 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
gtsam/geometry/triangulation.cpp
View File

@ -23,7 +23,8 @@
namespace gtsam { namespace gtsam {
Vector4 triangulateHomogeneousDLT(const std::vector<Matrix34>& projection_matrices, Vector4 triangulateHomogeneousDLT(
const std::vector<Matrix34>& projection_matrices,
const std::vector<Point2>& measurements, double rank_tol) { const std::vector<Point2>& measurements, double rank_tol) {
// number of cameras // number of cameras
@ -54,9 +55,10 @@ Vector4 triangulateHomogeneousDLT(const std::vector<Matrix34>& projection_matric
} }
Point3 triangulateDLT(const std::vector<Matrix34>& projection_matrices, Point3 triangulateDLT(const std::vector<Matrix34>& projection_matrices,
const std::vector<Point2>& measurements, double rank_tol) { const std::vector<Point2>& measurements, double rank_tol) {
Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol); Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements,
rank_tol);
// Create 3D point from homogeneous coordinates // Create 3D point from homogeneous coordinates
return Point3(sub((v / v(3)), 0, 3)); return Point3(sub((v / v(3)), 0, 3));
@ -90,6 +92,5 @@ Point3 optimize(const NonlinearFactorGraph& graph, const Values& values,
return result.at<Point3>(landmarkKey); return result.at<Point3>(landmarkKey);
} }
} // \namespace gtsam } // \namespace gtsam

95
gtsam/geometry/triangulation.h
View File

@ -26,18 +26,18 @@
namespace gtsam { namespace gtsam {
/// Exception thrown by triangulateDLT when SVD returns rank < 3 /// Exception thrown by triangulateDLT when SVD returns rank < 3
class TriangulationUnderconstrainedException : public std::runtime_error { class TriangulationUnderconstrainedException: public std::runtime_error {
public: public:
TriangulationUnderconstrainedException() TriangulationUnderconstrainedException() :
: std::runtime_error("Triangulation Underconstrained Exception.") { std::runtime_error("Triangulation Underconstrained Exception.") {
} }
}; };
/// Exception thrown by triangulateDLT when landmark is behind one or more of the cameras /// Exception thrown by triangulateDLT when landmark is behind one or more of the cameras
class TriangulationCheiralityException : public std::runtime_error { class TriangulationCheiralityException: public std::runtime_error {
public: public:
TriangulationCheiralityException() TriangulationCheiralityException() :
: std::runtime_error( std::runtime_error(
"Triangulation Cheirality Exception: The resulting landmark is behind one or more cameras.") { "Triangulation Cheirality Exception: The resulting landmark is behind one or more cameras.") {
} }
}; };
@ -49,9 +49,9 @@ class TriangulationCheiralityException : public std::runtime_error {
* @param rank_tol SVD rank tolerance * @param rank_tol SVD rank tolerance
* @return Triangulated point, in homogeneous coordinates * @return Triangulated point, in homogeneous coordinates
*/ */
GTSAM_EXPORT Vector4 triangulateHomogeneousDLT(const std::vector<Matrix34>& projection_matrices, GTSAM_EXPORT Vector4 triangulateHomogeneousDLT(
const std::vector<Point2>& measurements, const std::vector<Matrix34>& projection_matrices,
double rank_tol = 1e-9); const std::vector<Point2>& measurements, double rank_tol = 1e-9);
/** /**
* DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312 * DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312
@ -60,8 +60,9 @@ GTSAM_EXPORT Vector4 triangulateHomogeneousDLT(const std::vector<Matrix34>& proj
* @param rank_tol SVD rank tolerance * @param rank_tol SVD rank tolerance
* @return Triangulated Point3 * @return Triangulated Point3
*/ */
GTSAM_EXPORT Point3 triangulateDLT(const std::vector<Matrix34>& projection_matrices, GTSAM_EXPORT Point3 triangulateDLT(
const std::vector<Point2>& measurements, double rank_tol = 1e-9); const std::vector<Matrix34>& projection_matrices,
const std::vector<Point2>& measurements, double rank_tol = 1e-9);
/// ///
/** /**
@ -74,20 +75,19 @@ GTSAM_EXPORT Point3 triangulateDLT(const std::vector<Matrix34>& projection_matri
* @return graph and initial values * @return graph and initial values
*/ */
template<class CALIBRATION> template<class CALIBRATION>
std::pair<NonlinearFactorGraph, Values> triangulationGraph(const std::vector<Pose3>& poses, std::pair<NonlinearFactorGraph, Values> triangulationGraph(
boost::shared_ptr<CALIBRATION> sharedCal, const std::vector<Pose3>& poses, boost::shared_ptr<CALIBRATION> sharedCal,
const std::vector<Point2>& measurements, const std::vector<Point2>& measurements, Key landmarkKey,
Key landmarkKey, const Point3& initialEstimate) {
const Point3& initialEstimate) {
Values values; Values values;
values.insert(landmarkKey, initialEstimate); // Initial landmark value values.insert(landmarkKey, initialEstimate); // Initial landmark value
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
static SharedNoiseModel unit2(noiseModel::Unit::Create(2)); static SharedNoiseModel unit2(noiseModel::Unit::Create(2));
static SharedNoiseModel prior_model(noiseModel::Isotropic::Sigma(6, 1e-6)); static SharedNoiseModel prior_model(noiseModel::Isotropic::Sigma(6, 1e-6));
for (size_t i = 0; i < measurements.size(); i++) { for (size_t i = 0; i < measurements.size(); i++) {
const Pose3& pose_i = poses[i]; const Pose3& pose_i = poses[i];
PinholeCamera<CALIBRATION> camera_i(pose_i, *sharedCal); PinholeCamera<CALIBRATION> camera_i(pose_i, *sharedCal);
graph.push_back(TriangulationFactor<CALIBRATION> // graph.push_back(TriangulationFactor<CALIBRATION> //
(camera_i, measurements[i], unit2, landmarkKey)); (camera_i, measurements[i], unit2, landmarkKey));
} }
return std::make_pair(graph, values); return std::make_pair(graph, values);
@ -105,15 +105,16 @@ std::pair<NonlinearFactorGraph, Values> triangulationGraph(const std::vector<Pos
template<class CALIBRATION> template<class CALIBRATION>
std::pair<NonlinearFactorGraph, Values> triangulationGraph( std::pair<NonlinearFactorGraph, Values> triangulationGraph(
const std::vector<PinholeCamera<CALIBRATION> >& cameras, const std::vector<PinholeCamera<CALIBRATION> >& cameras,
const std::vector<Point2>& measurements, Key landmarkKey, const Point3& initialEstimate) { const std::vector<Point2>& measurements, Key landmarkKey,
const Point3& initialEstimate) {
Values values; Values values;
values.insert(landmarkKey, initialEstimate); // Initial landmark value values.insert(landmarkKey, initialEstimate); // Initial landmark value
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
static SharedNoiseModel unit2(noiseModel::Unit::Create(2)); static SharedNoiseModel unit2(noiseModel::Unit::Create(2));
static SharedNoiseModel prior_model(noiseModel::Isotropic::Sigma(6, 1e-6)); static SharedNoiseModel prior_model(noiseModel::Isotropic::Sigma(6, 1e-6));
for (size_t i = 0; i < measurements.size(); i++) { for (size_t i = 0; i < measurements.size(); i++) {
const PinholeCamera<CALIBRATION>& camera_i = cameras[i]; const PinholeCamera<CALIBRATION>& camera_i = cameras[i];
graph.push_back(TriangulationFactor<CALIBRATION> // graph.push_back(TriangulationFactor<CALIBRATION> //
(camera_i, measurements[i], unit2, landmarkKey)); (camera_i, measurements[i], unit2, landmarkKey));
} }
return std::make_pair(graph, values); return std::make_pair(graph, values);
@ -127,8 +128,8 @@ std::pair<NonlinearFactorGraph, Values> triangulationGraph(
* @param landmarkKey to refer to landmark * @param landmarkKey to refer to landmark
* @return refined Point3 * @return refined Point3
*/ */
GTSAM_EXPORT Point3 optimize(const NonlinearFactorGraph& graph, const Values& values, GTSAM_EXPORT Point3 optimize(const NonlinearFactorGraph& graph,
Key landmarkKey); const Values& values, Key landmarkKey);
/** /**
* Given an initial estimate , refine a point using measurements in several cameras * Given an initial estimate , refine a point using measurements in several cameras
@ -140,15 +141,14 @@ GTSAM_EXPORT Point3 optimize(const NonlinearFactorGraph& graph, const Values& va
*/ */
template<class CALIBRATION> template<class CALIBRATION>
Point3 triangulateNonlinear(const std::vector<Pose3>& poses, Point3 triangulateNonlinear(const std::vector<Pose3>& poses,
boost::shared_ptr<CALIBRATION> sharedCal, boost::shared_ptr<CALIBRATION> sharedCal,
const std::vector<Point2>& measurements, const std::vector<Point2>& measurements, const Point3& initialEstimate) {
const Point3& initialEstimate) {
// Create a factor graph and initial values // Create a factor graph and initial values
Values values; Values values;
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
boost::tie(graph, values) = triangulationGraph(poses, sharedCal, measurements, Symbol('p', 0), boost::tie(graph, values) = triangulationGraph(poses, sharedCal, measurements,
initialEstimate); Symbol('p', 0), initialEstimate);
return optimize(graph, values, Symbol('p', 0)); return optimize(graph, values, Symbol('p', 0));
} }
@ -161,15 +161,15 @@ Point3 triangulateNonlinear(const std::vector<Pose3>& poses,
* @return refined Point3 * @return refined Point3
*/ */
template<class CALIBRATION> template<class CALIBRATION>
Point3 triangulateNonlinear(const std::vector<PinholeCamera<CALIBRATION> >& cameras, Point3 triangulateNonlinear(
const std::vector<Point2>& measurements, const std::vector<PinholeCamera<CALIBRATION> >& cameras,
const Point3& initialEstimate) { const std::vector<Point2>& measurements, const Point3& initialEstimate) {
// Create a factor graph and initial values // Create a factor graph and initial values
Values values; Values values;
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
boost::tie(graph, values) = triangulationGraph(cameras, measurements, Symbol('p', 0), boost::tie(graph, values) = triangulationGraph(cameras, measurements,
initialEstimate); Symbol('p', 0), initialEstimate);
return optimize(graph, values, Symbol('p', 0)); return optimize(graph, values, Symbol('p', 0));
} }
@ -183,13 +183,13 @@ Point3 triangulateNonlinear(const std::vector<PinholeCamera<CALIBRATION> >& came
*/ */
template<class CALIBRATION> template<class CALIBRATION>
struct CameraProjectionMatrix { struct CameraProjectionMatrix {
CameraProjectionMatrix(const CALIBRATION& calibration) CameraProjectionMatrix(const CALIBRATION& calibration) :
: K_(calibration.K()) { K_(calibration.K()) {
} }
Matrix34 operator()(const Pose3& pose) const { Matrix34 operator()(const Pose3& pose) const {
return K_ * (pose.inverse().matrix()).block<3, 4>(0, 0); return K_ * (pose.inverse().matrix()).block<3, 4>(0, 0);
} }
private: private:
const Matrix3 K_; const Matrix3 K_;
}; };
@ -206,9 +206,10 @@ struct CameraProjectionMatrix {
* @return Returns a Point3 * @return Returns a Point3
*/ */
template<class CALIBRATION> template<class CALIBRATION>
Point3 triangulatePoint3(const std::vector<Pose3>& poses, boost::shared_ptr<CALIBRATION> sharedCal, Point3 triangulatePoint3(const std::vector<Pose3>& poses,
const std::vector<Point2>& measurements, double rank_tol = 1e-9, boost::shared_ptr<CALIBRATION> sharedCal,
bool optimize = false) { const std::vector<Point2>& measurements, double rank_tol = 1e-9,
bool optimize = false) {
assert(poses.size() == measurements.size()); assert(poses.size() == measurements.size());
if (poses.size() < 2) if (poses.size() < 2)
@ -252,9 +253,10 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses, boost::shared_ptr<CALI
* @return Returns a Point3 * @return Returns a Point3
*/ */
template<class CALIBRATION> template<class CALIBRATION>
Point3 triangulatePoint3(const std::vector<PinholeCamera<CALIBRATION> >& cameras, Point3 triangulatePoint3(
const std::vector<Point2>& measurements, double rank_tol = 1e-9, const std::vector<PinholeCamera<CALIBRATION> >& cameras,
bool optimize = false) { const std::vector<Point2>& measurements, double rank_tol = 1e-9,
bool optimize = false) {
size_t m = cameras.size(); size_t m = cameras.size();
assert(measurements.size() == m); assert(measurements.size() == m);
@ -267,7 +269,8 @@ Point3 triangulatePoint3(const std::vector<PinholeCamera<CALIBRATION> >& cameras
std::vector<Matrix34> projection_matrices; std::vector<Matrix34> projection_matrices;
BOOST_FOREACH(const Camera& camera, cameras) BOOST_FOREACH(const Camera& camera, cameras)
projection_matrices.push_back( projection_matrices.push_back(
CameraProjectionMatrix<CALIBRATION>(camera.calibration())(camera.pose())); CameraProjectionMatrix<CALIBRATION>(camera.calibration())(
camera.pose()));
Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol);
// The n refine using non-linear optimization // The n refine using non-linear optimization
@ -286,5 +289,5 @@ Point3 triangulatePoint3(const std::vector<PinholeCamera<CALIBRATION> >& cameras
return point; return point;
} }
} // \namespace gtsam } // \namespace gtsam

142
gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp
View File

@ -28,27 +28,31 @@
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
void BatchFixedLagSmoother::print(const std::string& s, const KeyFormatter& keyFormatter) const { void BatchFixedLagSmoother::print(const std::string& s,
const KeyFormatter& keyFormatter) const {
FixedLagSmoother::print(s, keyFormatter); FixedLagSmoother::print(s, keyFormatter);
// TODO: What else to print? // TODO: What else to print?
} }
/* ************************************************************************* */ /* ************************************************************************* */
bool BatchFixedLagSmoother::equals(const FixedLagSmoother& rhs, double tol) const { bool BatchFixedLagSmoother::equals(const FixedLagSmoother& rhs,
const BatchFixedLagSmoother* e = dynamic_cast<const BatchFixedLagSmoother*>(&rhs); double tol) const {
return e != NULL && FixedLagSmoother::equals(*e, tol) && factors_.equals(e->factors_, tol) const BatchFixedLagSmoother* e =
&& theta_.equals(e->theta_, tol); dynamic_cast<const BatchFixedLagSmoother*>(&rhs);
return e != NULL && FixedLagSmoother::equals(*e, tol)
&& factors_.equals(e->factors_, tol) && theta_.equals(e->theta_, tol);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Matrix BatchFixedLagSmoother::marginalCovariance(Key key) const { Matrix BatchFixedLagSmoother::marginalCovariance(Key key) const {
throw std::runtime_error("BatchFixedLagSmoother::marginalCovariance not implemented"); throw std::runtime_error(
"BatchFixedLagSmoother::marginalCovariance not implemented");
} }
/* ************************************************************************* */ /* ************************************************************************* */
FixedLagSmoother::Result BatchFixedLagSmoother::update(const NonlinearFactorGraph& newFactors, FixedLagSmoother::Result BatchFixedLagSmoother::update(
const Values& newTheta, const NonlinearFactorGraph& newFactors, const Values& newTheta,
const KeyTimestampMap& timestamps) { const KeyTimestampMap& timestamps) {
const bool debug = ISDEBUG("BatchFixedLagSmoother update"); const bool debug = ISDEBUG("BatchFixedLagSmoother update");
if (debug) { if (debug) {
@ -79,7 +83,8 @@ FixedLagSmoother::Result BatchFixedLagSmoother::update(const NonlinearFactorGrap
std::cout << "Current Timestamp: " << current_timestamp << std::endl; std::cout << "Current Timestamp: " << current_timestamp << std::endl;
// Find the set of variables to be marginalized out // Find the set of variables to be marginalized out
std::set<Key> marginalizableKeys = findKeysBefore(current_timestamp - smootherLag_); std::set<Key> marginalizableKeys = findKeysBefore(
current_timestamp - smootherLag_);
if (debug) { if (debug) {
std::cout << "Marginalizable Keys: "; std::cout << "Marginalizable Keys: ";
BOOST_FOREACH(Key key, marginalizableKeys) { BOOST_FOREACH(Key key, marginalizableKeys) {
@ -116,7 +121,8 @@ FixedLagSmoother::Result BatchFixedLagSmoother::update(const NonlinearFactorGrap
} }
/* ************************************************************************* */ /* ************************************************************************* */
void BatchFixedLagSmoother::insertFactors(const NonlinearFactorGraph& newFactors) { void BatchFixedLagSmoother::insertFactors(
const NonlinearFactorGraph& newFactors) {
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, newFactors) { BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, newFactors) {
Key index; Key index;
// Insert the factor into an existing hole in the factor graph, if possible // Insert the factor into an existing hole in the factor graph, if possible
@ -136,7 +142,8 @@ void BatchFixedLagSmoother::insertFactors(const NonlinearFactorGraph& newFactors
} }
/* ************************************************************************* */ /* ************************************************************************* */
void BatchFixedLagSmoother::removeFactors(const std::set<size_t>& deleteFactors) { void BatchFixedLagSmoother::removeFactors(
const std::set<size_t>& deleteFactors) {
BOOST_FOREACH(size_t slot, deleteFactors) { BOOST_FOREACH(size_t slot, deleteFactors) {
if (factors_.at(slot)) { if (factors_.at(slot)) {
// Remove references to this factor from the FactorIndex // Remove references to this factor from the FactorIndex
@ -149,8 +156,8 @@ void BatchFixedLagSmoother::removeFactors(const std::set<size_t>& deleteFactors)
availableSlots_.push(slot); availableSlots_.push(slot);
} else { } else {
// TODO: Throw an error?? // TODO: Throw an error??
std::cout << "Attempting to remove a factor from slot " << slot << ", but it is already NULL." std::cout << "Attempting to remove a factor from slot " << slot
<< std::endl; << ", but it is already NULL." << std::endl;
} }
} }
} }
@ -198,8 +205,8 @@ void BatchFixedLagSmoother::reorder(const std::set<Key>& marginalizeKeys) {
} }
// COLAMD groups will be used to place marginalize keys in Group 0, and everything else in Group 1 // COLAMD groups will be used to place marginalize keys in Group 0, and everything else in Group 1
ordering_ = Ordering::colamdConstrainedFirst( ordering_ = Ordering::colamdConstrainedFirst(factors_,
factors_, std::vector<Key>(marginalizeKeys.begin(), marginalizeKeys.end())); std::vector<Key>(marginalizeKeys.begin(), marginalizeKeys.end()));
if (debug) { if (debug) {
ordering_.print("New Ordering: "); ordering_.print("New Ordering: ");
@ -241,16 +248,17 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() {
// check if we're already close enough // check if we're already close enough
if (result.error <= errorTol) { if (result.error <= errorTol) {
if (debug) { if (debug) {
std::cout << "BatchFixedLagSmoother::optimize Exiting, as error = " << result.error << " < " std::cout << "BatchFixedLagSmoother::optimize Exiting, as error = "
<< errorTol << std::endl; << result.error << " < " << errorTol << std::endl;
} }
return result; return result;
} }
if (debug) { if (debug) {
std::cout << "BatchFixedLagSmoother::optimize linearValues: " << linearKeys_.size() std::cout << "BatchFixedLagSmoother::optimize linearValues: "
<< std::endl; << linearKeys_.size() << std::endl;
std::cout << "BatchFixedLagSmoother::optimize Initial error: " << result.error << std::endl; std::cout << "BatchFixedLagSmoother::optimize Initial error: "
<< result.error << std::endl;
} }
// Use a custom optimization loop so the linearization points can be controlled // Use a custom optimization loop so the linearization points can be controlled
@ -269,7 +277,8 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() {
while (true) { while (true) {
if (debug) { if (debug) {
std::cout << "BatchFixedLagSmoother::optimize trying lambda = " << lambda << std::endl; std::cout << "BatchFixedLagSmoother::optimize trying lambda = "
<< lambda << std::endl;
} }
// Add prior factors at the current solution // Add prior factors at the current solution
@ -284,7 +293,8 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() {
Matrix A = Matrix::Identity(dim, dim); Matrix A = Matrix::Identity(dim, dim);
Vector b = key_value.second; Vector b = key_value.second;
SharedDiagonal model = noiseModel::Isotropic::Sigma(dim, sigma); SharedDiagonal model = noiseModel::Isotropic::Sigma(dim, sigma);
GaussianFactor::shared_ptr prior(new JacobianFactor(key_value.first, A, b, model)); GaussianFactor::shared_ptr prior(
new JacobianFactor(key_value.first, A, b, model));
dampedFactorGraph.push_back(prior); dampedFactorGraph.push_back(prior);
} }
} }
@ -293,7 +303,8 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() {
gttic(solve); gttic(solve);
// Solve Damped Gaussian Factor Graph // Solve Damped Gaussian Factor Graph
newDelta = dampedFactorGraph.optimize(ordering_, parameters_.getEliminationFunction()); newDelta = dampedFactorGraph.optimize(ordering_,
parameters_.getEliminationFunction());
// update the evalpoint with the new delta // update the evalpoint with the new delta
evalpoint = theta_.retract(newDelta); evalpoint = theta_.retract(newDelta);
gttoc(solve); gttoc(solve);
@ -304,9 +315,10 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() {
gttoc(compute_error); gttoc(compute_error);
if (debug) { if (debug) {
std::cout << "BatchFixedLagSmoother::optimize linear delta norm = " << newDelta.norm() std::cout << "BatchFixedLagSmoother::optimize linear delta norm = "
<< std::endl; << newDelta.norm() << std::endl;
std::cout << "BatchFixedLagSmoother::optimize next error = " << error << std::endl; std::cout << "BatchFixedLagSmoother::optimize next error = " << error
<< std::endl;
} }
if (error < result.error) { if (error < result.error) {
@ -344,21 +356,23 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() {
lambda *= lambdaFactor; lambda *= lambdaFactor;
} }
} }
} // end while } // end while
} }
gttoc(optimizer_iteration); gttoc(optimizer_iteration);
if (debug) { if (debug) {
std::cout << "BatchFixedLagSmoother::optimize using lambda = " << lambda << std::endl; std::cout << "BatchFixedLagSmoother::optimize using lambda = " << lambda
<< std::endl;
} }
result.iterations++; result.iterations++;
} while (result.iterations < maxIterations } while (result.iterations < maxIterations
&& !checkConvergence(relativeErrorTol, absoluteErrorTol, errorTol, previousError, && !checkConvergence(relativeErrorTol, absoluteErrorTol, errorTol,
result.error, NonlinearOptimizerParams::SILENT)); previousError, result.error, NonlinearOptimizerParams::SILENT));
if (debug) { if (debug) {
std::cout << "BatchFixedLagSmoother::optimize newError: " << result.error << std::endl; std::cout << "BatchFixedLagSmoother::optimize newError: " << result.error
<< std::endl;
} }
if (debug) { if (debug) {
@ -414,15 +428,18 @@ void BatchFixedLagSmoother::marginalize(const std::set<Key>& marginalizeKeys) {
} }
if (debug) { if (debug) {
PrintSymbolicGraph(removedFactors, "BatchFixedLagSmoother::marginalize Removed Factors: "); PrintSymbolicGraph(removedFactors,
"BatchFixedLagSmoother::marginalize Removed Factors: ");
} }
// Calculate marginal factors on the remaining keys // Calculate marginal factors on the remaining keys
NonlinearFactorGraph marginalFactors = calculateMarginalFactors( NonlinearFactorGraph marginalFactors = calculateMarginalFactors(
removedFactors, theta_, marginalizeKeys, parameters_.getEliminationFunction()); removedFactors, theta_, marginalizeKeys,
parameters_.getEliminationFunction());
if (debug) { if (debug) {
PrintSymbolicGraph(removedFactors, "BatchFixedLagSmoother::marginalize Marginal Factors: "); PrintSymbolicGraph(removedFactors,
"BatchFixedLagSmoother::marginalize Marginal Factors: ");
} }
// Remove marginalized factors from the factor graph // Remove marginalized factors from the factor graph
@ -436,7 +453,8 @@ void BatchFixedLagSmoother::marginalize(const std::set<Key>& marginalizeKeys) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
void BatchFixedLagSmoother::PrintKeySet(const std::set<Key>& keys, const std::string& label) { void BatchFixedLagSmoother::PrintKeySet(const std::set<Key>& keys,
const std::string& label) {
std::cout << label; std::cout << label;
BOOST_FOREACH(gtsam::Key key, keys) { BOOST_FOREACH(gtsam::Key key, keys) {
std::cout << " " << gtsam::DefaultKeyFormatter(key); std::cout << " " << gtsam::DefaultKeyFormatter(key);
@ -445,7 +463,8 @@ void BatchFixedLagSmoother::PrintKeySet(const std::set<Key>& keys, const std::st
} }
/* ************************************************************************* */ /* ************************************************************************* */
void BatchFixedLagSmoother::PrintKeySet(const gtsam::FastSet<Key>& keys, const std::string& label) { void BatchFixedLagSmoother::PrintKeySet(const gtsam::FastSet<Key>& keys,
const std::string& label) {
std::cout << label; std::cout << label;
BOOST_FOREACH(gtsam::Key key, keys) { BOOST_FOREACH(gtsam::Key key, keys) {
std::cout << " " << gtsam::DefaultKeyFormatter(key); std::cout << " " << gtsam::DefaultKeyFormatter(key);
@ -454,7 +473,8 @@ void BatchFixedLagSmoother::PrintKeySet(const gtsam::FastSet<Key>& keys, const s
} }
/* ************************************************************************* */ /* ************************************************************************* */
void BatchFixedLagSmoother::PrintSymbolicFactor(const NonlinearFactor::shared_ptr& factor) { void BatchFixedLagSmoother::PrintSymbolicFactor(
const NonlinearFactor::shared_ptr& factor) {
std::cout << "f("; std::cout << "f(";
if (factor) { if (factor) {
BOOST_FOREACH(Key key, factor->keys()) { BOOST_FOREACH(Key key, factor->keys()) {
@ -467,7 +487,8 @@ void BatchFixedLagSmoother::PrintSymbolicFactor(const NonlinearFactor::shared_pt
} }
/* ************************************************************************* */ /* ************************************************************************* */
void BatchFixedLagSmoother::PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor) { void BatchFixedLagSmoother::PrintSymbolicFactor(
const GaussianFactor::shared_ptr& factor) {
std::cout << "f("; std::cout << "f(";
BOOST_FOREACH(Key key, factor->keys()) { BOOST_FOREACH(Key key, factor->keys()) {
std::cout << " " << gtsam::DefaultKeyFormatter(key); std::cout << " " << gtsam::DefaultKeyFormatter(key);
@ -476,8 +497,8 @@ void BatchFixedLagSmoother::PrintSymbolicFactor(const GaussianFactor::shared_ptr
} }
/* ************************************************************************* */ /* ************************************************************************* */
void BatchFixedLagSmoother::PrintSymbolicGraph(const NonlinearFactorGraph& graph, void BatchFixedLagSmoother::PrintSymbolicGraph(
const std::string& label) { const NonlinearFactorGraph& graph, const std::string& label) {
std::cout << label << std::endl; std::cout << label << std::endl;
BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, graph) { BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, graph) {
PrintSymbolicFactor(factor); PrintSymbolicFactor(factor);
@ -486,7 +507,7 @@ void BatchFixedLagSmoother::PrintSymbolicGraph(const NonlinearFactorGraph& graph
/* ************************************************************************* */ /* ************************************************************************* */
void BatchFixedLagSmoother::PrintSymbolicGraph(const GaussianFactorGraph& graph, void BatchFixedLagSmoother::PrintSymbolicGraph(const GaussianFactorGraph& graph,
const std::string& label) { const std::string& label) {
std::cout << label << std::endl; std::cout << label << std::endl;
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, graph) { BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, graph) {
PrintSymbolicFactor(factor); PrintSymbolicFactor(factor);
@ -495,64 +516,73 @@ void BatchFixedLagSmoother::PrintSymbolicGraph(const GaussianFactorGraph& graph,
/* ************************************************************************* */ /* ************************************************************************* */
NonlinearFactorGraph BatchFixedLagSmoother::calculateMarginalFactors( NonlinearFactorGraph BatchFixedLagSmoother::calculateMarginalFactors(
const NonlinearFactorGraph& graph, const Values& theta, const std::set<Key>& marginalizeKeys, const NonlinearFactorGraph& graph, const Values& theta,
const std::set<Key>& marginalizeKeys,
const GaussianFactorGraph::Eliminate& eliminateFunction) { const GaussianFactorGraph::Eliminate& eliminateFunction) {
const bool debug = ISDEBUG("BatchFixedLagSmoother calculateMarginalFactors"); const bool debug = ISDEBUG("BatchFixedLagSmoother calculateMarginalFactors");
if (debug) if (debug)
std::cout << "BatchFixedLagSmoother::calculateMarginalFactors START" << std::endl; std::cout << "BatchFixedLagSmoother::calculateMarginalFactors START"
<< std::endl;
if (debug) if (debug)
PrintKeySet(marginalizeKeys, PrintKeySet(marginalizeKeys,
"BatchFixedLagSmoother::calculateMarginalFactors Marginalize Keys: "); "BatchFixedLagSmoother::calculateMarginalFactors Marginalize Keys: ");
// Get the set of all keys involved in the factor graph // Get the set of all keys involved in the factor graph
FastSet<Key> allKeys(graph.keys()); FastSet<Key> allKeys(graph.keys());
if (debug) if (debug)
PrintKeySet(allKeys, "BatchFixedLagSmoother::calculateMarginalFactors All Keys: "); PrintKeySet(allKeys,
"BatchFixedLagSmoother::calculateMarginalFactors All Keys: ");
// Calculate the set of RemainingKeys = AllKeys \Intersect marginalizeKeys // Calculate the set of RemainingKeys = AllKeys \Intersect marginalizeKeys
FastSet<Key> remainingKeys; FastSet<Key> remainingKeys;
std::set_difference(allKeys.begin(), allKeys.end(), marginalizeKeys.begin(), std::set_difference(allKeys.begin(), allKeys.end(), marginalizeKeys.begin(),
marginalizeKeys.end(), std::inserter(remainingKeys, remainingKeys.end())); marginalizeKeys.end(), std::inserter(remainingKeys, remainingKeys.end()));
if (debug) if (debug)
PrintKeySet(remainingKeys, "BatchFixedLagSmoother::calculateMarginalFactors Remaining Keys: "); PrintKeySet(remainingKeys,
"BatchFixedLagSmoother::calculateMarginalFactors Remaining Keys: ");
if (marginalizeKeys.size() == 0) { if (marginalizeKeys.size() == 0) {
// There are no keys to marginalize. Simply return the input factors // There are no keys to marginalize. Simply return the input factors
if (debug) if (debug)
std::cout << "BatchFixedLagSmoother::calculateMarginalFactors FINISH" << std::endl; std::cout << "BatchFixedLagSmoother::calculateMarginalFactors FINISH"
<< std::endl;
return graph; return graph;
} else { } else {
// Create the linear factor graph // Create the linear factor graph
GaussianFactorGraph linearFactorGraph = *graph.linearize(theta); GaussianFactorGraph linearFactorGraph = *graph.linearize(theta);
// .first is the eliminated Bayes tree, while .second is the remaining factor graph // .first is the eliminated Bayes tree, while .second is the remaining factor graph
GaussianFactorGraph marginalLinearFactors = *linearFactorGraph.eliminatePartialMultifrontal( GaussianFactorGraph marginalLinearFactors =
std::vector<Key>(marginalizeKeys.begin(), marginalizeKeys.end())).second; *linearFactorGraph.eliminatePartialMultifrontal(
std::vector<Key>(marginalizeKeys.begin(), marginalizeKeys.end())).second;
// Wrap in nonlinear container factors // Wrap in nonlinear container factors
NonlinearFactorGraph marginalFactors; NonlinearFactorGraph marginalFactors;
marginalFactors.reserve(marginalLinearFactors.size()); marginalFactors.reserve(marginalLinearFactors.size());
BOOST_FOREACH(const GaussianFactor::shared_ptr& gaussianFactor, marginalLinearFactors) { BOOST_FOREACH(const GaussianFactor::shared_ptr& gaussianFactor, marginalLinearFactors) {
marginalFactors += boost::make_shared<LinearContainerFactor>(gaussianFactor, theta); marginalFactors += boost::make_shared<LinearContainerFactor>(
gaussianFactor, theta);
if (debug) { if (debug) {
std::cout << "BatchFixedLagSmoother::calculateMarginalFactors Marginal Factor: "; std::cout
<< "BatchFixedLagSmoother::calculateMarginalFactors Marginal Factor: ";
PrintSymbolicFactor(marginalFactors.back()); PrintSymbolicFactor(marginalFactors.back());
} }
} }
if (debug) if (debug)
PrintSymbolicGraph(marginalFactors, PrintSymbolicGraph(marginalFactors,
"BatchFixedLagSmoother::calculateMarginalFactors All Marginal Factors: "); "BatchFixedLagSmoother::calculateMarginalFactors All Marginal Factors: ");
if (debug) if (debug)
std::cout << "BatchFixedLagSmoother::calculateMarginalFactors FINISH" << std::endl; std::cout << "BatchFixedLagSmoother::calculateMarginalFactors FINISH"
<< std::endl;
return marginalFactors; return marginalFactors;
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
} /// namespace gtsam } /// namespace gtsam

54
gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp
View File

@ -25,11 +25,12 @@
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
void recursiveMarkAffectedKeys(const Key& key, const ISAM2Clique::shared_ptr& clique, void recursiveMarkAffectedKeys(const Key& key,
std::set<Key>& additionalKeys) { const ISAM2Clique::shared_ptr& clique, std::set<Key>& additionalKeys) {
// Check if the separator keys of the current clique contain the specified key // Check if the separator keys of the current clique contain the specified key
if (std::find(clique->conditional()->beginParents(), clique->conditional()->endParents(), key) if (std::find(clique->conditional()->beginParents(),
clique->conditional()->endParents(), key)
!= clique->conditional()->endParents()) { != clique->conditional()->endParents()) {
// Mark the frontal keys of the current clique // Mark the frontal keys of the current clique
@ -47,21 +48,24 @@ void recursiveMarkAffectedKeys(const Key& key, const ISAM2Clique::shared_ptr& cl
/* ************************************************************************* */ /* ************************************************************************* */
void IncrementalFixedLagSmoother::print(const std::string& s, void IncrementalFixedLagSmoother::print(const std::string& s,
const KeyFormatter& keyFormatter) const { const KeyFormatter& keyFormatter) const {
FixedLagSmoother::print(s, keyFormatter); FixedLagSmoother::print(s, keyFormatter);
// TODO: What else to print? // TODO: What else to print?
} }
/* ************************************************************************* */ /* ************************************************************************* */
bool IncrementalFixedLagSmoother::equals(const FixedLagSmoother& rhs, double tol) const { bool IncrementalFixedLagSmoother::equals(const FixedLagSmoother& rhs,
const IncrementalFixedLagSmoother* e = dynamic_cast<const IncrementalFixedLagSmoother*>(&rhs); double tol) const {
return e != NULL && FixedLagSmoother::equals(*e, tol) && isam_.equals(e->isam_, tol); const IncrementalFixedLagSmoother* e =
dynamic_cast<const IncrementalFixedLagSmoother*>(&rhs);
return e != NULL && FixedLagSmoother::equals(*e, tol)
&& isam_.equals(e->isam_, tol);
} }
/* ************************************************************************* */ /* ************************************************************************* */
FixedLagSmoother::Result IncrementalFixedLagSmoother::update(const NonlinearFactorGraph& newFactors, FixedLagSmoother::Result IncrementalFixedLagSmoother::update(
const Values& newTheta, const NonlinearFactorGraph& newFactors, const Values& newTheta,
const KeyTimestampMap& timestamps) { const KeyTimestampMap& timestamps) {
const bool debug = ISDEBUG("IncrementalFixedLagSmoother update"); const bool debug = ISDEBUG("IncrementalFixedLagSmoother update");
@ -84,7 +88,8 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update(const NonlinearFact
std::cout << "Current Timestamp: " << current_timestamp << std::endl; std::cout << "Current Timestamp: " << current_timestamp << std::endl;
// Find the set of variables to be marginalized out // Find the set of variables to be marginalized out
std::set<Key> marginalizableKeys = findKeysBefore(current_timestamp - smootherLag_); std::set<Key> marginalizableKeys = findKeysBefore(
current_timestamp - smootherLag_);
if (debug) { if (debug) {
std::cout << "Marginalizable Keys: "; std::cout << "Marginalizable Keys: ";
@ -102,7 +107,8 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update(const NonlinearFact
if (constrainedKeys) { if (constrainedKeys) {
for (FastMap<Key, int>::const_iterator iter = constrainedKeys->begin(); for (FastMap<Key, int>::const_iterator iter = constrainedKeys->begin();
iter != constrainedKeys->end(); ++iter) { iter != constrainedKeys->end(); ++iter) {
std::cout << DefaultKeyFormatter(iter->first) << "(" << iter->second << ") "; std::cout << DefaultKeyFormatter(iter->first) << "(" << iter->second
<< ") ";
} }
} }
std::cout << std::endl; std::cout << std::endl;
@ -119,17 +125,19 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update(const NonlinearFact
KeyList additionalMarkedKeys(additionalKeys.begin(), additionalKeys.end()); KeyList additionalMarkedKeys(additionalKeys.begin(), additionalKeys.end());
// Update iSAM2 // Update iSAM2
ISAM2Result isamResult = isam_.update(newFactors, newTheta, FastVector<size_t>(), constrainedKeys, ISAM2Result isamResult = isam_.update(newFactors, newTheta,
boost::none, additionalMarkedKeys); FastVector<size_t>(), constrainedKeys, boost::none, additionalMarkedKeys);
if (debug) { if (debug) {
PrintSymbolicTree(isam_, "Bayes Tree After Update, Before Marginalization:"); PrintSymbolicTree(isam_,
"Bayes Tree After Update, Before Marginalization:");
std::cout << "END" << std::endl; std::cout << "END" << std::endl;
} }
// Marginalize out any needed variables // Marginalize out any needed variables
if (marginalizableKeys.size() > 0) { if (marginalizableKeys.size() > 0) {
FastList<Key> leafKeys(marginalizableKeys.begin(), marginalizableKeys.end()); FastList<Key> leafKeys(marginalizableKeys.begin(),
marginalizableKeys.end());
isam_.marginalizeLeaves(leafKeys); isam_.marginalizeLeaves(leafKeys);
} }
@ -183,7 +191,8 @@ void IncrementalFixedLagSmoother::createOrderingConstraints(
} }
/* ************************************************************************* */ /* ************************************************************************* */
void IncrementalFixedLagSmoother::PrintKeySet(const std::set<Key>& keys, const std::string& label) { void IncrementalFixedLagSmoother::PrintKeySet(const std::set<Key>& keys,
const std::string& label) {
std::cout << label; std::cout << label;
BOOST_FOREACH(gtsam::Key key, keys) { BOOST_FOREACH(gtsam::Key key, keys) {
std::cout << " " << gtsam::DefaultKeyFormatter(key); std::cout << " " << gtsam::DefaultKeyFormatter(key);
@ -192,7 +201,8 @@ void IncrementalFixedLagSmoother::PrintKeySet(const std::set<Key>& keys, const s
} }
/* ************************************************************************* */ /* ************************************************************************* */
void IncrementalFixedLagSmoother::PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor) { void IncrementalFixedLagSmoother::PrintSymbolicFactor(
const GaussianFactor::shared_ptr& factor) {
std::cout << "f("; std::cout << "f(";
BOOST_FOREACH(gtsam::Key key, factor->keys()) { BOOST_FOREACH(gtsam::Key key, factor->keys()) {
std::cout << " " << gtsam::DefaultKeyFormatter(key); std::cout << " " << gtsam::DefaultKeyFormatter(key);
@ -201,8 +211,8 @@ void IncrementalFixedLagSmoother::PrintSymbolicFactor(const GaussianFactor::shar
} }
/* ************************************************************************* */ /* ************************************************************************* */
void IncrementalFixedLagSmoother::PrintSymbolicGraph(const GaussianFactorGraph& graph, void IncrementalFixedLagSmoother::PrintSymbolicGraph(
const std::string& label) { const GaussianFactorGraph& graph, const std::string& label) {
std::cout << label << std::endl; std::cout << label << std::endl;
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, graph) { BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, graph) {
PrintSymbolicFactor(factor); PrintSymbolicFactor(factor);
@ -211,7 +221,7 @@ void IncrementalFixedLagSmoother::PrintSymbolicGraph(const GaussianFactorGraph&
/* ************************************************************************* */ /* ************************************************************************* */
void IncrementalFixedLagSmoother::PrintSymbolicTree(const gtsam::ISAM2& isam, void IncrementalFixedLagSmoother::PrintSymbolicTree(const gtsam::ISAM2& isam,
const std::string& label) { const std::string& label) {
std::cout << label << std::endl; std::cout << label << std::endl;
if (!isam.roots().empty()) { if (!isam.roots().empty()) {
BOOST_FOREACH(const ISAM2::sharedClique& root, isam.roots()) { BOOST_FOREACH(const ISAM2::sharedClique& root, isam.roots()) {
@ -244,4 +254,4 @@ void IncrementalFixedLagSmoother::PrintSymbolicTreeHelper(
} }
/* ************************************************************************* */ /* ************************************************************************* */
} /// namespace gtsam } /// namespace gtsam

38
gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h
View File

@ -30,18 +30,17 @@ namespace gtsam {
* such that the active states are placed in/near the root. This base class implements a function * such that the active states are placed in/near the root. This base class implements a function
* to calculate the ordering, and an update function to incorporate new factors into the HMF. * to calculate the ordering, and an update function to incorporate new factors into the HMF.
*/ */
class GTSAM_UNSTABLE_EXPORT IncrementalFixedLagSmoother : public FixedLagSmoother { class GTSAM_UNSTABLE_EXPORT IncrementalFixedLagSmoother: public FixedLagSmoother {
public: public:
/// Typedef for a shared pointer to an Incremental Fixed-Lag Smoother /// Typedef for a shared pointer to an Incremental Fixed-Lag Smoother
typedef boost::shared_ptr<IncrementalFixedLagSmoother> shared_ptr; typedef boost::shared_ptr<IncrementalFixedLagSmoother> shared_ptr;
/** default constructor */ /** default constructor */
IncrementalFixedLagSmoother(double smootherLag = 0.0, const ISAM2Params& parameters = IncrementalFixedLagSmoother(double smootherLag = 0.0,
ISAM2Params()) const ISAM2Params& parameters = ISAM2Params()) :
: FixedLagSmoother(smootherLag), FixedLagSmoother(smootherLag), isam_(parameters) {
isam_(parameters) {
} }
/** destructor */ /** destructor */
@ -50,7 +49,7 @@ class GTSAM_UNSTABLE_EXPORT IncrementalFixedLagSmoother : public FixedLagSmoothe
/** Print the factor for debugging and testing (implementing Testable) */ /** Print the factor for debugging and testing (implementing Testable) */
virtual void print(const std::string& s = "IncrementalFixedLagSmoother:\n", virtual void print(const std::string& s = "IncrementalFixedLagSmoother:\n",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/** Check if two IncrementalFixedLagSmoother Objects are equal */ /** Check if two IncrementalFixedLagSmoother Objects are equal */
virtual bool equals(const FixedLagSmoother& rhs, double tol = 1e-9) const; virtual bool equals(const FixedLagSmoother& rhs, double tol = 1e-9) const;
@ -62,8 +61,8 @@ class GTSAM_UNSTABLE_EXPORT IncrementalFixedLagSmoother : public FixedLagSmoothe
* @param timestamps an (optional) map from keys to real time stamps * @param timestamps an (optional) map from keys to real time stamps
*/ */
Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(), Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(),
const Values& newTheta = Values(), // const Values& newTheta = Values(), //
const KeyTimestampMap& timestamps = KeyTimestampMap()); const KeyTimestampMap& timestamps = KeyTimestampMap());
/** Compute an estimate from the incomplete linear delta computed during the last update. /** 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 * This delta is incomplete because it was not updated below wildfire_threshold. If only
@ -109,7 +108,7 @@ class GTSAM_UNSTABLE_EXPORT IncrementalFixedLagSmoother : public FixedLagSmoothe
return isam_.marginalCovariance(key); return isam_.marginalCovariance(key);
} }
protected: protected:
/** An iSAM2 object used to perform inference. The smoother lag is controlled /** An iSAM2 object used to perform inference. The smoother lag is controlled
* by what factors are removed each iteration */ * by what factors are removed each iteration */
ISAM2 isam_; ISAM2 isam_;
@ -119,17 +118,20 @@ class GTSAM_UNSTABLE_EXPORT IncrementalFixedLagSmoother : public FixedLagSmoothe
/** Fill in an iSAM2 ConstrainedKeys structure such that the provided keys are eliminated before all others */ /** Fill in an iSAM2 ConstrainedKeys structure such that the provided keys are eliminated before all others */
void createOrderingConstraints(const std::set<Key>& marginalizableKeys, void createOrderingConstraints(const std::set<Key>& marginalizableKeys,
boost::optional<FastMap<Key, int> >& constrainedKeys) const; boost::optional<FastMap<Key, int> >& constrainedKeys) const;
private: private:
/** Private methods for printing debug information */ /** Private methods for printing debug information */
static void PrintKeySet(const std::set<Key>& keys, const std::string& label = "Keys:"); static void PrintKeySet(const std::set<Key>& keys, const std::string& label =
"Keys:");
static void PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor); static void PrintSymbolicFactor(const GaussianFactor::shared_ptr& factor);
static void PrintSymbolicGraph(const GaussianFactorGraph& graph, const std::string& label = static void PrintSymbolicGraph(const GaussianFactorGraph& graph,
"Factor Graph:"); const std::string& label = "Factor Graph:");
static void PrintSymbolicTree(const gtsam::ISAM2& isam, const std::string& label = "Bayes Tree:"); static void PrintSymbolicTree(const gtsam::ISAM2& isam,
static void PrintSymbolicTreeHelper(const gtsam::ISAM2Clique::shared_ptr& clique, const std::string& label = "Bayes Tree:");
const std::string indent = ""); static void PrintSymbolicTreeHelper(
const gtsam::ISAM2Clique::shared_ptr& clique, const std::string indent =
"");
}; };
// IncrementalFixedLagSmoother // IncrementalFixedLagSmoother