//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());
        if (i == 0)

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];

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?


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.