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

check if NonlinearFactor is valid before linearizing

release/4.3a0
Varun Agrawal 2024-10-29 01:56:03 -04:00
parent 3c50f9387c
commit 3c06f07551
1 changed files with 5 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
gtsam/hybrid/HybridNonlinearFactor.cpp
View File

@ -185,6 +185,11 @@ std::shared_ptr<HybridGaussianFactor> HybridNonlinearFactor::linearize(
[continuousValues]( [continuousValues](
const std::pair<sharedFactor, double>& f) -> GaussianFactorValuePair { const std::pair<sharedFactor, double>& f) -> GaussianFactorValuePair {
auto [factor, val] = f; auto [factor, val] = f;
// Check if valid factor. If not, return null and infinite error.
if (!factor) {
return {nullptr, std::numeric_limits<double>::infinity()};
}
if (auto gaussian = std::dynamic_pointer_cast<noiseModel::Gaussian>( if (auto gaussian = std::dynamic_pointer_cast<noiseModel::Gaussian>(
factor->noiseModel())) { factor->noiseModel())) {
return {factor->linearize(continuousValues), return {factor->linearize(continuousValues),