#include <math.h>
#include <time.h>
#include <stdio.h>
#include <stdlib.h>
#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/PointCloud2.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>
#include <opencv/cv.h>
#include <opencv2/highgui/highgui.hpp>
#include <pcl/ros/conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/kdtree/kdtree_flann.h>

Include dependency graph for laserOdometry.cpp:

Functions
void	laserCloudExtreCurHandler (const sensor_msgs::PointCloud2ConstPtr &laserCloudExtreCur2)
void	laserCloudLastHandler (const sensor_msgs::PointCloud2ConstPtr &laserCloudLast2)
int	main (int argc, char **argv)
void	TransformReset ()
void	TransformToEnd (pcl::PointXYZHSV pi, pcl::PointXYZHSV po, double startTime, double endTime)
void	TransformToStart (pcl::PointXYZHSV pi, pcl::PointXYZHSV po, double startTime, double endTime)
Variables
pcl::PointCloud < pcl::PointXYZHSV >::Ptr	coeffSel (new pcl::PointCloud< pcl::PointXYZHSV >())
const double	deg2rad = PI / 180
bool	imuInited = false
float	imuPitchCur = 0
float	imuPitchLast = 0
float	imuPitchStartCur = 0
float	imuPitchStartLast = 0
float	imuRollCur = 0
float	imuRollLast = 0
float	imuRollStartCur = 0
float	imuRollStartLast = 0
float	imuShiftFromStartXCur = 0
float	imuShiftFromStartXLast = 0
float	imuShiftFromStartYCur = 0
float	imuShiftFromStartYLast = 0
float	imuShiftFromStartZCur = 0
float	imuShiftFromStartZLast = 0
float	imuVeloFromStartXCur = 0
float	imuVeloFromStartXLast = 0
float	imuVeloFromStartYCur = 0
float	imuVeloFromStartYLast = 0
float	imuVeloFromStartZCur = 0
float	imuVeloFromStartZLast = 0
float	imuYawCur = 0
float	imuYawLast = 0
float	imuYawStartCur = 0
float	imuYawStartLast = 0
double	initTime
pcl::KdTreeFLANN < pcl::PointXYZHSV >::Ptr	kdtreeCornerLast (new pcl::KdTreeFLANN< pcl::PointXYZHSV >())
pcl::KdTreeFLANN < pcl::PointXYZHSV >::Ptr	kdtreeCornerLLast (new pcl::KdTreeFLANN< pcl::PointXYZHSV >())
pcl::KdTreeFLANN < pcl::PointXYZHSV >::Ptr	kdtreeSurfLast (new pcl::KdTreeFLANN< pcl::PointXYZHSV >())
pcl::KdTreeFLANN < pcl::PointXYZHSV >::Ptr	kdtreeSurfLLast (new pcl::KdTreeFLANN< pcl::PointXYZHSV >())
pcl::PointCloud < pcl::PointXYZHSV >::Ptr	laserCloudCornerLast (new pcl::PointCloud< pcl::PointXYZHSV >())
pcl::PointCloud < pcl::PointXYZHSV >::Ptr	laserCloudCornerLLast (new pcl::PointCloud< pcl::PointXYZHSV >())
pcl::PointCloud < pcl::PointXYZHSV >::Ptr	laserCloudExtreCur (new pcl::PointCloud< pcl::PointXYZHSV >())
pcl::PointCloud < pcl::PointXYZHSV >::Ptr	laserCloudExtreLast (new pcl::PointCloud< pcl::PointXYZHSV >())
pcl::PointCloud < pcl::PointXYZHSV >::Ptr	laserCloudExtreOri (new pcl::PointCloud< pcl::PointXYZHSV >())
pcl::PointCloud < pcl::PointXYZHSV >::Ptr	laserCloudLast (new pcl::PointCloud< pcl::PointXYZHSV >())
pcl::PointCloud < pcl::PointXYZHSV >::Ptr	laserCloudSurfLast (new pcl::PointCloud< pcl::PointXYZHSV >())
pcl::PointCloud < pcl::PointXYZHSV >::Ptr	laserCloudSurfLLast (new pcl::PointCloud< pcl::PointXYZHSV >())
bool	newLaserCloudExtreCur = false
bool	newLaserCloudLast = false
const double	PI = 3.1415926
const double	rad2deg = 180 / PI
double	startTimeCur
double	startTimeLast
bool	systemInited = false
double	timeLaserCloudExtreCur = 0
double	timeLaserCloudLast = 0
double	timeLasted
double	timeLastedRec
float	transform [6] = {0}
float	transformRec [6] = {0}
float	transformSum [6] = {0}