# g2o Graph optimisation, What edge to use for a distance constraint?

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


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() = optimizer.vertices()[distances[i].id_from];
edge->vertices() = 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?