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
• projecting the sigma points in the error space, and thus computing the information matrix of the labeled edge

Definition at line 20 of file edge_labeler.h.

Constructor & Destructor Documentation

g2o::EdgeLabeler::EdgeLabeler

(

SparseOptimizer *

optimizer

)

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

Parameters

optimizer

the optimizer

Definition at line 12 of file edge_labeler.cpp.

                                                     {
    _optimizer = optimizer;
  }

Member Function Documentation

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 41 of file edge_labeler.cpp.

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

                                                                                                    {
    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));
      }
    }
  }

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 59 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);
  }

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 73 of file edge_labeler.cpp.

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().

                                                                                               {

    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, Eigen::aligned_allocator<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, Eigen::aligned_allocator<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;
  }

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 16 of file edge_labeler.cpp.

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

                                                                 {
    // 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;
  }

Member Data Documentation

SparseOptimizer* g2o::EdgeLabeler::_optimizer

protected

Definition at line 47 of file edge_labeler.h.

---

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

• /home/xuezhisd/CLionProjects/g2o/g2o/apps/g2o_hierarchical/edge_labeler.h
• /home/xuezhisd/CLionProjects/g2o/g2o/apps/g2o_hierarchical/edge_labeler.cpp