#include "includes/CovarianceMatrix.h"
#include "float.h"
#include "math.h"
#include <iostream>
#include <fstream>
#include "pcl/io/pcd_io.h"
#include "includes/point_types.h"
#include "includes/genericUtils.h"
#include "pcl/filters/passthrough.h"
#include "pcl/filters/extract_indices.h"
#include "pcl/features/intensity_spin.h"
#include "pcl/features/normal_3d.h"
#include <opencv/cv.h>
#include <opencv/highgui.h>
#include "moveRobot.h"
#include <vector>
#include "sensor_msgs/point_cloud_conversion.h"
#include "includes/color.cpp"
#include "pcl/kdtree/kdtree.h"
#include "pcl/kdtree/tree_types.h"
#include <pcl_ros/io/bag_io.h>
#include "HOG.cpp"
#include "includes/CombineUtils.h"
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/dynamic_bitset.hpp>
#include "pcl_visualization/pcl_visualizer.h"
#include "time.h"
#include "includes/segmentAndLabel.h"
#include "openni_listener.h"
#include <octomap/octomap.h>
#include <octomap_ros/OctomapROS.h>
#include <octomap_ros/conversions.h>
#include <boost/foreach.hpp>
#include <boost/tokenizer.hpp>
#include "wallDistance.h"

Include dependency graph for openni_listener.cpp:

