3d simulation - no Gazebo

CameraDistanceEstimator class. Aruco detection, as it is implemented in OpenCV, provides us with distance to a detected marker and angle to it. But it does not provide us with error, so I had to implement this part myself.

Changes are relared to the fact that we now have aruco-based landmarks and expect more landmark types in future. So I pass the strLandmarksType as a parameter. This is not necessarily the best solution, it is possible that in future I will have few types of landmarks simultaneously, so landmark type will be detected rather than passed explicitely.

landmarks_step parameter. In earlier chapters, I used "step" parameters to simulate "sensor unavailable" situation. In other words, if GPS update frequency is set to 10 Hz and GPS step is 1, we feed UKF with GPS coordinates 10 times per second, but if gps_step is 10, we only do it once per second.

Landmarks were not processed this way, now I have fixed this problem. Note that this is for a non-Gazebo simulation (testing) only, as in Gazebo there are other ways to figure out which landmarks we can see.

Up until now, in a 2d simulator, landmarks had 2 coordinates, x and y. Now they have 3 coordinates. However, as it is inconvenient to provide 3 coordinates for landmarks, and as we have a 2.5d map, we can provide landmarks with Z coordinate set to zero:

Here, the Z element is non-zero just for convenience, it will be overwritten anyway.

Note that we also have 4th element. It is not necessarily a best implementation, but it is necessary. See, if we use map as described above, to get landmark's Z coordinate, we will get Z coordinate of the surface point where the landmark is placed. However, the landmark also has its own height; in case of "posts" in world_25d_landmarks.sdf world, its center is 1.5 meters above the ground.

Also note that this approach (get Z from the map and add 1.5) will work for aruco markers facing sideways, while for the top marker 2 should be added and not 1.5. This remains TBD (to be done) as in current section I simply do not use top markers (as my robot can not fly).

Then, in a constructor, when we load the 2.5d map, z coordinates are set based on the point's x and y.

We still need to implement 2m tall "posts" with a 1m cube covered with aruco markers on its top, in a world (in the .sdf file). For that, we need Z coordinates. As I do not have any utility for that (yet), here is the shortcut.

1. Run mar editor utility and create set of waypoints where you want your landmarks to be.
Click "Copy waypoints" button. You now have them in the clipboard. Paste it to a text editor and clean. Here is the code you want, one containing x and y coordinates of waypoints:

Paste this code to kalman.py and run it (the code is already there, just comment it out and put in coordinates you want. The code will print x, y and z coordinates.

Note the "+0.75": we want the bottom of the post to be on the surface, not its center.

In the code, you will find landmarks_dist2d, landmarks_dist3d_bearing0, and landmarks_dist3d_bearing2d. The idea is to either use 2d landmarks (landmarks_dist2d, obsolete and will be removed), or 3d landmarks while using distance to landmark but not bearing (landmarks_dist3d_bearing0, the only one currently implemented) or one using distance to landmark and bearing (landmarks_dist3d_bearing2d, not yet implemented).

As i mentioned above, UKF doesn't handle the situation when number of parameters we pass to it changes. So I chose to get fixed number of landmarks (equal to max. number passed in a constructor) and just make sure invisible landmarks do not affect UKF. To do it, we can set sigma to some huge value if landmark is not visible:

As UKF is follows the Markov logic, we can remove one landmark and add another. It will still work, as long as total number of landmarks does not change. This allows us to update landmarks as robot moves to different part of a map.

Landmarks and UKF

I have already provided all details, so let me just summarize. Plus, it is well commented in the code.

I pass certain number of landmarks. I run OpenCV aruco recognition to find markers. As I find them, I can get unique id (number encoded in this marker), distance and bearing to it. I put it in the data I feed to UKF.

For markers that robot doesn't see at the moment, large values are set for sigma, so it is not affecting UKF.