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

7
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
@ -56,7 +57,8 @@ 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

61
gtsam/geometry/triangulation.h
View File

@ -28,16 +28,16 @@ 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,7 +60,8 @@ 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<Matrix34>& projection_matrices,
const std::vector<Point2>& measurements, double rank_tol = 1e-9); const std::vector<Point2>& measurements, double rank_tol = 1e-9);
/// ///
@ -74,10 +75,9 @@ 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
@ -105,7 +105,8 @@ 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;
@ -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
@ -141,14 +142,13 @@ 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,8 +183,8 @@ 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);
@ -206,7 +206,8 @@ 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,
boost::shared_ptr<CALIBRATION> sharedCal,
const std::vector<Point2>& measurements, double rank_tol = 1e-9, const std::vector<Point2>& measurements, double rank_tol = 1e-9,
bool optimize = false) { bool optimize = false) {
@ -252,7 +253,8 @@ 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<PinholeCamera<CALIBRATION> >& cameras,
const std::vector<Point2>& measurements, double rank_tol = 1e-9, const std::vector<Point2>& measurements, double rank_tol = 1e-9,
bool optimize = false) { bool optimize = false) {
@ -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

124
gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp
View File

@ -28,26 +28,30 @@
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");
@ -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 = "
<< newDelta.norm() << std::endl;
std::cout << "BatchFixedLagSmoother::optimize next error = " << error
<< std::endl; << std::endl;
std::cout << "BatchFixedLagSmoother::optimize next error = " << error << std::endl;
} }
if (error < result.error) { if (error < result.error) {
@ -349,16 +361,18 @@ FixedLagSmoother::Result BatchFixedLagSmoother::optimize() {
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);
@ -495,13 +516,15 @@ 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,
@ -510,35 +533,41 @@ NonlinearFactorGraph BatchFixedLagSmoother::calculateMarginalFactors(
// 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 =
*linearFactorGraph.eliminatePartialMultifrontal(
std::vector<Key>(marginalizeKeys.begin(), marginalizeKeys.end())).second; 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());
} }
} }
@ -548,7 +577,8 @@ NonlinearFactorGraph BatchFixedLagSmoother::calculateMarginalFactors(
"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;
} }

46
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
@ -53,14 +54,17 @@ void IncrementalFixedLagSmoother::print(const std::string& s,
} }
/* ************************************************************************* */ /* ************************************************************************* */
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);

22
gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h
View File

@ -38,10 +38,9 @@ class GTSAM_UNSTABLE_EXPORT IncrementalFixedLagSmoother : public FixedLagSmoothe
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 */
@ -123,13 +122,16 @@ class GTSAM_UNSTABLE_EXPORT IncrementalFixedLagSmoother : public FixedLagSmoothe
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