Classes
class	BinningInfo
class	BinStumps
class	OriginalFrameInfo
class	SpectralProfile
Defines
#define	MAX_TRYS 20
#define	MAX_TURNS 2
#define	NUM_CLASSES 17
#define	SPAN 30
Typedefs
typedef pcl_visualization::PointCloudColorHandler < sensor_msgs::PointCloud2 >	ColorHandler
typedef ColorHandler::Ptr	ColorHandlerPtr
typedef pcl::KdTree< PointT >	KdTree
typedef pcl::KdTree< PointT >::Ptr	KdTreePtr
typedef pcl::PointXYZRGBCamSL	PointT
Functions
void	addToEdgeHeader (std::string featName, size_t numTimes=1)
void	addToNodeHeader (std::string featName, size_t numTimes=1)
void	apply_camera_filter (const pcl::PointCloud< PointT > &incloud, pcl::PointCloud< PointT > &outcloud, int camera)
void	apply_notsegment_filter (const pcl::PointCloud< PointT > &incloud, pcl::PointCloud< PointT > &outcloud, int segment)
void	apply_segment_filter (pcl::PointCloud< PointT > &incloud, pcl::PointCloud< PointT > &outcloud, int segment)
void	apply_segment_filter (pcl::PointCloud< PointT > &incloud, pcl::PointCloud< PointT > &outcloud, int segment, SpectralProfile &feats)
void	apply_segment_filter_and_compute_HOG (pcl::PointCloud< PointT > &incloud, pcl::PointCloud< PointT > &outcloud, int segment, SpectralProfile &feats)
void	buildOctoMap (const pcl::PointCloud< PointT > &cloud, OcTreeROS &tree)
void	concat_feats (vector< float > &features, vector< vector< float > > &feats)
void	concat_feats (vector< float > &features, vector< float > &feats)
void	createTrees (const std::vector< pcl::PointCloud< PointT > > &segment_clouds, std::vector< pcl::KdTreeFLANN< PointT >::Ptr > &trees)
void	evaluate (string inp, string out, int oclass)
int	findNeighbors (PointT centroid, const std::vector< pcl::PointCloud< PointT > > &segment_clouds, const std::vector< pcl::KdTreeFLANN< PointT >::Ptr > &trees, vector< int > &neighbor_map)
void	get_color_features (const pcl::PointCloud< PointT > &cloud, vector< float > &features, SpectralProfile &spectralProfileOfSegment)
void	get_feature_average (vector< vector< float > > &descriptor_results, vector< float > &avg_feats)
void	get_feature_histogram (vector< vector< float > > &descriptor_results, vector< vector< float > > &result, vector< BinningInfo > binningInfos)
void	get_global_features (const pcl::PointCloud< PointT > &cloud, vector< float > &features, SpectralProfile &spectralProfileOfSegment)
int	get_neighbors (const std::vector< pcl::PointCloud< PointT > > &segment_clouds, map< pair< int, int >, float > &distance_matrix, map< int, vector< int > > &neighbor_map)
float	get_occupancy_feature (const pcl::PointCloud< PointT > &cloud1, const pcl::PointCloud< PointT > &cloud2, OcTreeROS &tree)
void	get_pair_features (int segment_id, vector< int > &neighbor_list, map< pair< int, int >, float > &distance_matrix, std::map< int, int > &segment_num_index_map, vector< SpectralProfile > &spectralProfiles, map< int, vector< float > > &edge_features, OcTreeROS &tree)
int	getBinIndex (double value, double min, double step)
int	getBinIndex (pcl::PointXYZ value, pcl::PointXYZ min, pcl::PointXYZ step, int index)
Vector3d	getCenter (pcl::PointCloud< PointT > &cloud)
void	getCentroid (const pcl::PointCloud< PointT > &cloud, Eigen::Vector4f &centroid)
float	getDistanceToBoundary (const pcl::PointCloud< PointT > &cloud1, const pcl::PointCloud< PointT > &cloud2)
void	getMinMax (const pcl::PointCloud< PointT > &cloud, const pcl::PointIndices &indices, Eigen::Vector4f &min_p, Eigen::Vector4f &max_p)
void	getMinMax (const pcl::PointCloud< PointT > &cloud, Eigen::Vector4f &min_p, Eigen::Vector4f &max_p)
void	getMovement (bool lookFor)
void	getSegmentDistanceToBoundary (const pcl::PointCloud< PointT > &cloud, map< int, float > &segment_boundary_distance)
pair< float, int >	getSmallestDistance (PointT centroid, pcl::KdTreeFLANN< PointT >::Ptr tree)
pair< float, int >	getSmallestDistance (const pcl::PointCloud< PointT > &cloud1, const pcl::PointCloud< PointT > &cloud2)
void	getSpectralProfile (const pcl::PointCloud< PointT > &cloud, SpectralProfile &spectralProfile)
void	getSpectralProfileCent (const pcl::PointCloud< PointT > &cloud, SpectralProfile &spectralProfile)
boost::dynamic_bitset	labelsAlreadyLookedFor (NUM_CLASSES)
boost::dynamic_bitset	labelsFound (NUM_CLASSES)
boost::dynamic_bitset	labelsToFindBitset (NUM_CLASSES)
void	lookForClass (vector< int > &classes, pcl::PointCloud< pcl::PointXYZRGBCamSL > &cloud, vector< SpectralProfile > &spectralProfiles, map< int, int > &segIndex2label, const std::vector< pcl::PointCloud< PointT > > &segment_clouds, int scene_num, vector< pcl::PointXYZI > &maximas)
void	lookForClassInOriginalFrameOrthographic (vector< int > &classes, pcl::PointCloud< pcl::PointXYZRGBCamSL > &cloud, vector< SpectralProfile > &spectralProfiles, map< int, int > &segIndex2label, const std::vector< pcl::PointCloud< PointT > > &segment_clouds, int scene_num, vector< pcl::PointXYZI > &maximas)
void	lookForClassInOriginalFrameProjective (vector< int > &classes, pcl::PointCloud< pcl::PointXYZRGBCamSL > &cloud, vector< SpectralProfile > &spectralProfiles, map< int, int > &segIndex2label, const std::vector< pcl::PointCloud< PointT > > &segment_clouds, int scene_num, vector< pcl::PointXYZI > &maximas)
int	main (int argc, char **argv)
boost::dynamic_bitset	maximaChanged (NUM_CLASSES)
vector< int >	maximaFrames (NUM_CLASSES, 0)
vector< pcl::PointXYZI >	maximas (NUM_CLASSES)
void	parseAndApplyLabels (std::ifstream &file, pcl::PointCloud< pcl::PointXYZRGBCamSL > &cloud, std::vector< pcl::PointCloud< PointT > > &segment_clouds, map< int, int > &segIndex2label)
void	printLabelsFound (int turnCount)
void	printLabelsToLookFor ()
void	processPointCloud (const sensor_msgs::PointCloud2ConstPtr &point_cloud)
void	readAllStumpValues ()
void	readInvLabelMap (map< int, int > &invLabelMap, const string &file)
void	readLabelList (const string &file)
void	readStumpValues (vector< BinStumps > &featBins, const string &file)
void	readWeightVectors ()
void	robotMovementControl (const sensor_msgs::PointCloud2ConstPtr &point_cloud)
void	robotMovementControlRandom (const sensor_msgs::PointCloud2ConstPtr &point_cloud)
void	robotMovementControlSingleObject (const sensor_msgs::PointCloud2ConstPtr &point_cloud)
void	saveOriginalImages (const pcl::PointCloud< pcl::PointXYZRGBCamSL > &cloud, pcl::PointXYZ max, pcl::PointXYZ min, pcl::PointXYZ steps, int scene_num)
int	write_feats (TransformG transG, pcl::PointCloud< pcl::PointXYZRGBCamSL >::Ptr &cloud_ptr, int scene_num)
Variables
bool	addEdgeHeader = true
bool	addNodeHeader = true
bool	all_done = false
bool	BinFeatures = true
pcl::PointCloud< PointT >	cloudUntransformed
pcl::PointCloud< PointT >	cloudUntransformedUnfiltered
vector< pcl::PointCloud < pcl::PointXYZRGBCamSL > >	cloudVector
int	counts [640 *480]
double	currentAngle = 0
static const string	edgeBinFile = "binStumpsE.txt"
vector< int >	edgeFeatIndices
vector< std::string >	edgeFeatNames
vector< BinStumps >	edgeFeatStumps
Matrix< float, Dynamic, Dynamic > *	edgeWeights [NUM_CLASSES]
string	environment
bool	foundAny = false
TransformG	globalTransform
map< int, int >	invLabelMap
map< int, int >	labelMap
std::ofstream	labelsFoundFile
vector< int >	labelsToFind
vector< int >	labelsToLookFor
vector< vector< pcl::PointXYZI > >	locations
int	MIN_SEG_SIZE = 300
static const string	nodeBinFile = "binStumpsN.txt"
vector< int >	nodeFeatIndices
vector< std::string >	nodeFeatNames
vector< BinStumps >	nodeFeatStumps
Matrix< float, Dynamic, Dynamic > *	nodeWeights
int	NUM_ASSOCIATIVE_FEATS = 4 + 1
int	objCount = 0
OriginalFrameInfo *	originalFrame
bool	originalScan = true
ros::Publisher	pub
MoveRobot *	robot
vector< double >	rotations
vector< int >	sceneNumVector
map< int, double >	sceneToAngleMap
std::vector< std::map< int, int > >	segIndex2LabelVector
vector< vector < pcl::PointCloud< PointT > > >	segment_cloudsVector
vector< vector< SpectralProfile > >	spectralProfilesVector
int	step = 1
vector< double >	translations
bool	translationState = false
int	turnCount = 0
bool	UseVolFeats = false
pcl::PCDWriter	writer

