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

remove linearizedFactorGraph and use linearized unary and binary factors

release/4.3a0
Varun Agrawal 2024-10-30 13:51:39 -04:00
parent d48b1fc840
commit 7f07509388
1 changed files with 16 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -120,21 +120,13 @@ using MotionModel = BetweenFactor<double>;
// Test fixture with switching network. // Test fixture with switching network.
/// ϕ(X(0)) .. ϕ(X(k),X(k+1)) .. ϕ(X(k);z_k) .. ϕ(M(0)) .. ϕ(M(K-3),M(K-2)) /// ϕ(X(0)) .. ϕ(X(k),X(k+1)) .. ϕ(X(k);z_k) .. ϕ(M(0)) .. ϕ(M(K-3),M(K-2))
struct Switching { struct Switching {
private:
HybridNonlinearFactorGraph nonlinearFactorGraph_;
public: public:
size_t K; size_t K;
DiscreteKeys modes; DiscreteKeys modes;
HybridNonlinearFactorGraph unaryFactors, binaryFactors, modeChain; HybridNonlinearFactorGraph unaryFactors, binaryFactors, modeChain;
HybridGaussianFactorGraph linearizedFactorGraph; HybridGaussianFactorGraph linearUnaryFactors, linearBinaryFactors;
Values linearizationPoint; Values linearizationPoint;
// Access the flat nonlinear factor graph.
const HybridNonlinearFactorGraph &nonlinearFactorGraph() const {
return nonlinearFactorGraph_;
}
/** /**
* @brief Create with given number of time steps. * @brief Create with given number of time steps.
* *
@ -164,36 +156,33 @@ struct Switching {
// Create hybrid factor graph. // Create hybrid factor graph.
// Add a prior ϕ(X(0)) on X(0). // Add a prior ϕ(X(0)) on X(0).
nonlinearFactorGraph_.emplace_shared<PriorFactor<double>>( unaryFactors.emplace_shared<PriorFactor<double>>(
X(0), measurements.at(0), Isotropic::Sigma(1, prior_sigma)); X(0), measurements.at(0), Isotropic::Sigma(1, prior_sigma));
unaryFactors.push_back(nonlinearFactorGraph_.back());
// Add "motion models" ϕ(X(k),X(k+1),M(k)). // Add "motion models" ϕ(X(k),X(k+1),M(k)).
for (size_t k = 0; k < K - 1; k++) { for (size_t k = 0; k < K - 1; k++) {
auto motion_models = motionModels(k, between_sigma); auto motion_models = motionModels(k, between_sigma);
nonlinearFactorGraph_.emplace_shared<HybridNonlinearFactor>(modes[k], binaryFactors.emplace_shared<HybridNonlinearFactor>(modes[k],
motion_models); motion_models);
binaryFactors.push_back(nonlinearFactorGraph_.back());
} }
// Add measurement factors ϕ(X(k);z_k). // Add measurement factors ϕ(X(k);z_k).
auto measurement_noise = Isotropic::Sigma(1, prior_sigma); auto measurement_noise = Isotropic::Sigma(1, prior_sigma);
for (size_t k = 1; k < K; k++) { for (size_t k = 1; k < K; k++) {
nonlinearFactorGraph_.emplace_shared<PriorFactor<double>>( unaryFactors.emplace_shared<PriorFactor<double>>(X(k), measurements.at(k),
X(k), measurements.at(k), measurement_noise); measurement_noise);
unaryFactors.push_back(nonlinearFactorGraph_.back());
} }
// Add "mode chain" ϕ(M(0)) ϕ(M(0),M(1)) ... ϕ(M(K-3),M(K-2)) // Add "mode chain" ϕ(M(0)) ϕ(M(0),M(1)) ... ϕ(M(K-3),M(K-2))
modeChain = createModeChain(transitionProbabilityTable); modeChain = createModeChain(transitionProbabilityTable);
nonlinearFactorGraph_ += modeChain;
// Create the linearization point. // Create the linearization point.
for (size_t k = 0; k < K; k++) { for (size_t k = 0; k < K; k++) {
linearizationPoint.insert<double>(X(k), static_cast<double>(k + 1)); linearizationPoint.insert<double>(X(k), static_cast<double>(k + 1));
} }
linearizedFactorGraph = *nonlinearFactorGraph_.linearize(linearizationPoint); linearUnaryFactors = *unaryFactors.linearize(linearizationPoint);
linearBinaryFactors = *binaryFactors.linearize(linearizationPoint);
} }
// Create motion models for a given time step // Create motion models for a given time step
@ -224,6 +213,14 @@ struct Switching {
} }
return chain; return chain;
} }
HybridGaussianFactorGraph linearizedFactorGraph() {
HybridGaussianFactorGraph graph;
graph.push_back(linearUnaryFactors);
graph.push_back(linearBinaryFactors);
graph.push_back(modeChain);
return graph;
}
}; };
} // namespace gtsam } // namespace gtsam