Add odometry term to optimization problem (wip) #456

Mofef · 2017-08-16T11:31:25Z

This is to address #370 , but it's still work in progress. However since there are a lot of changes related to odometry happening these days I felt it's better to come forward with some intermediate state to avoid double work.
A detailed review on this is probably a waste of time but i'm happy for some directions

Unaddressed problems are:

New parameters should be extracted to configuration files.
So far I don't pay attention to the time stamps.
Removing the old consecutive scan distance minimization terms results in a broken trajectory
The translational scaling factor seams to take whatever lower bound I offer it without a visible effect to the map. So probably I have a mistake in the problem formulation.
Probably hard bounds on those parameters are not the best way to go. A penalty with a scaling parameter could be better.
Maybe you prefer odometry parameters to stay outside of cartographer entirely. I could then remove the related code from this PR and open a new issue.

wohe · 2017-08-16T15:14:21Z

cartographer/mapping_2d/sparse_pose_graph.cc

@@ -164,6 +164,15 @@ void SparsePoseGraph::AddImuData(const int trajectory_id,
  });
 }

+void SparsePoseGraph::AddOdometerData(const int trajectory_id,
+                                      const sensor::OdometryData odometry_data) {
+


Remove empty line?

wohe · 2017-08-16T15:15:02Z

cartographer/mapping_2d/sparse_pose_graph.h

@@ -97,6 +97,9 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {
      override EXCLUDES(mutex_);
  std::vector<Constraint> constraints() override EXCLUDES(mutex_);

+  void AddOdometerData(const int trajectory_id,


Move right after AddImuData() similar to the .cc file?

wohe · 2017-08-16T15:15:36Z

cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc

@@ -23,6 +23,7 @@
 #include <memory>
 #include <string>
 #include <vector>
+#include <cartographer/sensor/odometry_data.h>


Style: This belongs to the next group of includes in alphabetical order.

wohe · 2017-08-16T15:16:36Z

cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h

@@ -78,6 +79,9 @@ class OptimizationProblem {
  int num_trimmed_nodes(int trajectory_id) const;
  int num_trimmed_submaps(int trajectory_id) const;

+  void AddOdometerData(const int trajectory_id,


Move right after to AddImuData() as in the .cc?

wohe · 2017-08-16T15:17:10Z

cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h

@@ -22,6 +22,7 @@
 #include <map>
 #include <set>
 #include <vector>
+#include <cartographer/sensor/odometry_data.h>


Style: Move to the next block of includes.

wohe · 2017-08-16T15:19:15Z

cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc

+                      .pose,
+                  0.3, 30.})),
+          nullptr /* loss function */,
+          C_nodes[trajectory_id][odometry_data_index - 1].data(),


This seems wrong: indices of nodes and odometry data are different in general since usually odometry has a higher frequency. I think what you want to do is look at the time stamps of two consecutive nodes, then use the data in odometry_data_ to compute the relative change expected according to odometry. Since you most likely do not have the exact timestamp in the odometry data, you'd have to interpolate between two odometry poses, once for the timestamp of the first node and once for the timestamp of the second node.

Yes, thanks. I'm currently working on it.

wohe · 2017-08-16T15:21:16Z

cartographer/mapping_2d/sparse_pose_graph/spa_cost_function.h

@@ -81,6 +81,56 @@ class SpaCostFunction {
  const Constraint::Pose pose_;
 };

+class WarpedSpaCostFunction {


I suggest leaving this out for now. Let's keep the focus of this PR on introducing odometry to the optimization. Adding scaling factors can be done in a follow up PR.

wohe · 2017-08-16T15:25:10Z

cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc

+          C_odometry[trajectory_id].data());
+    }
+  }
+


Since we now tie together consecutive scans using odometry, the following code should be dropped/changed. As I see it odometry can take the place of

node_data_[trajectory_id][node_data_index - 1] .initial_point_cloud_pose.inverse() * node_data_[trajectory_id][node_data_index] .initial_point_cloud_pose

I'd suggest changing the code to have one loop and either use a new computation of this relative pose using odometry if available, or the old code otherwise.

wohe · 2017-08-16T15:28:30Z

If you would like to merge a smaller PR sooner, feel free to send a PR containing just the code that moves odometry data to the optimization problem without making use of it yet.

Mofef · 2017-08-17T09:37:59Z

Please take another look :)

wohe · 2017-08-18T13:12:50Z

cartographer/mapping_2d/sparse_pose_graph.h

