Joint vs. link:  the link moves in the joint's reference frame.
To go from joint i to link i, use apply_q.
To go from link i to joint i+1, use DH params.

Show collision detection with circles and polygons.

Show Iterate node and =Next=> transition.

GitHub: cozmo-tools source can be browsed there.

Particle filter: why do we use log weights?
  Maintain precision.
  Faster to do this when multiple landmarks are being
    evaluated; only do the exp() call at the end.

Different sensor models:
  Single distance sensor gives you distance ring but no heading info.
  Two distance sensors give reasonably tight location bound,
     still no heading since we could rotate in place and the
     distance values wouldn't change.
  Motion rewards particles whose predictions change the way the
  sensor changes; this is how heading is recovered.

  Single bearing sensor gives no location, but heading is in
    agreement with the location, e.g., if sensor says LM is
    straight ahead, all particles will be pointing at the landmark.
  Two bearing sensors give a weak constraint on location due to
    limited field of camera view.
  Motion eventually cleans things up.

  Combined sensor provides very fast, very clean localization.

Discuss answers to ArucoBearingSensor and ArucoCombinedSensor.

What are we missing?
  We're assuming point landmarks.  But they're not points.
  Cubes have a recoverable orientation value.  Can we use that?
  ArUco markers also have orientation information, and we know
    they're not visible over a range of roughly 200 degrees.
  Can we exploit this to build an even better sensor model?
  Can we use a single cube as our landmark?

World map:
  types of walls:  300, 450, and 600 mm.
  Unique ArUco markers give wall information.
  Constraints derived from multiple markers on the same wall.
  Constraints from knowing that walls meet at right angles.

====

IK for Cozmo.

Represent the world as a chain of revolute and prismatic joints attached
to the base frame.

Putting the lift at a point in space: work backwards.
Target point height governs lift height.
Lift angle determines base frame distance from lift.
Base can be anywhere on a circle with that radius.
Find point on circle where base doesn't collide.

====

SLAM

Show first SLAM video.

Data association problem: mapping sensed landmarks to the map's landmarks.
Hard for sonar/lidar.

We'll use unique landmarks so the problem is much easier.

Key idea: each particle represents a robot position and an EKF model
of each landmark.

Preface to second video:
  Bayesian filter = prediction (motion model) followed by correction (sensor model)
  Kalman filter = Bayesian filter for linear models where error is assumed Gaussian
  Extended Kalman filter: motion and sensor models can be non-linear; they are
    approximated with Taylor series.
  KF and EKF are both parameteric representations.
  Particle filter is non-parametric alternative.  

Show second SLAM video.