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