#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Imu.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_listener.h>
#include <tf/transform_broadcaster.h>
#include "cameraParameters.h"
#include "pointDefinition.h"

Include dependency graph for visualOdometry.cpp:

Functions
void	accumulateRotation (double cx, double cy, double cz, double lx, double ly, double lz, double &ox, double &oy, double &oz)
void	depthCloudHandler (const sensor_msgs::PointCloud2ConstPtr &depthCloud2)
void	diffRotation (double cx, double cy, double cz, double lx, double ly, double lz, double &ox, double &oy, double &oz)
void	imageDataHandler (const sensor_msgs::Image::ConstPtr &imageData)
void	imagePointsHandler (const sensor_msgs::PointCloud2ConstPtr &imagePoints2)
void	imuDataHandler (const sensor_msgs::Imu::ConstPtr &imuData)
int	main (int argc, char **argv)
Variables
double	angleSum [3] = {0}
pcl::PointCloud < pcl::PointXYZI >::Ptr	depthCloud (new pcl::PointCloud< pcl::PointXYZI >())
int	depthCloudNum = 0
double	depthCloudTime
pcl::PointCloud< DepthPoint >::Ptr	depthPointsCur (new pcl::PointCloud< DepthPoint >())
int	depthPointsCurNum = 0
pcl::PointCloud< DepthPoint >::Ptr	depthPointsLast (new pcl::PointCloud< DepthPoint >())
int	depthPointsLastNum = 0
ros::Publisher *	depthPointsPubPointer = NULL
pcl::PointCloud< DepthPoint >::Ptr	depthPointsSend (new pcl::PointCloud< DepthPoint >())
pcl::PointCloud< ImagePoint >::Ptr	imagePointsCur (new pcl::PointCloud< ImagePoint >())
int	imagePointsCurNum = 0
double	imagePointsCurTime
pcl::PointCloud< ImagePoint >::Ptr	imagePointsLast (new pcl::PointCloud< ImagePoint >())
int	imagePointsLastNum = 0
double	imagePointsLastTime
pcl::PointCloud< pcl::PointXYZ > ::Ptr	imagePointsProj (new pcl::PointCloud< pcl::PointXYZ >())
ros::Publisher *	imagePointsProjPubPointer = NULL
ros::Publisher *	imageShowPubPointer
bool	imuInited = false
double	imuPitch [imuQueLength] = {0}
double	imuPitchCur = 0
double	imuPitchLast = 0
int	imuPointerFront = 0
int	imuPointerLast = -1
const int	imuQueLength = 200
double	imuRoll [imuQueLength] = {0}
double	imuRollCur = 0
double	imuRollLast = 0
double	imuTime [imuQueLength] = {0}
double	imuYaw [imuQueLength] = {0}
double	imuYawCur = 0
double	imuYawInit = 0
double	imuYawLast = 0
std::vector< float > *	ipDepthCur = new std::vector<float>()
std::vector< float > *	ipDepthLast = new std::vector<float>()
std::vector< int >	ipInd
pcl::PointCloud < pcl::PointXYZHSV >::Ptr	ipRelations (new pcl::PointCloud< pcl::PointXYZHSV >())
pcl::PointCloud < pcl::PointXYZHSV >::Ptr	ipRelations2 (new pcl::PointCloud< pcl::PointXYZHSV >())
std::vector< float >	ipy2
pcl::KdTreeFLANN < pcl::PointXYZI >::Ptr	kdTree (new pcl::KdTreeFLANN< pcl::PointXYZI >())
const double	PI = 3.1415926
std::vector< int >	pointSearchInd
std::vector< float >	pointSearchSqrDis
const int	showDSRate = 2
pcl::PointCloud< ImagePoint >::Ptr	startPointsCur (new pcl::PointCloud< ImagePoint >())
pcl::PointCloud< ImagePoint >::Ptr	startPointsLast (new pcl::PointCloud< ImagePoint >())
pcl::PointCloud < pcl::PointXYZHSV >::Ptr	startTransCur (new pcl::PointCloud< pcl::PointXYZHSV >())
pcl::PointCloud < pcl::PointXYZHSV >::Ptr	startTransLast (new pcl::PointCloud< pcl::PointXYZHSV >())
tf::TransformBroadcaster *	tfBroadcasterPointer = NULL
double	transformSum [6] = {0}
ros::Publisher *	voDataPubPointer = NULL