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..dc9b00580 100644 --- a/gtsam_unstable/examples/FixedLagSmootherExample.cpp +++ b/gtsam_unstable/examples/FixedLagSmootherExample.cpp @@ -145,5 +145,19 @@ int main(int argc, char** argv) { cout << setprecision(5) << " Key: " << key_timestamp.first << " Time: " << key_timestamp.second << endl; } + // Here is an example of how to get the full Jacobian of the problem. + // First, get the linearization point. + Values result = smootherISAM2.calculateEstimate(); + + // Get the factor graph + auto &factorGraph = smootherISAM2.getFactors(); + + // Linearize to a Gaussian factor graph + boost::shared_ptr linearGraph = factorGraph.linearize(result); + + // Converts the linear graph into a Jacobian factor and extracts the Jacobian matrix + Matrix jacobian = linearGraph->jacobian().first; + cout << " Jacobian: " << jacobian << endl; + return 0; }