UP | HOME

Homework 4

Task S (Feature Detection)

The first tasks here are to enable you to sense landmarks that can then be used as sensor measurements for SLAM.

Task S.1 (Landmark Detection)

  1. Write a node called landmarks in the nuslam package to detect landmarks and publish their relative locations
    • This node takes 2D laser_scan data and outputs a landmark locations relative to the robots
    • You should have a way to display the detected landmark locations in rviz
  2. Write a landmark_detect.launch.xml
    • If the robot argument is nusim, it should launch your simulator and the landmarks node and display the sensed landmarks in rviz

Hints:

  1. Writing some of the test cases in S.2 while you are coding these algorithms can help you debug and make sure you don't introduce new bugs.
    • You may want to do some "test-driven-development (TDD)" and write some test cases for the circle detection algorithm prior to writing the algorithm itself.
  2. You can use any algorithm you want as long as it works. Below I describe algorithms that I know work.
  3. First solve an unsupervised learning problem: clustering points into groups corresponding to individual landmarks.
    • Cluster the laser scanner points based on a distance threshold.
    • Discard any clusters with fewer than 3 points, as these will not provide good results when we fit a circle
    • The laser scan data is ordered by increasing relative bearing angle. If two consecutive scan points are near each other they are part of the same cluster, otherwise they are in different clusters
    • Keep in mind that the scan wraps around, so you may be starting in the middle of a cluster
    • To help test this step, I recommend outputting markers to help visualize the clusters.
  4. Next, solve a supervised learning problem: circular regression.
    • To detect the circles, implement this circle fitting algorithm
    • It may help to break up each step into separate functions and test them separately.
  5. Solve a classification problem
    • Classify the clusters of points into Circle and Not Circle. This step helps to avoid false positives.
    • Algorithm below is from J. Xavier et. al., Fast line, arc/circle and leg detection from laser scan data in a Player driver, ICRA 2005
      1. Let \(P_1\) and \(P_2\) be the endpoints of a cluster and \(P\) be a point in the cluster.
      2. If the cluster is an arc of a circle, then the angle from \(P_1\) to \(P\) to \(P_2\) is always the same for all \(P\) (a consequence of the inscribed angle theorem)
      3. So, compute the mean and standard deviation of the angles between the endpoints and each point on the segment
      4. If standard deviation is below 0.15 radians and mean is between 90 and 135 degrees, then it is a circle (thresholds are from the paper, tuning them will help you as their thresholds will not necessarily work well in our application).
  6. Practical and Simple Extras to aid accuracy
    1. Feel free to use these ideas to increase accuracy. They will help in our specific setup but may not be as generally applicable to all situations.
    2. Eliminate circles with really large or small radii, since we know the approximate size of the landmarks (this increases the assumptions required for your algorithm to work but also is very helpful).
    3. Make the minimum cluster size larger to ensure better fits for the circle fitting algorithm

Task S.2 (Testing Landmark Detection)

  1. Add some tests for circle detection in a file called circle_tests.cpp in the nuslam package.
  2. The goal of testing is to break each of the landmark detection parts down into simpler components.
    • We will not attempt comprehensive testing here, but writing a few simple tests is well worth our time and can even help in debugging.
  3. The circle fitting algorithm is the easiest to test because the authors of the algorithm provide test data! (from https://people.cas.uab.edu/~mosya/cl/CPPcircle.html)
    • Write tests for the circle algorithm, here are the results you should get. (If you did not use this algorithm, you still must test whatever algorithm that you implement)
    • Test 1 Inputs: \(\{(1,7), (2,6), (5,8), (7,7), (9,5), (3,7)\}\). Outputs: center (4.615482, 2.807354), radius 4.8275
    • Test 2 Inputs: \(\{(-1,0), (-0.3, -0.06), (0.3, 0.1), (1, 0)\}\). Outputs: center (0.4908357, -22.15212) radius 22.17979
    • You can expect to come within \(10^{-4}\) of the results above.

Task L (SLAM)

Task L.3 (SLAM with unknown data association)

Here, you will implement SLAM based on the laser-scanner measurements, using the landmark detection algorithms developed in Task S.

  1. Implement a data association scheme (either based on mahalanobis distance, euclidean distance, or anything else you can think of) to associate measurements with landmarks and incorporate it into the SLAM algorithm
    • You should feel free to add any of the additional filtering techniques to make the data association more effective
    • See Data Association for a description of the basic algorithm
  2. Drive the robot around a closed path in the nusim and include a screencast of the result in rviz. Use your data association scheme and simulated laser scan data.
    • Try to drive the robot back to its initial starting position
    • Report the final pose error between the actual robot position and odometry in nuslam/README.md
    • Report the final pose error between the actual robot position and the SLAM estimate in nuslam/README.md
    • The screencast should show the actual and estimated marker positions, and the actual, estimated and odometry path, and robots.
  3. It is okay if your algorithm only works while driving slowly
  4. When you complete this task you have fully implemented EKF SLAM with unknown data association!
    • It may be helpful to temporarily reduce the noise of the laser scanner in the simulation to zero (you can do this in the xacro urdf) while testing.
  5. You should be able to launch this demo by running ros2 launch nuslam unknown_data_assoc.launch.xml

Task L.4 (SLAM in the Real World)

  1. I have provided some cylindrical obstacles for you to use
  2. Run your code on the turtlebot and drive it around in the real world
    • Complete a circuit of the environment and drive the robot back to its initial position
  3. Include a screenshot from RVIZ at the end of a run and a video of the turtlebot moving.
  4. Report the final pose \((x, y, \theta)\) as determined by the SLAM algorithm and as determined by the odometry in nuslam/README.md.
    • Did your SLAM implementation outperform the odometry? (If not, it may have a bug, require more tuning, or you did not drive the robot far enough)
    • You may have to drive slowly to achieve good results.
  5. You should be able to run this demo with
    • On the turtlebot: ros2 launch nuslam turtlebot_bringup.launch.xml
    • On your computer: ros2 launch nuslam pc_bringup.launch.xml

Author: Matthew Elwin