[Feature] Set SE3 priors with only position contraints (XYZ)

[TSC21]

Hi,

I see that EdgeSE2XYPrior can be use to setup a prior to a 2D pose with constraints in XY. I was expecting there could be something similar to 3D pose (EdgeSE3XYZPrior), but I couldn't find that type. I find that there's an EdgeXYZPrior type, but it expects vertexes of type VertexPointXYZ, while, on the graph I am building, I am adding VertexSE3.

The main application for this is adding GPS measurements as priors to constrain XYZ, but without constraining the orientation, since there's no orientation data being fed. Even feeding quite large covariances to the angular data leads to the priors condition the entire graph on orientation as well, even that one just wants to constrain the position.

Is there a solution that can be obtained with the current existent types, or something as EdgeSE3XYZPrior is required?

Thanks in advance for this!

[ferreram]

Hi there,

You can add something like this:

class G2O_TYPES_SBA_API EdgeXYZPriorSE3Expmap : public BaseUnaryEdge<3, SE3Quat, VertexSE3Expmap>{ public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

EdgeXYZPriorSE3Expmap();

bool read(std::istream& is);

bool write(std::ostream& os) const;

void computeError()  {
  const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[0]);

  SE3Quat C(_measurement);
  SE3Quat error_= v1->estimate().inverse()*C;
  auto err = error_.log();

  _error[0] = err[3];
  _error[1] = err[4];
  _error[2] = err[5];    
}

// virtual void linearizeOplus();

};

[sume-cn]

@ferreram What will be the Jacobian for this edge ? And why not use EdgeXYZPriorSE3Expmap : public BaseUnaryEdge<3, Vector3d, VertexSE3Expmap> since the GPS measurement does not have orientation?