Distance factor between two points

[S Chatzizacharias]

I want to add a distance factor between two Point2 variables. (i.e. distance between point 1 and point 2 is always 1.0 m) but I'm not sure how to implement it. I tried with emplaced_share like this:

graph.emplace_shared<BetweenFactor<Point2> >(point_1_key, point_2_key, [distance?], [noise_model?]);

but I'm not sure what type I should use for the distance and the noise model. Should I create a custom factor for distance between two points? And If yes, can you provide an example or a tutorial? I also tried to play with this example but I have the same questions as with the emplace_shared. 

I'm new with gtsam, so excuse me if the question is too trivial. 

[Dellaert, Frank]

Look at the templated Range factors. They do exactly what you want. Check out their unit tests.

Frank

---

From: gtsam...@googlegroups.com <gtsam...@googlegroups.com> on behalf of S Chatzizacharias <spyros...@gmail.com>
Sent: Tuesday, June 1, 2021 9:25:23 AM
To: gtsam users <gtsam...@googlegroups.com>
Subject: [GTSAM] Distance factor between two points

 

--
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/26fe73d2-0d0e-4778-9880-543f5d3a80ccn%40googlegroups.com.

[S Chatzizacharias]

I'm looking at the testRangeFactor.cpp from the slam test files and I can find 4 different uses of the range factor, but none of them is between two Point2 types.

// Create a noise model for the pixel error

static SharedNoiseModel model(noiseModel::Unit::Create(1));

typedef RangeFactor<Pose2, Point2> RangeFactor2D;

typedef RangeFactor<Pose3, Point3> RangeFactor3D;

typedef RangeFactorWithTransform<Pose2, Point2> RangeFactorWithTransform2D;

typedef RangeFactorWithTransform<Pose3, Point3> RangeFactorWithTransform3D;

// Keys are deliberately *not* in sorted order to test that case.

Key poseKey(2);

Key pointKey(1);

double measurement(10.0);

I'm I looking at the correct place? Or does the same factor can be used with <Point2> instead of <Pose2, Point2> ?

Regards,

Spyros

[Dellaert, Frank]

Hmmm, don’t even remember the last two. But, yes, try <Point2,Point2>. 

Frank

---

From: gtsam...@googlegroups.com <gtsam...@googlegroups.com> on behalf of S Chatzizacharias <spyros...@gmail.com>

Sent: Wednesday, June 2, 2021 5:38:57 AM
To: gtsam users <gtsam...@googlegroups.com>
Subject: Re: [GTSAM] Distance factor between two points

 

To view this discussion on the web visit https://groups.google.com/d/msgid/gtsam-users/83005cc8-cb93-4289-8d8c-e82b6df87558n%40googlegroups.com.

[S Chatzizacharias]

I tried this:

double door_width(1.0);

static SharedNoiseModel width_noise(noiseModel::Unit::Create(1));

graph.emplace_shared<RangeFactor<Point2, Point2>>(next_door_num_left, next_door_num_right, door_width, width_noise);

with next_door_num_left and next_door_num_right being integers used as variable keys. 

I get the following compilation error:

error: invalid use of incomplete type ‘struct gtsam::Range<Eigen::Matrix<double, 2, 1>, Eigen::Matrix<double, 2, 1> >’

     return Expression<T>(Range<A1, A2>(), a1_, a2_);

but I don't understand why. 

[Dellaert, Frank]

I added this in https://github.com/borglab/gtsam/pull/783

You can check out this branch or merge it into your branch, or wait until Fan approves it.

Best

Frank

---

From: gtsam...@googlegroups.com <gtsam...@googlegroups.com> on behalf of S Chatzizacharias <spyros...@gmail.com>

Sent: Tuesday, June 1, 2021 09:25

To: gtsam users <gtsam...@googlegroups.com>
Subject: [GTSAM] Distance factor between two points

--

[jiang...@gmail.com]

Hi all,

#783 is merged.

Best,

Fan

[S Chatzizacharias]

hello. Thank you for your fast response.

I add the changes in Point2.h and testRangeFactor.cpp but I stil get this compilation error:

/usr/local/include/gtsam/sam/RangeFactor.h:59:26: error: invalid use of incomplete type ‘struct gtsam::Range<Eigen::Matrix<double, 2, 1>, Eigen::Matrix<double, 2, 1> >’

     return Expression<T>(Range<A1, A2>(), a1_, a2_);

                          ^~~~~~~~~~~~~~~
In file included from /usr/local/include/gtsam/geometry/Pose2.h:23:0,
                 from /home/spyros/catkin_ws/src/door_detector_1/src/graph_creator_1.cpp:12:
/usr/local/include/gtsam/geometry/BearingRange.h:39:8: note: declaration of ‘struct gtsam::Range<Eigen::Matrix<double, 2, 1>, Eigen::Matrix<double, 2, 1> >’
 struct Range;

Should something change in RangeFactor.h as well?

Regards,

Spyros

You received this message because you are subscribed to a topic in the Google Groups "gtsam users" group.
To unsubscribe from this topic, visit https://groups.google.com/d/topic/gtsam-users/IDJtClsqYHs/unsubscribe.
To unsubscribe from this group and all its topics, send an email to gtsam-users...@googlegroups.com.
To view this discussion on the web visit https://groups.google.com/d/msgid/gtsam-users/214c3345-4772-4042-acf3-0b10cad99955n%40googlegroups.com.

[S Chatzizacharias]

The problem was solved by uninstalling gtsam with running 

xargs rm -rf < install_manifest.txt 

from the build directory of the gtsam (check this issue), then reinstalling gtsam from source (git clone https:.....) and then building the package as described here. (sudo was used in the install command: sudo make install)