diff --git a/gtsam/geometry/triangulation.cpp b/gtsam/geometry/triangulation.cpp index 7478f5512..c0f69217c 100644 --- a/gtsam/geometry/triangulation.cpp +++ b/gtsam/geometry/triangulation.cpp @@ -23,7 +23,8 @@ namespace gtsam { -Vector4 triangulateHomogeneousDLT(const std::vector& projection_matrices, +Vector4 triangulateHomogeneousDLT( + const std::vector& projection_matrices, const std::vector& measurements, double rank_tol) { // number of cameras @@ -54,9 +55,10 @@ Vector4 triangulateHomogeneousDLT(const std::vector& projection_matric } Point3 triangulateDLT(const std::vector& projection_matrices, - const std::vector& measurements, double rank_tol) { + const std::vector& 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(landmarkKey); } - } // \namespace gtsam diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index e1fada87e..0eb59f016 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -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& projection_matrices, - const std::vector& measurements, - double rank_tol = 1e-9); +GTSAM_EXPORT Vector4 triangulateHomogeneousDLT( + const std::vector& projection_matrices, + const std::vector& 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& proj * @param rank_tol SVD rank tolerance * @return Triangulated Point3 */ -GTSAM_EXPORT Point3 triangulateDLT(const std::vector& projection_matrices, - const std::vector& measurements, double rank_tol = 1e-9); +GTSAM_EXPORT Point3 triangulateDLT( + const std::vector& projection_matrices, + const std::vector& measurements, double rank_tol = 1e-9); /// /** @@ -74,20 +75,19 @@ GTSAM_EXPORT Point3 triangulateDLT(const std::vector& projection_matri * @return graph and initial values */ template -std::pair triangulationGraph(const std::vector& poses, - boost::shared_ptr sharedCal, - const std::vector& measurements, - Key landmarkKey, - const Point3& initialEstimate) { +std::pair triangulationGraph( + const std::vector& poses, boost::shared_ptr sharedCal, + const std::vector& 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 camera_i(pose_i, *sharedCal); - graph.push_back(TriangulationFactor // + graph.push_back(TriangulationFactor // (camera_i, measurements[i], unit2, landmarkKey)); } return std::make_pair(graph, values); @@ -105,15 +105,16 @@ std::pair triangulationGraph(const std::vector std::pair triangulationGraph( const std::vector >& cameras, - const std::vector& measurements, Key landmarkKey, const Point3& initialEstimate) { + const std::vector& 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& camera_i = cameras[i]; - graph.push_back(TriangulationFactor // + graph.push_back(TriangulationFactor // (camera_i, measurements[i], unit2, landmarkKey)); } return std::make_pair(graph, values); @@ -127,8 +128,8 @@ std::pair 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 Point3 triangulateNonlinear(const std::vector& poses, - boost::shared_ptr sharedCal, - const std::vector& measurements, - const Point3& initialEstimate) { + boost::shared_ptr sharedCal, + const std::vector& 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& poses, * @return refined Point3 */ template -Point3 triangulateNonlinear(const std::vector >& cameras, - const std::vector& measurements, - const Point3& initialEstimate) { +Point3 triangulateNonlinear( + const std::vector >& cameras, + const std::vector& 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 >& came */ template 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 -Point3 triangulatePoint3(const std::vector& poses, boost::shared_ptr sharedCal, - const std::vector& measurements, double rank_tol = 1e-9, - bool optimize = false) { +Point3 triangulatePoint3(const std::vector& poses, + boost::shared_ptr sharedCal, + const std::vector& 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& poses, boost::shared_ptr -Point3 triangulatePoint3(const std::vector >& cameras, - const std::vector& measurements, double rank_tol = 1e-9, - bool optimize = false) { +Point3 triangulatePoint3( + const std::vector >& cameras, + const std::vector& 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 >& cameras std::vector projection_matrices; BOOST_FOREACH(const Camera& camera, cameras) projection_matrices.push_back( - CameraProjectionMatrix(camera.calibration())(camera.pose())); + CameraProjectionMatrix(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 >& cameras return point; } -} // \namespace gtsam +} // \namespace gtsam diff --git a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp index 86d8d4162..563da4a9f 100644 --- a/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/BatchFixedLagSmoother.cpp @@ -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(&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(&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 marginalizableKeys = findKeysBefore(current_timestamp - smootherLag_); + std::set 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& deleteFactors) { +void BatchFixedLagSmoother::removeFactors( + const std::set& 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& 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& 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(marginalizeKeys.begin(), marginalizeKeys.end())); + ordering_ = Ordering::colamdConstrainedFirst(factors_, + std::vector(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& 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& marginalizeKeys) { } /* ************************************************************************* */ -void BatchFixedLagSmoother::PrintKeySet(const std::set& keys, const std::string& label) { +void BatchFixedLagSmoother::PrintKeySet(const std::set& 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& keys, const std::st } /* ************************************************************************* */ -void BatchFixedLagSmoother::PrintKeySet(const gtsam::FastSet& keys, const std::string& label) { +void BatchFixedLagSmoother::PrintKeySet(const gtsam::FastSet& 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& 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& marginalizeKeys, + const NonlinearFactorGraph& graph, const Values& theta, + const std::set& 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 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 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(marginalizeKeys.begin(), marginalizeKeys.end())).second; + GaussianFactorGraph marginalLinearFactors = + *linearFactorGraph.eliminatePartialMultifrontal( + std::vector(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(gaussianFactor, theta); + marginalFactors += boost::make_shared( + 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 diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp index 7f4fef574..23cd42a0a 100644 --- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp +++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.cpp @@ -25,11 +25,12 @@ namespace gtsam { /* ************************************************************************* */ -void recursiveMarkAffectedKeys(const Key& key, const ISAM2Clique::shared_ptr& clique, - std::set& additionalKeys) { +void recursiveMarkAffectedKeys(const Key& key, + const ISAM2Clique::shared_ptr& clique, std::set& 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(&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(&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 marginalizableKeys = findKeysBefore(current_timestamp - smootherLag_); + std::set 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::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(), constrainedKeys, - boost::none, additionalMarkedKeys); + ISAM2Result isamResult = isam_.update(newFactors, newTheta, + FastVector(), 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 leafKeys(marginalizableKeys.begin(), marginalizableKeys.end()); + FastList leafKeys(marginalizableKeys.begin(), + marginalizableKeys.end()); isam_.marginalizeLeaves(leafKeys); } @@ -183,7 +191,8 @@ void IncrementalFixedLagSmoother::createOrderingConstraints( } /* ************************************************************************* */ -void IncrementalFixedLagSmoother::PrintKeySet(const std::set& keys, const std::string& label) { +void IncrementalFixedLagSmoother::PrintKeySet(const std::set& 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& 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 diff --git a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h index ad2c7f4e5..31ae20c50 100644 --- a/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h +++ b/gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h @@ -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 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& marginalizableKeys, - boost::optional >& constrainedKeys) const; + boost::optional >& constrainedKeys) const; - private: +private: /** Private methods for printing debug information */ - static void PrintKeySet(const std::set& keys, const std::string& label = "Keys:"); + static void PrintKeySet(const std::set& 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