Define Documentation

#define MAX_TRYS 20

Definition at line 236 of file openni_listener.cpp.

#define MAX_TURNS 2

Definition at line 235 of file openni_listener.cpp.

#define NUM_CLASSES 17

Definition at line 62 of file openni_listener.cpp.

#define SPAN 30

Definition at line 261 of file openni_listener.cpp.

Typedef Documentation

typedef pcl_visualization::PointCloudColorHandler<sensor_msgs::PointCloud2> ColorHandler

Definition at line 32 of file openni_listener.cpp.

typedef ColorHandler::Ptr ColorHandlerPtr

Definition at line 33 of file openni_listener.cpp.

typedef pcl::KdTree< PointT > KdTree

Definition at line 35 of file openni_listener.cpp.

typedef pcl::KdTree< PointT >::Ptr KdTreePtr

Definition at line 36 of file openni_listener.cpp.

typedef pcl::PointXYZRGBCamSL PointT

Definition at line 26 of file openni_listener.cpp.

Function Documentation

void addToEdgeHeader	(	std::string	featName,
		size_t	numTimes = `1`
	)		`[inline]`

Definition at line 646 of file openni_listener.cpp.

void addToNodeHeader	(	std::string	featName,
		size_t	numTimes = `1`
	)

Definition at line 636 of file openni_listener.cpp.

