2-from-geographical-coordinates-to-pixels.md

Step 2: From geographical coordinates to pixels

Now, lets compute the connection between geographical coordinates and image pixels using real data!

The Sophus library

We will use Sophus to represent poses and orientations. It lets us perform operations on SO(3) and SE(3) matrix Lie groups with Eigen. Take a look at so3.hpp and se3.hpp for documentation. Ask the instructors when you have questions about Sophus!

Coordinate systems

In camera_pose_lab.cpp, we are given the geodetic position of a light pole:

We will create a local Cartesian NED coordinate system around this position using LocalCoordinateSystem. The helicopter (body) navigation data is given as a set of geodetic positions with the corresponding NED attitude. The LocalCoordinateSystem class lets us convert navigation data to poses in the local coordinate system F_l.

Take a look at the documentation in local_coordinate_system.h.

With LocalCoordinateSystem, we are able to represent the poses of the helicopter F_b in a common Cartesian coordinate system F_l.

The camera pose measurements in F_c are represented as Cartesian positions with corresponding NED attitudes relative to the body frame F_b.

For each image, we are given these data in DataElement. Take a look at DataElement in dataset.h. Then, get an overview of the data structures in attitude.h, geodetic_position.h and cartesian_position.h.

1. Convert geographical body position and attitude to local Cartesian pose

The first step is to convert the navigation data to the poses T_lb (pose of F_b relative to F_l). To accomplish this, we need to convert the orientation from roll-pitch-yaw Euler angles (θ₁, θ₂, θ₃) to a proper rotation matrix:

You will need to finish the Attitude::toQuaternion member function in attitude.cpp, which is used by Attitude::toSO3 to return a Sophus::SO3d orientation.

1. Convert attitude in Euler angles (roll-pitch-yaw) to a Quaternion. Use Eigen::AngleAxisd, which lets us create principal rotations, combine them and return quaternions.
2. Add body axes to the 3D viewer in camera_pose_lab.cpp using Viewer3D::addBodyAxes. Use the image number provided by the data element.
3. Compile and run. You should see the body axes for each image. The forward axis (is that x, y or z?) should point approximately towards the next pose. Check also that the other directions (right and down) look reasonable

2. Compute the pose of the camera

The next step is to convert the camera pose measurements to poses T_lc (pose of F_c relative to F_l).

You will need to finish CartesianPosition::toVector to return a translation vector between F_b and F_c. You can then use this translation and the corresponding SO(3) orientation from the camera's attitude to compute the SE(3) pose T_bc. This will let you compute and visualize the pose T_lc.

1. Convert the Cartesian position to a translation vector between F_b and F_c by finishing CartesianPosition::toVector in cartesian_position.cpp.
2. Construct the pose T_bc in the body coordinate system in camera_pose_lab.cpp.
3. Compute the pose T_lc in the local coordinate system in camera_pose_lab.cpp.
4. Add the camera axes to the 3D viewer in camera_pose_lab.cpp using Viewer3D::addCameraAxes.
5. Compile and run. You should see the smaller camera axes together with the body axes for each image. The optical axis (is that x, y or z?) should point approximately towards the origin of the local coordinate system, shown by the big axes in the center of the grid plane. Does the result look reasonable?

3. Undistort the images

Before we can project world points into the images, we need to undistort them. Why?

The calibration parameters have been read into Intrinsics objects. To use them, we need to finish Intrinsics::toCalibrationMatrix to return the camera calibration matrix K and Intrinsics::toDistortionVector to return an OpenCV distortion coefficient vector. We can then undistort the image using PerspectiveCameraModel. Take a look at the documentation in perspective_camera_model.h.

1. Finish Intrinsics::toCalibrationMatrix in intrinsics.cpp.
2. Finish Intrinsics::toDistortionVector in intrinsics.cpp. You should return a vector on the form [k₁, k₂, 0, 0, k₃], where k_i are the different radial distortion parameters.
3. Use the camera model to undistort the image in camera_pose_lab.cpp.
4. Compile and run. You will probably not notice any significant changes to the images, but if they still look reasonable, we are ready to continue!

4. Project a geographic world point into the images

We are now ready to project the position of the light pole into each image. Remember that the light pole is at the origin in the local coordinate system F_l.

First, we need to compute the camera projection matrix

in PerspectiveCameraModel::computeCameraProjectionMatrix.

Make certain that you are using the correct pose when you extract R and t. You might be interested in the se3::matrix3x4 member function.

We can then use the camera projection matrix to implement the perspective camera projection function in PerspectiveCameraModel::projectWorldPoint. Remember to use the homogenous representations in Eigen.

1. Finish PerspectiveCameraModel::computeCameraProjectionMatrix in perspective_camera_model.cpp.
2. Finish PerspectiveCameraModel::projectWorldPoint in perspective_camera_model.cpp.
3. Add the camera frustum to the 3D viewer using Viewer3D::addCameraFrustum in camera_pose_lab.cpp.
4. Compile and run. Does the camera frustums look reasonable? Is the light pole projected correctly into the images?

Congratulations! You have now established the connection between geographical coordinates and image pixels using the power of imaging geometry! Cool, right?

Extra

• Finish PerspectiveCameraModel::projectWorldPoints and project other geographical points into the images (see www.norgeskart.no).
• Create a virtual camera and project points into this camera.