33 using namespace gtsam;
42 bool GaussianFactorGraph::equals(
const This& fg,
double tol)
const
44 return Base::equals(fg,
tol);
57 std::map<Key, size_t> GaussianFactorGraph::getKeyDimMap()
const {
58 map<Key, size_t> spec;
61 map<Key,size_t>::iterator it2 = spec.find(*it);
62 if ( it2 == spec.end() ) {
63 spec.emplace(*it, gf->getDim(it));
72 double total_error = 0.;
121 size_t& ncols)
const {
122 gttic_(GaussianFactorGraph_sparseJacobian);
124 typedef std::map<Key, size_t> KeySizeMap;
126 for (
const auto&
factor : *
this) {
127 if (!
static_cast<bool>(
factor))
continue;
130 dims[*it] =
factor->getDim(it);
136 KeySizeMap columnIndices = dims;
138 columnIndices[
key] = ncols;
144 entries.reserve(30 *
size());
146 for (
const auto&
factor : *
this) {
147 if (!
static_cast<bool>(
factor))
continue;
151 std::dynamic_pointer_cast<JacobianFactor>(
factor));
152 if (!jacobianFactor) {
154 std::dynamic_pointer_cast<HessianFactor>(
factor));
158 throw std::invalid_argument(
159 "GaussianFactorGraph contains a factor that is neither a "
160 "JacobianFactor nor a HessianFactor.");
169 size_t column_start = columnIndices[*
key];
170 for (
size_t i = 0;
i < (
size_t)whitenedA.rows();
i++)
171 for (
size_t j = 0;
j < (
size_t)whitenedA.cols();
j++) {
172 double s = whitenedA(
i,
j);
174 entries.emplace_back(nrows +
i, column_start +
j,
s);
179 for (
size_t i = 0;
i < (
size_t)whitenedb.size();
i++) {
180 double s = whitenedb(
i);
181 if (
std::abs(
s) > 1
e-12) entries.emplace_back(nrows +
i, ncols,
s);
185 nrows += jacobianFactor->rows();
195 return sparseJacobian(
Ordering(this->
keys()), nrows, ncols);
199 Matrix GaussianFactorGraph::sparseJacobian_()
const {
200 gttic_(GaussianFactorGraph_sparseJacobian_matrix);
202 auto result = sparseJacobian();
208 const auto& entry =
result[k];
209 IJS(0, k) = double(std::get<0>(entry) + 1);
210 IJS(1, k) = double(std::get<1>(entry) + 1);
211 IJS(2, k) = std::get<2>(entry);
217 Matrix GaussianFactorGraph::augmentedJacobian(
225 Matrix GaussianFactorGraph::augmentedJacobian()
const {
232 pair<Matrix, Vector> GaussianFactorGraph::jacobian(
235 return make_pair(augmented.leftCols(augmented.cols() - 1),
236 augmented.col(augmented.cols() - 1));
240 pair<Matrix, Vector> GaussianFactorGraph::jacobian()
const {
241 Matrix augmented = augmentedJacobian();
242 return make_pair(augmented.leftCols(augmented.cols() - 1),
243 augmented.col(augmented.cols() - 1));
247 Matrix GaussianFactorGraph::augmentedHessian(
256 Matrix GaussianFactorGraph::augmentedHessian()
const {
264 pair<Matrix, Vector> GaussianFactorGraph::hessian(
267 size_t n = augmented.rows() - 1;
268 return make_pair(augmented.topLeftCorner(
n,
n), augmented.topRightCorner(
n, 1));
272 pair<Matrix, Vector> GaussianFactorGraph::hessian()
const {
273 Matrix augmented = augmentedHessian();
274 size_t n = augmented.rows() - 1;
275 return make_pair(augmented.topLeftCorner(
n,
n), augmented.topRightCorner(
n, 1));
290 map<Key,Matrix> GaussianFactorGraph::hessianBlockDiagonal()
const {
291 map<Key,Matrix> blocks;
294 map<Key,Matrix>
BD =
factor->hessianBlockDiagonal();
295 map<Key,Matrix>::const_iterator it =
BD.begin();
296 for (;it!=
BD.end();++it) {
298 const Matrix& Bj = it->second;
302 blocks.emplace(
j,Bj);
310 gttic(GaussianFactorGraph_optimize);
311 return BaseEliminateable::eliminateMultifrontal(Ordering::COLAMD,
function)
317 gttic(GaussianFactorGraph_optimize);
318 return BaseEliminateable::eliminateMultifrontal(
ordering,
function)->optimize();
324 gttic(GaussianFactorGraph_optimizeDensely);
335 size_t n = augmented.rows() - 1;
336 auto AtA = augmented.topLeftCorner(
n,
n);
337 auto eta = augmented.topRightCorner(
n, 1);
351 result = std::make_shared<JacobianFactor>(*gf);
363 Ai->transposeMultiplyAdd(1.0,
e,
g);
383 gttic(GaussianFactorGraph_optimizeGradientSearch);
385 gttic(Compute_Gradient);
388 double gradientSqNorm = grad.
dot(grad);
389 gttoc(Compute_Gradient);
396 gttic(Compute_minimizing_step_size);
399 gttoc(Compute_minimizing_step_size);
401 gttic(Compute_point);
404 gttoc(Compute_point);
414 e.push_back((*Ai) *
x);
420 void GaussianFactorGraph::multiplyHessianAdd(
double alpha,
428 multiplyInPlace(
x,
e.begin());
432 void GaussianFactorGraph::multiplyInPlace(
const VectorValues&
x,
const Errors::iterator&
e)
const {
433 Errors::iterator ei =
e;
445 J::shared_ptr jacobian(std::dynamic_pointer_cast<J>(
factor));
446 if (jacobian && jacobian->get_model() && jacobian->get_model()->isConstrained()) {
455 void GaussianFactorGraph::transposeMultiplyAdd(
double alpha,
const Errors&
e,
458 Errors::const_iterator ei =
e.begin();
461 Ai->transposeMultiplyAdd(
alpha, *(ei++),
x);
496 Errors::const_iterator ei =
e.begin();
501 pair<VectorValues::iterator, bool>
xi =
x.tryInsert(*
j,
Vector());
503 xi.first->second = Vector::Zero(Ai->getDim(
j));
504 xi.first->second += Ai->getA(
j).transpose() * *ei;
517 e.push_back(Ai->error_vector(
x));
523 void GaussianFactorGraph::printErrors(
526 const std::function<
bool(
const Factor* ,
528 printCondition)
const {
529 cout <<
str <<
"size: " <<
size() << endl << endl;
530 for (
size_t i = 0;
i < (*this).size();
i++) {
532 const double errorValue =
534 if (!printCondition(
factor.get(), errorValue,
i))
538 ss <<
"Factor " <<
i <<
": ";
544 cout <<
"error = " << errorValue <<
"\n";