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

Question

У меня есть проблема оптимизации, которую я пытаюсь решить с помощью 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());