The histogram of a submap is now stored in the submap (class and proto) itself. This change allows to accumulate the histogram of a submap in local SLAM by adding up the histogram of each new scan.
The main advantage is that the background thread doesn't have to loop over all `TrajectoryNode`s of a finished submap to compute the submap histogram for the `RotationalScanMatcher`. Instead this chunk of work is moved to the local SLAM thread but is split up into a few computations for each new scan. When running localization, the histogram of a submap can just be read from a map pbstream and does not have to be computed from the nodes.
In summary:
- This change improved the CPU time of offline SLAM by ~7%.
- Increases the readability of the code and performance of the background thread. (see `PoseGraph3D::ComputeConstraint`)
- No negative performance impacts on accuracy or finding loop-closures
However:
- With this change to the submap proto, old maps (pbstreams) are no longer supported and need to be re-created by running offline slam
This makes the map builder backwards compatible to the current map pbstream (version 1). The PR prepares for #1277, where pbstream version 2 will be introduced. Backwards compatibility was discussed in #1277.
When a map with pbstream version 1 is loaded, a rotational scan matcher histogram is generated for each submap using the histograms of all nodes that were inserted to the submap during local SLAM. Once this backwards compatibility is in place, I would like to introduce the new format with #1277.
We were passing the gravity estimate of the current tracking frame
to intialize the local submap pose. Fixing this improves the alignment
of submaps in the global (and approx. gravity-aligned) frame.
* Move tools/bazel.rc to prepare for bazel 0.17
Bazel 0.17 will no longer import the legacy "tools/bazel.rc" config
file. This change ensures compatibility with the new versions.
For users on bazel 0.16 and earlier who have a ~/.bazelrc, this change
will cause bazel to ignore their ~/.bazelrc and load this one instead.
Hopefully this won't break anyone. (famous last words)
The uplink server only receives the grid content of a submap after
that submap is finished for efficiency. Therefore, constraint
searches against that submap need to be skipped.
Also add checks to avoid this in the future.
FIXES=#1360
Adds a boolean argument to MapBuilderInterface::SerializeState() to indicate whether the caller wants to include unfinished submaps in the serialized state.
For cartographer_grpc this argument will be set to false since unfinished submaps do not have a Grid which would lead to a crash in the Submap::ToProto() function.
PR #1224 introduced checks for `collate_landmarks` and `collate_fixed_frame` in the `CollatedTrajectoryBuilder`. However, it falsely checks for `collate_landmarks` in the `AddSensorData(FixedFramePoseData)` function. This PR fixes that.
Added new constructor for `RotationalScanMatcher` and exposed `RotateHistogram`.
This PR prepares for PR #1277 where the constructor
`RotationalScanMatcher(const std::vector<std::pair<Eigen::VectorXf, float>>& histograms_at_angles)`
will be removed and only the new constructor
`RotationalScanMatcher(const Eigen::VectorXf& histogram)`
will remain. The unit tests will be updated to use the new constructor in #1277.
Submap::ToProto now returns the proto.
This PR makes the interfaces for serialization more consistent: In `mapping_state_serialization.cc` all `ToProto` methods return the proto except Submap::ToProto.
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.
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
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.
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.
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.
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.
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.
[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.
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)
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.
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.
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
Previously, NodeId and SubmapId could be (partially) uninitialized,
for instance like this:
```
NodeId node_id;
SubmapId submap{0}; // uninitialized submap_index
```
This introduces constructors to prevent this.
Add the parameter `OptimizationProblemOptions.use_online_imu_extrinsincs_in_3d`. Set it to true by default to not change existing behavior.
Using online IMU extrinsics is not always desirable, and particularly during localization, we have determined that it is preferable to turn this feature off.
* update TrajectoryState of trajectories that got 'trimmed' away to be
deleted in the PureLocalizationTrimmer
* update serialization to only serialize 'undeleted' trajectories and
corresponding options.
#1111
- Adds TSDF2D Grid
- Adds tests for TSDF2D Grid
- Introduces Grid2D::GrowLimits(...) for multiple grids to reduce code duplication between TSDF2D and Grid2D
Fixing "Optimizing: -nan%... #846" https://github.com/googlecartographer/cartographer/issues/846.
The issue was triggered in a multithreaded execution, when all nodes were processed, but the working queue was still non empty (having other kind of jobs).
Unfortunately I failed to reproduce the bug in tests, so no new tests were added.
to allow setting the GlobalSlamOptimizationCallback after MapBuilder and PoseGraph
creation. Also removes the GlobalSlamOptimizationCallback from the Constructor
since it is not used.
Prerequisite for implementing ReceiveGlobalSlamOptimizations() in gRPC MapBuilderInterface.
Making getters and non-modifying methods in PoseGraph* const.
Before most getters where marked non-const because of the need to acquire the mutex for thread safety. This forces all code using the PoseGraph to pass it around as a non-const object even if the consumer is not altering the object.
By making the mutexes mutable we can make getters and methods that do not change the PoseGraph const and thus enable consumer APIs to express how they are going to use the PoseGraph as well.
fixes#1021
Michael Grupp
Martin Schwörer
Christoph Schütte
danielsievers
Kevin Daun
gaschler
Juraj Oršulić
Rodrigo Queiro
Alexander Belyaev
Wolfgang Hess
Arno Fleck
Susanne Pielawa
Alexander Reimann
Sebastian Klose
sotnik-github
Robert Milijaš