32 #ifndef _LIDAR_EGOMOTION_H_
33 #define _LIDAR_EGOMOTION_H_
40 #include <laser_geometry/laser_geometry.h>
42 #include <sensor_msgs/LaserScan.h>
43 #include <sensor_msgs/PointCloud2.h>
45 #include <tf/transform_listener.h>
47 #include <pcl_ros/transforms.h>
48 #include <pcl/conversions.h>
49 #include <pcl/point_cloud.h>
50 #include <pcl/point_types.h>
51 #include <pcl_conversions/pcl_conversions.h>
53 #include <csm/csm_all.h>
54 #include <csm/laser_data.h>
void RaysToPMScan(ray_config_t *cfg, ray_measurment_t *src, PMScan *dst)
int RaysToMbICP(ray_config_t *cfg, ray_measurment_t *src, Tpfp *dst, bool test_angle=0, double angle=M_PI/2.2)
Nonholonomic motion model declaration.
Generic types declaration for use in the lidar_egomotion algorithm.
void CreateMeasurementFromDisplacement(double dx, double dy, double dtheta, double z[2], double dt, double l, double bwa)
MbICP scan matcher modified main header.
pcl::PointCloud< pcl::PointXYZ > wrapper_Laserscan2PointCloud(const sensor_msgs::LaserScan &scan)
void ConvertEstimatedToMeasurment(double vl, double dir, float *dx, float *dy, float *dtheta, double dt, double l, double bwa)
void laser_2_Handler(const sensor_msgs::LaserScan &scan)
void RaysToLDP(ray_definition *src, LDP dst, bool test_angle=0, double angle=M_PI/2.5)
void SetDefaultConfiguration(void)
void laser_3_Handler(const sensor_msgs::LaserScan &scan)
void ConfigurePLICP(struct sm_params *params, LDP ref, LDP sens)
Plplot auxiliary drawing class declaration.
void CleanZone(ray_definition *rays, double steering_angle)
Structure describing a laser scan.
External polar scan matcher algorithm main header.