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