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

Merge pull request #163 from michaelbosse/fix/variable_slots_bug 

Fix bug when constructing VariableSlots
release/4.3a0
Frank Dellaert 2019-11-10 13:42:17 +08:00 committed by GitHub
parent 0f790e8644 fab3b01756
commit 4ac78953ae
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 18 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.
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<size_t>()));
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;

14
gtsam_unstable/examples/FixedLagSmootherExample.cpp
View File

@ -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<GaussianFactorGraph> 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;
}