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

Fix formatting

release/4.3a0
Ankur Roy Chowdhury 2023-02-04 20:48:30 -08:00
parent b20827b2dc
commit c8124ec944
1 changed files with 30 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

72
gtsam/linear/SubgraphBuilder.cpp
View File

@ -365,50 +365,38 @@ SubgraphBuilder::Weights SubgraphBuilder::weights(
Weights weights; Weights weights;
weights.reserve(m); weights.reserve(m);
for (const GaussianFactor::shared_ptr &gf : gfg) for (const GaussianFactor::shared_ptr &gf : gfg) {
{ switch (parameters_.skeletonWeight) {
switch (parameters_.skeletonWeight) case SubgraphBuilderParameters::EQUAL:
{ weights.push_back(1.0);
case SubgraphBuilderParameters::EQUAL: break;
weights.push_back(1.0); case SubgraphBuilderParameters::RHS_2NORM: {
break; if (JacobianFactor::shared_ptr jf =
case SubgraphBuilderParameters::RHS_2NORM: std::dynamic_pointer_cast<JacobianFactor>(gf)) {
{ weights.push_back(jf->getb().norm());
if (JacobianFactor::shared_ptr jf = } else if (HessianFactor::shared_ptr hf =
std::dynamic_pointer_cast<JacobianFactor>(gf)) std::dynamic_pointer_cast<HessianFactor>(gf)) {
{ weights.push_back(hf->linearTerm().norm());
weights.push_back(jf->getb().norm()); }
} } break;
else if (HessianFactor::shared_ptr hf = case SubgraphBuilderParameters::LHS_FNORM: {
std::dynamic_pointer_cast<HessianFactor>(gf)) if (JacobianFactor::shared_ptr jf =
{ std::dynamic_pointer_cast<JacobianFactor>(gf)) {
weights.push_back(hf->linearTerm().norm()); weights.push_back(std::sqrt(jf->getA().squaredNorm()));
} } else if (HessianFactor::shared_ptr hf =
} std::dynamic_pointer_cast<HessianFactor>(gf)) {
break; weights.push_back(std::sqrt(hf->information().squaredNorm()));
case SubgraphBuilderParameters::LHS_FNORM: }
{ } break;
if (JacobianFactor::shared_ptr jf =
std::dynamic_pointer_cast<JacobianFactor>(gf))
{
weights.push_back(std::sqrt(jf->getA().squaredNorm()));
}
else if (HessianFactor::shared_ptr hf =
std::dynamic_pointer_cast<HessianFactor>(gf))
{
weights.push_back(std::sqrt(hf->information().squaredNorm()));
}
}
break;
case SubgraphBuilderParameters::RANDOM: case SubgraphBuilderParameters::RANDOM:
weights.push_back(std::rand() % 100 + 1.0); weights.push_back(std::rand() % 100 + 1.0);
break; break;
default: default:
throw std::invalid_argument( throw std::invalid_argument(
"utils::assign_weights: undefined weight scheme "); "SubgraphBuilder::weights(): undefined weight scheme ");
break; break;
} }
} }
return weights; return weights;