Request A Demo
Get a personalized demo and find out how to accelerate your time-to-market
Thank you! We'll be in touch soon.
Oops! Something went wrong while submitting the form.
Get a personalized demo and find out how to accelerate your time-to-market
In Part I and Part II of our series, we covered the theory, principles, mathematics, and some of the code required to create your own camera calibration module. If you have not yet read Part I and II, we strongly recommend doing so to ground yourself in these elements before exploring Part III, where we apply this to creating a model.
Because the Tangram Vision SDK is written in Rust (with wrappers for C and ROS, of course), we'll demonstrate how to create a calibration module in Rust. We’ll be using the NAlgebra linear algebra crate and the argmin optimization crate. You can follow along here, or with the full codebase at the Tangram Visions Blog repository. Let's get started.
In this tutorial we aren’t going to work with real images, but rather synthetically generate a few sets of image points using ground truth transforms and camera parameters.
To start we’ll generate some model points. The planar target lies in the XY plane. It will be a meter on each edge.
```rust
let mut source_pts: Vec<na::Point3<f64>> = Vec::new();
for i in -5..6 {
for j in -5..6 {
source_pts.push(na::Point3::<f64>::new(i as f64 * 0.1, j as f64 * 0.1, 0.0));
}
}
```
Then we’ll generate a few arbitrary camera-from-model transforms and some camera parameters.
```rust
let camera_model = na::Vector4::<f64>::new(540.0, 540.0, 320.0, 240.0); // fx, fy, cx, cy
let transforms = vec![
na::Isometry3::<f64>::new(
na::Vector3::<f64>::new(-0.1, 0.1, 2.0),//translation
na::Vector3::<f64>::new(-0.2, 0.2, 0.2),//rotation
),
na::Isometry3::<f64>::new(
na::Vector3::<f64>::new(-0.1, -0.1, 2.0),
na::Vector3::<f64>::new(0.2, -0.2, 0.2),
),
na::Isometry3::<f64>::new(
na::Vector3::<f64>::new(0.1, 0.1, 2.0),
na::Vector3::<f64>::new(-0.2, -0.2, -0.2),
),
];
```
Finally we’ll generate the imaged points by applying the image formation model.
```rust
let transformed_pts = transforms
.iter()
.map(|t| source_pts.iter().map(|p| t * p).collect::<Vec<_>>())
.collect::<Vec<_>>();
let imaged_pts = transformed_pts
.iter()
.map(|t_list| {
t_list
.iter()
.map(|t| project(&camera_model, t))
.collect::<Vec<na::Point2<f64>>>()
})
.collect::<Vec<_>>();
```
We’re going to use the `argmin` crate to solve the optimization problem. To build your own problem with `argmin`, you make a struct which implements the `ArgminOp` trait. The struct holds the data we're processing. Depending on the optimization algorithm you select, you’ll have to implement some of the trait’s functions. We’re going to use the Gauss-Newton algorithm which requires that we implement `apply()` which calculates the residual vector and `jacobian()` which calculates the Jacobian. The implementations for each are in the previous sections.
```rust
struct Calibration<'a> {
model_pts: &'a Vec<na::Point3<f64>>,
image_pts_set: &'a Vec<Vec<na::Point2<f64>>>,
}
```
During the optimization problem, the solver will update the parameters. The parameters are stored in a flat vector and thus it's useful to make a function that converts the parameter vector into easily-used objects for calculating the residual and Jacobian in the next iteration. Here's how we've done it:
```rust
impl<'a> Calibration<'a> {
/// Decode the camera model and transforms from the flattened parameter vector
fn decode_params(
&self,
param: &na::DVector<f64>,
) -> (na::Vector4<f64>, Vec<na::Isometry3<f64>>) {
let camera_model: na::Vector4<f64> = param.fixed_slice::<4, 1>(0, 0).clone_owned();
let transforms = self
.image_pts_set
.iter()
.enumerate()
.map(|(i, _)| {
let lie_alg_transform: na::Vector6<f64> =
param.fixed_slice::<6, 1>(4 + 6 * i, 0).clone_owned();
exp_map(&lie_alg_transform)
})
.collect::<Vec<_>>();
(camera_model, transforms)
}
}
```
Before we run the optimization problem we’ll have to initialize the parameters with some reasonable guesses. In the code block below, you'll see that we input four values for \\(f_x, f_y, c_x,\\) and \\(c_y\\).
```rust
let mut init_param = na::DVector::<f64>::zeros(4 + imaged_pts.len() * 6);
// Arbitrary guess for camera model
init_param[0] = 1000.0; // fx
init_param[1] = 1000.0; // fy
init_param[2] = 500.0; // cx
init_param[3] = 500.0; // cy
// Arbitrary guess for poses (3m in front of the camera with no rotation)
let init_pose = log_map(&na::Isometry3::translation(0.0, 0.0, 3.0));
// Populate poses with guess
init_param
.fixed_slice_mut::<6, 1>(4, 0)
.copy_from(&init_pose);
init_param
.fixed_slice_mut::<6, 1>(4 + 6, 0)
.copy_from(&init_pose);
init_param
.fixed_slice_mut::<6, 1>(4 + 6 * 2, 0)
.copy_from(&init_pose);
```
Next we build the `argmin` "executor" by constructing a solver and passing in the `ArgminOp` struct.
```rust
let cal_cost = Calibration {
model_pts: &source_pts,
image_pts_set: &imaged_pts,
};
let solver = argmin::solver::gaussnewton::GaussNewton::new()
.with_gamma(1e-1)
.unwrap();
let res = Executor::new(cal_cost, solver, init_param)
.add_observer(ArgminSlogLogger::term(), ObserverMode::Always)
.max_iters(2000)
.run()
.unwrap();
eprintln!("{}\\n\\n", res);
eprintln!("ground truth intrinsics: {}", camera_model);
eprintln!(
"optimized intrinsics: {}",
res.state().best_param.fixed_slice::<4, 1>(0, 0)
);
```
After letting the module run for over 100 iterations, the cost function will be quite small and we can see that we converged to the right answer.
```rust
ground truth intrinsics:
┌ ┐
│ 540 │
│ 540 │
│ 320 │
│ 240 │
└ ┘
optimized intrinsics:
┌ ┐
│ 539.9999999989419 │
│ 539.99999999895 │
│ 320.00000000071225 │
│ 240.0000000009506 │
└ ┘
```
So there you have it - the theory and the execution of a built-from-scratch calibration module for a single sensor. You can revisit Part I and Part II to review what we explored prior to demonstrating execution of the model above.
As we noted at the end of Part II, creating a calibration module for sensors is no trivial task. The simplified module and execution we've described in these two posts provide the building blocks for a single camera approach, without optimizations for calibration time, compute resources, or environmental variability. Adding these factors, and expanding calibration routines to multiple sensors, makes the calibration challenge significantly more difficult to achieve. That's why multi-sensor, multi-modal calibration is a key part of the Tangram Vision SDK. You can request access to the Tangram Vision SDK and test it on up to five device instances at no cost.