Amcl Improvements

Amcl update:

Main update is the calculation of the particle mean when the resampling is not done (but motion and observation is integrated) - so that amcl publishes corrections every time an observation is integrated. 

Also added drawing of the corrected laser points (for use when comparing with ground truth). 
hmm, sure - I had that code publish a new tf when the amcl was running -
but ended up not using it.


On Thu, Aug 14, 2014 at 8:29 PM, Dejan Pangercic <notifications@github.com>
wrote:

> In amcl/src/amcl_node.cpp:
>
> > +      //                              tf::Vector3(hyps[max_weight_hyp].pf_pose_mean.v[0],
> > +      //                                          hyps[max_weight_hyp].pf_pose_mean.v[1],
> > +      //                                               0.0));
> > +
> > +      //   fprintf(stdout, "Test Base to Map : %f,%f,%f\n", hyps[max_weight_hyp].pf_pose_mean.v[0],
> > +      //           hyps[max_weight_hyp].pf_pose_mean.v[1], hyps[max_weight_hyp].pf_pose_mean.v[2]);
> > +      //   tf::Stamped<tf::Pose> test_base_to_map(test_base_tf,
> > +      //                                          laser_scan->header.stamp,
> > +      //                                          test_frame_id_);
> > +      //   ros::Time transform_expiration = (laser_scan->header.stamp +
> > +      //                                     transform_tolerance_);
> > +      //   tf::StampedTransform test_base_tf_stamped(test_base_to_map.inverse(),
> > +      //                                             transform_expiration,
> > +      //                                             test_frame_id_, global_frame_id_);
> > +      //   this->tfb_->sendTransform(test_base_tf_stamped);
> > +      // }
>
> I would just get rid of this?
>
> —
> Reply to this email directly or view it on GitHub
> <https://github.com/bosch-ros-pkg/navigation/pull/12/files#r16279030>.
>