#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::PointXYZI >::Ptr	coeffSel (new pcl::PointCloud< pcl::PointXYZI >())
pcl::PointCloud < pcl::PointXYZI >::Ptr	cornerPointsLessSharp (new pcl::PointCloud< pcl::PointXYZI >())
pcl::PointCloud < pcl::PointXYZI >::Ptr	cornerPointsSharp (new pcl::PointCloud< pcl::PointXYZI >())
pcl::PointCloud< pcl::PointXYZ > ::Ptr	imuTrans (new pcl::PointCloud< pcl::PointXYZ >())
void	imuTransHandler (const sensor_msgs::PointCloud2ConstPtr &imuTrans2)
pcl::KdTreeFLANN < pcl::PointXYZI >::Ptr	kdtreeCornerLast (new pcl::KdTreeFLANN< pcl::PointXYZI >())
pcl::KdTreeFLANN < pcl::PointXYZI >::Ptr	kdtreeSurfLast (new pcl::KdTreeFLANN< pcl::PointXYZI >())
pcl::PointCloud < pcl::PointXYZI >::Ptr	laserCloudCornerLast (new pcl::PointCloud< pcl::PointXYZI >())
void	laserCloudFlatHandler (const sensor_msgs::PointCloud2ConstPtr &surfPointsFlat2)
pcl::PointCloud < pcl::PointXYZI >::Ptr	laserCloudFullRes (new pcl::PointCloud< pcl::PointXYZI >())
void	laserCloudFullResHandler (const sensor_msgs::PointCloud2ConstPtr &laserCloudFullRes2)
void	laserCloudLessFlatHandler (const sensor_msgs::PointCloud2ConstPtr &surfPointsLessFlat2)
void	laserCloudLessSharpHandler (const sensor_msgs::PointCloud2ConstPtr &cornerPointsLessSharp2)
pcl::PointCloud < pcl::PointXYZI >::Ptr	laserCloudOri (new pcl::PointCloud< pcl::PointXYZI >())
void	laserCloudSharpHandler (const sensor_msgs::PointCloud2ConstPtr &cornerPointsSharp2)
pcl::PointCloud < pcl::PointXYZI >::Ptr	laserCloudSurfLast (new pcl::PointCloud< pcl::PointXYZI >())
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)
pcl::PointCloud < pcl::PointXYZI >::Ptr	surfPointsFlat (new pcl::PointCloud< pcl::PointXYZI >())
pcl::PointCloud < pcl::PointXYZI >::Ptr	surfPointsLessFlat (new pcl::PointCloud< pcl::PointXYZI >())
void	TransformToEnd (pcl::PointXYZI pi, pcl::PointXYZI po)
void	TransformToStart (pcl::PointXYZI pi, pcl::PointXYZI po)
Variables
float	imuPitchLast = 0
float	imuPitchStart = 0
float	imuRollLast = 0
float	imuRollStart = 0
float	imuShiftFromStartX = 0
float	imuShiftFromStartY = 0
float	imuShiftFromStartZ = 0
float	imuVeloFromStartX = 0
float	imuVeloFromStartY = 0
float	imuVeloFromStartZ = 0
float	imuYawLast = 0
float	imuYawStart = 0
int	laserCloudCornerLastNum
int	laserCloudSurfLastNum
bool	newCornerPointsLessSharp = false
bool	newCornerPointsSharp = false
bool	newImuTrans = false
bool	newLaserCloudFullRes = false
bool	newSurfPointsFlat = false
bool	newSurfPointsLessFlat = false
const double	PI = 3.1415926
float	pointSearchCornerInd1 [40000]
float	pointSearchCornerInd2 [40000]
float	pointSearchSurfInd1 [40000]
float	pointSearchSurfInd2 [40000]
float	pointSearchSurfInd3 [40000]
int	pointSelCornerInd [40000]
int	pointSelSurfInd [40000]
const float	scanPeriod = 0.1
const int	skipFrameNum = 1
bool	systemInited = false
double	timeCornerPointsLessSharp = 0
double	timeCornerPointsSharp = 0
double	timeImuTrans = 0
double	timeLaserCloudFullRes = 0
double	timeSurfPointsFlat = 0
double	timeSurfPointsLessFlat = 0
float	transform [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 236 of file laserOdometry.cpp.

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

pcl::PointCloud<pcl::PointXYZI>::Ptr cornerPointsLessSharp ( new pcl::PointCloud< pcl::PointXYZI > () )

pcl::PointCloud<pcl::PointXYZI>::Ptr cornerPointsSharp ( new pcl::PointCloud< pcl::PointXYZI > () )

pcl::PointCloud<pcl::PointXYZ>::Ptr imuTrans ( new pcl::PointCloud< pcl::PointXYZ > () )

void imuTransHandler ( const sensor_msgs::PointCloud2ConstPtr & imuTrans2 )

Definition at line 305 of file laserOdometry.cpp.

pcl::KdTreeFLANN<pcl::PointXYZI>::Ptr kdtreeCornerLast ( new pcl::KdTreeFLANN< pcl::PointXYZI > () )

pcl::KdTreeFLANN<pcl::PointXYZI>::Ptr kdtreeSurfLast ( new pcl::KdTreeFLANN< pcl::PointXYZI > () )

pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudCornerLast ( new pcl::PointCloud< pcl::PointXYZI > () )

void laserCloudFlatHandler ( const sensor_msgs::PointCloud2ConstPtr & surfPointsFlat2 )

Definition at line 275 of file laserOdometry.cpp.

pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudFullRes ( new pcl::PointCloud< pcl::PointXYZI > () )

void laserCloudFullResHandler ( const sensor_msgs::PointCloud2ConstPtr & laserCloudFullRes2 )

Definition at line 295 of file laserOdometry.cpp.

void laserCloudLessFlatHandler ( const sensor_msgs::PointCloud2ConstPtr & surfPointsLessFlat2 )

Definition at line 285 of file laserOdometry.cpp.

void laserCloudLessSharpHandler ( const sensor_msgs::PointCloud2ConstPtr & cornerPointsLessSharp2 )

Definition at line 265 of file laserOdometry.cpp.

pcl::PointCloud<pcl::PointXYZI>::Ptr laserCloudOri ( new pcl::PointCloud< pcl::PointXYZI > () )

void laserCloudSharpHandler ( const sensor_msgs::PointCloud2ConstPtr & cornerPointsSharp2 )

Definition at line 255 of file laserOdometry.cpp.

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

int main	(	int	argc,
		char **	argv
	)

Definition at line 331 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
	)