Go to the documentation of this file.
29 for(
const Key&
key: gaussian->keys()) {
45 :
Base(jacobian, lin_points) {
48 size_t *dims = (
size_t *)alloca(
sizeof(
size_t) * (jacobian->size() + 1));
51 dims[index++] = jacobian->getDim(
iter);
58 Ab_.
matrix() = jacobian->augmentedJacobian();
64 std::cout <<
s << std::endl;
66 std::cout <<
"Nonlinear Keys: ";
68 std::cout << keyFormatter(
key) <<
" ";
69 std::cout << std::endl;
72 std::cout <<
"A[" << keyFormatter(*
key) <<
"]=\n" <<
A(*
key) << std::endl;
74 std::cout <<
"b=\n" <<
b() << std::endl;
99 return 0.5 * errorVector.dot(errorVector);
103 std::shared_ptr<GaussianFactor>
107 std::vector<std::pair<Key, Matrix> > terms;
109 terms.push_back(std::make_pair(
key, this->
A(
key)));
128 errorVector +=
A *
d;
143 :
Base(hessian, lin_points), info_(hessian->
info()) {}
148 std::cout <<
s << std::endl;
150 std::cout <<
"Nonlinear Keys: ";
152 std::cout << keyFormatter(
key) <<
" ";
153 std::cout << std::endl;
167 thisMatrix(thisMatrix.rows()-1, thisMatrix.cols()-1) = 0.0;
168 Matrix rhsMatrix =
e->info_.selfadjointView();
169 rhsMatrix(rhsMatrix.rows()-1, rhsMatrix.cols()-1) = 0.0;
185 for(
unsigned int i = 0;
i < this->
size(); ++
i){
190 index += linPt.
dim();
198 return 0.5 * (
f - 2.0 * xtg + xGx);
202 std::shared_ptr<GaussianFactor>
208 for(
unsigned int i = 0;
i < this->
size(); ++
i){
213 index += linPt.
dim();
223 std::vector<Vector> gs;
233 std::vector<Matrix> Gs;
Expression of a fixed-size or dynamic-size block.
A Gaussian factor using the canonical parameters (information form)
Array< double, 1, 3 > e(1./3., 0.5, 2.)
static const double d[K][N]
SymmetricBlockMatrix info_
virtual bool equals(const NonlinearFactor &f, double tol=1e-9) const
size_t dim() const override
Block range(DenseIndex startBlock, DenseIndex endBlock)
bool equal_with_abs_tol(const Eigen::DenseBase< MATRIX > &A, const Eigen::DenseBase< MATRIX > &B, double tol=1e-9)
Eigen::SelfAdjointView< constBlock, Eigen::Upper > selfadjointView(DenseIndex I, DenseIndex J) const
Return the square sub-matrix that contains blocks(i:j, i:j).
virtual size_t dim() const =0
std::shared_ptr< This > shared_ptr
A shared_ptr to this class.
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
const_iterator begin() const
size_t dim() const override
double error(const Values &c) const override
virtual Vector localCoordinates_(const Value &value) const =0
const ValueType at(Key j) const
void print(const Matrix &A, const string &s, ostream &stream)
LinearizedGaussianFactor()=default
LinearizedHessianFactor()
std::function< std::string(Key)> KeyFormatter
Typedef for a function to format a key, i.e. to convert it to a string.
Eigen::SelfAdjointView< constBlock, Eigen::Upper > squaredTerm() const
Vector error_vector(const Values &c) const
std::shared_ptr< This > shared_ptr
shared_ptr to this class
std::shared_ptr< GaussianFactor > linearize(const Values &c) const override
static shared_ptr Create(size_t dim)
LinearizedJacobianFactor()
bool equals(const Values &other, double tol=1e-9) const
DenseIndex nBlocks() const
Block count.
DenseIndex nBlocks() const
Block count.
constColumn linearTerm() const
const_iterator end() const
void g(const string &key, int i)
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate offset
const gtsam::Symbol key('X', 0)
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
double error(const Values &c) const override
const constBVector b() const
KeyVector::const_iterator const_iterator
Const iterator over keys.
void print(const std::string &str="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
void print(const std::string &s="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
const KeyVector & keys() const
Access the factor's involved variable keys.
iterator iter(handle obj)
ptrdiff_t DenseIndex
The index type for Eigen objects.
const Matrix & matrix() const
Eigen::SelfAdjointView< Block, Eigen::Upper > diagonalBlock(DenseIndex J)
Return the J'th diagonal block as a self adjoint view.
void insert(Key j, const Value &val)
double constantTerm() const
const constABlock A() const
bool equals(const NonlinearFactor &expected, double tol=1e-9) const override
std::shared_ptr< This > shared_ptr
shared_ptr to this class
DenseIndex getDim(DenseIndex block) const
Number of dimensions for variable on this diagonal block.
std::shared_ptr< GaussianFactor > linearize(const Values &c) const override
constBlock aboveDiagonalBlock(DenseIndex I, DenseIndex J) const
Get block above the diagonal (I, J).
std::uint64_t Key
Integer nonlinear key type.
bool equals(const NonlinearFactor &expected, double tol=1e-9) const override
A dummy factor that allows a linear factor to act as a nonlinear factor.
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:33:07