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

Now using add_factors

release/4.3a0
Frank Dellaert 2019-06-01 17:49:05 -04:00
parent 4b405728a7
commit 42b5f81633
2 changed files with 6 additions and 75 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
gtsam/nonlinear/ISAM2-impl.h
View File

@ -135,48 +135,6 @@ struct GTSAM_EXPORT UpdateImpl {
}
}
/// Perform the first part of the bookkeeping updates for adding new factors.
/// Adds them to the complete list of nonlinear factors, and populates the
/// list of new factor indices, both optionally finding and reusing empty
/// factor slots.
FactorIndices addFactorsStep1(const NonlinearFactorGraph& newFactors,
NonlinearFactorGraph* nonlinearFactors) const {
FactorIndices newFactorIndices(newFactors.size());
if (params_.findUnusedFactorSlots) {
size_t globalFactorIndex = 0;
for (size_t newFactorIndex = 0; newFactorIndex < newFactors.size();
++newFactorIndex) {
// Loop to find the next available factor slot
do {
// If we need to add more factors than we have room for, resize
// nonlinearFactors, filling the new slots with NULL factors.
// Otherwise, check if the current factor in nonlinearFactors is
// already used, and if so, increase globalFactorIndex. If the
// current factor in nonlinearFactors is unused, break out of the loop
// and use the current slot.
if (globalFactorIndex >= nonlinearFactors->size())
nonlinearFactors->resize(nonlinearFactors->size() +
newFactors.size() - newFactorIndex);
else if ((*nonlinearFactors)[globalFactorIndex])
++globalFactorIndex;
else
break;
} while (true);
// Use the current slot, updating nonlinearFactors and newFactorSlots.
(*nonlinearFactors)[globalFactorIndex] = newFactors[newFactorIndex];
newFactorIndices[newFactorIndex] = globalFactorIndex;
}
} else {
// We're not looking for unused slots, so just add the factors at the end.
for (size_t i = 0; i < newFactors.size(); ++i)
newFactorIndices[i] = i + nonlinearFactors->size();
nonlinearFactors->push_back(newFactors);
}
return newFactorIndices;
}
// 1. Add any new factors \Factors:=\Factors\cup\Factors'.
void pushBackFactors(const NonlinearFactorGraph& newFactors,
NonlinearFactorGraph* nonlinearFactors,
@ -185,8 +143,12 @@ struct GTSAM_EXPORT UpdateImpl {
ISAM2Result* result) const {
gttic(pushBackFactors);
// Add the new factor indices to the result struct
result->newFactorsIndices = addFactorsStep1(newFactors, nonlinearFactors);
// Perform the first part of the bookkeeping updates for adding new factors.
// Adds them to the complete list of nonlinear factors, and populates the
// list of new factor indices, both optionally finding and reusing empty
// factor slots.
result->newFactorsIndices = nonlinearFactors->add_factors(
newFactors, params_.findUnusedFactorSlots);
// Remove the removed factors
NonlinearFactorGraph removedFactors;

31
tests/testGaussianISAM2.cpp
View File

@ -283,37 +283,6 @@ bool isam_check(const NonlinearFactorGraph& fullgraph, const Values& fullinit, c
return nodeGradientsOk && expectedGradOk && totalGradOk && isamEqual && isamTreeEqual && consistent;
}
/* ************************************************************************* */
TEST(ISAM2, AddFactorsStep1) {
NonlinearFactorGraph nonlinearFactors;
nonlinearFactors += PriorFactor<double>(10, 0.0, model);
nonlinearFactors += NonlinearFactor::shared_ptr();
nonlinearFactors += PriorFactor<double>(11, 0.0, model);
NonlinearFactorGraph newFactors;
newFactors += PriorFactor<double>(1, 0.0, model);
newFactors += PriorFactor<double>(2, 0.0, model);
NonlinearFactorGraph expectedNonlinearFactors;
expectedNonlinearFactors += PriorFactor<double>(10, 0.0, model);
expectedNonlinearFactors += PriorFactor<double>(1, 0.0, model);
expectedNonlinearFactors += PriorFactor<double>(11, 0.0, model);
expectedNonlinearFactors += PriorFactor<double>(2, 0.0, model);
const FactorIndices expectedNewFactorIndices = list_of(1)(3);
ISAM2Params params;
ISAM2UpdateParams updateParams;
params.findUnusedFactorSlots = true;
UpdateImpl update(params, updateParams);
FactorIndices actualNewFactorIndices =
update.addFactorsStep1(newFactors, &nonlinearFactors);
EXPECT(assert_equal(expectedNonlinearFactors, nonlinearFactors));
EXPECT(assert_container_equality(expectedNewFactorIndices,
actualNewFactorIndices));
}
/* ************************************************************************* */
TEST(ISAM2, simple)
{