PCL ICP stuff. separate from ROS. first commit.