| 201 | } |
| 202 | |
| 203 | void Amcl::HandleInitialPoseMessage(Vec3d pf_init_pose_mean, |
| 204 | Mat3d pf_init_pose_cov) { |
| 205 | initial_pose_hyp_.reset(new AmclHyp()); |
| 206 | initial_pose_hyp_->pf_pose_mean = pf_init_pose_mean; |
| 207 | initial_pose_hyp_->pf_pose_cov = pf_init_pose_cov; |
| 208 | ApplyInitPose(); |
| 209 | } |
| 210 | |
| 211 | void Amcl::ApplyInitPose() { |
| 212 | if (initial_pose_hyp_ != nullptr && map_ptr_ != nullptr) { |