Commit Graph

522 Commits (363a337e67776d9762f29d762326bd2c53fb41fb)

Author SHA1 Message Date
Alexander Belyaev 986ac28ff8
Rename 'tranform' to 'landmark_to_tracking_transform'. (#836) 2018-01-22 18:30:14 +01:00
Alexander Belyaev 6d4649857a
Move implementation of ReadProto/WriteProto to .cc (#835) 2018-01-22 17:53:31 +01:00
Rodrigo Queiro 0c227097e7 Add a Bazel build for cartographer (#834)
This does not include cartographer_grpc, nor any of the binaries
(*_main.cc). It has been tested with Bazel 0.9.0 on Ubuntu 14.04 with
the latest (at time of writing) dazel/dazel container, by running:

```
bazel build //...
bazel test //...
```

While this is a large CL, the files under third_party are not new - they have been through internal review, and this is their open-source release. The other files come to 212 lines of code.
2018-01-22 16:47:07 +01:00
Alexander Belyaev 67850d25a0 Replace templates in io/proto_stream.h with upcasting. (#833) 2018-01-22 14:12:14 +01:00
Alexander Belyaev 746c9c83c8
Propagate LandmarksData to the PoseGraph. (#830)
Propagate LandmarkData to the PoseGraph.
2018-01-19 09:41:24 +01:00
gaschler de5937856d Collate_by_trajectory option (#828)
Adds an option to create a MapBuilder using TrajectoryCollator
(instead of Collator).

[RFC=0008](https://github.com/googlecartographer/rfcs/blob/master/text/0008-collator-interface.md)
2018-01-17 21:15:15 +01:00
gaschler d92040d0e6 Test Collator for multiple trajectories (#829) 2018-01-17 20:43:20 +01:00
gaschler 70e378b7c5 TrajectoryCollator (#827)
Introduces TrajectorCollator, which collates sensor data ignoring
other trajectories.
Tests the same.

[RFC=0008](https://github.com/googlecartographer/rfcs/blob/master/text/0008-collator-interface.md)
2018-01-17 17:48:20 +01:00
Wolfgang Hess ffdbf1c161 Fix debug output for 3D loop closure error. (#826)
PAIR=spielawa
2018-01-17 13:09:48 +01:00
gaschler f64eef876a Optional GetBlockingTrajectoryId (#820)
[RFC=0008](https://github.com/googlecartographer/rfcs/blob/master/text/0008-collator-interface.md)
2018-01-16 16:21:22 +01:00
Alexander Belyaev 53c2a6b58f
Move 'finished' getter to the base class. (#803)
* Move 'finished' getter to the base class.

* Ran clang-format.

* Ran clang-format.
2018-01-16 14:20:33 +01:00
Christoph Schütte 31ec89614c Add parameter to Submap::ToProto() to determine whether to include loop (#821) 2018-01-16 11:23:29 +01:00
Christoph Schütte 3660408ae6 Forward declare unique_ptr<LocalSlamResultData> (#824) 2018-01-16 10:20:51 +01:00
gaschler 057bd8ce6a Public SpaCostFunction::Compute* (#814) 2018-01-12 21:46:47 +01:00
Wolfgang Hess 19ff047a79 Allow FixedRatioSampler to drop all data. (#817)
This is useful to disable loop closure by setting
constraint_builder.sampling_ratio = 0.
2018-01-12 18:54:41 +01:00
Christoph Schütte 1de696d45f Introduce PoseGraphInterface::ToProto() (#813) 2018-01-12 13:55:14 +01:00
Christoph Schütte fee77c8a99 Add LocalSlamResultData retrieval and submap management (#810)
[RFC=0002](https://github.com/googlecartographer/rfcs/blob/master/text/0002-cloud-based-mapping-1.md)
2018-01-12 13:00:25 +01:00
gaschler 51ccee3e74 CollatorInterface (#808)
[RFC=0008](https://github.com/googlecartographer/rfcs/blob/master/text/0008-collator-interface.md)
2018-01-11 16:02:03 +01:00
Christoph Schütte 8fc64fdbb5 Wrap HybridGrid in unique_ptr to make Submap updatable (#809) 2018-01-11 14:34:56 +01:00
Christoph Schütte e1a182d1fa Implement LocalSlamResult data adding to PoseGraph. (#804) 2018-01-11 10:19:37 +01:00
Christoph Schütte d313af8674
Add InsertionResult to LocalSlamResult. (#801) 2018-01-10 17:26:04 +01:00
Susanne Pielawa c7a8c5fda9 Keep fixed frame across optimizations. (#807)
PAIR=wohe
2018-01-10 16:01:27 +01:00
Susanne Pielawa 286d16238e In optimization_problem, use a map instead of vector for trajectory data. (#805)
PAIR=wohe
2018-01-10 14:30:51 +01:00
Susanne Pielawa 67d26747cc Implement assignment operator for common::optional. (#800)
Implement assignment operator for common::optional.
2018-01-09 17:54:53 +01:00
Christoph Schütte 8c7c4e3d2a Make LocalTrajectoryBuilder optional. (#799)
This is to prepare for the cloud-based mapping case where the robot is running cartographer in pure-localization mode and the cloud instance only solves the global problem. In that case a `LocalTrajectoryBuilder` need to be instantiated in for the cloud instance.
2018-01-09 16:54:30 +01:00
Christoph Schütte 8165da873f Add GetConstraints() to gRPC service. (#798) 2018-01-09 14:25:28 +01:00
Christoph Schütte 78d05bf745 Add GetTrajectoryNodePoses() to gRPC service (#796) 2018-01-08 14:19:39 +01:00
Christoph Schütte d57c2441b8 Implement gRPC submap query. (#794) 2018-01-08 13:52:14 +01:00
Susanne Pielawa 91034eaf58 Making pose in FixedFramePoseData optional. (#792)
This is for the case that the GPS signal is not available.
2018-01-08 11:37:07 +01:00
Christoph Schütte 196b4b891c Add PoseGraphInterface::GetTrajectoryNodePoses() (#795) 2018-01-08 09:13:51 +01:00
Christoph Schütte 1a837ef3ab Introduce PoseGraphInterface::GetAllSubmapPoses() (#790) 2018-01-05 14:19:08 +01:00
Susanne Pielawa 63a80c9340 Adding a minimal implementation of std::optional. (#783)
We're using C++11, which doesn't have std::optional. We need a few
features of std::optional for improved GPS support. These are
implemented here.
2018-01-05 11:27:21 +01:00
Juraj Oršulić 58d94aaa68 Refactor calling optimization into DispatchOptimization. (#729)
I noticed that @jihoonl opened the PR #726 which performs a similar thing. As discussed in googlecartographer/cartographer_ros#613 (@cschuet  has already taken a look), I pulled this out of #481 (a really old PR whose merging has been postponed), which is an example where re-running optimization is triggered from elsewhere as well (besides from `ComputeConstraintsForNode`). This refactoring makes libcartographer friendlier for use cases such as that one.

An important detail is that I have changed the condition in `WaitForAllComputations` to also check if the work queue is empty. If there are other things on the worker queue besides `ComputeConstraintsForNode`, currently we will wrongfully conclude that all computations are done. (This detail was merged in #754, so it's no longer in the diff of this PR).

Also missing is the same thing for 3D. I can add that when we settle on this.

Also, I suggest that `run_loop_closure` gets renamed to `run_optimization`.
2018-01-05 10:43:56 +01:00
Jihoon Lee 2ad83662f2 Migrate SubmapTexture and SubmapSlice logics from cartographer_ros (#782)
Towards [RFC06](https://github.com/googlecartographer/rfcs/blob/master/text/0006-serve-ros-map-from-pbstream.md).

Migrates
* `FillSubmapSlice` from `pbstream_to_rosmap_main.cc` 
* `SubmapTexture` logics from cartographer_ros
2018-01-04 10:52:05 +01:00
Juraj Oršulić 9ee65293d2 Pose graph: do not mark all submaps finished when finalizing a trajectory. (#784)
I think this was a mistake in #563 (@jihoonl). Not all submaps should be marked as finished, only those pertaining to the trajectory being finalized.
2018-01-04 10:11:28 +01:00
gaschler dbb3f7cde4 Test TrajectoryBuilderStub (#780) 2018-01-03 14:56:56 +01:00
Christoph Schütte 5b5b290e9f Implement BlockingQueue::WaitUntilEmpty(). (#775)
PAIR=gaschler
2017-12-20 10:22:53 +01:00
Christoph Schütte 02734c296d Implement ReceiveLocalSlamResultsHandler. (#772) 2017-12-19 15:11:29 +01:00
Christoph Schütte 03751b3c9f Implement local SLAM subscriptions in MapBuilderServer (#766) 2017-12-18 16:32:34 +01:00
gaschler c6decd5b7b Test MapBuilderServer (#762)
Test to start and stop the MapBuilderServer.
Moves test helper functions in common with MapBuilderTest
to internal/mapping.
2017-12-18 11:52:17 +01:00
gaschler d49706944f FixedFramePoseData in gRPC server (#761)
Defines the FixedFramePoseData RPC and implements
the handler for the gRPC server.
2017-12-15 15:26:58 +01:00
gaschler 6817d22788 Test global 2D SLAM. (#751) 2017-12-15 14:45:56 +01:00
Juraj Oršulić 29e4395a5a Make WaitForAllComputation wait for work queue in 3D as well (#758)
Repeat the change in #754 by @gaschler for 3D as well.
2017-12-14 10:02:54 +01:00
gaschler f8452821d7 WaitForAllComputation waits for work queue (#754) 2017-12-13 18:49:22 +01:00
gaschler 59d1b968bc Unwarp by point in LocalTrajectoryBuilder. (#636) 2017-12-13 18:29:42 +01:00
Christoph Schütte c5ec086968 Introduce TimedPointCloudData. (#748)
This data structure is needed to forward RangeFinderData over gRPC.

[RFC=0002](https://github.com/googlecartographer/rfcs/blob/master/text/0002-cloud-based-mapping-1.md)
2017-12-08 19:28:47 +01:00
Christoph Schütte 3ae78563c6 Introduce TrajectoryBuilderInterface. (#736)
[RFC=0004](https://github.com/googlecartographer/rfcs/blob/master/text/0004-trajectory-builder-interface.md)
2017-12-08 11:52:29 +01:00
Christoph Schütte 96cdbde5bf Introduce PoseGraphInterface. (#744)
[RFC=0005](https://github.com/googlecartographer/rfcs/blob/master/text/0005-pose-graph-interface.md)
2017-12-08 11:13:48 +01:00
gaschler 549eef0000 Correct namespace (#742) 2017-12-07 21:17:30 +01:00
Michael Grupp 176fc42c3f Remove empty leftover of pose_estimate.h (#740)
Follow-up of #670.
2017-12-07 08:53:04 +01:00
Christoph Schütte 2a7a6ef934
Make sensor::Data dispatchable to TrajectoryBuilder. (#738)
This change allows to dispatch sensor::Data to implementations of mapping::TrajectoryBuilder, i.e. CollatedTrajectoryBuilder. We need this for cartographer_grpc as the incoming sensor data is inserted into a queue by the gRPC threads and dequeued by a SLAM threads that inserts them into a CollatedTrajectoryBuilder.
2017-12-06 16:36:36 +01:00
gaschler 4c999037b4 Integration tests for local slam. (#734) 2017-12-06 15:58:16 +01:00
gaschler 49d55d97c1 Test MapBuilder (#731) 2017-12-05 14:04:10 +01:00
gaschler b77a1f2178
Remove implementation from TrajectoryBuilder. (#728)
To simplify the implementation of a gRPC-based trajectory builder,
this moves the existing implementation to CollatedTrajectoryBuilder
and organizes the interface by sensor type.
2017-12-05 11:05:05 +01:00
gaschler ee9c61a736 Move LocalSlamResultCallback to AddTrajectory (#724) 2017-12-04 22:41:38 +01:00
gaschler 1ff8243802
Test RotationDeltaCostFunctor (#721) 2017-12-01 09:25:15 +01:00
gaschler 63e901d276
Create AutoDiffCostFunction in cost functions. (#718)
Creating a ceres::AutoDiffCostFunction requires specifying numbers
of residuals and variables, so it is safer to implement this within
the cost functions, which know best.
2017-11-30 15:41:57 +01:00
gaschler 85bfb888eb
Interface for MapBuilder (#715)
Defines an interface for MapBuilder that can be used to implement a gRPC stub.
[RFC=0002](https://github.com/googlecartographer/rfcs/blob/master/text/0002-cloud-based-mapping-1.md)
2017-11-30 14:41:48 +01:00
gaschler 24f253a2aa Remove GetBlockingTrajectoryId (#714) 2017-11-30 12:04:14 +01:00
Jihoon Lee 88805a301d Trim all submaps when pure localization trajectory is finished. (#563)
Fixes #560 
based on #562
2017-11-28 10:21:55 +01:00
Juraj Oršulić bebe021b04 Serialize fixed frame pose data. (#689)
Similar to #666 and #548.
2017-11-28 09:44:35 +01:00
gaschler 1c7183d5c4 VoxelFilter for TimedPointCloud, Filter method. (#710)
This makes the VoxelFilter ready for TimedPointCloud and
per-point unwarping.
2017-11-27 15:12:07 +01:00
gaschler c292d76f86 Test ImuTracker (#711)
Adds unit tests for ImuTracker to cover filtering of the gravity
vector and tracking angular velocity.
2017-11-27 14:46:32 +01:00
Wolfgang Hess 7e82ac3bbf Make IMU cost functions internal. (#708)
[RFC=0003](https://github.com/googlecartographer/rfcs/blob/master/text/0003-internal-headers.md)
2017-11-24 15:29:48 +01:00
Wolfgang Hess 79b83b92b2 Make the MotionFilter internal. (#707)
Also moves it from mapping_3d to mapping since it is used
in both 2D and 3D SLAM.

[RFC=0003](https://github.com/googlecartographer/rfcs/blob/master/text/0003-internal-headers.md)
2017-11-24 14:01:07 +01:00
gaschler 0084f14c1c CeresScanMatcher target_translation (#702)
For its translation cost function, CeresScanMatcher now takes
a target translation as an argument instead of a pose, which 
was confusing.
2017-11-24 12:26:14 +01:00
Wolfgang Hess 0819e52a9c Make the LocalTrajectoryBuilders internal. (#704)
[RFC=0003](https://github.com/googlecartographer/rfcs/blob/master/text/0003-internal-headers.md)
2017-11-24 10:39:12 +01:00
Wolfgang Hess 8e6101de5c Make the OccupiedSpaceCostFunction internal. (#703)
[RFC=0003](https://github.com/googlecartographer/rfcs/blob/master/text/0003-internal-headers.md)
2017-11-23 18:04:36 +01:00
Wolfgang Hess 70a0f41364 Remove unused code. (#699) 2017-11-23 16:39:55 +01:00
gaschler 9bfc52d878 Rename arguments of CostFunctors to target_ (#700)
Rename constant targets of cost functors to target_*.
Pass only translation to Translation cost functors.
2017-11-23 16:04:49 +01:00
Wolfgang Hess ec034b13bc Do not install global_trajectory_builder.h. (#698)
This moves the global_trajectory_builder.h header under
cartographer/internal and changes the CMakeLists.txt to
not install internal headers.

[RFC=0003](https://github.com/googlecartographer/rfcs/blob/master/text/0003-internal-headers.md)
2017-11-23 14:47:29 +01:00
Juraj Oršulić 8f9f1cf44e Fix debug build (#697)
Similar to #418.
2017-11-23 14:20:01 +01:00
gaschler 147c8166b6 ImuTracker uses conjugate. (#693)
Normalized quaternions can be inverted with conjugate, which is faster.
The motivation for optimizing ImuTracker::Advance is that
per-point unwarping will call this for every point.
2017-11-23 13:50:44 +01:00
gaschler a8bd98680e ExtrapolatePose uses fewer transforms. (#694)
ExtrapolatePose avoids conversion to Rigid3d and computes translation
and rotation directly, which is faster.
Per-point unwarping will call this function for every point, so we
optimize it.
2017-11-23 12:27:26 +01:00
Juraj Oršulić 31f28b5097 Remove PoseEstimate. (#670)
Replaces #620.

Depends on switching `cartographer_ros` to use the new callback API for handling the last scan and pose estimate (googlecartographer/cartographer_ros#594).
2017-11-17 16:47:06 +01:00
Wolfgang Hess aba4575d93 Rename 'submap_transforms' to follow terminology. (#688)
https://google-cartographer.readthedocs.io/en/latest/terminology.html

Related to #602.
2017-11-17 13:13:45 +01:00
Christoph Schütte b91ff8fe44 Extrapolate poses for ground truth relations. (#687) 2017-11-17 12:27:31 +01:00
Wolfgang Hess 055728af93 Rename options for consistency. (#686)
"scan" is better named "node" if it refers to a node in global SLAM.

"penalty factor" is renamed "weight" for consistency with other similar options.

"scans_per_accumulation" is renamed "num_accumulated_range_data" to match the code and since the accumulated data is not called "scan".
2017-11-17 10:52:22 +01:00
Holger Rapp 5ee830e8cc Cleanup and new features in Image code. (#685)
- Removes stride_, since we know for a fact that it is always width * 4.
- Adds a constructor from a cairo surface.
- Adds a function that rotates the image by 90 degrees.

This is in preparation of adding a tool that can generate an image from a .pbstream.
2017-11-16 17:05:18 +01:00
Holger Rapp 0b4e7a9443 Refactor image related code. (#684)
- Take immutable parameter as const ref instead of pointer.
- make kCairoFormat a global constant, since it is defined in many places in cartographer and cartographer_ros, but we rely on it actually always being the same value.
2017-11-16 15:07:05 +01:00
gaschler b9015f33a7 Filter early in AddRangeData. (#681)
Filter earlier in 3D.
Combine two transforms to one in 2D.
2017-11-16 13:35:44 +01:00
Wolfgang Hess 16d62f45f0 Rename sparse_pose_graph options. (#682)
The default pose graph configuration is now included as "pose_graph.lua"
and available as POSE_GRAPH.

[RFC=0001](https://github.com/googlecartographer/rfcs/blob/master/text/0001-renaming-sparse-pose-graph.md)
2017-11-16 11:48:25 +01:00
jie a08a370ef3 Check if filtered point clouds are empty. (#664)
The point cloud will be filtered by MaxRangeFilter before doing scan matching. For some special cases, the filtered point cloud maybe empty, it will cause crash:
real_time_correlative_scan_matcher.cc:159 Check failed: candidate.score > 0.f (nan vs. 0) 
This PR fixed this bug, check the point cloud size before scan matching, return false for empty point cloud.
2017-11-16 09:56:01 +01:00
Wolfgang Hess bdca2095c0 Rename proto::SparsePoseGraph. (#680)
[RFC=0001](https://github.com/googlecartographer/rfcs/blob/master/text/0001-renaming-sparse-pose-graph.md)
2017-11-15 14:58:49 +01:00
Wolfgang Hess c25379cd20 Rename mapping_{2,3}d::SparsePoseGraph. (#678)
[RFC=0001](https://github.com/googlecartographer/rfcs/blob/master/text/0001-renaming-sparse-pose-graph.md)
2017-11-15 13:50:18 +01:00
Wolfgang Hess 8c114d6eaf Rename mapping::SparsePoseGraph. (#677)
[RFC=0001](https://github.com/googlecartographer/rfcs/blob/master/text/0001-renaming-sparse-pose-graph.md)
2017-11-15 12:06:19 +01:00
Wolfgang Hess 36b9cf7f9a Move the mapping_3d/sparse_pose_graph directory. (#676)
[RFC=0001](https://github.com/googlecartographer/rfcs/blob/master/text/0001-renaming-sparse-pose-graph.md)
2017-11-15 11:30:35 +01:00
Wolfgang Hess 26db9d6210 Move the mapping_2d/sparse_pose_graph directory. (#675)
[RFC=0001](https://github.com/googlecartographer/rfcs/blob/master/text/0001-renaming-sparse-pose-graph.md)
2017-11-15 10:52:06 +01:00
catskul f6192e4735 replace implicit use of cartographer::string with explicit use of std::string (#673)
Fixes #622.
2017-11-15 10:17:59 +01:00
Wolfgang Hess 291c0f581b Move the mapping/sparse_pose_graph directory. (#669)
[RFC=0001](https://github.com/googlecartographer/rfcs/blob/master/text/0001-renaming-sparse-pose-graph.md)
2017-11-15 09:39:45 +01:00
gaschler 7904808d40 Reduce transforms in LocalTrajectoryBuilder. (#668)
This speeds up AddRangeData because we avoid one copy and loop over all points.
2017-11-15 09:15:02 +01:00
Juraj Oršulić 818e5e1a44 Add local SLAM result callback. (#574)
Depends on ~~#619~~ (merged) and ~~#617~~ (merged).
Related to #508.

Also, if cartographer_ros is going to use this, and we wish to serialize the saved range data, that will have to be handled in cartographer_ros, right?
2017-11-14 17:19:14 +01:00
Wolfgang Hess a4c0e4754e Rename scan to node. (#667)
Changes the naming from "scan" to "node" in the pose graph.
AddNode() adds a new node to the graph which might contain
data from multiple range sensors and not necessarily one scan.

Configuration and documentation changes might follow in a
separate PR.

Related to #280.
2017-11-14 16:53:31 +01:00
Juraj Oršulić 4b342eddd0 Miscellaneous: rename odometer->odometry (#665)
Since we're adding `sensor::OdometryData`, I think that the function should be called `AddOdometryData`.
2017-11-14 16:18:07 +01:00
Juraj Oršulić 5496cbdc0c Serialize odometry data (#666)
Replaces #550.
2017-11-14 15:18:39 +01:00
Juraj Oršulić 60c72cb445 Deserialize trajectory connectivity (#552) 2017-11-14 13:26:53 +01:00
Wolfgang Hess 3ec583a327 Change fixed_frame_pose_data_ to MapByTime. (#662) 2017-11-13 17:41:04 +01:00
gaschler fd5003b69b Omit odometry penalty within frozen trajectory. (#661)
In the case of pure localization, this reduces the size of the optimization problem by a large factor.
2017-11-13 14:19:15 +01:00
Wolfgang Hess 4a8607810e Change odometry_data_ to MapByTime. (#655) 2017-11-13 13:21:53 +01:00
damienrg d183ab737a Fix mismatched tag warning for MapByIndex (#658)
- [ X] Keep your PR rebased to master.
- [ X] Keep your PR under 200 lines of code and address a single concern.
- [ X] Add unit test(s) and documentation (these do not count toward your 200 lines).
- [ X] Adhere to the [Google C++ Style Guide](https://google.github.io/styleguide/cppguide.html).
- [ X] Run `clang-format -style=Google` on your code (this alone is not enough to ensure you've followed the style guide).
- [ X] Run `ninja test` or `catkin_make_isolated --install --use-ninja --pkg cartographer --make-args test` as appropriate.
2017-11-13 10:18:58 +01:00