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

Address review comments

release/4.3a0
Frank Dellaert 2024-10-23 09:24:47 -07:00
parent 2c9665fae6
commit 977ac0d762
2 changed files with 5 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
gtsam/hybrid/HybridGaussianConditional.cpp
View File

@ -275,9 +275,7 @@ std::shared_ptr<HybridGaussianFactor> HybridGaussianConditional::likelihood(
if (auto conditional = if (auto conditional =
std::dynamic_pointer_cast<GaussianConditional>(pair.first)) { std::dynamic_pointer_cast<GaussianConditional>(pair.first)) {
const auto likelihood_m = conditional->likelihood(given); const auto likelihood_m = conditional->likelihood(given);
// scalar is already correct. // pair.second == conditional->negLogConstant() - negLogConstant_
assert(pair.second ==
conditional->negLogConstant() - negLogConstant_);
return {likelihood_m, pair.second}; return {likelihood_m, pair.second};
} else { } else {
return {nullptr, std::numeric_limits<double>::infinity()}; return {nullptr, std::numeric_limits<double>::infinity()};
@ -320,8 +318,7 @@ HybridGaussianConditional::shared_ptr HybridGaussianConditional::prune(
}; };
FactorValuePairs prunedConditionals = factors().apply(pruner); FactorValuePairs prunedConditionals = factors().apply(pruner);
return std::shared_ptr<HybridGaussianConditional>( return std::make_shared<HybridGaussianConditional>(discreteKeys(), prunedConditionals);
new HybridGaussianConditional(discreteKeys(), prunedConditionals));
} }
/* *******************************************************************************/ /* *******************************************************************************/

4
gtsam/hybrid/HybridGaussianFactorGraph.cpp
View File

@ -367,7 +367,7 @@ HybridGaussianFactorGraph::eliminate(const Ordering &keys) const {
// any difference in noise models used. // any difference in noise models used.
HybridGaussianProductFactor productFactor = collectProductFactor(); HybridGaussianProductFactor productFactor = collectProductFactor();
auto isNull = [](const GaussianFactor::shared_ptr& ptr) { return !ptr; }; auto isNull = [](const GaussianFactor::shared_ptr &ptr) { return !ptr; };
// This is the elimination method on the leaf nodes // This is the elimination method on the leaf nodes
bool someContinuousLeft = false; bool someContinuousLeft = false;
@ -375,6 +375,8 @@ HybridGaussianFactorGraph::eliminate(const Ordering &keys) const {
[&](const std::pair<GaussianFactorGraph, double> &pair) -> Result { [&](const std::pair<GaussianFactorGraph, double> &pair) -> Result {
const auto &[graph, scalar] = pair; const auto &[graph, scalar] = pair;
// If any product contains a pruned factor, prune it here. Done here as it's
// non non-trivial to do within collectProductFactor.
if (graph.empty() || std::any_of(graph.begin(), graph.end(), isNull)) { if (graph.empty() || std::any_of(graph.begin(), graph.end(), isNull)) {
return {nullptr, 0.0, nullptr, 0.0}; return {nullptr, 0.0, nullptr, 0.0};
} }