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

38
gtsam/linear/SubgraphBuilder.cpp
View File

@ -365,41 +365,29 @@ 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: case SubgraphBuilderParameters::EQUAL:
weights.push_back(1.0); weights.push_back(1.0);
break; break;
case SubgraphBuilderParameters::RHS_2NORM: case SubgraphBuilderParameters::RHS_2NORM: {
{
if (JacobianFactor::shared_ptr jf = if (JacobianFactor::shared_ptr jf =
std::dynamic_pointer_cast<JacobianFactor>(gf)) std::dynamic_pointer_cast<JacobianFactor>(gf)) {
{
weights.push_back(jf->getb().norm()); weights.push_back(jf->getb().norm());
} } else if (HessianFactor::shared_ptr hf =
else if (HessianFactor::shared_ptr hf = std::dynamic_pointer_cast<HessianFactor>(gf)) {
std::dynamic_pointer_cast<HessianFactor>(gf))
{
weights.push_back(hf->linearTerm().norm()); weights.push_back(hf->linearTerm().norm());
} }
} } break;
break; case SubgraphBuilderParameters::LHS_FNORM: {
case SubgraphBuilderParameters::LHS_FNORM:
{
if (JacobianFactor::shared_ptr jf = if (JacobianFactor::shared_ptr jf =
std::dynamic_pointer_cast<JacobianFactor>(gf)) std::dynamic_pointer_cast<JacobianFactor>(gf)) {
{
weights.push_back(std::sqrt(jf->getA().squaredNorm())); weights.push_back(std::sqrt(jf->getA().squaredNorm()));
} } else if (HessianFactor::shared_ptr hf =
else if (HessianFactor::shared_ptr hf = std::dynamic_pointer_cast<HessianFactor>(gf)) {
std::dynamic_pointer_cast<HessianFactor>(gf))
{
weights.push_back(std::sqrt(hf->information().squaredNorm())); weights.push_back(std::sqrt(hf->information().squaredNorm()));
} }
} } break;
break;
case SubgraphBuilderParameters::RANDOM: case SubgraphBuilderParameters::RANDOM:
weights.push_back(std::rand() % 100 + 1.0); weights.push_back(std::rand() % 100 + 1.0);
@ -407,7 +395,7 @@ SubgraphBuilder::Weights SubgraphBuilder::weights(
default: default:
throw std::invalid_argument( throw std::invalid_argument(
"utils::assign_weights: undefined weight scheme "); "SubgraphBuilder::weights(): undefined weight scheme ");
break; break;
} }
} }