diff --git a/doc/g2o.pdf b/doc/g2o.pdf index 99a0baf8..1fd4224c 100644 Binary files a/doc/g2o.pdf and b/doc/g2o.pdf differ diff --git a/doc/g2o.tex b/doc/g2o.tex index 16120f6d..ba13f93c 100644 --- a/doc/g2o.tex +++ b/doc/g2o.tex @@ -307,7 +307,7 @@ \section{Least Squares Optimization} problems like SLAM or bundle adjustment. This may lead to sub-optimal solutions. In the remainder of this section we discuss first the general solution when the space of the parameters is Euclidean, and -subsequently we extend this solution to more general non-euclidean +subsequently we extend this solution to more general non-Euclidean spaces. \section{Considerations about the Structure of the Linearized System} @@ -994,7 +994,7 @@ \subsection{Identification of the State Variables} our landmark sensor is able to detect only the 2D pose of a landmark, but not its orientation. In other words the landmarks ``live'' in $\Re^2$. Conversely, the robot poses are parameterized by the robot -location $x-y$ on the plane and its orientation $\theta$, thus they +location $(x,y)$ on the plane and its orientation $\theta$, thus they belong to the group of 2D transformations $SE(2)$. More formally, the nodes of a 2D SLAM graph are of two types \begin{itemize} @@ -1037,8 +1037,8 @@ \subsection{Modeling of the Constraints} $\bx^\mathrm{s}_t$ to $\bx^\mathrm{s}_{t+1}$ \emph{measured} by the odometry. This measurement will be typically slightly different from the \emph{real} transformation between the two pose because of the -noise affecting the sensors. Being an odometry measurement an -euclidean transformation it is also a member of $SE(2)$ group. +noise affecting the sensors. Being an odometry measurement, an +Euclidean transformation, it is also a member of $SE(2)$ group. Assuming the noise affecting the measurement being white and Gaussian, it can be modeled by an $3 \times 3$ symmetric positive definite information matrix. In real applications the entries of this matrix @@ -1057,7 +1057,7 @@ \subsection{Modeling of the Constraints} If the robot senses a landmark $\bx^\mathrm{l}_i$ from the location $\bx^\mathrm{s}_t$, the corresponding measurement will be modeled by an edge going from the robot pose to the landmark. A measurement about -the landmark consists in a point in the $x-y$ plane, perceived in the +the landmark consists in a point in the $x$-$y$ plane, perceived in the robot frame. Thus a landmark measurement lives in $\Re^2$ as the landmarks do. Again, under white Gaussian noise assumption, the noise can be modeled by its inverse covariance. Accordingly, an edge between @@ -1085,7 +1085,7 @@ \subsection{Choice of the Parameterization for the Increments} \bx^\mathrm{l}_i \boxplus \bTDeltax^\mathrm{l}_i & \doteq & \bx^\mathrm{l}_i + \bTDeltax^\mathrm{l}_i \end{eqnarray} -The poses, conversely, live in the non euclidean space $SE(2)$. +The poses, conversely, live in the non-Euclidean space $SE(2)$. This space admits many parameterizations. Examples include: rotation matrix $\bR(\theta)$ and translation vector $(x \; y)^T$ or angle $\theta$ and translation vector $(x \; y)^T$. @@ -1097,14 +1097,16 @@ \subsection{Choice of the Parameterization for the Increments} three scalar parameters $x$, $y$ and $\theta$ of a pose as if they were a vector, and define the $\boxplus$ as the vector sum. There are many reasons why this is a poor choice. One of them is that the angles -are not euclidean, and one would need to re-normalize them after every +are not Euclidean, and one would need to re-normalize them after every addition. A better choice is to define the $\boxplus$ between a pose and a pose increment as the motion composition operator. Namely, given a robot pose $\bx^{s}_t = (x\; y\; \theta)^T$ and an increment $\bTDeltax^{s}_t -= (\Delta x\; \Delta y\; \Delta \theta)^T$, the operator can be defined as -follows: += (\Delta x\; \Delta y\; \Delta \theta)^T$, where $\Delta x$ is the +longitudinal displacement (i.e. in direction of the heading of the robot), +$\Delta y$ the lateral displacement and $\Delta \theta$ the rotational +change, the operator can be defined as follows: \begin{eqnarray} \bx^\mathrm{s}_t \boxplus \bTDeltax^\mathrm{s}_t & \doteq & \left( @@ -1133,7 +1135,7 @@ \subsection{Choice of the Parameterization for the Increments} \subsection{Design of the Error Functions} The last step in formalizing the problem is to design error functions -$\be(bx_k)$ that are ``reasonable''. A common way do is to define a +$\be(\bx_k)$ that are ``reasonable''. A common way to do this to define a so-called \emph{measurement} function $\bh_k(\bx_k)$ that ``predicts'' a measurement $\hat \bz_k$, given the knowledge of the vertices in the set $\bx_k$. Defining this function is usually rather easy, and can be @@ -1141,7 +1143,7 @@ \subsection{Design of the Error Functions} vector can be computed as the vector difference between the prediction $\hat \bz_k$ and the real measurement. This is a general approach to construct error functions and it works when the space of the errors is -locally euclidean around the origin of the measurement. If this is not +locally Euclidean around the origin of the measurement. If this is not the case one might want to replace the vector difference with some other operator which is more ``regular''. @@ -1151,17 +1153,20 @@ \subsection{Design of the Error Functions} $\bh^\mathrm{l}_{t,i}(\bx^\mathrm{s}_t, \bx^\mathrm{l}_i)$ that computes a ``virtual measurement''. This virtual measurement is the position of the landmark $\bx^\mathrm{l}_i$, seen from the robot position -$\bx^\mathrm{s}_t$. The equation for $\bh^\mathrm{l}_{t,i}(\cdot)$is the +$\bx^\mathrm{s}_t$. The equation for $\bh^\mathrm{l}_{t,i}(\cdot)$ is the following: \begin{eqnarray} - \bh^\mathrm{l}_{t,i}(\bx^\mathrm{s}_t, \bx^\mathrm{l}_i) & \doteq & - \left( + \bh^\mathrm{l}_{t,i}(\bx^\mathrm{s}_t, \bx^\mathrm{l}_i) & \doteq & + \left( \begin{array}{c} (x^\mathrm{s}_t - x_i) \cos \theta^\mathrm{s}_t + (y^\mathrm{s}_t - y_i) \sin \theta^\mathrm{s}_t \\ - - (x^\mathrm{s}_t - x_i) \sin \theta^\mathrm{s}_t + (y^\mathrm{s}_t - y_i) \cos \theta^\mathrm{s}_t + - (x^\mathrm{s}_t - x_i) \sin \theta^\mathrm{s}_t + (y^\mathrm{s}_t - y_i) \cos \theta^\mathrm{s}_t \end{array} \right) \end{eqnarray} +which converts the position of the landmark into the coordinate system of +the robot. + Since the landmarks live in an Euclidean space, it is reasonable to compute the error function as the normal vector difference. This leads to the following definition for the error functions of the @@ -1182,7 +1187,7 @@ \subsection{Design of the Error Functions} $\bx^\mathrm{s}_t$ to $\bx^\mathrm{s}_{t+1}$, that is the ``ideal'' odometry. Once again the error can be obtained as a difference between the measurement and the prediction. However, since our measurements -do not live in an euclidean space we can use the $\ominus$ instead of the vector difference. +do not live in an Euclidean space we can use $\ominus$ instead of the vector difference. \begin{eqnarray} \be^\mathrm{s}_{t,t+1}(\bx^\mathrm{s}_t, \bx^\mathrm{s}_{t+1}) & \doteq & \bz_{t,t+1} \ominus \bh^\mathrm{s}_{t,t+1}(\bx^\mathrm{s}_t, \bx^\mathrm{s}_{t+1}). @@ -1219,14 +1224,14 @@ \subsection{Putting things together} we define an \verb+operator*(...)+ that implements the motion composition operator $\oplus$, and an \verb+inverse()+ function that returns the inverse of a transformation. For convenience we also implement an -\verb+operator *+ that transforms 2D points. To convert the elements -from and to a minimal representation that utilizes an $Eigen::Vector3d$ +\verb+operator*+ that transforms 2D points. To convert the elements +from and to a minimal representation that utilizes an \verb+Eigen::Vector3d+ we define the methods \verb+fromVector(...)+ and \verb+toVector(...)+. The constructor initializes this class as a point in the origin oriented at 0 degrees. Note that having a separate class for a group is not mandatory in \gopt, but makes the code much more readable and reusable. The corresponding C++ class is reported in -Listing~\ref{lst:se2} +Listing~\ref{lst:se2}. \begin{lstlisting}[float,label=lst:se2,caption=\text{Helper class to represent $SE(2)$}.] class SE2 { public: @@ -1279,7 +1284,8 @@ \subsection{Putting things together} vertices. To this end we extend the \verb+BaseVertex<>+ class, and we derive the classes \verb+VertexSE2+ to represent a robot pose and \verb+VertexPointXY+ to represent a point landmark in the plane. -The class definition is reported in Listing~\ref{lst:vertexse2} +The class definition for robot pose vertices is reported in +Listing~\ref{lst:vertexse2}. The pose-vertex extends a template specialization of \verb+BaseVertex<>+. We should say to \gopt{} that the internal type has dimension 3 and that the estimate is of type \verb+SE2+. This @@ -1293,7 +1299,7 @@ \subsection{Putting things together} argument into an \verb+SE(2)+, then we multiply this increment at the right of the previous estimate. After that we should implement the read and write functions to a stream, but this is straight-forward and -you can look it up yourself in the code. +you can look it up yourself in the code. \begin{lstlisting}[float,label=lst:vertexse2,caption=Vertex representing a 2D robot pose]