g2o Оптимизация графика, какое ребро использовать для ограничения расстояния? - PullRequest
Новая
       84

g2o Оптимизация графика, какое ребро использовать для ограничения расстояния?

0 голосов
/ 19 июня 2019

У меня есть проблема оптимизации, которую я пытаюсь решить с помощью g2o

У меня есть набор данных, который представляет собой N шумных поз (XYZ) и известные (менее шумные) расстояния откаждую позу для каждой другой позы.

Я структурировал ее следующим образом: 

struct structDistance
{
    int id_from;
    int id_to;
    double distance;
};

struct structPose
{
    int id;
    Eigen::Vector3d position;
};

std::vector<structPose> poses;
std::vector<structDistance> distances;

Я установил позы как вершины на моем графике, исправив первую: 

for (size_t i = 0; i < poses.size(); ++i)
    {       
        Eigen::Vector3d xyz = poses[i].position;
        g2o::VertexPointXYZ* vertex(new g2o::VertexPointXYZ());
        vertex->setId(poses[i].id);
        vertex->setEstimate(xyz);
        if (i == 0)
        {
            vertex->setFixed(true);
        }
        optimizer.addVertex(vertex);        
}

Как края, я знаю расстояние от каждой позы до каждой другой позы, из измерений дальности.Теперь я хочу добавить эти расстояния как ограничения.

У меня есть это: 

//add edges. These are the distances between the poses.
    for (size_t i = 0; i < distances.size(); ++i)
    {       
            g2o::EdgePointXYZ* edge = new g2o::EdgePointXYZ();
            edge->setMeasurement(distances[i].distance); //THIS NEEDS VECTOR3D          
            edge->vertices()[0] = optimizer.vertices()[distances[i].id_from];
            edge->vertices()[1] = optimizer.vertices()[distances[i].id_to];
            optimizer.addEdge(edge);        
    }

Но edge->setMeasurement ожидает Vector3d, и у меня есть двойное значение в качестве показания расстояния.

Какое ребро я должен использовать для такого рода ограничений?Или мне нужно как-то получить Vector3d из моего измерения расстояния?

Кроме того, что делает эта линия?Мне это нужно? 

edge->setInformation(EdgePointXYZ::InformationType::Identity());
...