$$\newcommand{\vmath}[1]{\mathsf{#1}} \newcommand{\mapsfrom}{\leftarrow\!\shortmid} \newcommand{\maps}{\mapsto} \newcommand{\exc}{\vmath{\colF{exec}}} \newcommand{\eval}{\vmath{\colR{eval}}} \newcommand{\ufloor}{{\vmath{\colL floor}}} \newcommand{\uceil}{{\vmath{\colU ceil}}} \newcommand{\triv}{\text{Triv}} \newcommand{\idFunc}{\mathrm{Id}} \newcommand{\reals}{\mathbb{R}} \newcommand{\One}{\mathbf{1}} \renewcommand{\mathbbm}[1]{\mathbb{#1}} \renewcommand{\mathscr}[1]{\mathcal{#1}} \newcommand{\varocircle}{⦾} \newcommand{\varotimes}{⊗} \newcommand{\varovee}{(\vee)} \newcommand{\colR}{\color{darkred}} \newcommand{\colF}{\color{darkgreen}} \newcommand{\colH}{\color{blue}} \newcommand{\colI}{\color{orange}} \newcommand{\rtof}{{\colH{\varphi}}} \newcommand{\ftor}{{\colH{h}}} \newcommand{\ftoR}{{\colH{H}}} \newcommand{\Rcomp}{{\mathbb{R}}^{*}_{\small+}} \newcommand{\nonNegRealsComp}{\Rcomp} \newcommand{\nonNegReals}{\mathbb{R}_+} \newcommand{\Rcpu}[1]{\Rcomp{}[\textrm{#1}]} \newcommand{\funsp}{{\colF{\mathscr{F}}}} \newcommand{\impsp}{{\colI{\mathscr{I}}}} \newcommand{\ressp}{{\colR{\mathscr{R}}}} \newcommand{\funleq}{\posleq_{\funsp}} \newcommand{\fun}{\vmath{\colF{f}}} \newcommand{\res}{\vmath{\colR{r}}} \newcommand{\funtop}{\top_\funsp} \newcommand{\funbot}{\bot_\funsp} \newcommand{\imp}{\vmath{i}} \newcommand{\paramsp}{\mathscr{P}} \newcommand{\resleq}{\posleq_{\ressp}} \newcommand{\restop}{\top_\ressp} \newcommand{\resbot}{\bot_\ressp} \newcommand{\resspleq}{\resleq} \newcommand{\tressp}{\trof(\ressp)} \newcommand{\trof}{\mathscr{T}} \newcommand{\tres}{T} \newcommand{\tresleq}{\leq_{\trof}} \newcommand{\trleq}{\leq_{\trof}} \newcommand{\dpisp}{\ensuremath{\vmath{DPI}}} \newcommand{\cdpisp}{\ensuremath{\vmath{CDPI}}} \newcommand{\dprobsp}{\ensuremath{\vmath{DP}}} \newcommand{\dprob}{\vmath{dp}} \newcommand{\dpseries}{\vmath{series}} \newcommand{\dppar}{\vmath{par}} \newcommand{\dploop}{\vmath{loop}} \newcommand{\dploopb}{\vmath{loopb}} \newcommand{\cdprobsp}{\ensuremath{\vmath{CDP}}} \newcommand{\cdprob}{\vmath{cdp}} \newcommand{\dpatoms}{\vmath{atoms}} \newcommand{\resMin}{{\Min_{\resleq}}} \newcommand{\unconnectedfun}{\mathsf{UF}} \newcommand{\unconnectedres}{\mathsf{UR}} \newcommand{\Aressp}{{\mathsf{\colR A}\colR\ressp}} \newcommand{\Afunsp}{{\mathsf{\colF A}\colF\funsp}} \newcommand{\udpa}{\boldsymbol{u}_a} \newcommand{\udpb}{\boldsymbol{u}_b} \newcommand{\udpL}{\boldsymbol{\mathsf{L}}} \newcommand{\udpU}{\boldsymbol{\mathsf{U}}} \newcommand{\udpsp}{\vmath{UDP}} \newcommand{\udpleq}{\posleq_\udpsp} \newcommand{\dpsp}{\vmath{DP}} \newcommand{\dpleq}{\posleq_\dpsp} \newcommand{\terms}{\vmath{Terms}} \newcommand{\udpsem}{\Phi} \newcommand{\dpsem}{\varphi} \newcommand{\atoms}{\mathcal{A}} \newcommand{\atree}{\boldsymbol{\vmath{T}}} \newcommand{\val}{\boldsymbol{v}} \newcommand{\ops}{\vmath{ops}} \newcommand{\ftorL}{\ftor_L} \newcommand{\ftorU}{\ftor_U} \newcommand{\acprod}{\mathbin{\boldsymbol{\times}}} \newcommand{\oploop}{\dagger} \newcommand{\opseries}{\mathbin{\varocircle}} \newcommand{\oppar}{\mathbin{\varotimes}} \newcommand{\opcoprod}{\mathbin{\varovee}} \newcommand{\UId}{\vmath{UId}} \newcommand{\vdc}{\vmath{vdc}} \newcommand{\makedp}{\Gamma} \newcommand{\colU}{\color{purple}} \newcommand{\colL}{\color{orange}} \newcommand{\R}[1]{{\colR{#1}}} \newcommand{\F}[1]{{\colF{#1}}} \newcommand{\I}[1]{{\colI{#1}}} \newcommand{\cdpiN}{\mathcal{V}} \newcommand{\cdpin}{v} \newcommand{\cdpinA}{v_1} \newcommand{\cdpinB}{v_2} \newcommand{\cdpiresind}{i} \newcommand{\cdpifunind}{j} \newcommand{\cdpiresindA}{i_1} \newcommand{\cdpifunindB}{j_2} \newcommand{\dpinumf}{\vmath{nf}} \newcommand{\dpinumr}{\vmath{nr}} \newcommand{\cdpinnumf}{\dpinumf_\cdpin} \newcommand{\cdpinnumr}{\dpinumr_\cdpin} \newcommand{\cdpiE}{\mathcal{E}} \newcommand{\subto}{\text{s.t.}} \newcommand{\with}{\text{using}} \newcommand{\pset}{\mathscr{P}} \DeclareMathOperator*{\Min}{Min} \DeclareMathOperator*{\Inf}{Inf} \DeclareMathOperator*{\Sup}{Sup} \DeclareMathOperator*{\Max}{Max} \newcommand{\lowerbounds}{\vmath{lowerbounds}} \newcommand{\upperbounds}{\vmath{upperbounds}} \newcommand{\posMin}{\Min} \newcommand{\posleq}{\preceq} \newcommand{\poslt}{\prec} \newcommand{\posgeq}{\succeq} \newcommand{\posA}{\mathcal{P}} \newcommand{\posAleq}{\mathrel{{\posleq_\posA}}} \newcommand{\posAMin}{\mathop{{\posMin_{\posAleq}}}} \newcommand{\posAmin}{\mathop{{\min_{\posAleq}}}} \newcommand{\posAmax}{\mathop{{\max_{\posAleq}}}} \newcommand{\posB}{\mathcal{Q}} \newcommand{\posBleq}{\mathrel{{\posleq_\posB}}} \newcommand{\posC}{\mathcal{R}} \newcommand{\lfp}{\vmath{lfp}} \newcommand{\prefixed}{\vmath{prefixed}} \newcommand{\CPOs}{\textsc{CPO}s\xspace} \newcommand{\CPO}{\textsc{CPO}\xspace} \newcommand{\DCPOs}{\textsc{DCPO}s\xspace} \newcommand{\DCPO}{\textsc{DCPO}\xspace} \newcommand{\antichains}{\vmath{A}} \newcommand{\upsets}{\vmath{U}} \newcommand{\downsets}{\vmath{D}} \newcommand{\upresleq}{\posleq_{\upressp}} \newcommand{\upressp}{\upsets\ressp} \newcommand{\allupsets}{\vmath{Up}} \newcommand{\upit}{{\uparrow\,}} \newcommand{\stupit}{\dot{\upit}} \newcommand{\posetwidth}{\vmath{width}} \newcommand{\posetheight}{\vmath{height}} \newcommand{\posdef}[1]{\mathcal{P}_{#1}} \newcommand{\MR}{\M{R}} \newcommand{\myacronym}[1]{\textsc{#1}\xspace} \newcommand{\T}[1]{\boldsymbol{{\mathsf{#1}}}} \newcommand{\Tel}[1]{{\mathsf{#1}}} \newcommand{\Te}[1]{\Tel{#1}} \newcommand{\M}[1]{\mathbf{#1}} \newcommand{\Mel}[1]{\mathrm{#1}} \newcommand{\aset}[1]{\mathscr{#1}} \newcommand{\agroup}[1]{\mathrm{#1}} \newcommand{\aseq}[1]{\boldsymbol{#1}} \newcommand{\aseqe}[1]{#1} \newcommand{\dummyIndices}{} \newcommand{\aword}[1]{\mathsf{#1}} \newcommand{\vmath}[1]{\aword{#1}} \newcommand{\codefunc}[1]{\texttt{#1}\xspace} \newcommand{\swpackage}[1]{\textsc{#1}\xspace} \newcommand{\MA}{\M{A}} \newcommand{\MB}{\M{B}} \newcommand{\MC}{\M{C}} \newcommand{\MG}{\M{G}} \newcommand{\MH}{\M{H}} \newcommand{\ML}{\M{L}} \newcommand{\MQ}{\M{Q}} \newcommand{\MP}{\M{P}} \newcommand{\MS}{\M{S}} \newcommand{\MSigma}{\M{\Sigma}} \newcommand{\MV}{\M{V}} \newcommand{\MW}{\M{W}} \newcommand{\SP}{P_{\text{s}}} \newcommand{\AP}{P_{\text{a}}} \newcommand{\SE}{E} \newcommand{\ER}{r} \newcommand{\HP}{\Theta} \newcommand{\np}{n} \newcommand{\ones}{\boldsymbol{1}} \newcommand{\idMat}{\M{I}} \newcommand{\matTrace}{\vmath{Tr}} \newcommand{\angleFun}{\angle} \newcommand{\flatten}{\mathsf{vec}} \newcommand{\batterymass}{{\colR{m}}} \newcommand{\batterycapacity}{{\colF{C}}} \newcommand{\batterycost}{{\colR{c}}} \newcommand{\specificenergy}{{\colR{\rho}}} \newcommand{\specificcost}{{\colR{\alpha}}} \newcommand{\D}{\,\textrm{d}} \newcommand{\ex}{\mathbb{E}} \newcommand{\aset}[1]{\mathcal{#1}} \newcommand{\amat}[1]{\mathbf{#1}} \newcommand{\avec}[1]{\mathbf{#1}} \newcommand{\rv}[1]{\boldsymbol{#1}} \newcommand{\definedas}{\triangleq} \newcommand{\nats}{\mathbb{N}} \newcommand{\ints}{\mathbb{Z}} \newcommand{\rats}{\mathbb{Q}} \newcommand{\reals}{\mathbb{R}} \newcommand{\comp}{\mathbb{C}} \newcommand{\Time}{\mathbb{T}} \newcommand{\SEthree}{\text{SE}(3)} \newcommand{\SEtwo}{\text{SE}(2)} \newcommand{\sethree}{\text{se}(3)} \newcommand{\setwo}{\text{se}(2)} \newcommand{\SOthree}{\text{SO}(3)} \newcommand{\pose}{\boldsymbol{q}} \newcommand{\state}{\boldsymbol{x}} \newcommand{\statesp}{\mathcal{X}} \newcommand{\bmu}{\boldsymbol{\mu}} \newcommand{\bSigma}{\boldsymbol{\Sigma}} \newcommand{\tup}[1]{\langle#1\rangle}$$

Exercises - state estimation and sensor fusion

✎

Modified 2020-11-16 by Aleksandar Petrov

Build your own localization system through sensor fusion and the Apriltag library

In this exercise you will learn how to create estimators with both proprioceptive and exteroceptive sensors and how to manipulate the frame transformations tree to easily fuse these estimates.

Odometry with Wheel Encoders (unknown ref duckietown-robotics-development/odometry-modeling)

warning (14 of 17) index

warning

I will ignore this because it is an external link. 

 > I do not know what is indicated by the link '#duckietown-robotics-development/odometry-modeling'.

Location not known more precisely.

Created by function n/a in module n/a.

Modeling the Duckiebot and Representations (unknown ref duckietown-robotics-development/representations-modeling)

warning (15 of 17) index

warning

I will ignore this because it is an external link. 

 > I do not know what is indicated by the link '#duckietown-robotics-development/representations-modeling'.

Location not known more precisely.

Created by function n/a in module n/a.

Advanced Augmented Reality Exercise

Understand how to build a TF tree in ROS and visualize it in RViz.

Be able to create a multi-package Dockerized ROS workspace, and deploy it on the Duckiebot.

Experience with working with ROS Timers and Services

Create a wheel-encoder based estimator. Understand the benefits and drawbacks of such an estimator.

Create an Apriltag-based estimator. Understand the benefits and drawbacks of this estimator.

Create a sensor fusion node that fuses estimates from the above individual estimators.

The overall goal of this exercise is to create a state estimation system that localizes the Duckiebot in global coordinates. So far we have only seen how this can be done locally, relative to the lane. This has two major limitations. First, we don’t know how far we have travelled in the lane, as we only can get estimates on our lateral offset from the center of the lane. Second, we don’t know if we are in a straight or turning lane segment. In this exercise, however, we will work towards localizing the robot in a global coordinate frame.

To be able to perform such a complex task, we will have to use all sensors onboard the robot, namely the camera and the wheel encoders. We will achieve our localization goal in several incremental steps. First, we will develop an estimator using only wheel encoders. Then, we will develop an estimator using only camera information via the Apriltag detection library. Finally, we will fuse these estimates so that we get the best of both worlds.

Encoder localization package

✎

The first package you’ll create in your Dockerized ROS workspace is one that will contain a node that publishes a TransformStamped message at 30Hz with the following fields:

frame_id: map;
child_frame_id: encoder_baselink;
stamp: timestamp of the last wheel encoder tick;
transform: a 2D pose of the robot baselink (see Figure 4.2 for a definition of this frame).

The node will also have to broadcast the TF map-encoder_baselink.

Deliverables:

A screen recording similar to this Figure 4.1 where you drive the robot in a loop and try to “close the loop” (by driving to the point where you started from). Make sure that the video contains the camera images as well as the TF tree in Rviz. Use a visible landmark as the origin of the map frame.
A link to a Github repository containing a package called encoder_localization.

Hints:

For the estimates to be in a global frame, you will have to provide an initial pose estimate.
Use tf.TransformBroadcaster() to broadcast a TF which can be visualized in RViz. Note that you can pass the exact same message as the one that your publisher uses.
For the kinematic model of the Duckiebot, you’ll have to load the following parameters from the kinematics calibration file: baseline, radius.
To open RViz, simply run dts start_gui_tools hostname.local and run rviz from inside the container. Note that if you keep the container running you can save your RViz configuration file so that when you reopen it, it automatically displays your topics of interest.
Rviz uses the following color-code convention for frame axes: red for the x-axis, green for the y-axis, and blue for the z-axis.
To publish messages at a fixed frequency, consider using a ROS Timer. It works very similar to a subscriber except for the fact that the callback get called after a fixed duration of time.
This link contains many useful methods from the tf library that allow you to switch between representations (e.g. expressing an euler yaw angle as a quaternion).

Sensor fustion exercise: Encoders-only estimate — Example video deliverable for the encoder localization package

Position of the baselink and camera frames on the robot

As you have probably realized, some of the advantages of this localization system is that as long as the robot is moving, the wheel encoders will provide information about the state of the robot, at a high rate and with little delay. However, the pose of the robot is an integration of the wheel encoder measurements, meaning that it is subject to drift as any inaccuracies in measurement get propagated through time. When driving aggressively or through a slippery surface these inaccuracies are amplified (try this for yourself!).

A common way of getting rid of drift in mobile robots is to either use absolute position measurements (with GPS, for instance) or to use fixed landmarks in the world as reference. This is how we humans also navigate the environment. Since there is no GPS on-board the Duckiebot, we will have to use the latter approach. This is what we will explore in the next exercise, where our landmarks will be traffic signs with Apriltags (see Figure 4.3).

Example traffic sign to be used for Apriltag localization. In order to accurately construct your TF tree, please measure the height indicated

Apriltag localization package

✎

In this package you’ll have to place a node that subscribes to /camera_node/image/compressed and publishes a TransformStamped message (if an image with an Apriltag has been received) with the following fields:

frame_id: map;
child_frame_id: at_baselink;
stamp: timestamp of the last image received;
transform: a 3D pose of the robot baselink (see Figure 4.2 for a definition of this frame).

This node will also have to broadcast the following TFs: map-apriltag, apriltag-camera, camera-at_baselink. Make sure that when you place the Apriltag in front of the robot you get something that looks roughly like Figure 4.5. This will make it easier to fuse this pose with the pose from the encoders.

Deliverables:

A screen recording similar to the one in Figure 4.6 where you move the Apriltag in front of the robot from one side of the Field-of-View (FOV) to the other. Make sure that the video contains the camera images as well as the TF tree in Rviz.
Instead of directly using compressed images, rectify them before passing them to the Apriltag detector. You will see that this significantly improves the accuracy of the detector but at the cost of significant delay. Provide a screen recording similar to this Figure 4.7 where you move the Apriltag in front of the robot from one side of the FOV to the other. Make sure that the video contains the camera images as well as the TF tree in Rviz. You should observe that the map and baselink are now in the same plane (or at least much closer to it than with compressed images).
(Bonus) Offload the computation of the rectified image to the GPU of the Jetson Nano so that the improved accuracy can be obtained without significant delay.
A link to your Github repository containing a package called at_localization.

Hints:

A frame cannot have two parents. If you try to broadcast the following TFs: map-baselink and camera-baselink, you will see that you’ll only be able to see the individual frames in RViz.
To rectify images, use the following cv2 methods:

newCameraMatrix = cv2.getOptimalNewCameraMatrix(cameraMatrix, 
                                                distCoeffs, 
                                                (640, 480), 
                                                1.0)
cv2.initUndistortRectifyMap(cameraMatrix, 
                            distCoeffs, 
                            np.eye(3), 
                            newCameraMatrix, 
                            (640, 480), 
                            cv2.CV_32FC1)
cv2.remap(compressed_image, map1, map2, cv2.INTER_LINEAR)

To use the Apriltag detector, consult this link. You can extract the pose of the Apriltag in the camera frame with tag.pose_R (rotation matrix) and tag.pose_t (translation vector). Keep in mind that the coordinate frame convention (see Figure 4.4) is different than the one you are supposed to use in the deliverable!
To broadcast static transforms, use

self.static_tf_br = tf2_ros.StaticTransformBroadcaster()

To calculate the static transform between the map and apriltag frames () use Figure 4.3 as reference. To calculate the static transform between the baselink and camera frames, you can use Figure 4.2 as reference, or you can try to use the homography matrix obtained during extrinsic calibration.

To convert between frames, we recommend that you use 4x4 transformation matrices. An example of such a matrix is which transforms a vector in frame B to frame A. During your manipulations, keep in mind that and for any frames and .

Frame convention used by Apriltag library when returning pose.

Sensor fustion exercise: TF tree — Goal TF tree for the AT localization package with rectified images and the robot facing the Apriltag

Sensor fustion exercise: Compressed image — Example video deliverable for the AT localization package with compressed images

Sensor fustion exercise: Rectified image — Example video deliverable for the AT localization package with rectified images

The detected Apriltag can provide accurate pose estimates with respect to landmarks but at the cost of a significant delay and a low frequency. Moreover, one cannot continuously rely on such estimates since the Apriltag could go out of sight. To combat this, we can fuse the estimates of the Apriltag and the wheel encoders for continuous, accurate and robust localization. This is the goal of the next exercise.

Fused localization package

✎

In this package you’ll have to place a node that publishes a TransformStamped message with the following fields:

frame_id: map;
child_frame_id: fused_baselink;
stamp: current time,
transform: a 2D pose of the robot baselink.

The node will also have to broadcast the TF map-fused_baselink. As a minimum, your fusion node should have the following behaviour:

The first time the Apriltag becomes visible, you have to calibrate the encoder_baselink frame/estimate to match exactly the Apriltag pose. This should be done with a ROS Service provided by and encoder_localization_node (server) which fused_localization_node (client) can call. If you need an example of a Duckietown package that defines a similar service, check this out (pay a special attention to the CMakeLists.txt and package.xml files).
Publish the robot pose using the Apriltag estimate (when available) projected on the ground plane (recall that this node publishes 2D poses).
If the Apriltag is not visible, use the encoder estimates, starting from the last Apriltag pose received (there should be no pose jump if the Apriltag goes out of sight).
If the Apriltag becomes visible again, switch back to using Apriltag estimates (a jump in fused pose is allowed).
You don’t have to handle the delay or the variance of the individual estimates to complete the exercise, but you are more than welcome to!

Deliverables:

A screen recording similar to this Figure 4.8, where you drive the robot in a loop around the Apriltag with the virtual joystick. Start the recording with the Apriltag not visible, so that you validate that your calibration service is working and when it does bacome visible all the frames align. You should end the trajectory where you started it (feel free to use a marker on the ground). Make sure that the video contains the camera images as well as the TF tree in Rviz.
A link to a github repository containing a package called fused_localization