This chapter describes the work that was specifically developed to fulfill this dissertation's objectives, which include hardware modifications made to the existing \gls{ptu} and the tools and methods used to estimate verticality and then stabilize the \gls{ptu} (humanoid head).

\section{Hardware modifications to the PTU}
\label{sec:modifications}

The existing \gls{ptu} was built with the purpose to track a given target when the relative position between the \gls{ptu} and the target changes \cite{sousa_visual_2016}. However, this objectives of this work are different. After being able to estimate verticality using either visual or inertial data, the goal is to stabilize the unit where the camera is mounted on, so that the obtained image is aligned with the gravity vector (the ground would have to be parallel to the x-axis of the image). As such, modifications to the existing \gls{ptu} had to be made to turn it into a \gls{rtu}, since the degree of freedom associated with the pan angle is irrelevant when trying to balance a humanoid robot.

%\begin{figure}[H]
%	\centering
%	\includegraphics[width=0.4\textwidth]{./Imagens/image-missing-md.png}
%	\caption{A visual explanation of why the pan angle is irrelevant when trying to balance a humanoid robot. Taken from \cite{}.}
%	\label{fig:pan_explanation}
%\end{figure}



By making the roll and tilt angles variable, it would then be possible to send commands to the servomotors present in the \gls{rtu} so that the camera (and the image it records) would be aligned with the gravity vector.

Therefore, a structure made of aluminum was designed in order to change the degrees of freedom of the unit where the servomotors were mounted. This part had to fix itself on the previously designed parts and it was made to be as light and as short as possible, so that the torque the servomotors had to apply would be minimum. The camera's optical axis was also made to intersect both of the servomotor's axis (see Fig. \ref{fig:axis}), so that when the motors apply a rotation, the camera would stay in the same place. The technical drawings associated with this alteration are present in Appendix A (\gls{ptu} to \gls{rtu} transformation section).

