diff --git a/.gitignore b/.gitignore index 43e3bd1682..1c1fc6a813 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,5 @@ /build +/build-steam /debug* .idea *.pyc @@ -19,13 +20,26 @@ vcpkg_installed/ # for QtCreator: CMakeLists.txt.user* xcode/ -/Dockerfile +# /Dockerfile /python/gtsam/notebooks/.ipynb_checkpoints/ellipses-checkpoint.ipynb .cache/ .venv/ # MyST build outputs _build + +/lib +/include +*.egg-info +/results/*.csv +/examples/*.asv +uv.lock + +# analyses +analyses/build +analyses/results +analyses/plots +*callgrind.out* GEMINI.md doc/#*.lyx# /cmake-build-debug/ diff --git a/examples/AbcEquivariantFilterExample.cpp b/examples/AbcEquivariantFilterExample.cpp index 6aaf8c6a64..f2c31d2ebd 100644 --- a/examples/AbcEquivariantFilterExample.cpp +++ b/examples/AbcEquivariantFilterExample.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include diff --git a/examples/Data/.gitignore b/examples/Data/.gitignore index 2211df63dd..8b13789179 100644 --- a/examples/Data/.gitignore +++ b/examples/Data/.gitignore @@ -1 +1 @@ -*.txt + diff --git a/examples/Pose2SLAMExample.cpp b/examples/Pose2SLAMExample.cpp index f3d5559303..2476b4adc9 100644 --- a/examples/Pose2SLAMExample.cpp +++ b/examples/Pose2SLAMExample.cpp @@ -20,49 +20,87 @@ * A simple 2D pose slam example * - The robot moves in a 2 meter square * - The robot moves 2 meters each step, turning 90 degrees after each step - * - The robot initially faces along the X axis (horizontal, to the right in 2D) + * - The robot initially faces along the X axis (horizontal, to the right in + * 2D) * - We have full odometry between pose - * - We have a loop closure constraint when the robot returns to the first position + * - We have a loop closure constraint when the robot returns to the first + * position */ -// In planar SLAM example we use Pose2 variables (x, y, theta) to represent the robot poses +// In planar SLAM example we use Pose2 variables (x, y, theta) to represent the +// robot poses #include // We will use simple integer Keys to refer to the robot poses. #include -// In GTSAM, measurement functions are represented as 'factors'. Several common factors -// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems. -// Here we will use Between factors for the relative motion described by odometry measurements. -// We will also use a Between Factor to encode the loop closure constraint -// Also, we will initialize the robot at the origin using a Prior factor. +// In GTSAM, measurement functions are represented as 'factors'. Several common +// factors have been provided with the library for solving robotics/SLAM/Bundle +// Adjustment problems. Here we will use Between factors for the relative motion +// described by odometry measurements. We will also use a Between Factor to +// encode the loop closure constraint Also, we will initialize the robot at the +// origin using a Prior factor. #include -// When the factors are created, we will add them to a Factor Graph. As the factors we are using -// are nonlinear factors, we will need a Nonlinear Factor Graph. +// When the factors are created, we will add them to a Factor Graph. As the +// factors we are using are nonlinear factors, we will need a Nonlinear Factor +// Graph. #include -// Finally, once all of the factors have been added to our factor graph, we will want to -// solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values. -// GTSAM includes several nonlinear optimizers to perform this step. Here we will use the -// a Gauss-Newton solver +// Finally, once all of the factors have been added to our factor graph, we will +// want to solve/optimize to graph to find the best (Maximum A Posteriori) set +// of variable values. GTSAM includes several nonlinear optimizers to perform +// this step. Here we will use the a Gauss-Newton solver #include -// Once the optimized values have been calculated, we can also calculate the marginal covariance -// of desired variables +// Once the optimized values have been calculated, we can also calculate the +// marginal covariance of desired variables #include -// The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the -// nonlinear functions around an initial linearization point, then solve the linear system -// to update the linearization point. This happens repeatedly until the solver converges -// to a consistent set of variable values. This requires us to specify an initial guess -// for each variable, held in a Values container. +// The nonlinear solvers within GTSAM are iterative solvers, meaning they +// linearize the nonlinear functions around an initial linearization point, then +// solve the linear system to update the linearization point. This happens +// repeatedly until the solver converges to a consistent set of variable values. +// This requires us to specify an initial guess for each variable, held in a +// Values container. #include +#include +#include using namespace std; using namespace gtsam; +// --- Save Poses to CSV --- +int saveResultToFile(Values& result, NonlinearFactorGraph& graph, + const string& filename) { + // Get marginals + Marginals marginals(graph, result); + + // open file, print header + ofstream poses_file(filename); + if (poses_file.is_open()) { + poses_file + << "key,x,y,theta,C11,C12,C13,C22,C23,C33\n"; // Header for Pose2 + // filter results for pose2 + for (const auto& [key, pose] : result.extract()) { + Matrix cov = marginals.marginalCovariance(key); + poses_file << key << "," << setprecision(6) << pose.x() << "," + << setprecision(6) << pose.y() << "," << setprecision(6) + << pose.theta() << "," << setprecision(6) << cov(0, 0) << "," + << setprecision(6) << cov(0, 1) << "," << setprecision(6) + << cov(0, 2) << "," << setprecision(6) << cov(1, 1) << "," + << setprecision(6) << cov(1, 2) << "," << setprecision(6) + << cov(2, 2) << "\n"; + } + poses_file.close(); + return 1; + } else { + cerr << "Error opening file" << endl; + return 0; + } +} + int main(int argc, char** argv) { // 1. Create a factor graph container and add factors to it NonlinearFactorGraph graph; @@ -72,7 +110,8 @@ int main(int argc, char** argv) { auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); graph.addPrior(1, Pose2(0, 0, 0), priorNoise); - // For simplicity, we will use the same noise model for odometry and loop closures + // For simplicity, we will use the same noise model for odometry and loop + // closures auto model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); // 2b. Add odometry factors @@ -83,14 +122,16 @@ int main(int argc, char** argv) { graph.emplace_shared >(4, 5, Pose2(2, 0, M_PI_2), model); // 2c. Add the loop closure constraint - // This factor encodes the fact that we have returned to the same pose. In real systems, - // these constraints may be identified in many ways, such as appearance-based techniques - // with camera images. We will use another Between Factor to enforce this constraint: + // This factor encodes the fact that we have returned to the same pose. In + // real systems, these constraints may be identified in many ways, such as + // appearance-based techniques with camera images. We will use another Between + // Factor to enforce this constraint: graph.emplace_shared >(5, 2, Pose2(2, 0, M_PI_2), model); graph.print("\nFactor Graph:\n"); // print - // 3. Create the data structure to hold the initialEstimate estimate to the solution - // For illustrative purposes, these have been deliberately set to incorrect values + // 3. Create the data structure to hold the initialEstimate estimate to the + // solution For illustrative purposes, these have been deliberately set to + // incorrect values Values initialEstimate; initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2)); initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2)); @@ -105,7 +146,8 @@ int main(int argc, char** argv) { // system solver to use, and the amount of information displayed during // optimization. We will set a few parameters as a demonstration. GaussNewtonParams parameters; - // Stop iterating once the change in error between steps is less than this value + // Stop iterating once the change in error between steps is less than this + // value parameters.relativeErrorTol = 1e-5; // Do not perform more than N iteration steps parameters.maxIterations = 100; @@ -124,5 +166,17 @@ int main(int argc, char** argv) { cout << "x4 covariance:\n" << marginals.marginalCovariance(4) << endl; cout << "x5 covariance:\n" << marginals.marginalCovariance(5) << endl; + // save to file + string outfile = "../results/Pose2SLAMExample.csv"; + if (argc > 1) { + outfile = argv[1]; + } + int success = saveResultToFile(result, graph, outfile); + if (!success) { + cerr << "Failed to save results to file." << endl; + return 1; + } + cout << "Results saved to " << outfile << endl; + return 0; } diff --git a/gtsam/nonlinear/WNOAFactorGraph.cpp b/gtsam/nonlinear/WNOAFactorGraph.cpp new file mode 100644 index 0000000000..8ca346f48c --- /dev/null +++ b/gtsam/nonlinear/WNOAFactorGraph.cpp @@ -0,0 +1,497 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file WNOAFactorGraph.h + * @brief Factor graph that handles computation of interpolated states for WNOA + * @author Sven Lilge + */ + +#include // for GTSAM_USE_TBB +#include +#include + +#include // for Eigen::setNbThreads + +#ifdef GTSAM_USE_TBB +#include +#include +#include +#include +#include +#endif + +#include +#include +#include +#include +#include +#include + +namespace gtsam { + +/* ************************************************************************* */ +namespace { + +#ifdef GTSAM_USE_TBB +template +class _LinearizeOneFactor { + const WNOAFactorGraph& nonlinearGraph_; + const Values& linearizationPoint_; + GaussianFactorGraph& result_; + typename WNOAInterpFactor::PassedInterpData& passedInterpData_; + + public: + // Create functor with constant parameters + _LinearizeOneFactor( + const WNOAFactorGraph& graph, const Values& linearizationPoint, + GaussianFactorGraph& result, + typename WNOAInterpFactor::PassedInterpData& passedInterpData) + : nonlinearGraph_(graph), + linearizationPoint_(linearizationPoint), + result_(result), + passedInterpData_(passedInterpData) {} + // Operator that linearizes a given range of the factors + void operator()(const tbb::blocked_range& blocked_range) const { + for (size_t i = blocked_range.begin(); i != blocked_range.end(); ++i) { + if (nonlinearGraph_[i] && nonlinearGraph_[i]->sendable()) { + // Attempt dynamic cast to WNOAInterpFactor + auto wnoa_factor = std::dynamic_pointer_cast>( + nonlinearGraph_[i]); + if (wnoa_factor) { + result_[i] = + wnoa_factor->linearize(linearizationPoint_, &passedInterpData_); + } else { + result_[i] = nonlinearGraph_[i]->linearize(linearizationPoint_); + } + } else { + result_[i] = GaussianFactor::shared_ptr(); + } + } + } +}; +#endif + +} // namespace + +template +WNOAFactorGraph::WNOAFactorGraph( + std::unordered_map> interp_map, + const Eigen::Vector Q_psd, bool fixed_noise_model) + : interpolator_(Q_psd), + interp_to_borders_map_(std::move(interp_map)), + fixed_noise_model_(fixed_noise_model) { + // Collect unique keys for boundary states to avoid repeated Values::at + // lookups. + border_pose_keys_.reserve(interp_to_borders_map_.size() * 2); + border_vel_keys_.reserve(interp_to_borders_map_.size() * 2); + for (const auto& kv : interp_to_borders_map_) { + const auto& border_states = kv.second; + const auto& left = border_states.first; + const auto& right = border_states.second; + border_pose_keys_.insert(left.pose); + border_pose_keys_.insert(right.pose); + border_vel_keys_.insert(left.vel); + border_vel_keys_.insert(right.vel); + + double tau = kv.first.time; + double t_k = left.time; + double t_kp1 = right.time; + interp_to_LambdaPsi_vec_.emplace_back( + kv.first, std::make_shared( + interpolator_.getLambdaPsi(t_k, t_kp1, tau))); + } + + // Convert map to vector for optimal parallel access patterns + interp_to_borders_vec_ = + std::vector>>( + interp_to_borders_map_.begin(), interp_to_borders_map_.end()); + + // Build compact batch vector: for each unique border pair, list indices of + // interp states + struct LocalBorderHash { + size_t operator()( + const std::pair& p) const noexcept { + const size_t h1 = std::hash{}(p.first); + const size_t h2 = std::hash{}(p.second); + return h1 ^ (h2 + 0x9e3779b9 + (h1 << 6) + (h1 >> 2)); + } + }; + std::unordered_map, std::vector, + LocalBorderHash> + tmp; + tmp.reserve(interp_to_borders_vec_.size()); + for (size_t idx = 0; idx < interp_to_borders_vec_.size(); ++idx) { + const auto& borders = + interp_to_borders_vec_[idx].second; // pair + tmp[borders].push_back(idx); + } + border_batches_.clear(); + border_batches_.reserve(tmp.size()); + for (auto& kv : tmp) border_batches_.emplace_back(std::move(kv)); + } + +/* ************************************************************************* */ +template +std::shared_ptr WNOAFactorGraph::linearize( + const Values& linearizationPoint) const { + // Compute values, Jacobians and conditional covariances for all interpolated + // states + + auto passedInterpData = + std::make_shared::PassedInterpData>(); + + if (fixed_noise_model_) { + passedInterpData->values = getInterpolatedValues( + linearizationPoint, &passedInterpData->jacobians, nullptr); + } else { + passedInterpData->values = + getInterpolatedValues(linearizationPoint, &passedInterpData->jacobians, + &passedInterpData->condCovs); + } + + // create an empty linear FG + GaussianFactorGraph::shared_ptr linearFG = + std::make_shared(); + +#ifdef GTSAM_USE_TBB + + linearFG->resize(size()); + TbbOpenMPMixedScope + threadLimiter; // Limits OpenMP threads since we're mixing TBB and OpenMP + + // First linearize all sendable factors + tbb::parallel_for(tbb::blocked_range(0, size()), + _LinearizeOneFactor(*this, linearizationPoint, + *linearFG, *passedInterpData), + tbb::auto_partitioner()); + + // Linearize all non-sendable factors + for (size_t i = 0; i < size(); i++) { + auto& factor = (*this)[i]; + if (factor && !(factor->sendable())) { + // Attempt dynamic cast to WNOAInterpFactor + auto wnoa_factor = + std::dynamic_pointer_cast>(factor); + if (wnoa_factor) { + (*linearFG)[i] = + wnoa_factor->linearize(linearizationPoint, passedInterpData.get()); + } else { + (*linearFG)[i] = factor->linearize(linearizationPoint); + } + } + } + +#else + + linearFG->reserve(size()); + + // linearize all factors + for (const sharedFactor& factor : factors_) { + // Attempt dynamic cast to WNOAInterpFactor + if (factor) { + auto wnoa_factor = + std::dynamic_pointer_cast>(factor); + if (wnoa_factor) { + linearFG->push_back( + wnoa_factor->linearize(linearizationPoint, passedInterpData.get())); + } else { + linearFG->push_back(factor->linearize(linearizationPoint)); + } + } else + linearFG->push_back(GaussianFactor::shared_ptr()); + } + +#endif + + + return linearFG; +} + +template +double WNOAFactorGraph::error(const Values& values) const { + + double total_error = 0.; + + // Compute values, Jacobians and conditional covariances for all interpolated + // states + + auto passedInterpData = + std::make_shared::PassedInterpData>(); + if (fixed_noise_model_) { + passedInterpData->values = getInterpolatedValues(values, nullptr, nullptr); + } else { + passedInterpData->values = + getInterpolatedValues(values, nullptr, &passedInterpData->condCovs); + } + + // iterate over all the factors_ to accumulate the log probabilities + for (const sharedFactor& factor : factors_) { + if (factor) { + auto wnoa_factor = + std::dynamic_pointer_cast>(factor); + if (wnoa_factor) { + total_error += wnoa_factor->error(values, passedInterpData.get()); + } else { + total_error += factor->error(values); + } + } + } + + return total_error; +} +/* @brief Interpolate all interpolated states based on estimated states. + * Put their values in a Values structure and compute their Jacobians.*/ +template +Values WNOAFactorGraph::getInterpolatedValues( + const Values& values, + std::unordered_map>* InterpJacobians, + std::unordered_map* InterpCondCovs) const { + +#ifdef GTSAM_USE_TBB + // Parallelized version using TBB + // We parallelize over border batches + // Each batch corresponds to a pair of bordering states the set of interpolated states that lie between them + // This allows us to compute interpolated values and Jacobians for all states in a batch together + // This is more efficient than parallelizing over individual interpolated states, due to shared border state computations + // We use TBB's parallel_for with a blocked range to process batches in parallel + // We use thread-local storage to accumulate results before merging them into the final output containers + + TbbOpenMPMixedScope threadLimiter; + // Cache boundary values + std::unordered_map border_pose_cache; + border_pose_cache.reserve(border_pose_keys_.size()); + std::unordered_map border_vel_cache; + border_vel_cache.reserve(border_vel_keys_.size()); + for (Key k : border_pose_keys_) + border_pose_cache.emplace(k, values.at(k)); + for (Key k : border_vel_keys_) + border_vel_cache.emplace(k, values.at(k)); + + // Use precomputed border batches stored in the graph (built in constructor) + auto& batches = this->border_batches_; + + // Thread-local storage for accumulating interpolated values, Jacobians and conditional covariances before merging + struct TLS { + Values local_values; + std::vector>> jacs; + std::vector> condcovs; + std::vector H; + }; + tbb::enumerable_thread_specific tls; + + + // Determine grain size for parallel_for + // This controls the number of batches processed by each thread in one go. + // We want to balance load and overhead. + // Might give a minimal performance boost compared to the default auto_partitioner + unsigned nt = std::thread::hardware_concurrency(); + unsigned phys = nt ? std::max(1u, nt / 2) : 1u; + size_t grain = std::max(1, batches.size() / (8 * phys)); + { + tbb::parallel_for( + tbb::blocked_range(0, batches.size(), grain), + [&](const tbb::blocked_range& r) { + + // Allocate space for variables based on number of batches and batch size to avoid dynamic resizing in the loop + auto& local = tls.local(); + if (local.H.empty()) local.H.resize(8); + size_t expectedInterp = 0; + for (size_t bi = r.begin(); bi != r.end(); ++bi) + expectedInterp += batches[bi].second.size(); // number of interpolated states in this batch + if (InterpJacobians) + local.jacs.reserve(local.jacs.size() + expectedInterp * 2); // 2 factors per interpolated state (pose and velocity) + if (InterpCondCovs) + local.condcovs.reserve(local.condcovs.size() + expectedInterp); // 1 conditional covariance per interpolated state + + // Process each batch in the assigned range + for (size_t bi = r.begin(); bi != r.end(); ++bi) { + // Get border states for this batch from cache + const auto& bp = batches[bi].first; + const StateData& left = bp.first; + const StateData& right = bp.second; + + // Get the corresponding state values from the cache + const auto state_left = TimestampedPoseVelocity( + border_pose_cache[left.pose], border_vel_cache[left.vel], + left.time); + const auto state_right = TimestampedPoseVelocity( + border_pose_cache[right.pose], border_vel_cache[right.vel], + right.time); + + // Precompute local state vars and local-global Jacobians for this + // border pair + std::shared_ptr localGlobalStateJacsPreComp = + std::make_shared(); + std::shared_ptr localStateVecsPreComp = + std::make_shared(); + + if (InterpJacobians) { + // If we're computing Jacobians, we need to compute the local state vectors (xi, xi_dot at the border) and local-global Jacobians (at the border) for this border pair + // We can be reuse those for all interpolated states in this batch (only xi_tau, xi_dot_tau and the interpolation Jacobians change between interpolated states in the same batch) + *localStateVecsPreComp = interpolator_.computeLocalStateVecs( + state_left, state_right, localGlobalStateJacsPreComp.get()); + } else { + // If we're not computing Jacobians, we only need to compute the local state vectors (xi, xi_dot at the border) for this border pair once + // Again, we can reuse them for all interpolated states in this batch + *localStateVecsPreComp = interpolator_.computeLocalStateVecs( + state_left, state_right, nullptr); + } + + // Run through all interpolated states in this batch + for (size_t interpIdx : batches[bi].second) { + // Get the actual interpolated state data (time, keys) for this interpolated state index + const StateData& interp_state = + interp_to_borders_vec_[interpIdx].first; + PoseVelocity result; + if (InterpJacobians) { + // If we're computing Jacobians, we compute the interpolated state value and Jacobians for this interpolated state + // We can reuse the precomputed local state vectors and local-global Jacobians for the border states + // Only the interpolation Jacobians (xi_tau, xi_dot_tau) change between interpolated states in the same batch + // We can also rely on the precomputed Lambda and Psi interpolation matrices for this interpolated state that we stored in the graph during construction (only depends on tau and not the state values) + result = interpolator_.interpolatePoseAndVelocity( + state_left, state_right, interp_state.time, &local.H, + nullptr, nullptr, + interp_to_LambdaPsi_vec_[interpIdx].second, + localStateVecsPreComp, localGlobalStateJacsPreComp); + std::array Jpose = {local.H[0], local.H[1], + local.H[2], local.H[3]}; + std::array Jvel = {local.H[4], local.H[5], + local.H[6], local.H[7]}; + local.jacs.emplace_back(interp_state.pose, std::move(Jpose)); + local.jacs.emplace_back(interp_state.vel, std::move(Jvel)); + } else { + // If we're not computing Jacobians, we only compute the interpolated state value for this interpolated state + // We can reuse the precomputed local state vectors for the border states and the precomputed Lambda and Psi interpolation matrices for this interpolated state that we stored in the graph during construction + result = interpolator_.interpolatePoseAndVelocity( + state_left, state_right, interp_state.time, nullptr, + nullptr, nullptr, + interp_to_LambdaPsi_vec_[interpIdx].second, + localStateVecsPreComp, nullptr); + } + // Insert the computed interpolated state value into the thread-local Values structure + local.local_values.insert(interp_state.pose, + std::move(result.pose)); + local.local_values.insert(interp_state.vel, + std::move(result.vel)); + if (InterpCondCovs) { + // If we're computing conditional covariances, we also compute the conditional covariance for this interpolated state + auto state_tau = TimestampedPoseVelocity( + result, interp_state.time); + Matrix2N Sigma_tau = interpolator_.computeConditionalCov( + state_left, state_right, state_tau); + local.condcovs.emplace_back(interp_state, std::move(Sigma_tau)); + } + } + } + }); + } + + // Now we need to merge the thread-local results into the final output containers + // We reserved space in the thread-local containers based on the expected number of interpolated states to minimize dynamic resizing during merging + Values values_interp; + if (InterpJacobians) + InterpJacobians->reserve(interp_to_borders_vec_.size() * 2); + if (InterpCondCovs) InterpCondCovs->reserve(interp_to_borders_vec_.size()); + for (auto& local : tls) { + // Bulk-insert per-thread Values + values_interp.insert(local.local_values); + if (InterpJacobians) + for (auto& kv : local.jacs) + InterpJacobians->insert_or_assign(kv.first, std::move(kv.second)); + if (InterpCondCovs) + for (auto& sc : local.condcovs) + (*InterpCondCovs)[sc.first] = std::move(sc.second); + } + return values_interp; +#else + // Sequential version + // Logic is the same as in the parallel version, but we can directly insert results into the final output containers without needing thread-local storage or merging + // See comments above for more details on the logic + std::unordered_map border_pose_cache; + border_pose_cache.reserve(border_pose_keys_.size()); + std::unordered_map border_vel_cache; + border_vel_cache.reserve(border_vel_keys_.size()); + for (Key k : border_pose_keys_) + border_pose_cache.emplace(k, values.at(k)); + for (Key k : border_vel_keys_) + border_vel_cache.emplace(k, values.at(k)); + std::vector H(8); + Values values_interp; + if (InterpJacobians) + InterpJacobians->reserve(interp_to_borders_vec_.size() * 2); + if (InterpCondCovs) InterpCondCovs->reserve(interp_to_borders_vec_.size()); + for (const auto& kv : border_batches_) { + const StateData& left = kv.first.first; + const StateData& right = kv.first.second; + const auto state_left = TimestampedPoseVelocity( + border_pose_cache[left.pose], border_vel_cache[left.vel], left.time); + const auto state_right = TimestampedPoseVelocity( + border_pose_cache[right.pose], border_vel_cache[right.vel], right.time); + + // Precompute local state vars and local-global Jacobians for this border + // pair + std::shared_ptr localGlobalStateJacsPreComp = + std::make_shared(); + std::shared_ptr localStateVecsPreComp = + std::make_shared(); + + if (InterpJacobians) { + *localStateVecsPreComp = interpolator_.computeLocalStateVecs( + state_left, state_right, localGlobalStateJacsPreComp.get()); + } else { + *localStateVecsPreComp = + interpolator_.computeLocalStateVecs(state_left, state_right, nullptr); + } + + for (size_t interpIdx : kv.second) { + const StateData& interp_state = interp_to_borders_vec_[interpIdx].first; + PoseVelocity result; + if (InterpJacobians) { + result = interpolator_.interpolatePoseAndVelocity( + state_left, state_right, interp_state.time, &H, nullptr, nullptr, + interp_to_LambdaPsi_vec_[interpIdx].second, localStateVecsPreComp, + localGlobalStateJacsPreComp); + } else { + result = interpolator_.interpolatePoseAndVelocity( + state_left, state_right, interp_state.time, nullptr, nullptr, + nullptr, interp_to_LambdaPsi_vec_[interpIdx].second, + localStateVecsPreComp, nullptr); + } + values_interp.insert(interp_state.pose, result.pose); + values_interp.insert(interp_state.vel, result.vel); + if (InterpJacobians) { + (*InterpJacobians)[interp_state.pose] = + std::array{H[0], H[1], H[2], H[3]}; + (*InterpJacobians)[interp_state.vel] = + std::array{H[4], H[5], H[6], H[7]}; + } + if (InterpCondCovs) { + auto state_tau = + TimestampedPoseVelocity(result, interp_state.time); + Matrix2N Sigma_tau = interpolator_.computeConditionalCov( + state_left, state_right, state_tau); + (*InterpCondCovs)[interp_state] = std::move(Sigma_tau); + } + } + } + return values_interp; +#endif +} + +// Explicit template instantiation +template class WNOAFactorGraph; +template class WNOAFactorGraph; +template class WNOAFactorGraph; +template class WNOAFactorGraph; +template class WNOAFactorGraph; + +} // namespace gtsam diff --git a/gtsam/nonlinear/WNOAFactorGraph.h b/gtsam/nonlinear/WNOAFactorGraph.h new file mode 100644 index 0000000000..0dc318417e --- /dev/null +++ b/gtsam/nonlinear/WNOAFactorGraph.h @@ -0,0 +1,157 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file WNOAFactorGraph.h + * @brief Factor graph that handles computation of interpolated states for WNOA + * @author Sven Lilge + */ + +#pragma once + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace gtsam { + +/** + * @brief Factor graph specialized for WNOA interpolation-aware computation. + * + * `WNOAFactorGraph` wraps standard factor graph functionalities with utilities + * to compute interpolated pose/velocity states under a + * White-Noise-on-Acceleration (WNOA) motion prior. It stores the mapping from + * interpolated query times to their left/right bordering estimated states and + * precomputes interpolation helpers for efficient repeated evaluation. + * + * The graph provides optimized linearization and error computation routines + * that can exploit precomputed interpolation batches to reduce repeated + * work when evaluating many wrapper factors using the same query times. + * + * @tparam PoseType Pose group/type (e.g. `Pose2`, `Pose3`) used by the + * interpolator. + */ +template +class WNOAFactorGraph : public ExpressionFactorGraph { + private: + using This = WNOAFactorGraph; + using Base = ExpressionFactorGraph; + using VelocityType = typename gtsam::traits::TangentVector; + static constexpr int dim = traits::dimension; + + // Convenient matrices + using Matrix2N = Eigen::Matrix; + using MatrixN = Eigen::Matrix; + using VectorN = Eigen::Matrix; + + // Interpolator class + const Interpolator interpolator_; + + using LambdaPsiMats = typename Interpolator::LambdaPsiMats; + using LocalStateVecs = typename Interpolator::LocalStateVecs; + using LocalGlobalStateJacs = + typename Interpolator::LocalGlobalStateJacs; + + // map interpolated state to border states + std::unordered_map> interp_to_borders_map_; + std::vector>> + interp_to_borders_vec_; + std::vector>> + interp_to_LambdaPsi_vec_; + + // Precomputed batches of borders -> indices in interp_to_borders_vec_ + // Each entry contains the border pair and the list of interp indices that + // share those borders. + std::vector, std::vector>> + border_batches_; + + bool fixed_noise_model_ = false; + + std::unordered_set border_pose_keys_; + std::unordered_set border_vel_keys_; + + // Efficient storage for indices of WNOAInterpFactors + std::unordered_set wnoa_interp_factor_indices_; + + /** + * @brief Evaluate interpolated states from bordering estimated states. + * + * Builds a `Values` container containing interpolated pose and velocity + * entries for every `StateData` known in `interp_to_borders_map_`. When + * requested, also fills `InterpJacobians` with flattened Jacobian blocks + * (ordered [LPose,LVel,RPose,RVel]) and `InterpCondCovs` with the + * conditional covariance for each interpolated state. + * + * @param values Outer `Values` providing the bordering estimated states. + * @param InterpJacobians Optional output pointer populated with flattened + * jacobian blocks. + * @param InterpCondCovs Optional output pointer populated with + * per-interpolated-state covariances. + * @return Values Container with interpolated pose and velocity entries. + */ + Values getInterpolatedValues( + const Values& values, + std::unordered_map>* InterpJacobians, + std::unordered_map* InterpCondCovs = nullptr) const; + + public: + /** + * @brief Linearize the graph into a GaussianFactorGraph. + * + * This routine produces a linearized Gaussian factor graph evaluated at + * `linearizationPoint`. It exploits precomputed interpolation data and + * batching to reduce duplicated interpolation work across wrapper + * factors. + * + * @param linearizationPoint Values at which to linearize the nonlinear graph. + * @return std::shared_ptr Linearized Gaussian factor + * graph. + */ + std::shared_ptr linearize( + const Values& linearizationPoint) const; + + /** + * @brief Compute the unnormalized graph error (sum of factor losses). + * + * Computes the scalar error over all factors in the graph. When the + * graph contains interpolation wrapper factors this method uses the + * interpolator to evaluate interpolated states as part of the residual + * computation and can exploit precomputation to improve throughput. + * + * @param values Current `Values` used to evaluate the error. + * @return double Scalar unnormalized error (sum of factor losses). + */ + double error(const Values& values) const; + + /** + * @brief Construct a `WNOAFactorGraph` with interpolation metadata. + * + * @param interp_map Mapping from each interpolated `StateData` to its + * left/right bordering estimated `StateData`. + * @param Q_psd Diagonal PSD vector for the WNOA interpolator (size must match + * PoseType dimension). + * @param fixed_noise_model If true, the graph will not augment measurement + * noise for interpolation. + */ + WNOAFactorGraph( + std::unordered_map> interp_map, + const Eigen::Vector Q_psd, bool fixed_noise_model = false); + +}; + +} // namespace gtsam diff --git a/gtsam/nonlinear/WNOALevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/WNOALevenbergMarquardtOptimizer.cpp new file mode 100644 index 0000000000..78cbe7e018 --- /dev/null +++ b/gtsam/nonlinear/WNOALevenbergMarquardtOptimizer.cpp @@ -0,0 +1,329 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ +/** + * @file WNOALevenbergMarquardtOptimizer.cpp + * @brief A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme for WNOAFactorGraphs (derived from standard LevenbergMarquardtOptimizer) + * @author Sven Lilge + * @date Dec 6, 2012 + */ + + +#include +#include +#include +#include +#include +#include +#include +#include +#if GTSAM_USE_BOOST_FEATURES +#include +#endif + +#include +#include +#include +#include +#include +#include + +using namespace std; + +namespace gtsam { + +typedef internal::LevenbergMarquardtState State; + +/* ************************************************************************* */ +template +WNOALevenbergMarquardtOptimizer::WNOALevenbergMarquardtOptimizer(const WNOAFactorGraph& graph, + const Values& initialValues, + const LevenbergMarquardtParams& params) + : NonlinearOptimizer( + graph, std::unique_ptr(new State(initialValues, graph.error(initialValues), + params.lambdaInitial, params.lambdaFactor))), + wnoa_graph_(graph), + params_(LevenbergMarquardtParams::EnsureHasOrdering(params, graph)) {} + + +template +WNOALevenbergMarquardtOptimizer::WNOALevenbergMarquardtOptimizer(const WNOAFactorGraph& graph, + const Values& initialValues, + const Ordering& ordering, + const LevenbergMarquardtParams& params) + : NonlinearOptimizer( + graph, std::unique_ptr(new State(initialValues, graph.error(initialValues), + params.lambdaInitial, params.lambdaFactor))), + wnoa_graph_(graph), + params_(LevenbergMarquardtParams::ReplaceOrdering(params, ordering)) {} + +/* ************************************************************************* */ +template +void WNOALevenbergMarquardtOptimizer::initTime() { + // use chrono to measure time in microseconds + startTime_ = std::chrono::high_resolution_clock::now(); +} + +/* ************************************************************************* */ +template +double WNOALevenbergMarquardtOptimizer::lambda() const { + auto currentState = static_cast(state_.get()); + return currentState->lambda; +} + +/* ************************************************************************* */ +template +int WNOALevenbergMarquardtOptimizer::getInnerIterations() const { + auto currentState = static_cast(state_.get()); + return currentState->totalNumberInnerIterations; +} + +/* ************************************************************************* */ +template +GaussianFactorGraph::shared_ptr WNOALevenbergMarquardtOptimizer::linearize() const { + return wnoa_graph_.linearize(state_->values); +} + +/* ************************************************************************* */ +template +GaussianFactorGraph WNOALevenbergMarquardtOptimizer::buildDampedSystem( + const GaussianFactorGraph& linear, const VectorValues& sqrtHessianDiagonal) const { + gttic(damp); + auto currentState = static_cast(state_.get()); + + if (params_.verbosityLM >= LevenbergMarquardtParams::DAMPED) + std::cout << "building damped system with lambda " << currentState->lambda << std::endl; + + if (params_.dampingParams.diagonalDamping) + return currentState->buildDampedSystem(linear, sqrtHessianDiagonal); + else + return currentState->buildDampedSystem(linear); +} + +/* ************************************************************************* */ +// Log current error/lambda to file +template +inline void WNOALevenbergMarquardtOptimizer::writeLogFile(double currentError){ + auto currentState = static_cast(state_.get()); + + if (!params_.logFile.empty()) { + ofstream os(params_.logFile.c_str(), ios::app); + // use chrono to measure time in microseconds + auto currentTime = std::chrono::high_resolution_clock::now(); + // Get the time spent in seconds and print it + double timeSpent = std::chrono::duration_cast(currentTime - startTime_).count() / 1e6; + os << /*inner iterations*/ currentState->totalNumberInnerIterations << "," + << timeSpent << "," + << /*current error*/ currentError << "," << currentState->lambda << "," + << /*outer iterations*/ currentState->iterations << endl; + } +} + +/* ************************************************************************* */ +template +bool WNOALevenbergMarquardtOptimizer::tryLambda(const GaussianFactorGraph& linear, + const VectorValues& sqrtHessianDiagonal) { + auto currentState = static_cast(state_.get()); + bool verbose = (params_.verbosityLM >= LevenbergMarquardtParams::TRYLAMBDA); + +#if GTSAM_USE_BOOST_FEATURES +#ifdef GTSAM_USING_NEW_BOOST_TIMERS + boost::timer::cpu_timer lamda_iteration_timer; + lamda_iteration_timer.start(); +#else + boost::timer lamda_iteration_timer; + lamda_iteration_timer.restart(); +#endif +#else + auto start = std::chrono::high_resolution_clock::now(); +#endif + + if (verbose) + cout << "trying lambda = " << currentState->lambda << endl; + + // Build damped system for this lambda (adds prior factors that make it like gradient descent) + auto dampedSystem = buildDampedSystem(linear, sqrtHessianDiagonal); + + // Try solving + double modelFidelity = 0.0; + bool step_is_successful = false; + bool stopSearchingLambda = false; + double newError = numeric_limits::infinity(); + double costChange = 0.0; + Values newValues; + VectorValues delta; + + bool systemSolvedSuccessfully; + try { + // ============ Solve is where most computation happens !! ================= + delta = solve(dampedSystem, params_); + systemSolvedSuccessfully = true; + } catch (const IndeterminantLinearSystemException&) { + systemSolvedSuccessfully = false; + } + + if (systemSolvedSuccessfully) { + if (verbose) + cout << "linear delta norm = " << delta.norm() << endl; + if (params_.verbosityLM >= LevenbergMarquardtParams::TRYDELTA) + delta.print("delta"); + + // Compute the old linearized error as it is not the same + // as the nonlinear error when robust noise models are used. + double oldLinearizedError = linear.error(VectorValues::Zero(delta)); + double newlinearizedError = linear.error(delta); + + // cost change in the linearized system (old - new) + double linearizedCostChange = oldLinearizedError - newlinearizedError; + if (verbose) + cout << "newlinearizedError = " << newlinearizedError + << " linearizedCostChange = " << linearizedCostChange << endl; + + if (linearizedCostChange >= 0) { // step is valid + // update values + gttic(retract); + // ============ This is where the solution is updated ==================== + newValues = currentState->values.retract(delta); + // ======================================================================= + gttoc(retract); + + // compute new error + gttic(compute_error); + if (verbose) + cout << "calculating error:" << endl; + newError = wnoa_graph_.error(newValues); + gttoc(compute_error); + + if (verbose) + cout << "old error (" << currentState->error << ") new (tentative) error (" << newError + << ")" << endl; + + // cost change in the original, nonlinear system (old - new) + costChange = currentState->error - newError; + + if (linearizedCostChange > std::numeric_limits::epsilon() * oldLinearizedError) { + // the (linear) error has to decrease to satisfy this condition + // fidelity of linearized model VS original system between + modelFidelity = costChange / linearizedCostChange; + // if we decrease the error in the nonlinear system and modelFidelity is above threshold + step_is_successful = modelFidelity > params_.minModelFidelity; + if (verbose) + cout << "modelFidelity: " << modelFidelity << endl; + } // else we consider the step non successful and we either increase lambda or stop if error + // change is small + + double minAbsoluteTolerance = params_.relativeErrorTol * currentState->error; + // if the change is small we terminate + if (std::abs(costChange) < minAbsoluteTolerance) { + if (verbose) + cout << "abs(costChange)=" << std::abs(costChange) + << " minAbsoluteTolerance=" << minAbsoluteTolerance + << " (relativeErrorTol=" << params_.relativeErrorTol << ")" << endl; + stopSearchingLambda = true; + } + } + } // if (systemSolvedSuccessfully) + + if (params_.verbosityLM == LevenbergMarquardtParams::SUMMARY) { +#if GTSAM_USE_BOOST_FEATURES +// do timing +#ifdef GTSAM_USING_NEW_BOOST_TIMERS + double iterationTime = 1e-9 * lamda_iteration_timer.elapsed().wall; +#else + double iterationTime = lamda_iteration_timer.elapsed(); +#endif +#else + auto end = std::chrono::high_resolution_clock::now(); + double iterationTime = std::chrono::duration_cast(end - start).count() / 1e6; +#endif + if (currentState->iterations == 0) { + cout << "iter cost cost_change lambda success iter_time" << endl; + } + cout << setw(4) << currentState->iterations << " " << setw(12) << newError << " " << setw(12) << setprecision(2) + << costChange << " " << setw(10) << setprecision(2) << currentState->lambda << " " << setw(6) + << systemSolvedSuccessfully << " " << setw(10) << setprecision(2) << iterationTime << endl; + } + if (step_is_successful) { + // we have successfully decreased the cost and we have good modelFidelity + // NOTE(frank): As we return immediately after this, we move the newValues + // TODO(frank): make Values actually support move. Does not seem to happen now. + state_ = currentState->decreaseLambda(params_, modelFidelity, std::move(newValues), newError); + return true; + } else if (!stopSearchingLambda) { // we failed to solved the system or had no decrease in cost + if (verbose) + cout << "increasing lambda" << endl; + State* modifiedState = static_cast(state_.get()); + modifiedState->increaseLambda(params_); // TODO(frank): make this functional with Values move + + // check if lambda is too big + if (modifiedState->lambda >= params_.lambdaUpperBound) { + if (params_.verbosity >= NonlinearOptimizerParams::TERMINATION || + params_.verbosityLM == LevenbergMarquardtParams::SUMMARY) + cout << "Warning: Levenberg-Marquardt giving up because " + "cannot decrease error with maximum lambda" << endl; + return true; + } else { + return false; // only case where we will keep trying + } + } else { // the change in the cost is very small and it is not worth trying bigger lambdas + if (verbose) + cout << "Levenberg-Marquardt: stopping as relative cost reduction is small" << endl; + return true; + } +} + +/* ************************************************************************* */ +template +GaussianFactorGraph::shared_ptr WNOALevenbergMarquardtOptimizer::iterate() { + auto currentState = static_cast(state_.get()); + + gttic(LM_iterate); + + // Linearize graph + if (params_.verbosityLM >= LevenbergMarquardtParams::DAMPED) + cout << "linearizing = " << endl; + GaussianFactorGraph::shared_ptr linear = linearize(); + + if(currentState->totalNumberInnerIterations==0) { // write initial error + writeLogFile(currentState->error); + + if (params_.verbosityLM == LevenbergMarquardtParams::SUMMARY) { + cout << "Initial error: " << currentState->error + << ", values: " << currentState->values.size() << std::endl; + } + } + + // Only calculate diagonal of Hessian (expensive) once per outer iteration, if we need it + VectorValues sqrtHessianDiagonal; + if (params_.dampingParams.diagonalDamping) { + sqrtHessianDiagonal = linear->hessianDiagonal(); + for (auto& [key, value] : sqrtHessianDiagonal) { + value = value.cwiseMax(params_.dampingParams.minDiagonal).cwiseMin(params_.dampingParams.maxDiagonal).cwiseSqrt(); + } + } + + // Keep increasing lambda until we make make progress + while (!tryLambda(*linear, sqrtHessianDiagonal)) { + auto newState = static_cast(state_.get()); + writeLogFile(newState->error); + } + + return linear; +} + +// Explicit template instantiation to ensure the template gets compiled +template class WNOALevenbergMarquardtOptimizer; +template class WNOALevenbergMarquardtOptimizer; +template class WNOALevenbergMarquardtOptimizer; +template class WNOALevenbergMarquardtOptimizer; +template class WNOALevenbergMarquardtOptimizer; + +} /* namespace gtsam */ + diff --git a/gtsam/nonlinear/WNOALevenbergMarquardtOptimizer.h b/gtsam/nonlinear/WNOALevenbergMarquardtOptimizer.h new file mode 100644 index 0000000000..7b28722621 --- /dev/null +++ b/gtsam/nonlinear/WNOALevenbergMarquardtOptimizer.h @@ -0,0 +1,196 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file WNOALevenbergMarquardtOptimizer.h + * @brief A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme for WNOAFactorGraphs (derived from standard LevenbergMarquardtOptimizer) + * @author Sven Lilge + * @date Dec 6, 2012 + */ + +#pragma once + +#include +#include +#include +#include +#include + +class NonlinearOptimizerMoreOptimizationTest; + +namespace gtsam { + +/** + * @brief Levenberg-Marquardt optimizer tuned for WNOA interpolation graphs. + * + * `WNOALevenbergMarquardtOptimizer` implements the Levenberg-Marquardt + * trust-region algorithm and is specialized to operate on + * `WNOAFactorGraph`. It can leverage interpolation precomputation + * in the factor graph to reduce redundant work during linearization and + * error evaluation when many wrapper factors are present. + * + * @tparam PoseType Pose group/type used by the underlying WNOA graph + * (e.g., `Pose2`, `Pose3`). + */ +template +class GTSAM_EXPORT WNOALevenbergMarquardtOptimizer: public NonlinearOptimizer { + +protected: + /// Reference to the specialized WNOA factor graph. Must outlive this optimizer. + const WNOAFactorGraph& wnoa_graph_; + + /// Copied Levenberg-Marquardt parameters used by this optimizer. + const LevenbergMarquardtParams params_; ///< LM parameters + + /// Time point recording when the optimization started (for logging/timing). + std::chrono::time_point startTime_; ///< time when optimization started + + /// Initialize the `startTime_` to the current time. + void initTime(); + +public: + typedef std::shared_ptr shared_ptr; + + /// @name Constructors/Destructor + /// @{ + + /** + * @brief Construct optimizer for a `WNOAFactorGraph` with initial values. + * + * @param graph The `WNOAFactorGraph` to optimize. + * @param initialValues Initial variable assignments for the optimization. + * @param params Levenberg-Marquardt configuration parameters (optional). + */ + WNOALevenbergMarquardtOptimizer(const WNOAFactorGraph& graph, const Values& initialValues, + const LevenbergMarquardtParams& params = LevenbergMarquardtParams()); + + /** + * @brief Construct optimizer with explicit variable ordering. + * + * Variant that accepts an `Ordering` to control elimination/linearization + * ordering used during factor linearization and solving. + * + * @param graph The `WNOAFactorGraph` to optimize. + * @param initialValues Initial variable assignments. + * @param ordering Variable ordering to use for elimination/linearization. + * @param params Levenberg-Marquardt configuration parameters (optional). + */ + WNOALevenbergMarquardtOptimizer(const WNOAFactorGraph& graph, const Values& initialValues, + const Ordering& ordering, + const LevenbergMarquardtParams& params = LevenbergMarquardtParams()); + + /** + * @brief Virtual destructor. + */ + ~WNOALevenbergMarquardtOptimizer() override { + } + + /// @} + + /// @name Standard interface + /// @{ + + /** + * @brief Return current LM damping parameter (`lambda`). + * @return double Current damping value. + */ + double lambda() const; + + /** + * @brief Return number of inner LM iterations performed during the last step. + * @return int Number of inner iterations. + */ + int getInnerIterations() const; + + /** + * @brief Print optimizer header and parameters for debugging. + * @param str Optional prefix string printed before the header. + */ + void print(const std::string& str = "") const { + std::cout << str << "LevenbergMarquardtOptimizer" << std::endl; + this->params_.print(" parameters:\n"); + } + + /// @} + + /// @name Advanced interface + /// @{ + + /** + * @brief Perform a single LM iteration. + * + * Linearizes the current nonlinear graph, builds the damped linear system + * and attempts a step via the inner LM loop. Returns the linearized + * `GaussianFactorGraph` produced during this iteration. + * + * @return GaussianFactorGraph::shared_ptr Linearized Gaussian factor graph for this iteration. + */ + GaussianFactorGraph::shared_ptr iterate() override; + + /** + * @brief Read-only access to the LM parameters used by this optimizer. + * @return const LevenbergMarquardtParams& Reference to the stored parameters. + */ + const LevenbergMarquardtParams& params() const { + return params_; + } + + /** + * @brief Append a log entry capturing the current optimizer progress. + * @param currentError Current scalar error value to write to the log. + */ + void writeLogFile(double currentError); + + /** + * @brief Linearize the underlying nonlinear factor graph. + * + * This method leverages the efficient linearization routine of WNOAFactor Graph (e.g., leveraging precomputed interpolation batches). + * + * @return GaussianFactorGraph::shared_ptr Linearized Gaussian factor graph. + */ + virtual GaussianFactorGraph::shared_ptr linearize() const; + + /** + * @brief Build a damped (Levenberg-Marquardt) linear system for testing. + * + * Constructs a modified Gaussian factor graph where damping corresponding + * to a candidate `lambda` is applied (via `sqrtHessianDiagonal`). + * + * @param linear Linearized Gaussian factor graph + * @param sqrtHessianDiagonal Per-variable sqrt of the Hessian diagonal used for damping + * @return GaussianFactorGraph Damped Gaussian factor graph + */ + GaussianFactorGraph buildDampedSystem(const GaussianFactorGraph& linear, + const VectorValues& sqrtHessianDiagonal) const; + + /** + * @brief Inner LM loop: try a candidate lambda, apply or reject the step. + * + * Builds and solves the damped system, evaluates the step using the + * objective, and updates optimizer state based on acceptance criteria. + * + * @param linear Linearized Gaussian factor graph + * @param sqrtHessianDiagonal Per-variable sqrt of the Hessian diagonal + * @return bool True if the step was accepted (or algorithm terminates), false if rejected. + */ + bool tryLambda(const GaussianFactorGraph& linear, const VectorValues& sqrtHessianDiagonal); + + /// @} + +protected: + + /** Access the parameters (base class version) */ + const NonlinearOptimizerParams& _params() const override { + return params_; + } +}; + +} diff --git a/gtsam/nonlinear/WnoaInterpFactor.h b/gtsam/nonlinear/WnoaInterpFactor.h index dbe79bbcc1..ff5b671915 100644 --- a/gtsam/nonlinear/WnoaInterpFactor.h +++ b/gtsam/nonlinear/WnoaInterpFactor.h @@ -34,6 +34,8 @@ #include #include #include +#include + #include #include @@ -966,6 +968,131 @@ NonlinearFactorGraph interpolateFactorGraph( return new_graph; } +/** + * @brief Convert a factor graph to a `WNOAFactorGraph` with interpolated states + * removed. + * + * Similar to `interpolateFactorGraph`, this function replaces factors on + * interpolated states with wrapper factors acting on bordering estimated + * states and also inserts WNOA motion prior factors between estimated + * states. The returned graph is specialized as a `WNOAFactorGraph` which + * carries interpolation metadata (borders and PSD). + * + * @tparam PoseType Pose type used in the graph (e.g. `Pose2`, `Pose3`). + * @param graph Input factor graph possibly containing factors on interpolated + * states. + * @param estimated_states Ordered set of estimated `StateData` (main-solve + * states). + * @param interp_states Ordered set of `StateData` entries to be + * interpolated/removed. + * @param Q_psd Diagonal PSD vector for the WNOA motion prior (dimension must + * match PoseType). + * @param fixed_noise If true, do not augment measurement noise models for + * interpolation. + * @return WNOAFactorGraph Graph specialized for WNOA with + * interpolated states removed. + */ +template +WNOAFactorGraph interpolateWNOAFactorGraph( + const NonlinearFactorGraph& graph, + const std::set& estimated_states, + const std::set& interp_states, Vector Q_psd, + bool fixed_noise = false) { + // assert that the pose is the right kind of variable + static_assert( + std::is_same_v::structure_category, + lie_group_tag> || + std::is_same_v::structure_category, + vector_space_tag>, + "Pose type must be either a Lie group or vector space"); + // check dimension on the power spectral density matrix + assert(traits::dimension == Q_psd.size()); + + // Get map from keys to interpolated state, and interpolated state to + // estimated state. + std::unordered_map key_to_interp; + std::unordered_map> + interp_to_borders; + auto iter_est_state = estimated_states.begin(); + for (const StateData& state : interp_states) { + // search for estimated state that upper bound current interpolated state + iter_est_state = + std::lower_bound(iter_est_state, estimated_states.end(), state); + if (iter_est_state == estimated_states.begin()) { + throw std::runtime_error( + "Interpolated state time is before all estimated state times"); + } else if (iter_est_state == estimated_states.end()) { + throw std::runtime_error( + "Interpolated state time is after all estimated state times"); + } else { + // decrement iterator (point to left border) + iter_est_state--; + // map interp to left border index + interp_to_borders[state] = + std::pair(*iter_est_state, *std::next(iter_est_state)); + // map keys to interp state + key_to_interp[state.pose] = state; + key_to_interp[state.vel] = state; + } + } + + // Create new factor graph + WNOAFactorGraph new_graph(interp_to_borders, Q_psd, fixed_noise); + + // Add WNOA prior between all estimated states + auto iter_state = estimated_states.begin(); + while (std::next(iter_state) != estimated_states.end()) { + StateData state_k = *iter_state; + StateData state_kp1 = *std::next(iter_state); + // get time diff + double del_t = state_kp1.time - state_k.time; + // add factor + auto motion_factor = std::make_shared>( + state_k.pose, state_k.vel, state_kp1.pose, state_kp1.vel, del_t, Q_psd); + new_graph.add(motion_factor); + iter_state++; + } + // loop through factors and wrap factors on interpolated states + for (auto& factor : graph) { + // handle null factor + if (!factor) continue; + // if the factor is a WNOA motion factor, do not add it + if (std::dynamic_pointer_cast>(factor)) continue; + // get ordered sets of interpolated and estimated states + std::set factor_interp_states; + std::set factor_estimated_states; + for (Key& key : factor->keys()) { + // check if key is an interpolated value + if (key_to_interp.count(key) > 0) { + // add indices + StateData interp_state = key_to_interp[key]; + factor_interp_states.insert(interp_state); + auto [left, right] = interp_to_borders.at(interp_state); + factor_estimated_states.insert(left); + factor_estimated_states.insert(right); + } + } + // add factor to new graph + if (factor_interp_states.size() == 0) { + // factor does not require interpolation, just add factor as is + new_graph.add(factor); + } else { + // Downcast the NonlinearFactor to a NoiseModelFactor + auto nmfactor = std::dynamic_pointer_cast(factor); + assert(nmfactor && + "Defined factors must be NoiseModelFactor or derivative class"); + + // Define and add factor to new graph + const auto wrapped_factor = std::make_shared>( + nmfactor, factor_estimated_states, factor_interp_states, Q_psd, + fixed_noise); + new_graph.add(wrapped_factor); + } + } + + return new_graph; +} + /** * @brief Update a `Values` with interpolated pose and velocity entries. * diff --git a/gtsam/sam/LocBearingRangeFactor.h b/gtsam/sam/LocBearingRangeFactor.h new file mode 100644 index 0000000000..9a36b7cfe2 --- /dev/null +++ b/gtsam/sam/LocBearingRangeFactor.h @@ -0,0 +1,114 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file LocBearingRangeFactor.h + * @date Apr 1, 2010 + * @author Connor Holmes + * @brief a single factor contains both the bearing and the range to prevent + * handle to pair bearing and range factors. This version of the factor assumes + * that the second input is a fixed landmark, to allow localization. + */ + +#pragma once + +#include +#include + +namespace gtsam { + +/** + * Binary factor for a bearing/range measurement + * @ingroup sam + */ +template ::result_type, + typename R = typename Range::result_type> +class LocBearingRangeFactor + : public ExpressionFactorN, A1, A2> { + private: + typedef BearingRange T; + typedef ExpressionFactorN Base; + typedef LocBearingRangeFactor This; + A2 landmark; + + public: + typedef std::shared_ptr shared_ptr; + + /// Default constructor + LocBearingRangeFactor() {} + + /// Construct from BearingRange instance + LocBearingRangeFactor(Key key1, const A2& landmark, const T& bearingRange, + const SharedNoiseModel& model) + : Base({{key1}}, model, T(bearingRange)), landmark(landmark) { + this->initialize(expression({{key1}})); + } + + /// Construct from separate bearing and range + LocBearingRangeFactor(Key key1, const A2& landmark, const B& measuredBearing, + const R& measuredRange, const SharedNoiseModel& model) + : Base({{key1}}, model, T(measuredBearing, measuredRange)), + landmark(landmark) { + this->initialize(expression({{key1}})); + } + + ~LocBearingRangeFactor() override {} + + /// @return a deep copy of this factor + gtsam::NonlinearFactor::shared_ptr clone() const override { + return std::static_pointer_cast( + gtsam::NonlinearFactor::shared_ptr(new This(*this))); + } + + // Return measurement expression + Expression expression( + const typename Base::ArrayNKeys& keys) const override { + return Expression(T::Measure, Expression(keys[0]), + Expression(landmark)); + } + + Vector evaluateError(const A1& a1, const A2& a2, + OptionalMatrixType H1 = OptionalNone, + OptionalMatrixType H2 = OptionalNone) const { + std::vector Hs(2); + const auto& keys = Factor::keys(); + const Vector error = this->unwhitenedError( + {{keys[0], genericValue(a1)}}, Hs); + if (H1) *H1 = Hs[0]; + if (H2) *H2 = Hs[1]; + return error; + } + + /// print + void print(const std::string& s = "", + const KeyFormatter& kf = DefaultKeyFormatter) const override { + std::cout << s << "LocBearingRangeFactor" << std::endl; + Base::print(s, kf); + } + + private: +#if GTSAM_ENABLE_BOOST_SERIALIZATION + friend class boost::serialization::access; + template + void serialize(ARCHIVE& ar, const unsigned int /*version*/) { + ar& boost::serialization::make_nvp( + "Base", boost::serialization::base_object(*this)); + } +#endif +}; // LocBearingRangeFactor + +/// traits +template +struct traits > + : public Testable > {}; + +} // namespace gtsam diff --git a/gtsam/sam/tests/testLocBearingRangeFactor.cpp b/gtsam/sam/tests/testLocBearingRangeFactor.cpp new file mode 100644 index 0000000000..bd04c433a0 --- /dev/null +++ b/gtsam/sam/tests/testLocBearingRangeFactor.cpp @@ -0,0 +1,81 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testLocBearingRangeFactor.cpp + * @brief Unit tests for LocBearingRangeFactor Class + * @author Connor Holmes + * @date July 2025 + */ + +#include +#include +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; + +namespace { +Key poseKey(1); +Point2 point2(-4.0, 11.0); +Point3 point3(1, 0, 0); + +typedef LocBearingRangeFactor BearingRangeFactor2D; +static SharedNoiseModel model2D(noiseModel::Isotropic::Sigma(2, 0.5)); +BearingRangeFactor2D factor2D(poseKey, point2, 1, 2, model2D); + +typedef LocBearingRangeFactor BearingRangeFactor3D; +static SharedNoiseModel model3D(noiseModel::Isotropic::Sigma(3, 0.5)); +BearingRangeFactor3D factor3D(poseKey, point3, + Pose3().bearing(Point3(1, 0, 0)), 1, model3D); +} + +/* ************************************************************************* */ +TEST(LocBearingRangeFactor, 2D) { + // Set the linearization point + Values values; + values.insert(poseKey, Pose2(1.0, 2.0, 0.57)); + + EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor2D.expression({poseKey}), + values, 1e-7, 1e-5); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor2D, values, 1e-7, 1e-5); +} + +/* ************************************************************************* */ +// TODO(frank): this test is disabled (for now) because the macros below are +// incompatible with the Unit3 localCoordinates. See testBearingFactor... +//TEST(BearingRangeFactor, 3D) { +// // Serialize the factor +// std::string serialized = serializeXML(factor3D); +// +// // And de-serialize it +// BearingRangeFactor3D factor; +// deserializeXML(serialized, factor); +// +// // Set the linearization point +// Values values; +// values.insert(poseKey, Pose3()); +// +// EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression({poseKey, pointKey}), +// values, 1e-7, 1e-5); +// EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5); +//} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */