ISAM2 recalculatelaBatch() BUG .When using Inc smoother, CPU usage increases about gtsam

[x724172556]

open findunusedSlot option.In ISAM2.cpp,Fixed ISAM2.cpp(on line 664):
GaussianFactorGraph factorsToAdd;
std::vector<boost::shared_ptr> tf;
for (const auto& key_factors : marginalFactors) {
for (const auto& factor : key_factors.second) {
if (factor) {
factorsToAdd.push_back(factor);
if (marginalFactorsIndices)
marginalFactorsIndices->push_back(nonlinearFactors_.size());
#if 0
nonlinearFactors_.push_back(
boost::make_shared(factor));
#else

    tf.push_back( boost::make_shared<LinearContainerFactor>(factor));

#endif
if (params_.cacheLinearizedFactors) linearFactors_.push_back(factor);
for (Key factorKey : *factor) {
fixedVariables_.insert(factorKey);
}
}
}
}
#if 1
auto newFactorIndexs=nonlinearFactors_.add_factors(tf,params_.findUnusedFactorSlots );
// nonlinearFactors_.add_factors(
// factorsToAdd, params_.findUnusedFactorSlots);
variableIndex_.augment(factorsToAdd,newFactorIndexs); // Augment the variable index

#else
variableIndex_.augment(factorsToAdd); // Augment the variable index
#endif

[x724172556]

GaussianFactorGraph factorsToAdd;
std::vector<boost::shared_ptr> tf;
for (const auto& key_factors : marginalFactors) {
for (const auto& factor : key_factors.second) {
if (factor) {
factorsToAdd.push_back(factor);
if (marginalFactorsIndices)
marginalFactorsIndices->push_back(nonlinearFactors_.size());
#if 0
nonlinearFactors_.push_back(
boost::make_shared(factor));
#else

    tf.push_back( boost::make_shared<LinearContainerFactor>(factor));
#endif
if (params_.cacheLinearizedFactors) linearFactors_.push_back(factor);
for (Key factorKey : *factor) {
fixedVariables_.insert(factorKey);
}
}
}
}
#if 1
auto newFactorIndexs=nonlinearFactors_.add_factors(tf,params_.findUnusedFactorSlots );
// nonlinearFactors_.add_factors(
// factorsToAdd, params_.findUnusedFactorSlots);
variableIndex_.augment(factorsToAdd,newFactorIndexs); // Augment the variable index

#else
variableIndex_.augment(factorsToAdd); // Augment the variable index
#endif