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

update switching example to accept measurements in constructor

release/4.3a0
Varun Agrawal 2022-10-14 16:44:06 -04:00
parent 0faf2226d4
commit d387d2cf3f
1 changed files with 11 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -131,17 +131,25 @@ struct Switching {
* @param between_sigma The stddev between poses. * @param between_sigma The stddev between poses.
* @param prior_sigma The stddev on priors (also used for measurements). * @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,
std::vector<double> measurements = {})
: 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.
for (size_t k = 0; k <= K; k++) { for (size_t k = 0; k <= K; k++) {
modes.emplace_back(M(k), 2); modes.emplace_back(M(k), 2);
} }
// If measurements are not provided, we just have the robot moving forward.
if (measurements.size() == 0) {
for (size_t k = 1; k <= K; k++) {
measurements.push_back(k - 1);
}
}
// Create hybrid factor graph. // Create hybrid factor graph.
// Add a prior on X(1). // Add a prior on X(1).
auto prior = boost::make_shared<PriorFactor<double>>( auto prior = boost::make_shared<PriorFactor<double>>(
X(1), 0, noiseModel::Isotropic::Sigma(1, prior_sigma)); X(1), measurements.at(0), noiseModel::Isotropic::Sigma(1, prior_sigma));
nonlinearFactorGraph.push_nonlinear(prior); nonlinearFactorGraph.push_nonlinear(prior);
// Add "motion models". // Add "motion models".
@ -160,7 +168,7 @@ struct Switching {
auto measurement_noise = noiseModel::Isotropic::Sigma(1, prior_sigma); auto measurement_noise = noiseModel::Isotropic::Sigma(1, prior_sigma);
for (size_t k = 2; k <= K; k++) { for (size_t k = 2; k <= K; k++) {
nonlinearFactorGraph.emplace_nonlinear<PriorFactor<double>>( nonlinearFactorGraph.emplace_nonlinear<PriorFactor<double>>(
X(k), 1.0 * (k - 1), measurement_noise); X(k), measurements.at(k - 1), measurement_noise);
} }
// Add "mode chain" // Add "mode chain"