void apply_camera_filter	(	const pcl::PointCloud< PointT > &	incloud,
		pcl::PointCloud< PointT > &	outcloud,
		int	camera
	)

Definition at line 826 of file openni_listener.cpp.

void apply_notsegment_filter	(	const pcl::PointCloud< PointT > &	incloud,
		pcl::PointCloud< PointT > &	outcloud,
		int	segment
	)

Definition at line 796 of file openni_listener.cpp.

void apply_segment_filter	(	pcl::PointCloud< PointT > &	incloud,
		pcl::PointCloud< PointT > &	outcloud,
		int	segment
	)

Definition at line 676 of file openni_listener.cpp.

void apply_segment_filter	(	pcl::PointCloud< PointT > &	incloud,
		pcl::PointCloud< PointT > &	outcloud,
		int	segment,
		SpectralProfile &	feats
	)

Definition at line 753 of file openni_listener.cpp.

void apply_segment_filter_and_compute_HOG	(	pcl::PointCloud< PointT > &	incloud,
		pcl::PointCloud< PointT > &	outcloud,
		int	segment,
		SpectralProfile &	feats
	)

Definition at line 710 of file openni_listener.cpp.

void buildOctoMap	(	const pcl::PointCloud< PointT > &	cloud,
		OcTreeROS &	tree
	)

Definition at line 656 of file openni_listener.cpp.

void concat_feats	(	vector< float > &	features,
		vector< vector< float > > &	feats
	)

Definition at line 1190 of file openni_listener.cpp.

void concat_feats	(	vector< float > &	features,
		vector< float > &	feats
	)

Definition at line 1200 of file openni_listener.cpp.

void createTrees	(	const std::vector< pcl::PointCloud< PointT > > &	segment_clouds,
		std::vector< pcl::KdTreeFLANN< PointT >::Ptr > &	trees
	)

Definition at line 311 of file openni_listener.cpp.

void evaluate	(	string	inp,
		string	out,
		int	oclass
	)

Definition at line 2798 of file openni_listener.cpp.

int findNeighbors	(	PointT	centroid,
		const std::vector< pcl::PointCloud< PointT > > &	segment_clouds,
		const std::vector< pcl::KdTreeFLANN< PointT >::Ptr > &	trees,
		vector< int > &	neighbor_map
	)

Definition at line 348 of file openni_listener.cpp.

void get_color_features	(	const pcl::PointCloud< PointT > &	cloud,
		vector< float > &	features,
		SpectralProfile &	spectralProfileOfSegment
	)

Definition at line 1207 of file openni_listener.cpp.

void get_feature_average	(	vector< vector< float > > &	descriptor_results,
		vector< float > &	avg_feats
	)

Definition at line 1114 of file openni_listener.cpp.

void get_feature_histogram	(	vector< vector< float > > &	descriptor_results,
		vector< vector< float > > &	result,
		vector< BinningInfo >	binningInfos
	)

Definition at line 1140 of file openni_listener.cpp.

