Refactor and correct prediction code and transforms.

The current code for taking in odom or velocity messages and generating predictions is incorrect. I think this is likely the root cause of or has already been called out in several issues:

These types of prediction inputs are necessary when the environment is somewhat ambiguous, such as a hallway.

Here is an example of the scan matcher struggling to track the actual robot motion down a hallway:

no predictions: current w/o predictions (video)

This case performs reasonably, but can't track down the long axis of the hallway when there aren't features nearby.

velocity based predictions: current w/ velocity predictions (video) :thinking:

odom based predictions: current w/ odom predictions (video)

:thinking:

Clearly something is wrong with the predictions.

As has been pointed out in the other issues the code here is wrong: https://github.com/CCNYRoboticsLab/scan_tools/blob/b5d9938/laser_scan_matcher/src/laser_scan_matcher.cpp#L456-L470

  getPrediction(pr_ch_x, pr_ch_y, pr_ch_a, dt);

  // the predicted change of the laser's position, in the fixed frame

  tf::Transform pr_ch;
  createTfFromXYTheta(pr_ch_x, pr_ch_y, pr_ch_a, pr_ch);

  // account for the change since the last kf, in the fixed frame

  pr_ch = pr_ch * (f2b_ * f2b_kf_.inverse());

  // the predicted change of the laser's position, in the laser frame

  tf::Transform pr_ch_l;
  pr_ch_l = laser_to_base_ * f2b_.inverse() * pr_ch * f2b_ * base_to_laser_ ;

The handling of the velocity message to create a prediction is also incorrect because the way the code is currently structured, the prediction is the differential in the fixed frame instead of the base_link frame, but the velocity will be in the base_link frame and isn't transformed into the fixed frame in any way.

Finally, the code is extra confusing because the variable names are either cryptic, or imply the opposite transform, or both.

This PR fixes all of that and also adds TF as an option for prediction input.

The TF input works just like the odom input, but uses tf instead of subscribing to an odom. The TF transform if base link in the fixed frame would probably be provided by a kalman filter of various localization sensors, like robot localization. That way the kalman filter can deal with the various nuances of transforming data into the correct frame and fusing it together.

Results

velocity based predictions: fixed w/ velocity predictions (video)

odom based predictions: fixed w/ odom predictions (video)

@130s

CCNYRoboticsLab / scan_tools

Refactor and correct prediction code and transforms. #79

Results