diff --git a/gtsam/inference/VariableSlots.h b/gtsam/inference/VariableSlots.h index f9297d300..a665890c2 100644 --- a/gtsam/inference/VariableSlots.h +++ b/gtsam/inference/VariableSlots.h @@ -99,7 +99,9 @@ VariableSlots::VariableSlots(const FG& factorGraph) // factor does not involve that variable. size_t jointFactorPos = 0; for(const typename FG::sharedFactor& factor: factorGraph) { - assert(factor); + if (!factor) { + continue; + } size_t factorVarSlot = 0; for(const Key involvedVariable: *factor) { // Set the slot in this factor for this variable. If the @@ -111,7 +113,7 @@ VariableSlots::VariableSlots(const FG& factorGraph) iterator thisVarSlots; bool inserted; boost::tie(thisVarSlots, inserted) = this->insert(std::make_pair(involvedVariable, FastVector())); if(inserted) - thisVarSlots->second.resize(factorGraph.size(), Empty); + thisVarSlots->second.resize(factorGraph.nrFactors(), Empty); thisVarSlots->second[jointFactorPos] = factorVarSlot; if(debug) std::cout << " var " << involvedVariable << " rowblock " << jointFactorPos << " comes from factor's slot " << factorVarSlot << std::endl; ++ factorVarSlot; diff --git a/gtsam_unstable/examples/FixedLagSmootherExample.cpp b/gtsam_unstable/examples/FixedLagSmootherExample.cpp index 71153ee31..a6d5b699d 100644 --- a/gtsam_unstable/examples/FixedLagSmootherExample.cpp +++ b/gtsam_unstable/examples/FixedLagSmootherExample.cpp @@ -145,5 +145,18 @@ int main(int argc, char** argv) { cout << setprecision(5) << " Key: " << key_timestamp.first << " Time: " << key_timestamp.second << endl; } + // get the linearization point + Values result = smootherISAM2.calculateEstimate(); + + // create the factor graph + auto &factor_graph = smootherISAM2.getFactors(); + + // linearize to a Gaussian factor graph + boost::shared_ptr linear_graph = factor_graph.linearize(result); + + // this is where the segmentation fault occurs + Matrix A = linear_graph->jacobian().first; + cout << " A = " << A << endl; + return 0; }