Introduce [Timed]RangefinderPoint. #1357

ojura · 2018-07-31T13:31:07Z

ojura · 2018-07-31T16:05:34Z

So, this PR is about getting rid of direct uses of Vector3/4 throughout the Cartographer codebase and using a new class [Timed]RangefinderPoint instead. Also, I made sure not to interrupt the data flow of points - wherever we recalculate the position inside RangefinderPoint, the structure itself is preserved (using overloading of operator*). This enables libcartographer users to extend RangefinderPoint with their own stuff and have that additional metadata survive the entire local SLAM process (so we get the same RangefinderPoints inside a LocalSlamResult).

Sorry for the PR linediff size. Vector3/4s were sprinkled all over the codebase. It would be great if you could review it quickly so it doesn't rot away :)

ojura · 2018-07-31T16:18:10Z

cartographer/sensor/proto/sensor.proto

@@ -30,14 +39,14 @@ message CompressedPointCloud {
 message TimedPointCloudData {
  int64 timestamp = 1;
  transform.proto.Vector3f origin = 2;
-  repeated transform.proto.Vector4f point_data = 3;
+  repeated TimedRangefinderPoint point_data = 3;


Is it ok to break compatibility here?
If not, perhaps we could rename the third field to point_data_legacy and have the new TimedRangefinderPoint as the 4th field?

yes, please do that. Let's not break it for now. We will have plenty of opportunities to do so :)

ojura · 2018-07-31T16:19:00Z

cartographer/sensor/range_data.cc

-  std::transform(
-      proto.misses().begin(), proto.misses().end(), std::back_inserter(misses),
-      static_cast<Eigen::Vector3f (*)(const transform::proto::Vector3f&)>(
-          transform::ToEigen));


I'd say that this monstrosity disappearing is the highlight of this PR

I cannot stop being surprised that we have such code in the first place... :)

ojura · 2018-07-31T16:22:21Z

cartographer/mapping/internal/3d/local_trajectory_builder_3d.cc

@@ -190,7 +190,8 @@ LocalTrajectoryBuilder3D::AddRangeData(
        // maximum range. This way the free space up to the maximum range will
        // be updated.
        accumulated_range_data_.misses.push_back(
-            origin_in_local + options_.max_range() / range * delta);
+            hit_in_local.RestrictDistanceFromOrigin(origin_in_local,


We avoid breaking the data flow and discarding possible additional metadata here by
(1) computing hit_in_local using operator*(Rigid, RangefinderPoint) which is metadata-friendly
(2) avoiding creating a completely new RangefinderPoint by using a RestrictDistanceFromOrigin method, which is again metadata-friendly

gaschler · 2018-08-02T09:26:45Z

@ojura Please consider presenting your design in the open house (today or in two weeks) to make sure implementation goes in the right direction.

ojura · 2018-08-03T07:21:22Z

To address some concerns from yesterday:
Performance:
30 runs of roslaunch cartographer_ros offline_backpack_2d.launch bag_filenames:=b2-2016-04-05-14-44-52.bag no_rviz:=true
with [Timed]RangefinderPoint: μ = 64.64 s, σ = 0.2
master: μ = 65.06 s, σ = 0.31

I believe this change does not increase complexity significantly and that it makes code more readable. It is made clear that a rangefinder point is being passed, and not just any Vector3f. Also, I think .time() is more readable than accessing the fourth element ([3]) of a Vector4f.

ojura · 2018-08-07T18:49:27Z

@pifon2a @gaschler @cschuet friendly ping, PTAL?

pifon2a

I like what you're doing here.

pifon2a · 2018-08-08T15:08:27Z

cartographer/io/points_batch.h

@@ -52,7 +53,7 @@ struct PointsBatch {
  int trajectory_id;

  // Geometry of the points in the map frame.
-  std::vector<Eigen::Vector3f> points;
+  cartographer::sensor::PointCloud points;


nit: just sensor::PointCloud.

pifon2a · 2018-08-08T15:11:48Z

cartographer/sensor/point_cloud.h

@@ -29,14 +30,14 @@ namespace sensor {

 // Stores 3D positions of points.
 // For 2D points, the third entry is 0.f.
-typedef std::vector<Eigen::Vector3f> PointCloud;
+typedef std::vector<RangefinderPoint> PointCloud;


nit:
using PointCloud = std::... here and below

pifon2a · 2018-08-08T15:23:50Z

cartographer/sensor/proto/sensor.proto

@@ -20,6 +20,15 @@ option java_outer_classname = "Sensor";

 import "cartographer/transform/proto/transform.proto";

+message RangefinderPoint {
+  transform.proto.Vector3f position = 1;


I am not sure I like 'position' name here. In ROS it was called just a point.

Sure. We're not bound by ROS conventions here, though. A RangefinderPoint having a point member sounds kind of odd to me.

When I see position I always confuse it with pose which also has orientation. What about x_y_z or coordinates? Or leave it as position.

At the robotics classes at my uni, we were taught the following definition of a robot pose: The combination of position and orientation is referred to as the pose of an object.

So if that's okay with you, I'd leave it as position :-)

Yeah, it is ok. :) I did not have robotics classes :)

pifon2a · 2018-08-08T15:24:53Z

cartographer/sensor/proto/sensor.proto

+}
+
+message TimedRangefinderPoint {
+  RangefinderPoint rangefinder_point = 1;


inline the type: transform.proto.Vector3f point = 1; . It can happen that RangefinderPoint has some metadata that we do not really want in TimedRangefinderPoint.

Okay, should the C++ implementation follow this as well? I.e. TimedRangefinderPoint contains a Vector3f?

pifon2a · 2018-08-08T15:26:11Z

cartographer/sensor/proto/sensor.proto

@@ -30,14 +39,14 @@ message CompressedPointCloud {
 message TimedPointCloudData {
  int64 timestamp = 1;
  transform.proto.Vector3f origin = 2;
-  repeated transform.proto.Vector4f point_data = 3;
+  repeated TimedRangefinderPoint point_data = 3;


yes, please do that. Let's not break it for now. We will have plenty of opportunities to do so :)

pifon2a · 2018-08-08T15:41:13Z

cartographer/sensor/range_data.cc

-  std::transform(
-      proto.misses().begin(), proto.misses().end(), std::back_inserter(misses),
-      static_cast<Eigen::Vector3f (*)(const transform::proto::Vector3f&)>(
-          transform::ToEigen));


I cannot stop being surprised that we have such code in the first place... :)

pifon2a · 2018-08-08T15:43:30Z

cartographer/sensor/timed_point_cloud_data.h

@@ -32,7 +32,7 @@ struct TimedPointCloudData {

 struct TimedPointCloudOriginData {
  struct RangeMeasurement {
-    Eigen::Vector4f point_time;
+    sensor::TimedRangefinderPoint point_time;


omit sensor::.

pifon2a · 2018-08-08T15:48:41Z

cartographer/sensor/rangefinder_point.h

+namespace cartographer {
+namespace sensor {
+
+// Stores 3D position of a point observed by a rangefinder sensor.


Please, use composition here.

All data members should be in private section of the class.

Try to get rid of friend functions.

About the friend operator*: we typically apply a multiplication with a Rigid3 pose estimate from the left. Unfortunately, a member operator* only allows this as the left operand. So I see two solutions: either an overloaded friend operator* like this, or a member function e.g. RangefinderPoint::ToLocal(Rigid3 pose_estimate) which would do a left multiplication.

Also, about composition: you mentioned you would inline the Vector3f in TimedRangefinderPoint proto. Does this mean that the C++ class should be the same?

Slicing a TimedRangefinderPoint into a RangefinderPoint then means a little bit of more work. Should I put it in a TimedRangefinderPoint::ToRangefinderPoint member?

Make them structs. As in proto.

struct TimedRangefinderPoint { proto::RangefinderPoint ToProto() const; Eigen::Vector3f position_; float time_; }; struct RangefinderPoint { proto::RangefinderPoint ToProto() const; Eigen::Vector3f position_; };

Isn't _ only for private members? Also, I think structs shouldn't have methods, IIRC ToProto/FromProto are implemented as functions outside structs.

In structs, you do not need _, sorry, i just copied. Structs should not have complicated logic, yes. But serialization/deserialization can be put there. Up to you.

Okay, cool. I'm only missing one more detail: where to implement discarding time? Member TimedRangefinderPoint::ToRangefinderPoint or standalone function?

Do you need it? Isn't it just a call to constructor of RangefinderPoint(timed_point.position)?

ojura · 2018-08-08T21:12:43Z

Okay, the nits should be fixed. I updated the two new classes to be structs. FromProto([Range|TimedPointCloud]Data) should be able to handle old .pbstreams.

ojura · 2018-08-08T22:00:30Z

cartographer/sensor/range_data.cc

+    for (const auto& point_proto : proto.returns())
+      returns.push_back(FromProto(point_proto));
+  } else {
+    returns.reserve(proto.returns_legacy().size());


This should take care of deserializing old .pbstreams

ojura · 2018-08-09T05:15:40Z

cartographer/sensor/rangefinder_point.h

+  return lhs.position == rhs.position && lhs.time == rhs.time;
+}
+
+RangefinderPoint FromProto(


@pifon2a WDYT about inlining all of these? Point processing functions seem like they could cause a bottleneck if not inlined.

pifon2a · 2018-08-08T18:26:19Z

cartographer/sensor/rangefinder_point.h

+namespace cartographer {
+namespace sensor {
+
+// Stores 3D position of a point observed by a rangefinder sensor.


Do you need it? Isn't it just a call to constructor of RangefinderPoint(timed_point.position)?

pifon2a · 2018-08-09T10:59:13Z

cartographer/mapping/internal/2d/local_trajectory_builder_2d.cc

    const Eigen::Vector3f origin_in_local =
        range_data_poses[i] *
        synchronized_data.origins.at(synchronized_data.ranges[i].origin_index);
-    const Eigen::Vector3f hit_in_local = range_data_poses[i] * hit.head<3>();
-    const Eigen::Vector3f delta = hit_in_local - origin_in_local;
+    sensor::RangefinderPoint hit_in_local =


If I am not mistaken, you can write it like:
sensor::RangefinderPoint hit_in_local{range_data_poses[i] * hit.position}

I disagree with this and the comment below. The whole point of this PR is to enable adding metadata by changing only rangefinder_point.h. The rest of Cartographer should be written with an assumption that there is some hidden metadata in RangefinderPoint. This metadata should be preserved throughout the SLAM pipeline. By constructing a completely new point here, and not using operator*, we would discard this data.

Could you provide an example of such metadata? And when it will be added to Cartographer.

Please see the comment below. My primary motivation for this is keeping the laser ID (i.e. which laser of the e.g. 16 lasers on the lidar has observed the point). Currently, I do not plan on upstreaming this, but I would like to be able to do this with minimal interventions to Cartographer, hence this PR. But if there's interest, sure, why not.

Another possible metadata that we might want to add is the sensor ID, to help resolve issues like #947.

pifon2a · 2018-08-09T11:03:46Z

cartographer/sensor/rangefinder_point.h

+  float time;
+};
+
+template <class T>


since there is no metadata yet, I doubt that the operators*() are needed at all.

pifon2a · 2018-08-09T11:05:04Z

cartographer/sensor/rangefinder_point.h

+  return proto;
+}
+
+inline RangefinderPoint ToRangefinderPoint(


ToRangefinderPoint/ToTimedRangefinderPoint are also not needed.

For my use case, they absolutely are. If I decide to add an int LaserId to [Timed]RangefinderPoint in my fork, I want to be able to do this only by updating rangefinder_point.h. In this case, I would update ToRangefinderPoint to also copy over LaserId. I want to avoid having to update the rest of Cartographer codebase for this. This is why I insist on using operator* and these two functions.

pifon2a · 2018-08-09T11:05:37Z

cartographer/sensor/timed_point_cloud_data.cc

+  if (proto.point_data().size() > 0) {
+    timed_point_cloud.reserve(proto.point_data().size());
+    for (const auto& timed_point_proto : proto.point_data())
+      timed_point_cloud.push_back(FromProto(timed_point_proto));


nit: unfortunately, style guide requires {} here.

ojura · 2018-08-10T11:49:06Z

@pifon2a Have I managed to convince you about operator* and ToRangefinderPoint? Ready to merge?

pifon2a · 2018-08-10T11:50:36Z

Juraj, I want to take another look and then we merge today. Let me finish some stuff first. -- Cheers, Alex

…

On Fri, Aug 10, 2018 at 1:49 PM Juraj Oršulić ***@***.***> wrote: @pifon2a <https://github.com/pifon2a> Have I managed to convince you? Ready to merge? — You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub <#1357 (comment)>, or mute the thread <https://github.com/notifications/unsubscribe-auth/AfA_b56DJ25SeEkqkXxDphZVLeEAhDCsks5uPXNNgaJpZM4VoQha> .

pifon2a

ok, looks fine.

googlebot added the cla: yes label Jul 31, 2018

ojura mentioned this pull request Jul 31, 2018

Follow cartographer#1357 cartographer-project/cartographer_ros#964

Merged

ojura commented Jul 31, 2018

View reviewed changes

ojura added 7 commits August 7, 2018 20:47

Introduce [Timed]RangefinderPoint.

51db8cf

Fix tests.

8efb89d

Finish fixing up tests

93222da

get rid of warning

26ea5ef

fix grpc test

c65bb4b

fix grpc again

6dff4a9

update tsdf

f2edc50

ojura force-pushed the introduce-rangefinderpoint branch from aeb815c to f2edc50 Compare August 7, 2018 18:47

pifon2a suggested changes Aug 8, 2018

View reviewed changes

ojura added 7 commits August 8, 2018 19:41

purge sensor:: from sensor/

d5726f3

typedef -> using

cd16c71

cleanup points_batch.h

7ae467a

missed one typedef

aea858b

implementation with structs

a11d5de

fix further tests

b0f70d7

update year

51e495c

add comment for ToRangefinderPoint

59cf08e

ojura commented Aug 8, 2018

View reviewed changes

ojura commented Aug 9, 2018

View reviewed changes

Inline rangefinder point functions.

4c99b50

ojura and others added 4 commits August 9, 2018 08:11

add missing header

70d59b2

add missing header to the right place

302158b

re-run CI

88ad420

Ran clang-format.

29319b9

pifon2a suggested changes Aug 9, 2018

View reviewed changes

ojura and others added 5 commits August 9, 2018 13:22

address brackets nit

1c2d18b

Merge branch 'master' into introduce-rangefinderpoint

79476d9

Ran clang-format.

616c01e

Merge branch 'master' into introduce-rangefinderpoint

10ab386

Merge branch 'master' into introduce-rangefinderpoint

6a399f3

ojura added 5 commits August 10, 2018 14:34

Inline operator*

2b6ea17

Run clang-format.

e49b9c4

rerun CI

e5f8db8

rerun CI again

6296f2a

Merge branch 'master' into introduce-rangefinderpoint

bc927a0

pifon2a approved these changes Aug 10, 2018

View reviewed changes

Merge branch 'master' into introduce-rangefinderpoint

d7cb478

pifon2a merged commit 73c3d47 into cartographer-project:master Aug 10, 2018

ojura deleted the introduce-rangefinderpoint branch August 10, 2018 21:30

akshay-prsd pushed a commit to BossaNova/cartographer that referenced this pull request Jul 18, 2019

Introduce [Timed]RangefinderPoint. (cartographer-project#1357)

2628bdd

maxschnettler mentioned this pull request Aug 8, 2019

Wrong point cloud origin when accumulating data of different sensors #947

Open

schayapathy pushed a commit to schayapathy/cartographer that referenced this pull request Aug 15, 2019

Introduce [Timed]RangefinderPoint. (cartographer-project#1357)

9b2780c

Introduce [Timed]RangefinderPoint. #1357

Introduce [Timed]RangefinderPoint. #1357

Conversation

ojura commented Jul 31, 2018

ojura commented Jul 31, 2018 • edited Loading

ojura Jul 31, 2018 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

gaschler commented Aug 2, 2018

ojura commented Aug 3, 2018 • edited Loading

ojura commented Aug 7, 2018

pifon2a left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

ojura commented Aug 8, 2018 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

ojura Aug 9, 2018 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

ojura commented Aug 10, 2018 • edited Loading

pifon2a commented Aug 10, 2018 via email

pifon2a left a comment

Choose a reason for hiding this comment

ojura commented Jul 31, 2018 •

edited

Loading

ojura Jul 31, 2018 •

edited

Loading

ojura commented Aug 3, 2018 •

edited

Loading

ojura commented Aug 8, 2018 •

edited

Loading

ojura Aug 9, 2018 •

edited

Loading

ojura commented Aug 10, 2018 •

edited

Loading