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 =
std::dynamic_pointer_cast<GaussianConditional>(pair.first)) {
const auto likelihood_m = conditional->likelihood(given);
// scalar is already correct.
assert(pair.second ==
conditional->negLogConstant() - negLogConstant_);
// pair.second == conditional->negLogConstant() - negLogConstant_
return {likelihood_m, pair.second};
} else {
return {nullptr, std::numeric_limits<double>::infinity()};
@ -320,8 +318,7 @@ HybridGaussianConditional::shared_ptr HybridGaussianConditional::prune(
};
FactorValuePairs prunedConditionals = factors().apply(pruner);
return std::shared_ptr<HybridGaussianConditional>(
new HybridGaussianConditional(discreteKeys(), prunedConditionals));
return std::make_shared<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.
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
bool someContinuousLeft = false;
@ -375,6 +375,8 @@ HybridGaussianFactorGraph::eliminate(const Ordering &keys) const {
[&](const std::pair<GaussianFactorGraph, double> &pair) -> Result {
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)) {
return {nullptr, 0.0, nullptr, 0.0};
}