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 {
Vector4 triangulateHomogeneousDLT(const std::vector<Matrix34>& projection_matrices,
Vector4 triangulateHomogeneousDLT(
const std::vector<Matrix34>& projection_matrices,
const std::vector<Point2>& measurements, double rank_tol) {
// number of cameras
@ -54,9 +55,10 @@ Vector4 triangulateHomogeneousDLT(const std::vector<Matrix34>& projection_matric
}
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
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);
}
} // \namespace gtsam

95
gtsam/geometry/triangulation.h
View File

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

142
gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp
View File

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

54
gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp
View File

@ -25,11 +25,12 @@
namespace gtsam {
/* ************************************************************************* */
void recursiveMarkAffectedKeys(const Key& key, const ISAM2Clique::shared_ptr& clique,
std::set<Key>& additionalKeys) {
void recursiveMarkAffectedKeys(const Key& key,
const ISAM2Clique::shared_ptr& clique, std::set<Key>& additionalKeys) {
// 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()) {
// 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,
const KeyFormatter& keyFormatter) const {
const KeyFormatter& keyFormatter) const {
FixedLagSmoother::print(s, keyFormatter);
// TODO: What else to print?
}
/* ************************************************************************* */
bool IncrementalFixedLagSmoother::equals(const FixedLagSmoother& rhs, double tol) const {
const IncrementalFixedLagSmoother* e = dynamic_cast<const IncrementalFixedLagSmoother*>(&rhs);
return e != NULL && FixedLagSmoother::equals(*e, tol) && isam_.equals(e->isam_, tol);
bool IncrementalFixedLagSmoother::equals(const FixedLagSmoother& rhs,
double tol) const {
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,
const Values& newTheta,
const KeyTimestampMap& timestamps) {
FixedLagSmoother::Result IncrementalFixedLagSmoother::update(
const NonlinearFactorGraph& newFactors, const Values& newTheta,
const KeyTimestampMap& timestamps) {
const bool debug = ISDEBUG("IncrementalFixedLagSmoother update");
@ -84,7 +88,8 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update(const NonlinearFact
std::cout << "Current Timestamp: " << current_timestamp << std::endl;
// 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) {
std::cout << "Marginalizable Keys: ";
@ -102,7 +107,8 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update(const NonlinearFact
if (constrainedKeys) {
for (FastMap<Key, int>::const_iterator iter = constrainedKeys->begin();
iter != constrainedKeys->end(); ++iter) {
std::cout << DefaultKeyFormatter(iter->first) << "(" << iter->second << ") ";
std::cout << DefaultKeyFormatter(iter->first) << "(" << iter->second
<< ") ";
}
}
std::cout << std::endl;
@ -119,17 +125,19 @@ FixedLagSmoother::Result IncrementalFixedLagSmoother::update(const NonlinearFact
KeyList additionalMarkedKeys(additionalKeys.begin(), additionalKeys.end());
// Update iSAM2
ISAM2Result isamResult = isam_.update(newFactors, newTheta, FastVector<size_t>(), constrainedKeys,
boost::none, additionalMarkedKeys);
ISAM2Result isamResult = isam_.update(newFactors, newTheta,
FastVector<size_t>(), constrainedKeys, boost::none, additionalMarkedKeys);
if (debug) {
PrintSymbolicTree(isam_, "Bayes Tree After Update, Before Marginalization:");
PrintSymbolicTree(isam_,
"Bayes Tree After Update, Before Marginalization:");
std::cout << "END" << std::endl;
}
// Marginalize out any needed variables
if (marginalizableKeys.size() > 0) {
FastList<Key> leafKeys(marginalizableKeys.begin(), marginalizableKeys.end());
FastList<Key> leafKeys(marginalizableKeys.begin(),
marginalizableKeys.end());
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;
BOOST_FOREACH(gtsam::Key key, keys) {
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(";
BOOST_FOREACH(gtsam::Key key, factor->keys()) {
std::cout << " " << gtsam::DefaultKeyFormatter(key);
@ -201,8 +211,8 @@ void IncrementalFixedLagSmoother::PrintSymbolicFactor(const GaussianFactor::shar
}
/* ************************************************************************* */
void IncrementalFixedLagSmoother::PrintSymbolicGraph(const GaussianFactorGraph& graph,
const std::string& label) {
void IncrementalFixedLagSmoother::PrintSymbolicGraph(
const GaussianFactorGraph& graph, const std::string& label) {
std::cout << label << std::endl;
BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, graph) {
PrintSymbolicFactor(factor);
@ -211,7 +221,7 @@ void IncrementalFixedLagSmoother::PrintSymbolicGraph(const GaussianFactorGraph&
/* ************************************************************************* */
void IncrementalFixedLagSmoother::PrintSymbolicTree(const gtsam::ISAM2& isam,
const std::string& label) {
const std::string& label) {
std::cout << label << std::endl;
if (!isam.roots().empty()) {
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
* 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 boost::shared_ptr<IncrementalFixedLagSmoother> shared_ptr;
/** default constructor */
IncrementalFixedLagSmoother(double smootherLag = 0.0, const ISAM2Params& parameters =
ISAM2Params())
: FixedLagSmoother(smootherLag),
isam_(parameters) {
IncrementalFixedLagSmoother(double smootherLag = 0.0,
const ISAM2Params& parameters = ISAM2Params()) :
FixedLagSmoother(smootherLag), isam_(parameters) {
}
/** destructor */
@ -50,7 +49,7 @@ class GTSAM_UNSTABLE_EXPORT IncrementalFixedLagSmoother : public FixedLagSmoothe
/** Print the factor for debugging and testing (implementing Testable) */
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 */
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
*/
Result update(const NonlinearFactorGraph& newFactors = NonlinearFactorGraph(),
const Values& newTheta = Values(), //
const KeyTimestampMap& timestamps = KeyTimestampMap());
const Values& newTheta = Values(), //
const KeyTimestampMap& timestamps = KeyTimestampMap());
/** 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
@ -109,7 +108,7 @@ class GTSAM_UNSTABLE_EXPORT IncrementalFixedLagSmoother : public FixedLagSmoothe
return isam_.marginalCovariance(key);
}
protected:
protected:
/** An iSAM2 object used to perform inference. The smoother lag is controlled
* by what factors are removed each iteration */
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 */
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 */
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 PrintSymbolicGraph(const GaussianFactorGraph& graph, const std::string& label =
"Factor Graph:");
static void PrintSymbolicTree(const gtsam::ISAM2& isam, const std::string& label = "Bayes Tree:");
static void PrintSymbolicTreeHelper(const gtsam::ISAM2Clique::shared_ptr& clique,
const std::string indent = "");
static void PrintSymbolicGraph(const GaussianFactorGraph& graph,
const std::string& label = "Factor Graph:");
static void PrintSymbolicTree(const gtsam::ISAM2& isam,
const std::string& label = "Bayes Tree:");
static void PrintSymbolicTreeHelper(
const gtsam::ISAM2Clique::shared_ptr& clique, const std::string indent =
"");
};
// IncrementalFixedLagSmoother