DEV Community: Adam Rodnitzky

Why Rust for Robots?

Adam Rodnitzky — Fri, 09 Apr 2021 18:21:37 +0000

Robotics largely runs on C++. Or at least it does for now. Here at Tangram Vision, we believe that there is better language for robotics, and that language is Rust.

C++ for Robotics

For robotic platforms where the intention is to deploy commercially and at scale, C++ has emerged as the standard over the last few decades, and there are a few key reasons why. For one, the level of ubiquity has become a self-reinforcing mechanism, with an ecosystem of libraries, tools and engineers that almost exclusively work in C++. In fact, ROS (the main client library of the most popular framework used by most roboticists) is primarily written in C++. Similarly, the popular OpenCV computer vision library is also written in and accessed with C++.

However, ubiquity of tools and engineers alone doesn't explain the popularity of C++ for robotics. There is a more fundamental reason: most robotics platforms must meet acceptable performance thresholds under known resource constraints. C++ is well suited for these embedded applications because of how "close to the metal" the language can get.

This "close to the metal" advantage also makes C++ a potentially huge hazard. Testing, architecture, and memory management for C++ isn't consistent across well-used dependencies; yet, these dependencies might capture and manipulate essential low-level resources. This means it becomes very easy to inadvertently build in a critical bug without realizing it... that is, until you're well into production. That critical bug won't be discovered until the product has been deployed in the real world, where an edge case scenario that slipped past QA triggers a memory leak, or system crash, or [your favorite disaster scenario here].

Unless you resemble this Venn diagram, there's a good chance that you'll inadvertently program in some level of trouble as you build out your robotics codebase in C++.

This is where Rust starts to make sense.

Enter Rust for Robotics

Rust is a relatively new language for robotics, but there's a rapidly growing set of projects and libraries that provide key frameworks for robotics development. Why the change?

For starters, the biggest benefit of building with Rust is memory safety and management. You have to try very hard to create a memory leak or race condition in Rust. Common gotchas like null pointers and data races are blocked altogether and won't compile. Likewise, the memory management approach in Rust is to use the stack to keep track of the program, and then use pointer references aimed at heaps where larger data structures are contained.

To access a data structure, ownership has to be established, thereby preventing multiple variables from accessing or modifying data structures simultaneously. Efficient? Yes. Safe? Also, yes. The best part: developers maintain that "close to the metal" access that they would normally go to C++ for. This makes Rust a highly-efficient, extremely safe language that also allows low-level access, something well-suited to the world of robotics where resource constraints and code safety are critical.

Rust is an obvious choice for robotics, but the transition to platforms written in Rust will take time. Rust itself is just over a decade old, whereas C++ has been around for nearly four decades. That's a lot of inertia to work against, but the Rust community is moving fast.

Resources for Rust in Robotics

There's a small but growing community of companies and developers (Tangram Vision included!) that are taking some of robotics' most commonly used libraries and tools and making them Rust compatible, as well as developing new tools to ease the development path for creating a Rust-programmed robot.

Here are a few of our favorites that cover some of the critical areas of robotics development. We've chosen to highlight resources that are still actively maintained within the last year.

Frameworks

OpenRR: An open-source Rust robotics platform

ROS

rosrust: A pure Rust implementation of the ROS client library
ros2-rust: Bindings, a code generator and code examples for ROS2
rustros_tf: A Rust port of the ROS tf library for keeping track of three dimensional transforms
Optimization Engine: Embedded optimization for robots and autonomous systems

Computer Vision

realsense-rust: High-level bindings for using Intel RealSense depth cameras (disclosure: Tangram Vision maintains this library!)
opencv-ros-camera: An OpenCV-compatible geometric model of a camera
adskalman: Kalman filter smoothing
cam-geom: Geometric models of cameras
bayes_estimate: A Bayesian estimation library

Collision Detection

openrr-planner: Path planning with collision avoidance

Controls

stepper: A universal stepper motor driver and controller interface for Rust

Simulation

nphysics: A 2D and 3D physics engine that can be used for robot simulation

Mathematics

Nalgebra: It's linear algebra...for Rust
petgraph: Graph data structure library, compatible with Rust

If there are other Rust resources that you've used for a robotics product that worked well, let us know! We plan on keeping this list curated and updated over time.

Should you build your Robot in Rust?

We think the answer will increasingly be yes. The fundamental benefits of the language already make it a great fit for the needs of roboticists and robots. The growing set of libraries and resources make it easier than ever to get started with foundations — and that includes the perception sensor tools and APIs that we are building here at Tangram Vision. Finally, more and more engineers are adopting Rust as a language of choice, particularly for embedded scenarios like robots. So, yes, the future is robots. And it will also be robots, built with Rust.

The Current State of Event Cameras

