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

Fix bug when constructing VariableSlots 

from a GaussianFactorGraph that has null factors.
release/4.3a0
Mike Bosse 2019-11-09 16:58:44 -08:00 committed by Michael Bosse
parent 357e739127
commit 8572cd17ad
2 changed files with 17 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/inference/VariableSlots.h
View File

@ -99,7 +99,9 @@ VariableSlots::VariableSlots(const FG& factorGraph)
// factor does not involve that variable. // factor does not involve that variable.
size_t jointFactorPos = 0; size_t jointFactorPos = 0;
for(const typename FG::sharedFactor& factor: factorGraph) { for(const typename FG::sharedFactor& factor: factorGraph) {
assert(factor); if (!factor) {
continue;
}
size_t factorVarSlot = 0; size_t factorVarSlot = 0;
for(const Key involvedVariable: *factor) { for(const Key involvedVariable: *factor) {
// Set the slot in this factor for this variable. If the // Set the slot in this factor for this variable. If the
@ -111,7 +113,7 @@ VariableSlots::VariableSlots(const FG& factorGraph)
iterator thisVarSlots; bool inserted; iterator thisVarSlots; bool inserted;
boost::tie(thisVarSlots, inserted) = this->insert(std::make_pair(involvedVariable, FastVector<size_t>())); boost::tie(thisVarSlots, inserted) = this->insert(std::make_pair(involvedVariable, FastVector<size_t>()));
if(inserted) if(inserted)
thisVarSlots->second.resize(factorGraph.size(), Empty); thisVarSlots->second.resize(factorGraph.nrFactors(), Empty);
thisVarSlots->second[jointFactorPos] = factorVarSlot; thisVarSlots->second[jointFactorPos] = factorVarSlot;
if(debug) std::cout << " var " << involvedVariable << " rowblock " << jointFactorPos << " comes from factor's slot " << factorVarSlot << std::endl; if(debug) std::cout << " var " << involvedVariable << " rowblock " << jointFactorPos << " comes from factor's slot " << factorVarSlot << std::endl;
++ factorVarSlot; ++ factorVarSlot;

13
gtsam_unstable/examples/FixedLagSmootherExample.cpp
View File

@ -145,5 +145,18 @@ int main(int argc, char** argv) {
cout << setprecision(5) << " Key: " << key_timestamp.first << " Time: " << key_timestamp.second << endl; 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<GaussianFactorGraph> 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; return 0;
} }