#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_conversions/pcl_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	AccumulateRotation (float cx, float cy, float cz, float lx, float ly, float lz, float &ox, float &oy, float &oz)
pcl::PointCloud < pcl::PointXYZHSV >::Ptr	coeffSel (new pcl::PointCloud< pcl::PointXYZHSV >())
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 >())
void	laserCloudExtreCurHandler (const sensor_msgs::PointCloud2ConstPtr &laserCloudExtreCur2)
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 >())
void	laserCloudLastHandler (const sensor_msgs::PointCloud2ConstPtr &laserCloudLast2)
pcl::PointCloud < pcl::PointXYZHSV >::Ptr	laserCloudSurfLast (new pcl::PointCloud< pcl::PointXYZHSV >())
pcl::PointCloud < pcl::PointXYZHSV >::Ptr	laserCloudSurfLLast (new pcl::PointCloud< pcl::PointXYZHSV >())
int	main (int argc, char **argv)
void	PluginIMURotation (float bcx, float bcy, float bcz, float blx, float bly, float blz, float alx, float aly, float alz, float &acx, float &acy, float &acz)
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
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
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}

Function Documentation

void AccumulateRotation	(	float	cx,
		float	cy,
		float	cz,
		float	lx,
		float	ly,
		float	lz,
		float &	ox,
		float &	oy,
		float &	oz
	)

Definition at line 186 of file laserOdometry.cpp.

pcl::PointCloud<pcl::PointXYZHSV>::Ptr coeffSel ( new pcl::PointCloud< pcl::PointXYZHSV > () )

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 > () )

void laserCloudExtreCurHandler ( const sensor_msgs::PointCloud2ConstPtr & laserCloudExtreCur2 )

Definition at line 265 of file laserOdometry.cpp.

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 > () )

void laserCloudLastHandler ( const sensor_msgs::PointCloud2ConstPtr & laserCloudLast2 )

Definition at line 315 of file laserOdometry.cpp.

pcl::PointCloud<pcl::PointXYZHSV>::Ptr laserCloudSurfLast ( new pcl::PointCloud< pcl::PointXYZHSV > () )

pcl::PointCloud<pcl::PointXYZHSV>::Ptr laserCloudSurfLLast ( new pcl::PointCloud< pcl::PointXYZHSV > () )

int main	(	int	argc,
		char **	argv
	)

Definition at line 385 of file laserOdometry.cpp.

void PluginIMURotation	(	float	bcx,
		float	bcy,
		float	bcz,
		float	blx,
		float	bly,
		float	blz,
		float	alx,
		float	aly,
		float	alz,
		float &	acx,
		float &	acy,
		float &	acz
	)