Adam Rodnitzky — Wed, 30 Dec 2020 21:22:27 +0000

We took one last moment before 2020 disappears to reflect on why event cameras have yet to take the sensor world by storm.

Event Cameras: Where Are They Now? | by Adam Rodnitzky | Tangram Visions | Dec, 2020 | Medium

Adam Rodnitzky ・ Dec 30, 2020 ・ 8 min read
Medium

One-to-Many Sensor Trouble, Part 2

Adam Rodnitzky — Mon, 07 Dec 2020 21:48:14 +0000

Phase 2: Updating Your Prediction

From State to Sensors

While we were busy predicting, the GPS on our RC car was giving us positional data updates. These sensor measurements $Zt\text{Z}_{t}$ give us valuable information about our world.

However, $zt\text{z}{t}$ and our state vector $xt+1\text{x}{t+1}$ may not actually correspond; our measurements might be in one space, and our state in another! For instance, what if we’re measuring our state in meters, but all of our measurements are in feet? We need some way to remedy this.

Let’s handle this by converting our state vector into our measurement space using an observation matrix H:

These equations represent the mean $μ{ₑₓₚ}$ and covariance $Σ{ₑₓₚ}$ of our predicted measurements.

For our RC car, we’re going from meters to feet, in both position and velocity. 1 meter is around 3.28 ft, so we would shape H to reflect this:

…leaving us with predicted measurements that we can now compare to our sensor measurements $Zt\text{Z}_{t}$ . Note that H is entirely dependent on what’s in your state and what’s being measured, so it can change from problem to problem.

Fig. 1: Moving our state from state space (meters, bottom left PDF) to measurement space (feet, top right PDF).

Our RC car example is a little simplistic, but this ability to translate our predicted state into predicted measurements is a big part of what makes Kalman filters so powerful. We can effectively compare our state with any and all sensor measurements, from any sensor. That’s powerful stuff!

An aside: The behavior of H is important. Vanilla Kalman filters use a linear Fₜ and H; that is, there is only one set of equations relating the estimated state to the predicted state (Fₜ), and predicted state to predicted measurement (H).

If the system is non-linear, then this assumption doesn’t hold. Fₜ and H might change every time our state does! This is where innovations like the Extended Kalman Filter (EKF) and the Unscented Kalman Filter come into play. EKFs are the de facto standard in sensor fusion for this reason.

Let’s add one more term for good measure: $R_{t}$ , our sensor measurement covariance. This represents the noise from our measurements. Everything is uncertain, right? It never ends.

The Beauty of PDFs

Since we converted our state space into the measurement space, we now have 2 comparable Gaussian PDFs:

$μ{ₑₓₚ}$ and $Σ{ₑₓₚ}$ , which make up the Gaussian PDF for our predicted measurements
$Z_{t}$ and $R_{t}$ , which make up the PDF for our sensor measurements

The strongest probability for our future state is the overlap between these two PDFs. How do we get this overlap?

Fig. 2: Our predicted state in measurement space is in red. Our measurements z are in blue. Notice how our measurements z have a much smaller covariance; this is going to come in handy.

We multiply them together! The product of two Gaussian functions is just another Gaussian function. Even better, this common Gaussian has a smaller covariance than either the predicted PDF or the sensor PDF, meaning that our state is now much more certain.

ISN’T THAT NEAT.

Update Step, Solved.

We’re not out of the woods yet; we still need to derive the math! Suffice to say… it’s a lot. The basic gist is that multiplying two Gaussian functions results in its own Gaussian function. Let’s do this with two PDFs now, Gaussian functions with means μ₁, μ₂ and variances σ₁², σ₂². Multiplying these two PDFs leaves us with our final Gaussian PDF, and thus our final mean and covariance terms:

When we substitute in our derived state and covariance matrices, these equations represent the update step of a Kalman filter.

See reference (Bromiley, P.A.) for a good explanation on how >to multiply two PDFs and derive the above. It takes a good >page to write out; you’ve been warned.

This is admittedly pretty painful to read as-is. We can simplify this by defining the Kalman Gain $K_{t}$ :

With $K_{t}$ in the mix, we find that our equations are much kinder on the eyes:

We’ve done it! Combined, the new $X$ and P create our final Gaussian distribution, the one that crunches all of that data to give us our updated state. See how our updated state spikes in probability (the blue spike in Fig. 3). That’s the power of Kalman Filters!

Fig. 3: Original state in green. Predicted state in red. Updated final state in blue. Look at how certain we are now! Such. Wow.

What Now?

Well… do it again! The Kalman filter is recursive, meaning that it uses its output as the next input to the cycle. In other words, the final $X$ is your new $X_{t}$ ! The cycle continues in the next prediction state.

Kalman filters are great for all sorts of reasons:

