g2o::EdgeLabeler Struct Reference

#include <edge_labeler.h>

Collaboration diagram for g2o::EdgeLabeler:

Public Member Functions
	EdgeLabeler (SparseOptimizer *optimizer)
int	labelEdges (std::set< OptimizableGraph::Edge * > &edges)

Protected Member Functions
void	augmentSparsePattern (std::set< std::pair< int, int > > &pattern, OptimizableGraph::Edge *e)
bool	computePartialInverse (SparseBlockMatrix< Eigen::MatrixXd > &spinv, const std::set< std::pair< int, int > > &pattern)
bool	labelEdge (const SparseBlockMatrix< Eigen::MatrixXd > &spinv, OptimizableGraph::Edge *e)

Protected Attributes
SparseOptimizer *	_optimizer

Detailed Description

This class implements the functions to label an edge (measurement) based on the actual configuration of the nodes. It does so by

• computing the expected mean of the measurement (_measurement) based on the state variables
• computing the joint covariance matrix of all sstate variables on which the measurement depends.
• extracting the sigma points from this covariance matrix (which is in the space if the increments used by \oplus
• projecting the sigma points in the error space, and thus computing the information matrix of the labeled edge

Definition at line 46 of file edge_labeler.h.

Constructor & Destructor Documentation

EdgeLabeler()

g2o::EdgeLabeler::EdgeLabeler

(

SparseOptimizer *

optimizer

)

constructs an edge labeler that operates on the optimizer passed as argument

Parameters

optimizer

the optimizer

Definition at line 41 of file edge_labeler.cpp.

{ _optimizer = optimizer; }

References _optimizer.

Member Function Documentation

augmentSparsePattern()

void g2o::EdgeLabeler::augmentSparsePattern ( std::set< std::pair< int, int > > &  pattern, OptimizableGraph::Edge *  e  )

protected

helper function that augments the sparse pattern of the inverse based on an edge

Parameters

pattern	the blocks of the inverse covered by the edge
e	the edge

Definition at line 69 of file edge_labeler.cpp.

                                                                {
  for (size_t i = 0; i < e->vertices().size(); i++) {
    const OptimizableGraph::Vertex* v =
        (const OptimizableGraph::Vertex*)e->vertices()[i];
    int ti = v->hessianIndex();
    if (ti == -1) continue;
    for (size_t j = i; j < e->vertices().size(); j++) {
      const OptimizableGraph::Vertex* v =
          (const OptimizableGraph::Vertex*)e->vertices()[j];
      int tj = v->hessianIndex();
      if (tj == -1) continue;
      if (tj < ti) swap(ti, tj);
      pattern.insert(std::make_pair(ti, tj));
    }
  }
}

References g2o::OptimizableGraph::Vertex::hessianIndex(), and g2o::HyperGraph::Edge::vertices().

Referenced by labelEdges().

computePartialInverse()

bool g2o::EdgeLabeler::computePartialInverse ( SparseBlockMatrix< Eigen::MatrixXd > &  spinv, const std::set< std::pair< int, int > > &  pattern  )

protected

helper function that computes the inverse based on the sparse pattenrn

Parameters

spinv	the output block inverse
pattern	the blocks of the inverse covered by the edge

Returns

true on successm, false on failure, .

Definition at line 87 of file edge_labeler.cpp.

                                               {
  std::vector<std::pair<int, int> > blockIndices(pattern.size());
  // Why this does not work???
  // std::copy(pattern.begin(),pattern.end(),blockIndices.begin());
 
  int k = 0;
  for (std::set<std::pair<int, int> >::const_iterator it = pattern.begin();
       it != pattern.end(); ++it) {
    blockIndices[k++] = *it;
  }
 
  // cerr << "sparse pattern contains " << blockIndices.size() << " blocks" <<
  // endl;
  return _optimizer->computeMarginals(spinv, blockIndices);
}

References _optimizer, and g2o::SparseOptimizer::computeMarginals().

Referenced by labelEdges().

labelEdge()

bool g2o::EdgeLabeler::labelEdge ( const SparseBlockMatrix< Eigen::MatrixXd > &  spinv, OptimizableGraph::Edge *  e  )

protected

helper function that labes a specific edge based on the marginals in the sparse block inverse

Returns

true on success, false on failure

Definition at line 105 of file edge_labeler.cpp.

                                                     {
  Eigen::Map<MatrixXd> info(e->informationData(), e->dimension(),
                            e->dimension());
  // cerr << "original information matrix" << endl;
  // cerr << info << endl;
 
  int maxDim = 0;
  for (size_t i = 0; i < e->vertices().size(); i++) {
    const OptimizableGraph::Vertex* v =
        (const OptimizableGraph::Vertex*)e->vertices()[i];
    int ti = v->hessianIndex();
    if (ti == -1) continue;
    maxDim += v->minimalEstimateDimension();
  }
 
  // cerr << "maxDim= " << maxDim << endl;
  MatrixXd cov(maxDim, maxDim);
  int cumRow = 0;
  for (size_t i = 0; i < e->vertices().size(); i++) {
    const OptimizableGraph::Vertex* vr =
        (const OptimizableGraph::Vertex*)e->vertices()[i];
    int ti = vr->hessianIndex();
    if (ti > -1) {
      int cumCol = 0;
      for (size_t j = 0; j < e->vertices().size(); j++) {
        const OptimizableGraph::Vertex* vc =
            (const OptimizableGraph::Vertex*)e->vertices()[j];
        int tj = vc->hessianIndex();
        if (tj > -1) {
          // cerr << "ti=" << ti << " tj=" << tj
          //    << " cumRow=" << cumRow << " cumCol=" << cumCol << endl;
          if (ti <= tj) {
            assert(spinv.block(ti, tj));
            // cerr << "cblock_ptr" << spinv.block(ti, tj) << endl;
            // cerr << "cblock.size=" << spinv.block(ti, tj)->rows() << "," <<
            // spinv.block(ti, tj)->cols() << endl; cerr << "cblock" << endl;
            // cerr << *spinv.block(ti, tj) << endl;
            cov.block(cumRow, cumCol, vr->minimalEstimateDimension(),
                      vc->minimalEstimateDimension()) = *spinv.block(ti, tj);
          } else {
            assert(spinv.block(tj, ti));
            // cerr << "cblock.size=" << spinv.block(tj, ti)->cols() << "," <<
            // spinv.block(tj, ti)->rows() << endl; cerr << "cblock" << endl;
            // cerr << spinv.block(tj, ti)->transpose() << endl;
            cov.block(cumRow, cumCol, vr->minimalEstimateDimension(),
                      vc->minimalEstimateDimension()) =
                spinv.block(tj, ti)->transpose();
          }
          cumCol += vc->minimalEstimateDimension();
        }
      }
      cumRow += vr->minimalEstimateDimension();
    }
  }
 
  // cerr << "covariance assembled" << endl;
  // cerr << cov << endl;
  // now cov contains the aggregate marginals of the state variables in the edge
  VectorXd incMean(maxDim);
  incMean.fill(0);
  std::vector<MySigmaPoint> incrementPoints;
  if (!sampleUnscented(incrementPoints, incMean, cov)) {
    cerr << "sampleUnscented fail" << endl;
    return false;
  }
  // now determine the zero-error measure by applying the error function of the
  // edge with a zero measurement
  // TODO!!!
  bool smss = e->setMeasurementFromState();
  if (!smss) {
    cerr << "FATAL: Edge::setMeasurementFromState() not implemented" << endl;
  }
  assert(smss && "Edge::setMeasurementFromState() not implemented");
 
  // std::vector<MySigmaPoint> globalPoints(incrementPoints.size());
  std::vector<MySigmaPoint> errorPoints(incrementPoints.size());
 
  // for each sigma point, project it to the global space, by considering those
  // variables that are involved
  // cerr << "sigma points are extracted, remapping to measurement space" <<
  // endl;
  for (size_t i = 0; i < incrementPoints.size(); i++) {
    int cumPos = 0;
    // VectorXd globalPoint(maxDim);
 
    // push all the "active" state variables
    for (size_t j = 0; j < e->vertices().size(); j++) {
      OptimizableGraph::Vertex* vr =
          (OptimizableGraph::Vertex*)e->vertices()[j];
      int tj = vr->hessianIndex();
      if (tj == -1) continue;
      vr->push();
    }
 
    for (size_t j = 0; j < e->vertices().size(); j++) {
      OptimizableGraph::Vertex* vr =
          (OptimizableGraph::Vertex*)e->vertices()[j];
      int tj = vr->hessianIndex();
      if (tj == -1) continue;
      vr->oplus(&incrementPoints[i]._sample[cumPos]);
      // assert(vr->getMinimalEstimateData(&globalPoint[cumPos]) &&
      // "Vertex::getMinimalEstimateData(...) not implemented");
      cumPos += vr->minimalEstimateDimension();
    }
 
    // construct the sigma point in the global space
    // globalPoints[i]._sample=globalPoint;
    // globalPoints[i]._wi=incrementPoints[i]._wi;
    // globalPoints[i]._wp=incrementPoints[i]._wp;
 
    // construct the sigma point in the error space
    e->computeError();
    Map<VectorXd> errorPoint(e->errorData(), e->dimension());
 
    errorPoints[i]._sample = errorPoint;
    errorPoints[i]._wi = incrementPoints[i]._wi;
    errorPoints[i]._wp = incrementPoints[i]._wp;
 
    // pop all the "active" state variables
    for (size_t j = 0; j < e->vertices().size(); j++) {
      OptimizableGraph::Vertex* vr =
          (OptimizableGraph::Vertex*)e->vertices()[j];
      int tj = vr->hessianIndex();
      if (tj == -1) continue;
      vr->pop();
    }
  }
 
  // reconstruct the covariance of the error by the sigma points
  MatrixXd errorCov(e->dimension(), e->dimension());
  VectorXd errorMean(e->dimension());
  reconstructGaussian(errorMean, errorCov, errorPoints);
 
  // info=errorCov.inverse();
  //  cerr << "remapped information matrix" << endl;
  //  cerr << info << endl;
  return true;
}

References g2o::SparseBlockMatrix< MatrixType >::block(), g2o::OptimizableGraph::Edge::computeError(), g2o::OptimizableGraph::Edge::dimension(), g2o::OptimizableGraph::Edge::errorData(), g2o::OptimizableGraph::Vertex::hessianIndex(), g2o::OptimizableGraph::Edge::informationData(), g2o::OptimizableGraph::Vertex::minimalEstimateDimension(), g2o::OptimizableGraph::Vertex::oplus(), g2o::OptimizableGraph::Vertex::pop(), g2o::OptimizableGraph::Vertex::push(), g2o::reconstructGaussian(), g2o::sampleUnscented(), g2o::OptimizableGraph::Edge::setMeasurementFromState(), and g2o::HyperGraph::Edge::vertices().

Referenced by labelEdges().

labelEdges()

int g2o::EdgeLabeler::labelEdges

(

std::set< OptimizableGraph::Edge * > &

edges

)

Labels the set of edges passed as argument. It computes the cholesky information matrix. This method only woorks aftec having called an optimize(...) in the connected optimizer. The labeling is performed based on the actual configuration of the nodes in the optimized subgraph.

Parameters

edges

the edges to label

Returns

-1 if the inverse cholesky cannot be computed, otherwise the number of edges where the labeling was successful

Definition at line 43 of file edge_labeler.cpp.

                                                                {
  // assume the system is "solved"
  // compute the sparse pattern of the inverse
  std::set<std::pair<int, int> > pattern;
  for (std::set<OptimizableGraph::Edge*>::iterator it = edges.begin();
       it != edges.end(); ++it) {
    augmentSparsePattern(pattern, *it);
  }
 
  SparseBlockMatrix<MatrixXd> spInv;
 
  bool result = computePartialInverse(spInv, pattern);
  // cerr << "partial inverse computed = " << result << endl;
  // cerr << "non zero blocks" << spInv.nonZeroBlocks() << endl;
 
  if (!result) {
    return -1;
  }
  int count = 0;
  for (std::set<OptimizableGraph::Edge*>::iterator it = edges.begin();
       it != edges.end(); ++it) {
    count += labelEdge(spInv, *it) ? 1 : 0;
  }
  return count;
}

References augmentSparsePattern(), computePartialInverse(), and labelEdge().

Referenced by g2o::Star::labelStarEdges().

Member Data Documentation

_optimizer

SparseOptimizer* g2o::EdgeLabeler::_optimizer

protected

Definition at line 81 of file edge_labeler.h.

Referenced by computePartialInverse(), and EdgeLabeler().

---

The documentation for this struct was generated from the following files:

• /build/reproducible-path/g2o-0~20230806/g2o/examples/g2o_hierarchical/edge_labeler.h
• /build/reproducible-path/g2o-0~20230806/g2o/examples/g2o_hierarchical/edge_labeler.cpp