void MainWindow::setRobustKernel()
{
SparseOptimizer* optimizer = viewer->graph;
bool robustKernel = cbRobustKernel->isChecked();
double huberWidth = leKernelWidth->text().toDouble();
//odometry edges are those whose node ids differ by 1
bool onlyLoop = cbOnlyLoop->isChecked();
if (robustKernel) {
QString strRobustKernel = coRobustKernel->currentText();
AbstractRobustKernelCreator* creator = RobustKernelFactory::instance()->creator(strRobustKernel.toStdString());
if (! creator) {
cerr << strRobustKernel.toStdString() << " is not a valid robust kernel" << endl;
return;
}
for (SparseOptimizer::EdgeSet::const_iterator it = optimizer->edges().begin(); it != optimizer->edges().end(); ++it) {
OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);
if (onlyLoop) {
if (e->vertices().size() >= 2 && std::abs(e->vertex(0)->id() - e->vertex(1)->id()) != 1) {
e->setRobustKernel(creator->construct());
e->robustKernel()->setDelta(huberWidth);
}
} else {
e->setRobustKernel(creator->construct());
e->robustKernel()->setDelta(huberWidth);
}
}
} else {
for (SparseOptimizer::EdgeSet::const_iterator it = optimizer->edges().begin(); it != optimizer->edges().end(); ++it) {
OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);
e->setRobustKernel(0);
}
}
}
void MainWindow::setRobustKernel()
{
SparseOptimizer* optimizer = viewer->graph;
bool robustKernel = cbRobustKernel->isChecked();
double huberWidth = leKernelWidth->text().toDouble();
if (robustKernel) {
QString strRobustKernel = coRobustKernel->currentText();
AbstractRobustKernelCreator* creator = RobustKernelFactory::instance()->creator(strRobustKernel.toStdString());
if (! creator) {
cerr << strRobustKernel.toStdString() << " is not a valid robust kernel" << endl;
return;
}
for (SparseOptimizer::EdgeSet::const_iterator it = optimizer->edges().begin(); it != optimizer->edges().end(); ++it) {
OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);
e->setRobustKernel(creator->construct());
e->robustKernel()->setDelta(huberWidth);
}
} else {
for (SparseOptimizer::EdgeSet::const_iterator it = optimizer->edges().begin(); it != optimizer->edges().end(); ++it) {
OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);
e->setRobustKernel(0);
}
}
}
void SparseOptimizer::computeActiveErrors()
{
for (EdgeContainer::const_iterator
it = _activeEdges.begin();
it != _activeEdges.end();
it++)
{
OptimizableGraph::Edge* e = *it;
e->computeError();
if (e->robustKernel()) e->robustifyError();
}
}
