I'm trying to use the Reference Frame Factor to try and do ICP between corresponding points in two different frames. It might work but I get weird solutions which seem wrong, so I think I have a misunderstanding about the usage of this factor and its capabilities.
My points are 2D and I am relating them by a Pose2D. I have a list of N corresponding points and I define N ReferenceFrameFactors<Point2, Pose2> in my graph. Theres not a lot of noise in my points, but sometimes there's a significant disparity when I plot both sets of points back in the global frame.
I was at one stage setting a prior on my points values so that they have noise but I found issues whether I did that or not.
Is there a more appropriate factor to use? Or a better way to use it?
cheers
Peter