void get_global_features	(	const pcl::PointCloud< PointT > &	cloud,
		vector< float > &	features,
		SpectralProfile &	spectralProfileOfSegment
	)

Definition at line 1245 of file openni_listener.cpp.

int get_neighbors	(	const std::vector< pcl::PointCloud< PointT > > &	segment_clouds,
		map< pair< int, int >, float > &	distance_matrix,
		map< int, vector< int > > &	neighbor_map
	)

Parameters:

segment_clouds
distance_matrix	: it will be populated by this method
neighbor_map	: it will be populated by this method(in adjacency list format)

Returns:: : number of edges

Definition at line 939 of file openni_listener.cpp.

float get_occupancy_feature	(	const pcl::PointCloud< PointT > &	cloud1,
		const pcl::PointCloud< PointT > &	cloud2,
		OcTreeROS &	tree
	)

Definition at line 1288 of file openni_listener.cpp.

void get_pair_features	(	int	segment_id,
		vector< int > &	neighbor_list,
		map< pair< int, int >, float > &	distance_matrix,
		std::map< int, int > &	segment_num_index_map,
		vector< SpectralProfile > &	spectralProfiles,
		map< int, vector< float > > &	edge_features,
		OcTreeROS &	tree
	)

Definition at line 1339 of file openni_listener.cpp.

int getBinIndex	(	double	value,
		double	min,
		double	step
	)

Definition at line 1545 of file openni_listener.cpp.

int getBinIndex	(	pcl::PointXYZ	value,
		pcl::PointXYZ	min,
		pcl::PointXYZ	step,
		int	index
	)

Definition at line 1548 of file openni_listener.cpp.

Vector3d getCenter ( pcl::PointCloud< PointT > & cloud )

Definition at line 2780 of file openni_listener.cpp.

void getCentroid	(	const pcl::PointCloud< PointT > &	cloud,
		Eigen::Vector4f &	centroid
	)

Definition at line 1042 of file openni_listener.cpp.

float getDistanceToBoundary	(	const pcl::PointCloud< PointT > &	cloud1,
		const pcl::PointCloud< PointT > &	cloud2
	)

Definition at line 856 of file openni_listener.cpp.

void getMinMax	(	const pcl::PointCloud< PointT > &	cloud,
		const pcl::PointIndices &	indices,
		Eigen::Vector4f &	min_p,
		Eigen::Vector4f &	max_p
	)

Definition at line 1012 of file openni_listener.cpp.

void getMinMax	(	const pcl::PointCloud< PointT > &	cloud,
		Eigen::Vector4f &	min_p,
		Eigen::Vector4f &	max_p
	)

Definition at line 1027 of file openni_listener.cpp.

void getMovement ( bool lookFor )

Definition at line 2486 of file openni_listener.cpp.

void getSegmentDistanceToBoundary	(	const pcl::PointCloud< PointT > &	cloud,
		map< int, float > &	segment_boundary_distance
	)

Definition at line 970 of file openni_listener.cpp.

pair<float, int> getSmallestDistance	(	PointT	centroid,
		pcl::KdTreeFLANN< PointT >::Ptr	tree
	)

Definition at line 322 of file openni_listener.cpp.

pair<float, int> getSmallestDistance	(	const pcl::PointCloud< PointT > &	cloud1,
		const pcl::PointCloud< PointT > &	cloud2
	)

Definition at line 871 of file openni_listener.cpp.

void getSpectralProfile	(	const pcl::PointCloud< PointT > &	cloud,
		SpectralProfile &	spectralProfile
	)

Definition at line 1057 of file openni_listener.cpp.

void getSpectralProfileCent	(	const pcl::PointCloud< PointT > &	cloud,
		SpectralProfile &	spectralProfile
	)

Definition at line 1104 of file openni_listener.cpp.

boost::dynamic_bitset labelsAlreadyLookedFor ( NUM_CLASSES )