@@ -78,6 +78,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {

  // Adds new IMU data to be used in the optimization.
  void AddImuData(int trajectory_id, const sensor::ImuData& imu_data);
+  void AddOdometerData(const int trajectory_id,
+                       const sensor::OdometryData data);


Take by const&? And please use the same argument name as in the implementation for readability: odometry_data. Similar in the optimization problem.

wohe · 2017-08-18T13:14:08Z

cartographer/mapping_2d/sparse_pose_graph.h

@@ -78,6 +78,8 @@ class SparsePoseGraph : public mapping::SparsePoseGraph {

  // Adds new IMU data to be used in the optimization.
  void AddImuData(int trajectory_id, const sensor::ImuData& imu_data);
+  void AddOdometerData(const int trajectory_id,


Drop the const of the const int argument? It is an implementation detail not needed in the header file. Similar in the optimization problem.

wohe · 2017-08-18T14:19:33Z

cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc

+              node_data_[trajectory_id][node_data_index].time) &&
+          odometry_data_[trajectory_id].Has(
+              node_data_[trajectory_id][node_data_index - 1].time)) {
+        problem.AddResidualBlock(


This is a lot of duplication. How about extracting the relative_pose, e.g.

const transform::Rigid2d relative_pose = odometry_available ? ... : ... problem.AddResidualBlock( ...

Mofef · 2017-08-18T15:55:09Z

Ok ready for another round :)

wohe

Just two nits.

wohe · 2017-08-18T16:02:15Z

cartographer/mapping_2d/sparse_pose_graph/optimization_problem.h

@@ -59,6 +61,8 @@ class OptimizationProblem {
  OptimizationProblem& operator=(const OptimizationProblem&) = delete;

  void AddImuData(int trajectory_id, const sensor::ImuData& imu_data);
+  void AddOdometerData(int trajectory_id,
+                       const sensor::OdometryData odometry_data);


wohe · 2017-08-18T16:03:12Z

cartographer/mapping_2d/sparse_pose_graph/optimization_problem.cc

  for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
       ++trajectory_id) {
    for (size_t node_data_index = 1;
         node_data_index < node_data_[trajectory_id].size();
         ++node_data_index) {
+      bool odometry_available =


Mofef · 2017-08-18T16:35:58Z

Done

@ojura

Implementation of cartographer-project#422 by @ojura. Original feature idea and implementation by @jihoonl.

Add odometry term to optimization problem

d4b9013

googlebot added the cla: yes label Aug 16, 2017

Fix segfault when no odometry is available

79f9fa1

wohe reviewed Aug 16, 2017

View reviewed changes

Mofef added 5 commits August 16, 2017 18:58

Properly match odometry data to node data

34cde01

Use interpolation buffer to store odometry data

06dff40

Remove odometry scale optimization parameters

3082209

Run clang-format

007b502

Address further style issues

1457955

wohe reviewed Aug 18, 2017

View reviewed changes

Address style improvements

241c05d

wohe reviewed Aug 18, 2017

View reviewed changes

Address reviewers comments

9fce905

wohe added 2 commits August 21, 2017 14:50

Update optimization_problem.cc

65648d5

Merge branch 'master' into optimization-odometry

df9cee6

wohe approved these changes Aug 21, 2017

View reviewed changes

wohe merged commit 0ef372d into cartographer-project:master Aug 21, 2017

jihoonl deleted the optimization-odometry branch August 21, 2017 13:21

wohe mentioned this pull request Aug 22, 2017

decreased map quality after angular velocity estimation(#458) #467

Closed

jihoonl mentioned this pull request Aug 30, 2017

Add Cartographer high level structure diagram #486

Merged

ojura mentioned this pull request Sep 15, 2017

Odometry in optimisation decreases quality, makes last submap pose jumpy #534

Closed

damienrg pushed a commit to damienrg/cartographer that referenced this pull request Nov 8, 2017

Add toggleable trajectories to RViz plugin (cartographer-project#456)

7e272f2

Implementation of cartographer-project#422 by @ojura. Original feature idea and implementation by @jihoonl.

ojura mentioned this pull request Aug 4, 2018

Global SLAM vs Local SLAM cartographer-project/cartographer_ros#967

Closed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Add odometry term to optimization problem (wip) #456

Add odometry term to optimization problem (wip) #456

Mofef commented Aug 16, 2017 •

edited

Loading

wohe Aug 16, 2017

wohe Aug 16, 2017

wohe Aug 16, 2017

wohe Aug 16, 2017

wohe Aug 16, 2017

wohe Aug 16, 2017

Mofef Aug 16, 2017

wohe Aug 16, 2017

wohe Aug 16, 2017

wohe commented Aug 16, 2017

Mofef commented Aug 17, 2017

wohe Aug 18, 2017

wohe Aug 18, 2017

wohe Aug 18, 2017

Mofef commented Aug 18, 2017

wohe left a comment

wohe Aug 18, 2017

wohe Aug 18, 2017

Mofef commented Aug 18, 2017

Add odometry term to optimization problem (wip) #456

Add odometry term to optimization problem (wip) #456

Conversation

Mofef commented Aug 16, 2017 • 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

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

wohe commented Aug 16, 2017

Mofef commented Aug 17, 2017

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Mofef commented Aug 18, 2017

wohe left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Mofef commented Aug 18, 2017

Mofef commented Aug 16, 2017 •

edited

Loading