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

g2o图形优化，用于距离限制的边缘是什么？

0 个答案: