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.
To simplify the implementation of a gRPC-based trajectory builder,
this moves the existing implementation to CollatedTrajectoryBuilder
and organizes the interface by sensor type.
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.
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.
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.
"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".
- 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.
- 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.
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.
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?
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.
- [ 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.
- [ 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.
This adds information about which nodes are contained in which
deserialized submaps. This is necessary for the rotational scan
matcher.
Also skips frozen trajectories when tying together nodes of a
trajectory with IMU data which is unavailable in this case.
- introduce InsertIntoSubmap for 2D
- clarify some variable names in 3D
- move rotational_scan_matcher_histogram calculation
to InsertIntoSubmap for 3D
- refactor last version of range data before insertion into
range_data_in_local (filtered_range_data_in_local for 3D)
This changes the origin of accumulated range data from the
zero vector (which could be far off) to the origin of the
first range data in the accumulation.
Adds a type TimedPointCloud that holds 4-dimensional vectors where measurement time of individual points are stored in the fourth entry.
Uses TimedRangeData to pass TimedPointCloud of incoming measurements to LocalTrajectoryBuilder.
Fixes#573.
This speeds up the asset writer significantly:
For the 3D sample bag:
~~~
/usr/bin/time roslaunch cartographer_ros assets_writer_backpack_3d.launch \
bag_filenames:=${HOME}/Downloads/b3-2016-04-05-14-14-00.bag \
pose_graph_filename:=${HOME}/Downloads/b3-2016-04-05-14-14-00.bag.pbstream
master:
2100.96user 26.79system 35:43.60elapsed 99%CPU (0avgtext+0avgdata 3657028maxresident)k
17682632inputs+32080outputs (0major+20021051minor)pagefaults 0swaps
this CL:
1937.46user 11.77system 32:22.49elapsed 100%CPU (0avgtext+0avgdata 3659412maxresident)k
1352inputs+32088outputs (0major+7042032minor)pagefaults 0swaps
~~~
So ~10%. For pipelines that do less this can be even 30% or more.
- Refactor ProbabilityGridPointsProcessor for code reuse.
- Adds Image::width and height.
- Adds GetFilename for FileWriter.
Related to googlecartographer/cartographer_ros#475.
* Adds DeleteScanMatcher() to 3D constraint builder
* Add possibility to remove submaps and scans from the 3D optimization
problem.
* Implements TrimmingHandle in SPA.
Before, the relative change in pose due to odometry in the
2D pose graph optimization problem was incorrectly done in
the tracking frame. Instead, the gravity aligned frame in
which the 2D poses are stored must be used.
Fixes#515.
PAIR=cschuet
This PR introduces a new option that specifies the number of seconds
after which global matcher searches are performed if no recent global
constraints have been found between the submap's and the node's
trajectory.
This changes submap_visualization.proto that multiple textures can be provided.
As of now, the first texture is generated from the high resolution
grid, which is the same as before.
The second texture is generated from the low resolution grid.
Updating the connectivity needs the data of nodes for which
constraints were added, so we postpone trimming to after the
connectivity update.
Also makes sure the mutex is held as necessary.
This PR fixes a crash when cartographer is run in localization mode due
to the map trajectory not having been properly added to the
ConnectedComponents.
* Introduce TrajectoryConnectivityState.
This class will be used to track the connectivity state (including the
last connection time) between pairs of trajectories.
* Remove connected_components_ from SPG.
This PR removes connected_components_ from the SparsePoseGraph. Since
the connectivity structure is no longer updated by the
ConstraintBuilders it is no longer necessary for SPG to keep a copy of
the connected components for data consistency.
Remove reverse_connected_components from SparsePoseGraphs and update
TrajectoryConnectivity to return "connected" for the reflexive case even
if trajectories are unknown.
This modifies the ConstraintBuilder interface so that Maybe*Constraint
is not responsible anymore for updating the TrajectoryConnectivity
member of SparsePoseGraph and moves this responsibility into the
WhenDone callback.
This changes the trajectory nodes to contain the complete 'pose'
including 'tracking_to_tracking_2d' applied already. Similar for
the 'zbar_ij' as it is returned by 'constraints()'. This allows
2D and 3D to be handled in the same way.
This PR unifies GlobalTrajectoryBuilder across 2D and 3D by templating
it with the LocalTrajectoryBuilder and the SparsePoseGraph. It thus
includes small changes to interfaces in LocalTrajectoryBuilder and
SparsePoseGraph.
This reduces the amount of code that needs to be written when a
new type of sensor data is introduced by a tiny bit.
Rangefinder data is not yet following this pattern.
Christoph Schütte
gaschler
Jihoon Lee
Juraj Oršulić
Wolfgang Hess
Wolfgang Hess
Holger Rapp
jie
catskul
damienrg
Alexander Belyaev
jie
gaschler
Mac Mason