#include <3d_ndt_mcl.h>
Public Member Functions | |
| Eigen::Affine3d | getMean () |
| Eigen::Vector3d | getMeanVector () |
| void | initializeFilter (double x, double y, double z, double roll, double pitch, double yaw, double x_e, double y_e, double z_e, double roll_e, double pitch_e, double yaw_e, unsigned int numParticles) |
| NDTMCL3D (double map_resolution, lslgeneric::NDTMap &nd_map, double zfilter) | |
| void | updateAndPredict (Eigen::Affine3d Tmotion, pcl::PointCloud< pcl::PointXYZ > &cloud) |
| void | updateAndPredictEff (Eigen::Affine3d Tmotion, pcl::PointCloud< pcl::PointXYZ > &cloud, double subsample_level) |
Public Attributes | |
| int | counter |
| bool | forceSIR |
| lslgeneric::NDTMap | map |
| This is my map. | |
| ParticleFilter3D | pf |
| This is the particle filter. | |
| double | resolution |
| double | resolution_sensor |
| double | zfilt_min |
Private Member Functions | |
| double | getDoubleTime () |
Private Attributes | |
| bool | isInit |
| int | sinceSIR |
NDT MCL - Class implementation
Definition at line 21 of file 3d_ndt_mcl.h.
| NDTMCL3D::NDTMCL3D | ( | double | map_resolution, |
| lslgeneric::NDTMap & | nd_map, | ||
| double | zfilter | ||
| ) | [inline] |
Constructor
First, lets make our target map match the given map This is done because we want (possibly) lower resolution target map
Definition at line 33 of file 3d_ndt_mcl.h.
| double NDTMCL3D::getDoubleTime | ( | ) | [inline, private] |
Definition at line 109 of file 3d_ndt_mcl.h.
| Eigen::Affine3d NDTMCL3D::getMean | ( | ) | [inline] |
Definition at line 98 of file 3d_ndt_mcl.h.
| Eigen::Vector3d NDTMCL3D::getMeanVector | ( | ) | [inline] |
Definition at line 92 of file 3d_ndt_mcl.h.
| void NDTMCL3D::initializeFilter | ( | double | x, |
| double | y, | ||
| double | z, | ||
| double | roll, | ||
| double | pitch, | ||
| double | yaw, | ||
| double | x_e, | ||
| double | y_e, | ||
| double | z_e, | ||
| double | roll_e, | ||
| double | pitch_e, | ||
| double | yaw_e, | ||
| unsigned int | numParticles | ||
| ) | [inline] |
Intialize filter to some pose and with some number of particles
Definition at line 82 of file 3d_ndt_mcl.h.
| void NDTMCL3D::updateAndPredict | ( | Eigen::Affine3d | Tmotion, |
| pcl::PointCloud< pcl::PointXYZ > & | cloud | ||
| ) |
Definition at line 3 of file 3d_ndt_mcl.cpp.
| void NDTMCL3D::updateAndPredictEff | ( | Eigen::Affine3d | Tmotion, |
| pcl::PointCloud< pcl::PointXYZ > & | cloud, | ||
| double | subsample_level | ||
| ) |
#pragma
Definition at line 94 of file 3d_ndt_mcl.cpp.
Definition at line 27 of file 3d_ndt_mcl.h.
| bool NDTMCL3D::forceSIR |
Definition at line 29 of file 3d_ndt_mcl.h.
bool NDTMCL3D::isInit [private] |
Definition at line 107 of file 3d_ndt_mcl.h.
This is my map.
Definition at line 23 of file 3d_ndt_mcl.h.
This is the particle filter.
Definition at line 24 of file 3d_ndt_mcl.h.
| double NDTMCL3D::resolution |
Definition at line 25 of file 3d_ndt_mcl.h.
| double NDTMCL3D::resolution_sensor |
Definition at line 26 of file 3d_ndt_mcl.h.
int NDTMCL3D::sinceSIR [private] |
Definition at line 108 of file 3d_ndt_mcl.h.
| double NDTMCL3D::zfilt_min |
Definition at line 28 of file 3d_ndt_mcl.h.