Beispiel #1
0
 void EdgeSE3PointXYZCov::initialEstimate(const g2o::OptimizableGraph::VertexSet& from_, g2o::OptimizableGraph::Vertex* /*to_*/)
 {
   VertexSE3*       from = static_cast<VertexSE3*>(_vertices[0]);
   VertexPointXYZCov* to = static_cast<VertexPointXYZCov*>(_vertices[1]);
   if (from_.count(from) > 0)
     to->setEstimate(from->estimate() * measurement());
   else
     std::cerr << __PRETTY_FUNCTION__ << std::endl;
 }
void SmoothEstimatePropagator::SmoothPropagateAction::
  operator()(g2o::OptimizableGraph::Edge* e_, const g2o::OptimizableGraph::VertexSet& from_, g2o::OptimizableGraph::Vertex* to_) const
{
  if (to_->fixed())
    return;

  // Static cast to SE3, this must be ensure beforehand using the propagator.
  g2o::VertexSE3* from = static_cast<g2o::VertexSE3*>(e_->vertex(0));
  g2o::VertexSE3* to = static_cast<g2o::VertexSE3*>(e_->vertex(1));
  g2o::EdgeSE3* e = static_cast<g2o::EdgeSE3*>(e_);

  if (from_.count(from) > 0){
    auto entry = adjacency->find(to);
    double distance = entry == adjacency->end() ? 0 : entry->second.distance(); // this shouldnt happen! "to" must be on the adjacency map
    to->setEstimate(exponencialInterpolation(to->estimate(), from->estimate() * e->measurement(), distance));
  }else{
    auto entry = adjacency->find(from);
    double distance = entry == adjacency->end() ? 0 : entry->second.distance(); // this shouldnt happen! "to" must be on the adjacency map
    from->setEstimate(exponencialInterpolation(from->estimate(), to->estimate() * e->measurement().inverse(), distance));
  }
}