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

Documentation and headers

release/4.3a0
Frank Dellaert 2011-10-29 04:00:14 +00:00
parent 2d5602bbc8
commit 58d83f704f
2 changed files with 13 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
gtsam/linear/JacobianFactor.cpp
View File

@ -15,17 +15,17 @@
* @date Dec 8, 2010 * @date Dec 8, 2010
*/ */
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/inference/VariableSlots.h>
#include <gtsam/inference/FactorGraph-inl.h>
#include <gtsam/base/debug.h> #include <gtsam/base/debug.h>
#include <gtsam/base/timing.h> #include <gtsam/base/timing.h>
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
#include <gtsam/base/FastMap.h> #include <gtsam/base/FastMap.h>
#include <gtsam/base/cholesky.h> #include <gtsam/base/cholesky.h>
#include <gtsam/inference/VariableSlots.h>
#include <gtsam/inference/FactorGraph-inl.h>
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <boost/format.hpp> #include <boost/format.hpp>
@ -33,6 +33,7 @@
#include <boost/lambda/bind.hpp> #include <boost/lambda/bind.hpp>
#include <boost/lambda/lambda.hpp> #include <boost/lambda/lambda.hpp>
#include <cmath>
#include <sstream> #include <sstream>
#include <stdexcept> #include <stdexcept>
@ -337,8 +338,11 @@ namespace gtsam {
// find first column index for this key // find first column index for this key
size_t column_start = columnIndices[*var]; size_t column_start = columnIndices[*var];
for (size_t i = 0; i < (size_t) whitenedA.rows(); i++) for (size_t i = 0; i < (size_t) whitenedA.rows(); i++)
for (size_t j = 0; j < (size_t) whitenedA.cols(); j++) for (size_t j = 0; j < (size_t) whitenedA.cols(); j++) {
entries.push_back(boost::make_tuple(i, column_start+j, whitenedA(i,j))); double s = whitenedA(i,j);
if (std::abs(s) > 1e-12) entries.push_back(
boost::make_tuple(i, column_start + j, s));
}
} }
Vector whitenedb(model_->whiten(getb())); Vector whitenedb(model_->whiten(getb()));

2
gtsam/linear/JacobianFactor.h
View File

@ -223,7 +223,7 @@ namespace gtsam {
* Return vector of i, j, and s to generate an m-by-n sparse matrix * Return vector of i, j, and s to generate an m-by-n sparse matrix
* such that S(i(k),j(k)) = s(k), which can be given to MATLAB's sparse. * such that S(i(k),j(k)) = s(k), which can be given to MATLAB's sparse.
* As above, the standard deviations are baked into A and b * As above, the standard deviations are baked into A and b
* @param first column index for each variable * @param columnIndices First column index for each variable.
*/ */
std::vector<boost::tuple<size_t, size_t, double> > std::vector<boost::tuple<size_t, size_t, double> >
sparse(const std::vector<size_t>& columnIndices) const; sparse(const std::vector<size_t>& columnIndices) const;