They extend to anything that can be modeled and measured. Automotives, touchscreens, econometrics, etc etc.
Your sensor data can come from anywhere. As long as there’s a connection between state and measurement, new data can be folded in.
Kalman filters also allow for the selective use of data. Did your sensor fail? Don’t count that sensor for this iteration! Easy.
They are a good way to model prediction. After completing one pass of the prediction step, just do it again (and again, and again) for an easy temporal model.

Of course, if this is too much and you’d rather do… anything else, Tangram Vision is developing solutions to these very same problems! We’re creating tools that help any vision-enabled system operate to its best. If you like this article, you’ll love seeing what we’re up to.

Code Examples and Graphics

The code used to render these graphs and figures is hosted on Tangram Vision’s public repository for the blog. Head down, check it out, and play around with the math yourself! If you can improve on our implementations, even better; we might put it in here. And be sure to tweet at us with your improvements.

Check it out here:

Tangram-Vision / Tangram-Vision-Blog

Code pertaining to posts made on our official blog!

Tangram Visions Blog

This repo holds code to generate the assets used in Tangram Visions, the company blog of @Tangram-Vision!

The Blog

Found here in its native Notion format
Found here on Medium
Found here on Dev

Find us wherever fine publications are syndicated.

Demo and Contribute

We take pride in our posts, but we realize that they aren't perfect. If you have an improvement or interesting modification in mind, write it up and submit a merge request! We might incorporate it here, and better yet, into the blog.

Examples in different programming languages
Improved rendering
Improved mathematics and numerical techniques
etc, etc, etc

View on GitHub

Multi-Sensor Calibration Pt 1 (Or, How to Kalman Filter Your Way Out)

Adam Rodnitzky — Wed, 02 Dec 2020 01:06:26 +0000

In the world of automated vision, there's only so much one can do with a single sensor. Optimize for one thing, and lose the other; and even with one-size-fits-all attempts, it's hard to paint a full sensory picture of the world at 30fps.

We use sensor combinations to overcome this restriction. This intuitively seems like the right play; more sensors mean more information. The jump from one camera to two, for instance, unlocks binocular vision, or the ability to see behind as well as in front. Better yet, use three cameras to do both at once. Add in a LiDAR unit, and see farther. Add in active depth, and see with more fidelity. Tying together multiple data streams is so valuable this act of Sensor Fusion is a whole discipline in itself.

Yet this boon in information often makes vision-enabled systems harder to build, not easier. Binocular vision relies on stable intrinsic and extrinsic camera properties, which cameras don't have. Depth sensors lose accuracy with distance. A sensor can fail entirely, like LiDAR on a foggy day.

This means that effective sensor fusion involves constructing vision architecture in a way that minimizes uncertainty in uncertain conditions. Sensors aren't perfect, and data can be noisy. It's the job of the engineer to sort this out and derive assurances about what is actually true. This challenge is what makes sensor fusion so difficult: it takes competency in information theory, geometry, optimization, fault tolerance, and a whole mess of other things to get right.

So how do we start?

Guessing

...just kidding. Though you would be surprised how many times an educated guess gets thrown in! No, we're talking

Kalman filters

So let's review our predicament:

We have a rough idea of our current state (e.g. the position of our robot), and we have a model of how that state changes through time.
We have multiple sensor modalities, each with their own data streams.
All of these sensors give noisy and uncertain data.

Nonetheless, this is all that we have to work with. This seems troubling; we can't be certain about anything!

Instead, what we can do is minimize our uncertainty. Through the beauty of mathematics, we can combine all of this knowledge and actually come out with a more certain idea of our state through time than if we used any one sensor or model.

This 👆 is the magic of Kalman filters.

Warning: Math.

Phase 1: Prediction

Model Behavior

Let's pretend that we're driving an RC car in a completely flat, very physics-friendly line.

There are two things that we can easily track about our car's state: its position pt and velocity vt.

We can speed up our robot by punching the throttle, something we do frequently. We do this by exerting a force f on the RC car's mass m, resulting in an acceleration a (see Newton's Second Law of Motion).

With just this information, we can derive a model for how our car will act over a time period $Δt\Delta t$ using some classical physics:

pt+1=pt+(vtΔt)+fΔt22m(1.1)\tag{1.1} p_{t+1} = p_{t} + (v_{t} \Delta t)+ \frac{f \Delta t^2} {2m}

vt+1=vt+fΔtm(1.2)\tag{1.2} v_{t+1} = v_{t} + \frac{ f \Delta t} {m}

We can simplify this for ourselves using some convenient matrix notation. Let's put the values we can track, position pt and velocity vt, into a state vector:

xt=[ptvt](1.3)\tag{1.3} \textbf{x}_{t} = \begin{bmatrix} p_{t} \\ v_{t} \end{bmatrix}

...and let's put out applied forces into a control vector that represents all the outside influences affecting our state:

