Commit Graph

930 Commits (6c31420b974a3ead500052cdb0a1b96b7a5dd29c)

Author SHA1 Message Date
Steve Wolter 6c31420b97 Log status of failed RPCs in PoseGraphStub. (#1288)
Log status of failed RPCs in PoseGraphStub.

This improves the usefulness of error messages in the log.
2018-07-17 13:38:37 +02:00
Christoph Schütte 0ee06ba561
Fix writing state of trajectories with unfinished submaps. (#1286) 2018-07-17 08:39:50 +02:00
danielsievers 482f912d95 Introduce separate mutex for PoseGraph3D work queue access (#1284)
Introduce separate mutex for PoseGraph3D work queue access.

This reduces lock contention, see issue #1250.
In particular higher frequency call to AddWorkItem
don't need to take the main mutex from the froground
thread anymore.
2018-07-16 17:15:28 +02:00
Alexander Belyaev 31d0a6acc7 [GenericPoseGraph] Add IMU Calibration and Pose3D node types. (#1278) 2018-07-16 11:07:37 +02:00
Juraj Oršulić ad572336d4 Include <array> in pose_graph/node.h and pose_2d.h (#1280) 2018-07-16 10:03:54 +02:00
Susanne Pielawa 07a9efee6b Add metrics: real time ratio and cpu time ratio. (#1275)
This PR introduces two metrics to measure performance of local slam: real time ratio, and cpu real time ratio. The latter is introduced to measure the headroom. 

Real time ratio tells us how fast local slam processes the incoming sensor data. When SLAM is falling behind, this is below 100%. In the steady state it is 100% (The real time ratio is above 100% only when slam is 'catching up'.). So this does not tell us how much faster than real time slam processes the data. For this purpose the cpu real time ratio is introduced. It measures how much cpu time slam used, compared to the real time passed to collect the sensor data. Thus it can be more than 100% if slam is faster than real time. For example, a cpu real time ratio of 200% means that slam could process the data twice as fast as it comes in. 

Three durations are measured:
- sensor_duration: the physical time it took to collect the sensor data. 
- wall_time_duration: the time it took to process that data
- thread_cpu_duration: CPU time it took to process that data. 

And the metrics then are: 
real time ratio = sensor_duration / wall_time_duration
cpu real time ratio = sensor_duration / thread_cpu_duration
2018-07-14 12:07:14 +02:00
Kevin Daun df12154542 Fix EqualCellProperties matcher. (#1276) 2018-07-14 10:34:46 +02:00
danielsievers 7b48b66a65 Fined-grained locking for PoseGraph3D constraints (#1279)
When preparing constraints make holding of the PoseGraph3D
mutex fine-grained to address lock contention from issue #1250.
That includes not holding the mutex when acquring the constraint
builder mutex.
2018-07-14 09:27:07 +02:00
Alexander Belyaev d4376e765b
[GenericPoseGraph] Move 'EqualsProto' matcher to test_helpers. (#1271) 2018-07-13 15:05:22 +02:00
Kevin Daun a08037dc47 Add TSDF interpolator. (#1274)
Based on [interpolated_grid.h](https://github.com/googlecartographer/cartographer/blob/master/cartographer/mapping/internal/3d/scan_matching/interpolated_grid.h).
2018-07-13 13:47:09 +02:00
danielsievers 8c9104568a Take PoseGraph3D mutex inside instead of outside work item (#1264)
Take PoseGraph3D mutex inside instead of outside work item
    
This refactors the code but does not alter behavior except
that the shared mutex is separately taken for work_queue_ access,
released, and then re-acquired inside each work item.
    
As a consequence AddWorkItem() also needs to EXCLUDE() the lock and
acquire it internally, because for the case where we run the task
in the foreground we cannot hold the lock and recursively acquire it again
inside the task being run.
    
This prepares for making locking more fine-grained, see issue #1250.
2018-07-13 11:53:25 +02:00
Christoph Schütte 61a89d8ab8 Check client_id in sensor data handlers (#1266) 2018-07-13 09:57:35 +02:00
Christoph Schütte 2b80767b57 Upgrade async_grpc (#1272) 2018-07-13 08:48:48 +02:00
Susanne Pielawa dce713b442 Added function GetThreadCpuTimeSeconds in common/time.h (#1267)
This will be needed for measuring SLAM performance.
2018-07-13 00:11:31 +02:00
Kevin Daun 5abd413310 Add value conversion tables (#1255)
Adds value conversion tables to perform lazy computations of lookup tables for mapping from a uint16 value to a float in [`lower_bound`, `upper_bound`].
Owners of the value conversion tables are `SubmapController`, `PoseGraph2D`, `ActiveSubmaps` and `ProbabilityGridPointsProcessor`. For concurrency reasons, having only a single owner is not possible.
Follow up PR in `cartographer_ros`  is prepared.
2018-07-12 22:11:55 +02:00
Michael Grupp 8602bf9430 Nit: no hardcoded version and current year in Sphinx docs. (#1268)
`Cartographer 1.0.0 documentation` -> `Cartographer documentation`
`Copyright 2016 The Cartographer Authors` -> `Copyright <year> The Cartographer Authors`
2018-07-12 20:43:36 +02:00
Alexander Belyaev c4c5fab5b2 [GenericPoseGraph] Add a base class for all nodes and Pose2D node. (#1265) 2018-07-12 15:21:59 +02:00
Susanne Pielawa 151a78d7be Added TODO for multi-trajectory case. (#1269) 2018-07-12 14:25:00 +02:00
Michael Grupp 30bc6bd97a Add equations for landmark cost function. (#1254) 2018-07-12 13:25:57 +02:00
Christoph Schütte 1b88fb8e90 Introduce client_id in SensorMetadata (#1257) 2018-07-12 00:21:31 +02:00
Wolfgang Hess 5ad2088561 Get rid of std::bind. (#1261)
`std::bind` is bug prone and should be avoided.
Lambdas are a more general and safer replacement.
2018-07-11 20:18:09 +02:00
Martin Schwörer a905036a00 Create first submap with first scan (#1253)
The first submap is now created with first call to `ActiveSubmaps:InsertRangeData`.

Originally the first submap was created at the origin. Creating the first submap with the first range data insertion allows to align the first submap with the gravity estimate of the first scan. This change makes the interface of ActiveSubmaps most consistent, as `ActiveSubmaps::submaps()` will return the correct active submaps after insertion.

The change affects the result of cartographer. The results were verified and provide the same quality as current master.
2018-07-11 19:44:37 +02:00
Alexander Belyaev 288328ef14 [GenericPoseGraph] Add protos for nodes/constraints. (#1258)
Does not cover all constraints yet.
2018-07-11 18:06:28 +02:00
danielsievers 9f1039221c Refactor PoseGraph work queue optimization dispatching (#1252)
Unwrap the logic to dispatch optimization and the deferred
logic to create the work queue (when kicked off in foreground)
to happen in AddWorkItem() and HandleWorkQueue().
The work items will instead return whether they need optimization
to be dispatched or not.
2018-07-11 17:33:41 +02:00
Kevin Daun bb380ae947 Make kSqrtTwoPi const. (#1259) 2018-07-11 13:02:11 +02:00
Wolfgang Hess 88850ec2d7 Add .clang-format file. (#1249)
This adds a .clang-format file, so that git clang-format uses
Google style without the need to remember the commandline flag.

Moreover, this ensures correct formatting if the Cartographer
code is in a subtree of a repo.
2018-07-11 12:28:53 +02:00
Christoph Schütte 35abfccc6b Introduce client_id in cartographer grpc (#1241) 2018-07-10 20:41:00 +02:00
Susanne Pielawa 120c216c47 Measure accumulation_duration from last accumulation stop (#1251)
[PR 946](https://github.com/googlecartographer/cartographer/pull/946) introduced metrics in local trajectory builder. The accumulation duration was measured from accumulation_start to accumulation_stop. 

This PR changes this to measure the entire time elapsed between two accumulations (i.e. from accumulation_stop to the next accumulation_stop). 

Mostly the two measurements result in roughly the same, with the new way measuring slightly larger durations, as expected. But occasionally the measurements differ significantly. This is probably due to a lock contention somewhere outside of what was measured previously. Since we are interested in real time metrics, we need to track the entire time passed.
2018-07-10 16:24:57 +02:00
Susanne Pielawa a04b6cbc48 use common::ToSeconds in local_trajectory_builder and rate_timer.h (#1248)
use the new overload of ToSeconds (introduced in 
[PR1244](https://github.com/googlecartographer/cartographer/pull/1244/files)) in local_trajectory_builder.
2018-07-10 13:14:54 +02:00
Wolfgang Hess afa3ba5336 Fix the name of real_time_correlative metric in 2D. (#1247)
This was called fast_correlative before, but the metric was about
the score of the real-time correlative scan matcher in local SLAM.
2018-07-10 10:53:23 +02:00
Martin Schwörer 1fed98727d extracted ScanMatch method in LocalTrajectoryBuilder3D (#1245)
Extracted the ScanMatch method in the LocalTrajectoryBuilder3D.

This is a short refactoring. With that change, the code of LocalTrajectoryBuilder3D resembles LocalTrajectoryBuilder2D more. (see LocalTrajectoryBuilder2D::ScanMatch)
2018-07-10 10:19:25 +02:00
Susanne Pielawa 3ee7c6710f Adding metrics (#1240)
to measure fraction of sensor time that is taken up by voxel filter, scan matcher, and inserting into submap.
2018-07-09 21:20:58 +02:00
Martin Schwörer aa3ac7e837 Removed redundant 2d voxel filtering (#1243)
Removed redundant adaptive voxel fitering in trajectory builder 2d.

Adaptive voxel filtering of the lidar point cloud was performed in InsertIntoSubmap and ScanMatch. Both methods are called from AddAccumulatedRangeData. Now, adaptive voxel filtering is done only once in AddAccumulatedRangeData and the filtered point cloud is then forwarded to InsertIntoSubmap and ScanMatch.
2018-07-09 20:45:55 +02:00
Arno Fleck 9e54a981c6 Introduce common::ToSeconds for std::chrono::steady_clock::duration. (#1244)
Introduce a new overload of ToSeconds for std::chrono::steady_clock and use it for the work queue delay metric.
2018-07-09 20:13:17 +02:00
Michael Grupp eff103f663 Ignore initial poses on gRPC server side. (#1234)
Fixes a crash if a client initializes with an initial pose inside a frozen map
that isn't known to the server.
2018-07-09 15:03:40 +02:00
Kevin Daun 7dfe404278 Add TSDF RangeDataInserter (#1236)
Adds TSDF RangeDataInserter, tests and integrates the configuration files.
2018-07-09 13:46:46 +02:00
Juraj Oršulić 3bf9ba0a69 Ensure async_grpc is find_packaged() for Cartographer users (#1105) 2018-07-09 13:11:51 +02:00
Arno Fleck 8b2c755a13 Remove "_internal" from the metric family names. (#1237)
As discussed and requested in #1218, this PR removes the substring "_internal" from all the metric family names.
2018-07-07 00:20:20 +02:00
Martin Schwörer 044f53735c removed unused member matching_submap_index (ActiveSubmaps3D,ActiveSubmaps2D) (#1238)
Removed the unused member matching_submap_index of ActiveSubmaps3D and ActiveSubmaps2D.
Adjusted the test accordingly.
2018-07-06 11:48:25 +02:00
Martin Schwörer 902459e95c fixed casting of high_resolution_max_range (SubmapsOptions3D) (#1239)
Fixed unintentional casting of high_resolution_max_range from double to int, to float

In SubmapsOptions3D the parameter "high_resolution_max_range" is defined as double. In the code it gets casted to int when calling Submap3D::InsertRangeData and to float when calling FilterRangeDataByMaxRange.
2018-07-06 11:02:38 +02:00
Arno Fleck 627ebb39b9 Add function to get the work queue delay from PoseGraph. (#1218) 2018-07-06 09:40:44 +02:00
Christoph Schütte bad8c96bc6
Add token_file_path parameter and enable auth (#1235) 2018-07-05 13:14:11 +02:00
Kevin Daun cc9fc75757 Separate raycast and grid update (#1216)
Separate raycast and grid update logic.
Rename raycast to ray_to_pixel_mask.
2018-07-03 19:51:39 +02:00
Wolfgang Hess d2f3c1ea31 Fix the local SLAM latency metric. (#1231)
Before, the metric was in integer seconds and since measurements
are usually quite small, it was mostly outputting 0.
2018-07-03 16:35:58 +02:00
Alexander Belyaev ddce76d9fb Make old pbstreams compatible with the new pure_localization_trimmer options. (#1230) 2018-07-03 14:27:38 +02:00
Alexander Belyaev ffb5034390 Mark TSDF::ToProto with override. (#1227) 2018-07-03 12:46:30 +02:00
Alexander Belyaev 0a253d24f6 Remove unnecessary std::move. (#1228) 2018-07-03 10:56:33 +02:00
Alexander Belyaev 3540996e91
Add options to (not)collate fixed-frame/landmarks data. (#1224) 2018-07-02 21:49:00 +02:00
Alexander Reimann 0981620d8f Add max_submaps_with_pure_localization parameter (#1141)
Adds a parameter for the currently hard coded `kSubmapsToKeep` in `pure_localization` mode.

The new parameter `max_submaps_with_pure_localization` allows to specify the history size of submaps in `pure_localization` mode.

Closes #1116
2018-07-02 20:32:23 +02:00
gaschler d5ed8a9ded Add further tests for MapById (#1226)
Adds tests for MapById for the cases: empty MapById, invalid trajectory_id, deleting all entries for a trajectory.
2018-07-02 19:37:44 +02:00