0
$\begingroup$

//cross posted from stackoverflow. If that is not ok, I will delete!

I have an optimisation problem that I am trying to solve with g2o

I have a data set which is N noisy Poses (XYZ), and known (less noisy) distances from every Pose to every other Pose.

I have it structured like:

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;

I set up the Poses as Vertices in my graph, fixing the first one:

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

As edges, I know the distance from each Pose to every other Pose, from range measurements. I want to now add these distances as constraints.

I have this:

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

But the edge->setMeasurement expects a Vector3d, and I have a double value as a distance reading.

What edge should I be using for this kind of constraint? Or do i need to get a Vector3d from my distance measurement somehow?

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.