Introduction to Visual SLAM: Chapter 2— 3D Rigid Body Motion
Hey there! Welcome back to our continuation on the SLAM series. Get ready because the topic for today is… 🥁🥁🥁 Body Geometry!! It’s not something I love, but we’ll have to get through it in our journey to become Visual SLAM experts!
Make sure you checked the previous chapter to get the most out of this one! Introduction to Visual SLAM: Chapter 1 — Introduction to SLAM.
I will assume you know some basics about 3D geometry and transformations and I will summarize the chapter into specific areas of interest. You can refer to the book for good explanations on that otherwise I might drive you crazy! Let’s get started with some interesting concepts!
Skew-symmetric matrix
The skew-symmetric operator (^) simply turns an outer vector product into a linear operation by transforming the first vector into a matrix. In the case of the dot product ‘c = a x b’, we can represent it such that ‘c = a ^ b’ where the vector ‘a’ is given as:
The main property of a skew-symmetric matrix is that it satisfies:
Coordinate Transforms
Coordinate transformations are quite a tricky part in robotics, but they are essential in the field! Just some quick notes on this, as I will not go through an entire explanation on space transforms.
The first thing to take into account is the two applications of a transformation. It can be used to transform one point by applying a rotation and translation, and the second application is to represent one point with respect to a different frame. Both applications will result in inverse transformation matrices if applied between the same frames.
In other words, the transformation from coordinate frame W to coordinate frame C is the same as the rotation of a point from frame C to frame W. Let’s see it in an image:
In here, the rotation matrix rotates the first frame around the X axis to get the second frame. This frame rotation matrix is inverse of the representation of a point in the second frame in the first frame. You can try and write it by yourself, and you will end up getting it ;)
As another example between coordinate transforms, let’s assume we have three coordinate systems, the world inertial system (W), robot 1 mobile frame (R1) and robot 2 mobile frame (R2), and the transformation matrices between W and R1, and between W and R2, T_R1_W and T_R2_W. Imagine we have a point, ‘p_R1’, observed from R1 frame, and we want to know how is it viewed from R2 frame. The point expressed in the R2 frame will be given by:
And T_W_R1 will be the inverse matrix of T_R1_W.
Note that transformation T cointains the rotation R (left blue box) and translation t (right blue box):
Got it? Okay time to move on!
Euler angles
We have already seen the rotation matrix representation in the previous section, but… have you ever heard of Euler Angles? Maybe something related to planes and that stuff right? The Euler angles provide an easy way to see the rotation represented in a three dimensional vector. The most used convention in robotics and aeronautics is Yaw — Pitch — Roll ([y, p, r]).
They are really easy to manipulate, but they have one big inconvenient, when pitch angle is +90º or -90º, we lose a degree of freedom! So that leads to a singularity adding a difficulty to our problem ughh… So how can we solve this?
Quaternions
May the magic begin! Quaternions are here to stay! As it’s not possible to find a 3-component rotation representation without a singularity, the quaternion adds a fourth component that works as the imaginary part of a complex number.
This quaternion can also be represented as
where ‘s’ acts as the imaginary part. This is how we rotate a point using quaternion representation:
In order to compose these transformations, we should now explore the different operations with quaternions:
- Addition and Substraction:
2. Multiplication:
3. Norm:
4. Conjugate:
5. Inverse:
6. Scalar Multiplication:
Transformations between representations
You might be wondering, okay everybody should agree on a rotation representation right? But what if this doesn’t happen? I’ve got good news for ya, it’s possible to transform between all these systems! Let’s see how to switch between representations.
- Quaternion to rotation matrix
2. Quaternion to rotation vector
Transformation properties
Sometimes, it is necessary to perform transformations outside of the Euclidean group (rotation + translation). You can see them summarized in the table below. Each one will keep the invariance of specific properties, so watch out for that! This is quite typical for image processing applications.
Eigen C++ Library
You might be wondering, I can write matrices by hand like a pro, but how can I use them in C++? Well… Eigen is the perfect library for that! You can find its installation guide in the official website, as it will help us working with matrices, transformations and geometries.
The book demonstrates some useful examples to learn about the library. You can find the code in the chapter 3 github repo.
It also shows a coordinate transform example that you can find below. Observe how the .pretranslate(t) method applies the translation component to the Euclidean transformation.
int main(int argc, char **argv) {Quaterniond q1(0.35, 0.2, 0.3, 0.1), q2(-0.5, 0.4, -0.1, 0.2);
q1.normalize();
q2.normalize();
Vector3d t1(0.3, 0.1, 0.1), t2(-0.1, 0.5, 0.3);
Vector3d p1(0.5, 0, 0.2);// Create homogeneous transformation matrix from rotation and translation
Isometry3d T1w(q1), T2w(q2);
T1w.pretranslate(t1);
T2w.pretranslate(t2);// Apply the transformation R1 to World, World to R2
Vector3d p2 = T2w * T1w.inverse() * p1;
cout << "p_R2 = T_R2_W * T_W_R1 * P_R1 = " << p2.transpose() << endl;return 0;}
Let’s…get…VISUAL!
Time to display something! Aren’t you excited? We are going to finally see some plots!! But wait… we still have to install Pangolin!
Pangolin is a set of lightweight and portable utility libraries for prototyping 3D, numeric or video based programs and algorithms. It is used quite widely in the field of Computer Vision as a means to remove platform-specific boilerplate and make it easy to visualize data. It’s based on OpenGL.
In order to install it, follow the GitHub guide and you should be good with that. Do not hesitate to look up any error in Google as there are sometimes C++ version incompatibility. You should end up having a directory usr/local/include/pangolin.
Make sure you download the trajectory text file also found in the chapter 3 github repo. As it already contains the code for the plotTrajectory script, I will only provide you with the CMakeLists.txt that helped me getting it to work. It took me a while to figure out how to include the pybind11::embed library.
cmake_minimum_required( VERSION 2.8 )project( geomtetry )set(CMAKE_CXX_FLAGS "-std=c++14")
include_directories( "usr/include/eigen3" )add_executable( eigenMatrix eigenMatrix.cpp )add_executable( useGeometry useGeometry.cpp )add_executable( coordinateTransform coordinateTransform.cpp )# Pangolinfind_package(Pangolin REQUIRED)
find_package(pybind11 REQUIRED)
include_directories( ${Pangolin_INCLUDE_DIRS} )
add_executable( plotTrajectory plotTrajectory.cpp)
target_link_libraries(plotTrajectory ${Pangolin_LIBRARIES} pybind11::embed)
The result should be something like this
You can play around with the parameters to see how they change the frame visualization.
Summary
Uuugh! What a long session! As you have seen these are the basics of rigid body geometry and trajectory visualization. The libraries in here will be extremely useful for future chapters on geometry operations, and in other robotics applications in general. Thank you so much for keeping up with the reading, and see you in the next chapter!