Fit one discrete set of points to another by shifting and rotating/tilting

123 views
Skip to first unread message

Mikhail Timofeev

unread,
Apr 15, 2020, 4:18:29 AM4/15/20
to gtsam users
I have two sets of measurements where every data point A, B consists of (x, y, timestamp) and I need to fit one of them to the other, i.e. find a transformation (shifting and rotating) that will bring A as close as possible to B. Timestamps provide a way to match the points between sets, i.e. I know which point from set A corresponds to which point from the set B. The problem has multiple local minima. Does anybody have an example of GTSAM code with a problem similar to this?

José Luis Blanco-Claraco

unread,
Apr 15, 2020, 5:23:27 AM4/15/20
to Mikhail Timofeev, gtsam users
Hi,

Something like this with expressions should work:

using namespace gtsam;
using gtsam::symbol_shorthand::T; // Pose3's
using gtsam::symbol_shorthand::P; // Point3's

// for each pairing:
newfactors.addExpressionFactor(compose(Pose3_(T(posekey)),
Point3_(P(localPointIdx))), Point3(globalPointValue), noiseModel);



JL

On Wed, Apr 15, 2020 at 10:18 AM Mikhail Timofeev
<mikhail.a...@gmail.com> wrote:
>
> I have two sets of measurements where every data point A, B consists of (x, y, timestamp) and I need to fit one of them to the other, i.e. find a transformation (shifting and rotating) that will bring A as close as possible to B. Timestamps provide a way to match the points between sets, i.e. I know which point from set A corresponds to which point from the set B. The problem has multiple local minima. Does anybody have an example of GTSAM code with a problem similar to this?
>
> --
> You received this message because you are subscribed to the Google Groups "gtsam users" group.
> To unsubscribe from this group and stop receiving emails from it, send an email to gtsam-users...@googlegroups.com.
> To view this discussion on the web visit https://groups.google.com/d/msgid/gtsam-users/8eeb48c0-8a48-48ff-8f96-42ef9dc3e26b%40googlegroups.com.



--

/**
* Jose Luis Blanco-Claraco
* Universidad de Almería - Departamento de Ingeniería
* [Homepage]( https://w3.ual.es/~jlblanco/ )
* [GH profile]( https://github.com/jlblancoc )
*/

Dellaert, Frank

unread,
Apr 15, 2020, 7:46:43 AM4/15/20
to José Luis Blanco-Claraco, Mikhail Timofeev, gtsam users
You can do nonlinear optimization as Jose indicates, but we have closed form solutions for this, for use in ICP. Search for “Align” in Pose3. Forget whether we have a Pose2 version but I think so.

Frank

From: 'José Luis Blanco-Claraco' via gtsam users <gtsam...@googlegroups.com>
Sent: Wednesday, April 15, 2020 5:23:15 AM
To: Mikhail Timofeev <mikhail.a...@gmail.com>
Cc: gtsam users <gtsam...@googlegroups.com>
Subject: Re: [GTSAM] Fit one discrete set of points to another by shifting and rotating/tilting
 

albert_xu

unread,
Feb 19, 2021, 7:47:10 AM2/19/21
to gtsam users
Could you give more details, I want to know how can we get globalPointValue

Dellaert, Frank

unread,
Feb 19, 2021, 10:01:19 AM2/19/21
to albert_xu, gtsam users
Again, here the align methods might help, resp in Pose2.h and Pose3.h:

/**
 * Calculate pose between a vector of 2D point correspondences (p,q)
 * where q = Pose2::transformFrom(p) = t + R*p
 */
typedef std::pair<Point2,Point2> Point2Pair;
static boost::optional<Pose2> align(const std::vector<Point2Pair>& pairs);

/**
   *  Create Pose3 by aligning two point pairs
   *  A pose aTb is estimated between pairs (a_point, b_point) such that a_point = aTb * b_point
   *  Note this allows for noise on the points but in that case the mapping will not be exact.
   */
boost::optional<Pose3> Align(const std::vector<Point3Pair>& abPointPairs);

and in Similarity3 if there is an unknown scale:

/**
   *  Create Similarity3 by aligning at least three point pairs
   */
  GTSAM_UNSTABLE_EXPORT static Similarity3 Align(const std::vector<Point3Pair>& abPointPairs);
 
  /**
   *  Create Similarity3 by aligning at least two pose pairs
   */
  GTSAM_UNSTABLE_EXPORT static Similarity3 Align(const std::vector<Pose3Pair>& abPosePairs);


From: gtsam...@googlegroups.com <gtsam...@googlegroups.com> on behalf of albert_xu <xsx...@gmail.com>
Sent: Friday, February 19, 2021 07:47
To: gtsam users <gtsam...@googlegroups.com>

Subject: Re: [GTSAM] Fit one discrete set of points to another by shifting and rotating/tilting

albert_xu

unread,
Feb 20, 2021, 9:33:43 PM2/20/21
to gtsam users
Thank you. What I want to ask is that if I define a new function(such as,closetpoint) instead of using  align methods to get the globalPointValue.Could I continue to use the function(addExpressionFactor)?
Because I think that the second parameter(that is measurement) in the function(addExpressionFactor) should be determinate(or fixed) during all iterative optimization processes.
But I hope it will change once at every iteration.In other words, during the optimization process, my measurement values ​​are also always changing like the first parameter(that is hypothesis). And if the function(addExpressionFactor) is not suitable, which function can be used to add the factor(constraints) into the graph? This is the core question I want to ask, and ICP is just an example.

Dellaert, Frank

unread,
Feb 20, 2021, 9:37:18 PM2/20/21
to albert_xu, gtsam users
If you’re really just trying to implement ICP, the align methods are the way to go.

Frank

Sent: Saturday, February 20, 2021 9:33:43 PM

albert_xu

unread,
Feb 21, 2021, 10:29:33 PM2/21/21
to gtsam users
Is it possible to do  dynamic data association during optimization progress?

Dellaert, Frank

unread,
Feb 21, 2021, 11:00:05 PM2/21/21
to albert_xu, gtsam users
No, but (vanilla) ICP is just:
1) align
2) find closest points
3) repeat if assignment changed
if you’re trying to do fancier I’m not understanding what it is.

Frank

Sent: Sunday, February 21, 2021 10:29:33 PM

albert_xu

unread,
Feb 22, 2021, 12:18:18 AM2/22/21
to gtsam users
Thank you
Reply all
Reply to author
Forward
0 new messages