gtsam
linear
SparseEigen.h
Go to the documentation of this file.
1
/* ----------------------------------------------------------------------------
2
3
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
4
* Atlanta, Georgia 30332-0415
5
* All Rights Reserved
6
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
8
* See LICENSE for the license information
9
10
* -------------------------------------------------------------------------- */
11
24
#pragma once
25
26
#include <
gtsam/linear/GaussianFactorGraph.h
>
27
#include <
gtsam/linear/VectorValues.h
>
28
29
#include <Eigen/Sparse>
30
31
namespace
gtsam
{
32
35
typedef
Eigen::SparseMatrix<double, Eigen::ColMajor, int>
SparseEigen
;
36
38
SparseEigen
sparseJacobianEigen
(
39
const
GaussianFactorGraph
&gfg,
const
Ordering
&
ordering
) {
40
gttic_
(SparseEigen_sparseJacobianEigen);
41
// intermediate `entries` vector is kind of unavoidable due to how expensive
42
// factor->rows() is, which prevents us from populating SparseEigen directly.
43
size_t
nrows, ncols;
44
auto
entries = gfg.
sparseJacobian
(
ordering
, nrows, ncols);
45
// declare sparse matrix
46
SparseEigen
Ab
(nrows, ncols);
47
// See Eigen::set_from_triplets. This is about 5% faster.
48
// pass 1: count the nnz per inner-vector
49
std::vector<int> nnz(ncols, 0);
50
for
(
const
auto
&entry : entries) nnz[std::get<1>(entry)]++;
51
Ab
.reserve(nnz);
52
// pass 2: insert the elements
53
for
(
const
auto
&entry : entries)
54
Ab
.insert(std::get<0>(entry), std::get<1>(entry)) = std::get<2>(entry);
55
return
Ab
;
56
}
57
58
SparseEigen
sparseJacobianEigen
(
const
GaussianFactorGraph
&gfg) {
59
gttic_
(SparseEigen_sparseJacobianEigen_defaultOrdering);
60
return
sparseJacobianEigen
(gfg,
Ordering
(gfg.
keys
()));
61
}
62
63
}
// namespace gtsam
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
Eigen::SparseMatrix
A versatible sparse matrix representation.
Definition:
SparseMatrix.h:96
gtsam::GaussianFactorGraph::keys
Keys keys() const
Definition:
GaussianFactorGraph.cpp:48
gtsam::SparseEigen
Eigen::SparseMatrix< double, Eigen::ColMajor, int > SparseEigen
Definition:
SparseEigen.h:35
gtsam::GaussianFactorGraph
Definition:
GaussianFactorGraph.h:73
gttic_
#define gttic_(label)
Definition:
timing.h:245
VectorValues.h
Factor Graph Values.
ordering
static enum @1096 ordering
gtsam
traits
Definition:
SFMdata.h:40
exampleQR::Ab
Matrix Ab
Definition:
testNoiseModel.cpp:207
gtsam::GaussianFactorGraph::sparseJacobian
std::vector< std::tuple< int, int, double > > sparseJacobian(const Ordering &ordering, size_t &nrows, size_t &ncols) const
Definition:
GaussianFactorGraph.cpp:119
gtsam::Ordering
Definition:
inference/Ordering.h:33
gtsam::sparseJacobianEigen
SparseEigen sparseJacobianEigen(const GaussianFactorGraph &gfg, const Ordering &ordering)
Constructs an Eigen-format SparseMatrix of a GaussianFactorGraph.
Definition:
SparseEigen.h:38
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:35:42