boost::dynamic_bitset labelsFound ( NUM_CLASSES )

boost::dynamic_bitset labelsToFindBitset ( NUM_CLASSES )

void lookForClass	(	vector< int > &	classes,
		pcl::PointCloud< pcl::PointXYZRGBCamSL > &	cloud,
		vector< SpectralProfile > &	spectralProfiles,
		map< int, int > &	segIndex2label,
		const std::vector< pcl::PointCloud< PointT > > &	segment_clouds,
		int	scene_num,
		vector< pcl::PointXYZI > &	maximas
	)

Definition at line 1969 of file openni_listener.cpp.

void lookForClassInOriginalFrameOrthographic	(	vector< int > &	classes,
		pcl::PointCloud< pcl::PointXYZRGBCamSL > &	cloud,
		vector< SpectralProfile > &	spectralProfiles,
		map< int, int > &	segIndex2label,
		const std::vector< pcl::PointCloud< PointT > > &	segment_clouds,
		int	scene_num,
		vector< pcl::PointXYZI > &	maximas
	)

Definition at line 1551 of file openni_listener.cpp.

void lookForClassInOriginalFrameProjective	(	vector< int > &	classes,
		pcl::PointCloud< pcl::PointXYZRGBCamSL > &	cloud,
		vector< SpectralProfile > &	spectralProfiles,
		map< int, int > &	segIndex2label,
		const std::vector< pcl::PointCloud< PointT > > &	segment_clouds,
		int	scene_num,
		vector< pcl::PointXYZI > &	maximas
	)

Definition at line 1756 of file openni_listener.cpp.

int main	(	int	argc,
		char **	argv
	)

Definition at line 2872 of file openni_listener.cpp.

boost::dynamic_bitset maximaChanged ( NUM_CLASSES )

vector<int> maximaFrames	(	NUM_CLASSES	,
		0
	)

vector<pcl::PointXYZI> maximas ( NUM_CLASSES )

void parseAndApplyLabels	(	std::ifstream &	file,
		pcl::PointCloud< pcl::PointXYZRGBCamSL > &	cloud,
		std::vector< pcl::PointCloud< PointT > > &	segment_clouds,
		map< int, int > &	segIndex2label
	)

Definition at line 1414 of file openni_listener.cpp.

void printLabelsFound ( int turnCount )

Definition at line 2470 of file openni_listener.cpp.

void printLabelsToLookFor ( )

Definition at line 2460 of file openni_listener.cpp.

void processPointCloud ( const sensor_msgs::PointCloud2ConstPtr & point_cloud )

Definition at line 2437 of file openni_listener.cpp.

void readAllStumpValues ( )

Definition at line 433 of file openni_listener.cpp.

void readInvLabelMap	(	map< int, int > &	invLabelMap,
		const string &	file
	)

Definition at line 411 of file openni_listener.cpp.

void readLabelList ( const string & file )

Definition at line 2406 of file openni_listener.cpp.

void readStumpValues	(	vector< BinStumps > &	featBins,
		const string &	file
	)

Definition at line 373 of file openni_listener.cpp.

void readWeightVectors ( )

Definition at line 393 of file openni_listener.cpp.

void robotMovementControl ( const sensor_msgs::PointCloud2ConstPtr & point_cloud )

Definition at line 2566 of file openni_listener.cpp.

void robotMovementControlRandom ( const sensor_msgs::PointCloud2ConstPtr & point_cloud )

Definition at line 2748 of file openni_listener.cpp.

void robotMovementControlSingleObject ( const sensor_msgs::PointCloud2ConstPtr & point_cloud )

Definition at line 2698 of file openni_listener.cpp.

void saveOriginalImages	(	const pcl::PointCloud< pcl::PointXYZRGBCamSL > &	cloud,
		pcl::PointXYZ	max,
		pcl::PointXYZ	min,
		pcl::PointXYZ	steps,
		int	scene_num
	)