\begin{figure}[H]
	\centering
	\includegraphics[width=\textwidth]{./Imagens/axis-intersection.PNG}
	\caption{Visual representation of the axes in the RTU. ``Axis1'' corresponds to the roll servo axis and the camera's optical axis and ``Axis2'' corresponds to the tilt servo axis.}
	\label{fig:axis}
\end{figure}

It is important to note that from this point on in the dissertation, the unit where the camera is mounted will now be called a \gls{rtu} instead of a \gls{ptu}. Besides this transformation from a \gls{ptu} to \gls{rtu}, some of the mechanical parts had to be replaced, because some screw threads were deteriorated from excessive use. To rebuild these parts, the drawings on Appendix A of \cite{sousa_visual_2016} were used.

\section{ROS packages}

The developed software was organized in different \gls{ros} packages, so that if another work needs to only use a specific part of the solution, it is possible to do it. Note that many \gls{ros} packages used in this dissertation are needed for the remaining packages to work and are therefore considered dependencies.

%(dependencies are listed in Appendix B)

What follows is a list of the packages, and a brief explanation of the nodes developed specifically by the author of this work:


\subsubsection{\texttt{head\_stabilization} package}

This package contains the \texttt{motor\_control} node which subscribes a \gls{ros} topic with the measured error in position and performs calculations based on a \gls{pd} control to command the \gls{rtu}'s servomotors to a certain angular position, with a given angular speed. It also reads the motors' current position and publishes it to a \gls{ros} topic.

The package also contains the \texttt{pose\_estimation} node, which subscribes to \gls{ros} topics containing coordinates of a chessboard square's corners, if there is a chessboard present in the camera image. It then calculates the geometrical transformation between the camera in the \gls{rtu} and a chessboard present in the camera's image, allowing the user to know the chessboard's roll and tilt angles relative to the camera.
	 
\subsubsection{\texttt{image\_converter} package}

This package contains the \texttt{image\_converter} node which subscribes to an image message and performs an undistortion of the image, based on the parameters found when previously calibrating the camera. It also detects the presence of a chessboard in the image and calculates the chessboard square's corner positions. It then publishes some of these point coordinates into different \gls{ros} topics, which can then be used by other nodes to, for example, initialize tracking operations.

\subsubsection{\texttt{movingedges} package}

This package contains the \texttt{movingedges} node which also subscribes to \gls{ros} topics containing the coordinates of a chessboard which is present on the camera image. It then performs the tracking of a line made by the connection of these points, allowing the user to know the chessboard's roll angle. It then publishes this angle into another \gls{ros} topic.

\section{Analyzing an image to estimate verticality}

After some time was spent studying the problem at hand it was possible to conclude that there are mainly two ways to estimate verticality through an image.

The first way implies doing image analysis when nothing is known about the objects in the scene. However, this comes with a problem: if there are no special features to analyze in the image (e.g. the robot is staring at a blank wall), then there is no way to actually perform an estimation of verticality. In the previous example, the robot may even be falling to its side, but since there is nothing worth analyzing on the image captured by the camera, one would have to resort to inertial data in order to prevent the robot's fall.

The second way implies having a known object present in the image and also to know the object orientation in relation to the environment. This was the way explored in this dissertation and in order to do so a chessboard previously used to perform camera calibrations was the object used (Fig. \ref{fig:chessboard}).

\begin{figure}[H]
	\centering
	\includegraphics[width=\textwidth]{./Imagens/chessboard.png}
	\caption{The chessboard used as a known object. Taken from \cite{matos_development_2016}.}
	\label{fig:chessboard}
\end{figure}

If, for example, the chessboard is aligned with the ground in the real world, then it is easy to also know the camera orientation in relation to the ground. To do it, one would just have to obtain the chessboard orientation in relation to the camera by analyzing the image, and the camera orientation to the ground would be that same value.

In order to obtain the chessboard orientation, two methods were used (section \ref{sec:methods}). This is because the first method would only be able to obtain the roll orientation while the second method made it possible to obtain both the roll and tilt orientation of the chessboard, and therefore the camera roll and tilt orientation too.

\section{Image acquisition and preprocessing}

In order to obtain images from the camera in a format which is transferable in the \gls{ros} framework, a node built in a previous work \cite{silva_multisensor_2016} was used. This node is called \texttt{Point\_Grey} and its source file is \texttt{pointgrey\_fl3-ge-28s4-c\_driver.cpp} located in the \texttt{pointgrey\_fl3\_ge\_28s4\_c} \gls{ros} package.

This node also configures the camera's parameters, therefore allowing the user to choose the desired image format. The parameters used in the work where this node was created also fit the needs of this dissertation, so they were kept the same. As such, the video mode used was mode 1 with a 964x728 resolution which allows for a maximum of 14 frames per second (FPS) \cite{noauthor_flir_2017}(page 77).

After retrieving the next image frame into an \gls{opencv} image, this node converts it to a \gls{ros} image message using the \texttt{cv\_bridge} \gls{ros} package (Fig. \ref{fig:cv_bridge}). The resulting image message is published on a topic named \texttt{/RawImage}, which can then be used by other nodes to acess the image and analyze it.

\begin{figure}[H]
	\centering
	\includegraphics[width=0.4\textwidth]{./Imagens/cvbridge.png}
	\caption{\texttt{cv\_bridge} application example. Taken from \cite{noauthor_cv_bridge_nodate}.}
	\label{fig:cv_bridge}
\end{figure}

 Since both of the methods used to estimate verticality need some information of where the chessboard is in the image in order to be initialized, the image processing needed to obtain the chessboard corners is performed on the \texttt{image\_converter} node, described next.

%\newpage
\subsubsection{\texttt{image\_converter} node}

\begin{sloppypar}
	The source file of this node is \texttt{image\_converter.cpp} and it is located in the \texttt{image\_converter} \gls{ros} package. Its main function is to provide other nodes with the coordinates of the internal chessboard corners present in an image so they can begin tracking (or other functions). In order to do so, it subscribes to the previously mentioned \texttt{/RawImage} topic, which contains the image captured by the camera in the \gls{ros} image message format. Then it converts that image message back into an actual \gls{opencv} image, using the \texttt{cv\_bridge} \gls{ros} package once again. Firstly, the image was undistorted based on the intrinsic parameters and distortion coefficients of the camera. To obtain these values, the camera was calibrated using the \texttt{camera\_calibration} \gls{ros} package (Fig. \ref{fig:calib}). In order to perform this calibration, the tutorial found in \cite{noauthor_camera_calibration/tutorials/monocularcalibration_nodate} was used. The results of the calibration are shown in Table \ref{tab:calib};
\end{sloppypar}

\begin{figure}[H]
	\centering
	\includegraphics[width=0.8\textwidth]{./Imagens/camera_calib.png}
	\caption{A screenshot of the camera calibration process. Taken from \cite{silva_multisensor_2016}}
	\label{fig:calib}
\end{figure}
\newpage
\begin{table}[H]
	\centering
	\caption{Intrinsic parameters and distortion coefficients obtained from camera calibration.}
	\label{tab:calib}
	\begin{tabular}{lr}
		\toprule \textbf{Parameters} & \textbf{Values (in pixels)} \\ \midrule
		$f_x$ & 503.406178 \\
		$f_y$ & 504.095921 \\
		$c_x$ & 488.641027 \\
		$c_y$ & 362.208972 \\ \midrule
		\textbf{Distortion coefficients} & \textbf{Values} \\ \midrule
		$k_1$ & -0.301046 \\
		$k_2$ & 0.069974 \\
		$p_1$ & 0.000667 \\
		$p_2$ & -0.000580 \\
		$k_3$ & 0.000000 \\ \bottomrule
	\end{tabular}
\end{table}
\noindent where $f_x$ and $f_y$ are the focal length in pixels;\\
$c_x$ and $c_y$ are the optical centers in pixels;\\
$k_1,k_2$ and $k_3$ are radial distortion coefficients;\\
and $p_1$ and $p_2$ are tangential distortion coefficients.

After removing the distortion with the \texttt{undistort()} \gls{opencv} function, the image was converted from the RGB to the grayscale space so that the \texttt{findChessboardCorners()} \gls{opencv} function could be used.\\

\noindent \textbf{Syntax:} bool \textbf{findChessboardCorners}(InputArray \textbf{image}, Size \textbf{patternSize}, OutputArray \textbf{corners}, int \textbf{flags})\\

\noindent
\begin{tabularx}{\textwidth}{l>{\raggedright\arraybackslash}X}
\textbf{Parameters:}& $\bullet$ \textbf{image} is an 8-bit grayscale or color image.;\\
					& $\bullet$ \textbf{patternSize} is the number of points per row and column of the chessboard in the CvSize format;\\
					& $\bullet$ \textbf{corners} is the output array where the corner coordinates are stored;\\
					& $\bullet$ \textbf{flags} is an integer which determines if certain optional operations are performed.
\end{tabularx}


\noindent This function outputs a non-zero value if all the corners are found, and zero otherwise.

After using the \texttt{findChessboardCorners()} function, a simple representation of the most important points are displayed on the screen (Fig. \ref{fig:chess_points}).

\begin{figure}[t]
	\centering
	\includegraphics[width=\textwidth]{./Imagens/chesspoints.png}
	\caption{The chessboard points used to initialise both tracking methods.}
	\label{fig:chess_points}
\end{figure}

The points are then published into separate topics, each topic containing the x and y coordinates of a corner. These topics contain a \gls{ros} message of the type \texttt{Num}, which was created specifically for this work. Its definition is made in the \texttt{Num.msg} file, located in the \texttt{msg} folder of the \texttt{image\_converter} package. Messages of this type can contain two numbers of the built-in float64 type, which are equivalent to a double in the C++ language.

The process described above is repeated for every new frame that arrives from the \texttt{/RawImage} topic.

\section{ViSP tracking methods}\label{sec:methods}
\subsection{Line Tracking method}

The first method of verticality estimation consists of using the line tracking capabilities of the \gls{visp} library. \gls{visp} contains a tracker for moving edges which can track either lines or ellipses. To initiate line tracking, it is necessary to provide two points belonging to the line to be tracked. \gls{visp} allows for these points to be provided manually with the user's interaction (selecting the two points with the mouse in a screenshot of the camera feed) or automatically by providing the coordinates in the image of these two points. The points used were points one and two present in Figure \ref{fig:chess_points}. These two points form a line in the chessboard (which is made of the sides of the chessboard squares) and the tracking itself is done on the \texttt{movingedges} node, described next.

\subsubsection{\texttt{movingedges node}}

The source file of this node is \texttt{movingedges.cpp} and it is located in the \texttt{movingedges} \gls{ros} package. Its main function is to track a line present in an image and publish that line orientation to be used by other nodes.

The tracking is made by first subscribing to the \texttt{/RawImage} topic and then initializing the moving edges parameters with the lines of code shown in code snippet \ref{code:meparams}.
	
\begin{lstlisting}[caption=Moving edges parameters initialization, label=code:meparams]
vpMe me;
me.setRange(10);
me.setThreshold(45000);
me.setSampleStep(3);
int nbLines = 1;
vpMeLine line[nbLines];
\end{lstlisting}


\noindent where \textbf{me} is the moving edges object of the vpMe class;\\
\textbf{setRange} is a method of the vpMe class which sets the seek range (in pixels) on both sides of the tracked line;\\
\textbf{setThreshold} is a method of the vpMe class which determines if the moving edge is valid or not;\\
\textbf{setSampleStep} is a method of the vpMe class which sets the minimum distance (in pixels) between two line points.\\
\textbf{nbLines} is the number of lines to track;\\
\textbf{line} is the tracker object of the vpMeLine class;\\

The values used above were the ones that delivered the best results after testing a wide range of different values. After initializing the moving edges parameters, the tracker is initialized with the instructions presented in code snippet \ref{code:tracker_init}.
\begin{lstlisting}[caption=Line tracker initialization, label=code:tracker_init]
line[0].setMe(&me);
line[0].setDisplay(vpMeSite::RANGE_RESULT);
do{
	cout << "Didn't find the chessboard points, retrying.." << endl;
	usleep(1000000);
}while( (pt1.get_i()==0 && pt1.get_j()==0) && (pt2.get_i()==0 && pt2.get_j()==0) );

line[0].initTracking(I,pt1,pt2);

line[0].display(I, vpColor::red) ;
\end{lstlisting}

In code snippet \ref{code:tracker_init}, the first two lines associate the tracker object with the moving edges parameters and allow it to display additional information on the image. Afterwards, the node awaits the reception of the coordinates of the points used to initialize the tracking, since without them, the tracker object wouldn't start and the node would shutdown.

If the point coordinates are received, the tracking is initialized and the tracking results are displayed on the image with a red color (Fig. \ref{fig:linetracking}).

\begin{figure}[H]
	\centering
	\includegraphics[width=\textwidth]{./Imagens/linetracking.png}
	\caption{Results of the ViSP line tracking method.}
	\label{fig:linetracking}
\end{figure}

The angle that the tracked red line does with the x-axis is equal (in absolute value) to the roll orientation of our camera. The roll angle is then published to a \gls{ros} topic named \texttt{/angle} which contains a message of the \texttt{Num} type, defined previously in the \texttt{image\_converter} package.

\subsection{Pose Estimation method}

The second method of verticality estimation consists of using the pose estimation capabilities of the \gls{visp} library. \gls{visp} contains a module that can estimate the pose (i.e. geometrical transformation) of an object in relation to the camera which is recording the tracked object. To initiate pose estimation, it is necessary to provide four points that belong to the plane of the object and their real world coordinates as well. The points used were points three, four, five and six present in Figure \ref{fig:chess_points}. The method itself is applied on the \texttt{pose\_estimation} node, described next.

\subsubsection{\texttt{pose\_estimation} node}

\begin{sloppypar}
	The source file of this node is \texttt{pose\_estimation.cpp} and it is located in the \texttt{head\_stabilization} \gls{ros} package. Its main function is to estimate the pose of the chessboard present in \gls{lar} (since the 3D coordinates stored belong to this chessboard in particular) in relation to the camera which is recording it. It then publishes that chessboard's roll and tilt angles into a \gls{ros} topic.
\end{sloppypar}

The pose estimation is made by first subscribing to the \texttt{/RawImage} topic and then initializing some variables, as shown in code snippet \ref{code:variable_init}.

\begin{lstlisting}[caption=Variable initialization for pose estimation method, label=code:variable_init]
// Parameters of our camera
vpCameraParameters cam(503.406178, 504.095921, 488.641027, 362.208972);

// The pose container
vpHomogeneousMatrix cMo;

std::vector<vpPoint> point;
double W = 0.37;
double H = 0.27;
point.push_back( vpPoint(-W, -H, 0) );
point.push_back( vpPoint( W, -H, 0) );
point.push_back( vpPoint( W,  H, 0) );
point.push_back( vpPoint(-W,  H, 0) );
\end{lstlisting}

To perform a pose estimation, the intrinsic camera parameters are necessary, as well as inputting four real world coordinates of chessboard corners. The points that are input here must correspond to the points whose pixel coordinates are supplied by the \texttt{image\_converter} node. Since the chessboard measures about 74 centimeters horizontally from point 3 to point 4 and about 54 centimeters vertically from point 3 to point 6, and we want the object reference frame to be in the center of the chessboard, the points were created as shown in code snippet \ref{code:variable_init}. Point 3 (from Fig. \ref{fig:chess_points}) is going to correspond to point[0], whose coordinates are (-0.37,-0.27). Point 4 corresponds to point[1] and so on. An important thing to note is that points 3 and 4 have corresponding negative y-axis values because it is a common conception that the y-axis grows from the top to the bottom in an image.

The \texttt{cMo vpHomogeneousMatrix} object is the homogeneous matrix where the pose estimation is going to be stored. After these variable initializations, the \texttt{computePose()} function is called as shown in code snippet \ref{code:compute_call}.

\begin{lstlisting}[caption=Call of the \texttt{computePose()} function, label=code:compute_call]
computePose(point, dot, cam, first_time, cMo);
\end{lstlisting}
\newpage
The \texttt{computePose()} function is defined as shown in code snippet \ref{code:compute_def}.

\begin{lstlisting}[caption=Definition of the \texttt{computePose()} function, label=code:compute_def]
void computePose(std::vector<vpPoint> &point, const std::vector<vpImagePoint> &dot, const vpCameraParameters &cam, bool first_time, vpHomogeneousMatrix &cMo){
	vpPose pose;     double x=0, y=0;
	for (unsigned int i=0; i < point.size(); i ++) {
		vpPixelMeterConversion::convertPoint(cam, dot[i], x, y);
		point[i].set_x(x);
		point[i].set_y(y);
		pose.addPoint(point[i]);
	}

	if (first_time == true) {
		vpHomogeneousMatrix cMo_dem;
		vpHomogeneousMatrix cMo_lag;
		pose.computePose(vpPose::DEMENTHON, cMo_dem);
		pose.computePose(vpPose::LAGRANGE, cMo_lag);
		double residual_dem = pose.computeResidual(cMo_dem);
		double residual_lag = pose.computeResidual(cMo_lag);
		if (residual_dem < residual_lag)
			cMo = cMo_dem;
		else
			cMo = cMo_lag;
	}
	pose.computePose(vpPose::VIRTUAL_VS, cMo);
}
\end{lstlisting}


In short, this function converts each of the four points from pixel coordinates to meters by using \texttt{vpPixelMeterConversion::convertPoint()}, updates each points real world coordinates and adds them to the \texttt{pose} object of the \texttt{vpPose} class. After all points are added to the \texttt{pose} object, the pose is estimated, and is stored in the \texttt{cMo} homogeneous matrix. If it is the first time this function is called, a first estimation of the pose is made by two different approaches: the \texttt{vpPose::DEMENTHON} and the \texttt{vpPose::LAGRANGE} approaches. The approach which produces the best result, (measured by \texttt{vpPose::computeResidual()}) will be the one chosen as the first estimation.


After the pose estimation has been made, the results are displayed in the screen (Fig. \ref{fig:pose_estimation}) and the roll and tilt angles of the chessboard are extracted from the pose matrix and published on a \gls{ros} topic named \texttt{/angle} which contains a message of the \texttt{Num} type, defined previously in the \texttt{image\_converter} package.

\begin{figure}[H]
	\centering
	\includegraphics[width=\textwidth]{./Imagens/pose_estimation.png}
	\caption{Results of the ViSP pose estimation method.}
	\label{fig:pose_estimation}
\end{figure}


\section{Stabilizing the RTU (Humanoid Head)}\label{sec:stabilization}

After obtaining an estimation of the roll and tilt camera angles in relation to the ground, it is necessary to stabilize the \gls{rtu} using the mounted servomotors. The initial approach was to control both of the servomotors and read the inertial sensors through the same Arduino board. However, when implementing a solution with the Arduino Servo library, it was discovered that the \texttt{read()} function only returned the value of the last call to \texttt{write()}. As such, it doesn't return the actual current position of the servomotor, which is absolutely necessary for a robust control algorithm. Additionally, when receiving the inertial readings through serial communication, there seemed to be some spike in voltage in the PWM pins, which would make the servomotors jitter for an instant.

With the problems described above, it was decided to use the same infrastructure that was previously used to control the servomotors. All the details on how to connect each component to each other are described in section 3.1 of \cite{cruz_haptic_2012}, while all the functions used to control the servomotors (e.g. read position, set position, read servo ID, set speed...) were developed by the same author in a \gls{ros} package named \texttt{hitec5980sg}, available in \gls{lar} toolkit v4 \cite{noauthor_lar_nodate}. Two functions that are important for the understanding of this work will now be described:\\

\noindent \textbf{Syntax:} short unsigned int hitec\_5980SG::\textbf{SetPosition}(int \textbf{id},int \textbf{position})\\

\noindent
\begin{tabularx}{\textwidth}{l>{\raggedright\arraybackslash}X}
	\textbf{Parameters:}& $\bullet$ \textbf{id} is the id of the servomotor;\\
	& $\bullet$ \textbf{position} is the position value to which the user wants to send the servo to (600-2400).\\
\end{tabularx}\\

\noindent The \texttt{SetPosition()} function returns a response from the servo, if the message was well received, or 0xFFFF if an error ocurred.\\

\noindent \textbf{Syntax:} short unsigned int hitec\_5980SG::\textbf{SetSpeedPosition}(int \textbf{id},int \textbf{speed})\\

\noindent
\begin{tabularx}{\textwidth}{l>{\raggedright\arraybackslash}X}
	\textbf{Parameters:}& $\bullet$ \textbf{id} is the id of the servomotor;\\
	& $\bullet$ \textbf{speed} is the speed value the user wants the servo to go with to the next position (0-255).\\
\end{tabularx}\\

\noindent The \texttt{SetSpeedPosition()} function returns a response from the servo, if the message was well received, or 0xFFFF if an error ocurred. This response also includes the current position of the servomotor.

Before beginning to describe the node that implements the stabilization process itself, it is important to introduce some specific characteristics of these servomotors and their range of motion. The servomotor responsible for altering the roll angle has a full span of 180\textdegree. However, the functions developed in the \texttt{hitech5980sg} package only accept position values from 600 $\mu$s to 2400 $\mu$s (pulse width control). The 600 $\mu$s position corresponds to the -90\textdegree\space position and 2400 $\mu$s corresponds to +90\textdegree\space position. This means that a 1\textdegree\space increase signifies an increase of 10 units in the servomotors own unit system.
Furthermore, the servomotor responsible for altering the tilt angle only has about 40\textdegree\space of range motion, counting from the +90\textdegree\space position, which means that its position values can range from 2000 $\mu$s to 2400 $\mu$s. This happens because of the way that the servomotor is installed in the \gls{rtu} and reducing the position values any further would imply that some parts collide with each other, which is not admissible.

It was also intended that both servos could be operated at the same time, for stabilizing movements which were composed of both a roll and a tilt component at the same time. However, this proved impossible since with the current \gls{rtu} setup, the roll servomotor also rotates the tilt servomotor. To better understand this problem, three representations of the camera in a scene with a global reference frame are shown in Figure \ref{fig:composite_movement}. When the tilt servo is actuated and the roll servo is on a non-zero position, the camera's optical axis leaves the $y=0$ plane, which is undesirable. As such, the stabilization process can only be done to one servo at a time.

\begin{figure}[H]
	\centering
	\includegraphics[width=\textwidth]{./Imagens/explicacao_v2.png}
	\caption{An explanation to why a change in both the roll and tilt angles makes the camera's optical axis leave the y=0 plane.}
	\label{fig:composite_movement}
\end{figure}

To further explain the stabilization process implemented in the \texttt{motor\_control} node, it will be described next.

\subsubsection{\texttt{motor\_control} node}\label{sec:motor_control}

\begin{sloppypar}
	The source file of this node is \texttt{motor\_control.cpp} and it is located in the \texttt{head\_stabilization} \gls{ros} package. Its main function is to stabilize the \gls{rtu} by controlling the servomotors' position and speed. It then publishes both of the servomotor's current positions into a \gls{ros} topic. It has a roll and a tilt mode, depending on the angle which is going to be stabilized.
\end{sloppypar}

A first version of this node implemented a control algorithm with just a proportional component. However, this proved to be insufficient as will be concluded in the experiments in section \ref{sec:prop_eval}. Nevertheless, a short explanation of how it worked will be described in the following paragraph.

First, this node retrieves the roll and tilt angle errors from the \texttt{/angle} \gls{ros} topic, and then a calculation of the roll motor's position setpoint is done, as shown in equation (\ref{eq:old_control}).

\begin{equation} \label{eq:old_control}
\theta^{1}_\mathrm{setpoint}=\theta^{1}_\mathrm{error}\times10+\theta^{1}_\mathrm{current}
\end{equation}

\noindent In equation (\ref{eq:old_control}), $\theta^{1}$ stands for the position of the motor which is responsible for the roll angle stabilization. In an example where the current motor position is 1600 $\mu$s and the roll error is -10\textdegree, the resulting setpoint would be 1500 $\mu$s, which would mean that the servo was on its 10\textdegree\space position and the setpoint is now 0\textdegree. After this setpoint was calculated, if the setpoint was within admissible position values (600 $\mu$s to 2400 $\mu$s), the servomotor was simply sent to the resulting position via the \texttt{hitec\_5980SG::SetPosition()} function.

This first control method worked well for slower motor speeds, but when setting motor speeds higher, overshoots would occur, resulting in general instability of the image (See section \ref{sec:prop_eval} for more details).

To solve these problems, a more refined control algorithm had to be implemented. As the final solution to the \gls{rtu} stabilization problem, a \gls{pd} control algorithm was used. Some small tests were made to see if an integrative component should be used, however it always resulted in a worse response from the system.

To determine the position setpoint for the servo responsible for the roll angle (using the roll mode), equation (\ref{eq:PD_m1control}) was used.

\begin{equation} \label{eq:PD_m1control}
\theta^{1}_\mathrm{setpoint}=(\theta^{1}_\mathrm{Pterm}+\theta^{1}_\mathrm{Dterm})\times10+\theta^{1}_\mathrm{current}
\end{equation}

\noindent In equation (\ref{eq:PD_m1control}), $\theta^{1}_\mathrm{Pterm}$ is calculated using equation (\ref{eq:m1pterm}) and $\theta^{1}_\mathrm{Dterm}$ is calculated using equation (\ref{eq:m1dterm}).

\begin{equation} \label{eq:m1pterm}
\theta^{1}_\mathrm{Pterm}=K_p \times \theta^{1}_\mathrm{error}
\end{equation}

\begin{equation} \label{eq:m1dterm}
\theta^{1}_\mathrm{Dterm}=K_d \times (\theta^{1}_\mathrm{error}-\theta^{1}_\mathrm{prev-error})
\end{equation}

In equation (\ref{eq:m1dterm}), $\theta^{1}_\mathrm{prev-error}$ is the roll error that happened in the previous iteration of the control sequence. $K_p$ and $K_d$ are the proportional and derivative coefficients, and their best values, after extensive testing (section \ref{sec:pd_eval}), are $K_p=0.7$, $K_d=0.15$ when using the line tracking method and $K_p=0.5$, $K_d=0.4$ when using the pose estimation method.

\begin{sloppypar}
The servo responsible for the roll angle was then sent to this setpoint, if the setpoint was within admissible position values (600 $\mu$s to 2400 $\mu$s), using the aforementioned \texttt{hitec\_5980SG::SetPosition()} function, with its maximum speed set by the \texttt{hitec\_5980SG::SetSpeedPosition()} function.
\end{sloppypar}

For the control of the servo which is responsible for the tilt angle, a \gls{pd} control algorithm was also used, but this time the control variable would be the servomotor's speed and not its position. This is because when trying to apply equation (\ref{eq:PD_m1control}) to the tilt servo, it would not always respond. A probable cause for this is the excessive torque it had to do in order to lift the camera, since the distance between the camera's center of gravity and the servo axis (moment arm) was too big, despite the efforts to minimize such distance.

The solution to this problem is sending the servo to one of its extreme positions (2000 $\mu$s or 2400 $\mu$s), depending on the sign of the tilt error, but with a varying velocity, depending on the magnitude of this error. To determine the speed setpoint for the servo responsible for the tilt angle (using the tilt mode), equation (\ref{eq:PD_m2control}) was used.

\begin{equation} \label{eq:PD_m2control}
\dot{\theta}^{2}_\mathrm{setpoint}=(\dot{\theta}^{2}_\mathrm{Pterm}+\dot{\theta}^{2}_\mathrm{Dterm})\times12.75
\end{equation}

\noindent In equation (\ref{eq:PD_m2control}), $\dot{\theta}^{2}_\mathrm{Pterm}$ is calculated using equation (\ref{eq:m2pterm}) and $\dot{\theta}^{2}_\mathrm{Dterm}$ is calculated using equation (\ref{eq:m2dterm}). The 12.75 constant originates from the fact that it was intended that if the tilt error was equal to 20\textdegree\space the servo speed should be 255. Therefore, if $K_p=1$, and $\dot{\theta}^{2}_\mathrm{Dterm}=0$ then $\dot{\theta}^{2}_\mathrm{setpoint}=255$.

\begin{equation} \label{eq:m2pterm}
\dot{\theta}^{2}_\mathrm{Pterm}=K_p \times \theta^{2}_\mathrm{error}
\end{equation}

\begin{equation} \label{eq:m2dterm}
\dot{\theta}^{2}_\mathrm{Dterm}=K_d \times (\theta^{2}_\mathrm{error}-\theta^{2}_\mathrm{prev-error})
\end{equation}

Since this is a totally different kind of control, the $K_p$ and $K_d$ are expected to change. Their best values, after extensive testing (section \ref{sec:pd_eval}), are $K_p=0.12$, $K_d=0.1$.

After all the control algorithm steps are done, the node sends the current motor positions to a \gls{ros} topic named \texttt{current\_motor\_position} which contains a message of the \texttt{Num} type, defined previously in the \texttt{image\_converter} package.

In order to change the mode in which this node operates, the user should to go to its source file and change the \texttt{mode} string variable's value to either ``roll'' or ``tilt'' and compile the package once again. The corresponding $K_p$ and $K_d$ must also be assigned. To make this task easier, a commented version of these changes is present on the file.

%\section{Estimating verticality using inertial data}
%
%At the beginning of this dissertation, estimating verticality using inertial data was one of its objectives, although one with less importance (as explained in section \ref{sec:obj}). Since all the other tasks took more time than was expected, it was not possible to do an estimation of verticality using only inertial data. However, to test if the inertial sensors were still working properly in case this is to be addressed in a future work, the Arduino board was connected to the computer and the relevant \gls{ros} nodes developed previously in \cite{rafeiro_rede_2013} were compiled. The Arduino code which would read the sensor's values was also uploaded to the board and it was possible to visualize the inertial readings in real-time on the computer screen.

\section{Controlling the robotic manipulator}

To perform the experiments described in chapter \ref{ch:4}, it was necessary to control the FANUC robotic manipulator. To do so, the \texttt{fanuc\_control} \gls{ros} package, developed in \cite{peixoto_visual_2016} was used and slightly altered. This package integrates the robCOMM language (sec. \ref{sec:robCOMM}) used to communicate with the manipulator's controller with the \gls{ros} framework. This makes it possible to publish the current value of the robot's joints to a \gls{ros} topic, which in turn allows the use of this data as an extremely precise ground truth for conducting experiments.

The only alteration made to the package was to insert the names of the console programs which contain the instructions to move the robot in each experiment into the \texttt{fanuc.cpp} file. When a different experiment is to be conducted, it is necessary to change the \texttt{tppname} variable's value accordingly and then compile the package again.

The \texttt{fanuc\_control} node publishes the cartesian coordinates of the robot's end effector into the \texttt{fanuc\_cart} \gls{ros} topic and the robot's joint values into the \texttt{fanuc\_joint} \gls{ros} topic.