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

more docs for Switching example

release/4.3a0
Varun Agrawal 2022-10-04 12:34:53 -04:00
parent 9002b68291
commit 6238a1f901
1 changed files with 9 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
gtsam/hybrid/tests/Switching.h
View File

@ -115,7 +115,6 @@ inline std::pair<KeyVector, std::vector<int>> makeBinaryOrdering(
/* *************************************************************************** /* ***************************************************************************
*/ */
using MotionModel = BetweenFactor<double>; using MotionModel = BetweenFactor<double>;
// using MotionMixture = MixtureFactor<MotionModel>;
// Test fixture with switching network. // Test fixture with switching network.
struct Switching { struct Switching {
@ -125,7 +124,13 @@ struct Switching {
HybridGaussianFactorGraph linearizedFactorGraph; HybridGaussianFactorGraph linearizedFactorGraph;
Values linearizationPoint; Values linearizationPoint;
/// Create with given number of time steps. /**
* @brief Create with given number of time steps.
*
* @param K The total number of timesteps.
* @param between_sigma The stddev between poses.
* @param prior_sigma The stddev on priors (also used for measurements).
*/
Switching(size_t K, double between_sigma = 1.0, double prior_sigma = 0.1) Switching(size_t K, double between_sigma = 1.0, double prior_sigma = 0.1)
: K(K) { : K(K) {
// Create DiscreteKeys for binary K modes, modes[0] will not be used. // Create DiscreteKeys for binary K modes, modes[0] will not be used.
@ -166,6 +171,8 @@ struct Switching {
linearizationPoint.insert<double>(X(k), static_cast<double>(k)); linearizationPoint.insert<double>(X(k), static_cast<double>(k));
} }
// The ground truth is robot moving forward
// and one less than the linearization point
linearizedFactorGraph = *nonlinearFactorGraph.linearize(linearizationPoint); linearizedFactorGraph = *nonlinearFactorGraph.linearize(linearizationPoint);
} }