diff --git a/README.md b/README.md index 386eb256f..07dca0d10 100644 --- a/README.md +++ b/README.md @@ -1,3 +1,161 @@ # fuse The fuse stack provides a general architecture for performing sensor fusion live on a robot. Some possible applications include state estimation, localization, mapping, and calibration. + +## Overview +fuse is a ROS framework for performing sensor fusion using nonlinear least squares optimization techniques. In +particular, fuse provides: +* a plugin-based system for modeling sensor measurements +* a similar plugin-based system for motion models +* a plugin-based system for publishing optimized state values +* an extensible state variable definition +* a "contract" on how an optimizer will interact with the above components +* and some common implementations to get everyone started + +(unpresented) ROSCon 2018 Lightning Talk [slides](doc/fuse_lightning_talk.pdf) + + +Data flows through the system approximately like this: +* A sensor model receives raw sensor data. The sensor model generates a constraint and sends it to the optimizer. +* The optimizer receives the new sensor constraint. A request is sent to each configured motion model to generate + a constraint between the previous state and the new state involved in the sensor constraint. +* The motion model receives the request and generates the required constraints to connect the new state to the + previously generated motion model chain. The motion model constraints are sent to the optimizer. +* The optimizer adds the new sensor model and motion model constraints and variables to the graph and + computes the optimal values for each state variable. +* The optimal state values are sent to each configured publisher (as well as the sensor models and motion models). +* The publishers receive the optimized state values and publish any derived quantities on ROS topics. +* Repeat + +It is important to note that much of this flow happens asynchronously in practice. Sensors are expected to operate +independently from each other, so each sensor will be sending constraints to the optimizer at its own frequency. The +optimizer will cache the constraints and process them in small batches on some schedule. The publishers may +require considerable processing time, introducing a delay between the completion of the optimization cycle and the +publishing of data to the ROS topic. + +![fuse sequence diagram](doc/fuse_sequence_diagram.png) + +## Example +Let's consider a simple robotics example to illustrate this. Assume we have a typical indoor differential-drive robot. +This robot has wheel encoders and a horizontal laser. + +The first thing we must do is define our state variables. At a minimum, we want the robot pose at each timestamp. +We model the pose using a 2D position and an orientation. Each 2D pose _at a specific time_ gets a unique variable +name. For ease of notation, let's call the pose variables `X1`, `X2`, `X3`, etc. (In reality, each variable gets +a UUID, but those are much harder to write down.) Each 2D pose is instantiated as a `example_robot::Pose2D` which is +derived from the `fuse_core::Variable` base class. (`fuse` ships with several basic variables, such as 2D and 3D +versions of position, orientation, and velocity variables, but you can derive your own variable types as you need them.) + +Next we need to decide how to model our sensors. We can model the wheel encoders as providing an incremental +pose measurement. Given a starting pose, `X1`, and a wheel encoder delta, `z`, we predict the pose `X2'` using some +measurement function `f`. + +`X2' = f(X1, z)` + +The error term for our constraint is the difference between the predicted pose `X2'` and the actual pose `X2` + +`error = X2'^-1 * X2` + +where `X2'^-1` is the inverse of the pose `X2'` + +We derive a `fuse_core::Constraint` that implements that error function. Similarly, we perform scan-to-scan matching +using out laser data and create an incremental pose constraint between consecutive scans. + +In the simplest example, the sensors are synchronized, i.e. the laser and the wheel encoders are sampled at the same +time. This is enough to construct our first `fuse` system. Below is the constraint graph generated from this first +system. The large circles represent state variables at a given time, while the small squares represent measurements. +The graph connectivity indicates which variables are involved in what measurements. + +![fuse graph](doc/fuse_graph_1.png) + +The two sensor models are configured as plugins to an optimizer implementation. The optimizer performs the required +computation to generate the optimal state variable values based on the provided sensor constraints. We will never be +able to exactly satisfy both the wheel encoder constraints and the laserscan constraints. Instead we minimize the error +of all the constraints using nonlinear least squares optimization. + +![fuse optimizer](doc/fuse_optimizer_1.png) + +While our `fuse` system is optimizing constraints from two different sensors, it is not yet publishing any data back +out to ROS. In order to publish data to ROS, we derive a `fuse_core::Publisher` class and add it to the +optimizer. Derived publishers have access to the optimized values of all state variables. The specific publisher +implementation determines what type of messages are published and at what frequency. For our example system, +we would like visualize the current pose of the robot in RViz, so we create a `fuse` publisher that finds the most +recent pose and converts it into a `geometry_msgs::PoseStamped` message, then publishes the message to a topic. + +![fuse optimizer](doc/fuse_optimizer_2.png) + +We finally have something that is starting to be useful. + +### Adaptation #1: Asynchronous sensors + +Typically the laser measurements and the wheel encoder measurements are not synchronized. The encoder measurements are +sampled faster than the laser, and are sampled at different times using a different clock. If we do not do anything +different in this situation, the constraint graph becomes disconnected. + +![fuse graph](doc/fuse_graph_2.png) + +This is where motion models come into play. A motion model differs from a sensor model in that constraints can be +generated between any two requested timestamps. Motion model constraints are generated upon request, not due to their +own internal clock. We use the motion model to connect the states introduced by the other sensor measurements. We +derive a class from the `fuse_core::MotionModel` base class and implement a differential drive kinematic +constraint for our robot. + +![fuse optimizer](doc/fuse_optimizer_3.png) + +The motion models are also configured as plugins to the optimizer. The optimizer requests motion models constraints +from the configured plugins whenever new states are created by the sensor models. + +![fuse graph](doc/fuse_graph_3.png) + +### Adaptation #2: Full path publishing + +Nothing about the `fuse` framework limits you to having a single publisher. What if you want to visualize the entire +robot trajectory, instead of just the most recent pose? Well, we can create a new derived `fuse_core::Publisher` class +that publishes all of the robot poses using a `nav_msgs::Path` message. + +![fuse optimizer](doc/fuse_optimizer_4.png) + +### Adaptation #3: Changing kinematics + +In your spare time, you also build [autonomous power wheels racers](http://www.powerracingseries.org/). But race cars +don't use differential drive; you need a different motion model. Easy enough. We simply derive a new +`fuse_core::MotionModel` class that implements an Ackermann steering model. Everything else can be reused. + +![fuse optimizer](doc/fuse_optimizer_5.png) + +![fuse graph](doc/fuse_graph_4.png) + +### Adaptation #4: Online calibration + +Over time you notice that the accuracy of the odometry measurements is decreasing. After some investigation you realize +that the soft rubber racing tires are wearing, decreasing the diameter of the wheels over time. It sure would be nice +if the odometry system could compensate for that automatically. To do that, we derive a new variable type from +`fuse_core::Variable` that holds a single scalar value representing a wheel diameter at a specific point in time. For +ease of notation, we refer to this new variable as `D1, D2, ...`, etc. We also need to derive a new wheel encoder sensor +model from the `fuse_core::SensorModel` base class. This new sensor model involves the previous pose and next pose as +before, but it also involves the previous wheel diameter. Finally, we need a `fuse_core::MotionModel` that describes +how the wheel diameter is expected to change over time. Maybe some sort of exponential decay? And for good measure, we +derive a new publisher plugin from `fuse_core::Publisher` that publishes the current wheel diameter. This allows us to +plot how the wheel diameter changes over the length of the race. + +![fuse optimizer](doc/fuse_optimizer_6.png) + +![fuse graph](doc/fuse_graph_5.png) + +Now our system estimates the wheel diameters at each time step as well as the robot's pose. + +## The Math +Internally `fuse` uses Google's [Ceres Solver](http://ceres-solver.org) to perform the nonlinear least squares +optimization, which produces the optimal state variable values. I direct any interested parties to the Ceres Solver +["Non-linear Least Squares"](http://ceres-solver.org/nnls_tutorial.html) tutorial for an excellent primer on the core +concepts and involved math. + +## Summary +The purpose of `fuse` is to provide a framework for performing sensor fusion tasks, allowing common components to be +reused between systems, while also allowing components to be customized for different use cases. The goal is to allow +end users to concentrate on modelling the robot, sensor, system, etc. and spend less time wiring the different +sensor models together into runable code. And since all of the models are implemented as plugins, separate plugin +libraries can be shared or kept private at the discretion of their authors. + +## API +Class API details coming soon... diff --git a/doc/fuse_graph_1.png b/doc/fuse_graph_1.png new file mode 100644 index 000000000..76bbe3dea Binary files /dev/null and b/doc/fuse_graph_1.png differ diff --git a/doc/fuse_graph_2.png b/doc/fuse_graph_2.png new file mode 100644 index 000000000..ba9e7addf Binary files /dev/null and b/doc/fuse_graph_2.png differ diff --git a/doc/fuse_graph_3.png b/doc/fuse_graph_3.png new file mode 100644 index 000000000..77e53305a Binary files /dev/null and b/doc/fuse_graph_3.png differ diff --git a/doc/fuse_graph_4.png b/doc/fuse_graph_4.png new file mode 100644 index 000000000..75b845301 Binary files /dev/null and b/doc/fuse_graph_4.png differ diff --git a/doc/fuse_graph_5.png b/doc/fuse_graph_5.png new file mode 100644 index 000000000..e73bd461a Binary files /dev/null and b/doc/fuse_graph_5.png differ diff --git a/doc/fuse_lightning_talk.pdf b/doc/fuse_lightning_talk.pdf new file mode 100644 index 000000000..a3b0a5906 Binary files /dev/null and b/doc/fuse_lightning_talk.pdf differ diff --git a/doc/fuse_optimizer_1.png b/doc/fuse_optimizer_1.png new file mode 100644 index 000000000..31ecb8358 Binary files /dev/null and b/doc/fuse_optimizer_1.png differ diff --git a/doc/fuse_optimizer_2.png b/doc/fuse_optimizer_2.png new file mode 100644 index 000000000..0984cfc81 Binary files /dev/null and b/doc/fuse_optimizer_2.png differ diff --git a/doc/fuse_optimizer_3.png b/doc/fuse_optimizer_3.png new file mode 100644 index 000000000..9fbab7f92 Binary files /dev/null and b/doc/fuse_optimizer_3.png differ diff --git a/doc/fuse_optimizer_4.png b/doc/fuse_optimizer_4.png new file mode 100644 index 000000000..5e0214cba Binary files /dev/null and b/doc/fuse_optimizer_4.png differ diff --git a/doc/fuse_optimizer_5.png b/doc/fuse_optimizer_5.png new file mode 100644 index 000000000..b6d582dba Binary files /dev/null and b/doc/fuse_optimizer_5.png differ diff --git a/doc/fuse_optimizer_6.png b/doc/fuse_optimizer_6.png new file mode 100644 index 000000000..4e0d354e7 Binary files /dev/null and b/doc/fuse_optimizer_6.png differ diff --git a/doc/fuse_sequence_diagram.png b/doc/fuse_sequence_diagram.png new file mode 100644 index 000000000..6ef60bf42 Binary files /dev/null and b/doc/fuse_sequence_diagram.png differ