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

use Switching constructor arguments for noise covariances

release/4.3a0
Varun Agrawal 2022-08-26 18:42:31 -04:00
parent 74bddd9582
commit 129fa68627
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -145,7 +145,7 @@ struct Switching {
// Add "motion models". // Add "motion models".
for (size_t k = 1; k < K; k++) { for (size_t k = 1; k < K; k++) {
KeyVector keys = {X(k), X(k + 1)}; KeyVector keys = {X(k), X(k + 1)};
auto motion_models = motionModels(k); auto motion_models = motionModels(k, between_sigma);
std::vector<NonlinearFactor::shared_ptr> components; std::vector<NonlinearFactor::shared_ptr> components;
for (auto &&f : motion_models) { for (auto &&f : motion_models) {
components.push_back(boost::dynamic_pointer_cast<NonlinearFactor>(f)); components.push_back(boost::dynamic_pointer_cast<NonlinearFactor>(f));
@ -155,7 +155,7 @@ struct Switching {
} }
// Add measurement factors // Add measurement factors
auto measurement_noise = noiseModel::Isotropic::Sigma(1, 0.1); 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), 1.0 * (k - 1), measurement_noise);