ut=[fm]=fm(1.4)\tag{1.4} \textbf{u}_{t} = \begin{bmatrix} \frac{f}{m} \end{bmatrix} = \frac{f}{m}

Now, with a little rearranging, we can organize our motion model for position and velocity into something a bit more compact:

[pt+1vt+1]=[1Δt01]⏟Ft[ptvt]+[Δt22Δt]⏟Btfm(1.5)\tag{1.5} \begin{bmatrix} p_{t+1} \\ v_{t+1} \end{bmatrix} = \underbrace{ \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix} }_{F_{t}} \begin{bmatrix} p_{t} \\ v_{t} \end{bmatrix} + \underbrace{ \begin{bmatrix} \frac{\Delta t^2}{2} \\ \Delta t \end{bmatrix}}_{B_{t}} \frac{f}{m}

⇒xt+1=Ftxt+Btut(1.6)\tag{1.6} \Rightarrow \textbf{x}_{t+1} = F_{t}\textbf{x}_{t} + B_{t}\textbf{u}_{t}

By rolling up these terms, we get some handy notation that we can use later:

$F_{t}$ is called our prediction matrix. It models what our system would do over $Δt\Delta t$ , given its current state.
$B_{t}$ is called our control matrix. This relates the forces in our control vector $ut\textbf{u}_t$ to the state prediction over $Δt\Delta t$ .

Uncertainty through PDFs

However, we’re not exactly sure whether or not our state values are true to life; there’s uncertainty! Let’s make some assumptions about what this uncertainty might look like in our system:

Any error we might get in an observation is inherently random; that is, there isn’t a bias towards one result.
Errors are independent of one another.

These two assumptions mean that our uncertainty follows the Central Limit Theorem! We can therefore assume that our error follows a Normal Distribution, aka a Gaussian curve.

We will use our understanding of Gaussian curves later to great effect, so take note!

We’re going to give this uncertainty model a special name: a probability density function (PDF). This represents how probable it is that certain states are the true state. Peaks in our function correspond to the states that have the highest probability of occurrence.

Fig. 1. Our first PDF

Our state vector $xt\text{x}_{t}$ represents the mean $μ$ of this PDF. To derive the rest of the function, we can model our state uncertainty using a covariance matrix $P_{t}$ :

There are some interesting properties here in $P_{t}$ . The diagonal elements ( $Σ_{pp}$ , $Σ_{vv}$ ) represent how much these variables deviate from their own mean. We call this variance.

The off-diagonal elements of $P_{t}$ express covariance between state elements. If $Σ_{pv}$ is zero, for instance, then we know that an error in velocity won’t influence an error in position. If it’s any other value, we can safely say that one affects the other in some way. PDFs without covariance terms look like Figure 1 above, with major and minor axes aligned with our world axes. PDFs with covariance are skewed off-axis depending on how extreme the covariance is:
Fig. 2. A PDF with non-zero covariance. Notice the ‘tilt’ in the major and minor axes of the ellipsis.

Variance, covariance, and the related correlation of variables are valuable, as they make our PDF more information-dense.

We know how to predict $xt+1\text{x}_{t+1}$ , but we also need the predicted covariance $P_{t+1}$ if we’re going to describe our state fully. We can derive it from $xt+1\text{x}_{t+1}$ using some (drastically simplified) linear algebra:

Notice that $B_{t}u_{t}$ got tossed out! Control has no uncertainty that we can directly observe, so we can’t use the same math that we did on $FtxtF_{t}\text{x}_{t}$ .

However, we can factor in the effects of noisy control inputs another way: by adding a process noise covariance matrix $Q_{t}$ :

Yes, we are literally adding noise.

Prediction step, solved?

We have now derived the full prediction step:

Our results are… ok. We got a good guess at our new state out of this process, sure, but we’re a lot more uncertain than we used to be!

Fig. 3. Starting state PDF in red, predicted state PDF in blue. Notice how the distribution is more spread out. Our state is less certain than before.

There’s a good reason for that: everything up to this point has been a sort of “best guess”. We have our state, and we have a model of how the world works; all that we’re doing is using both to predict what might happen over time. We still need something to support these predictions outside of our model.

Something like sensor measurements, for instance.

. . .

We’re getting there! So far, this post has covered:

The motivation for using a Kalman filter in the first place
A toy use case, in this case our physics-friendly RC car
The Kalman filter prediction step: our best guess at where we’ll be, given our state

We’ll keep it going in Part II (posted later this week) by bringing in our sensor measurements (finally). We will use these measurements, along with our PDFs, to uncover the true magic of Kalman filters!

Spoiler: it’s not magic. It’s just more math.

Note: this excellent post was written by Tangram Vision CEO Brandon Minor

Brandon MinorFollow

Founder of Tangram Vision