-
Notifications
You must be signed in to change notification settings - Fork 2
/
plane_segmenter.h
151 lines (118 loc) · 6.2 KB
/
plane_segmenter.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
#ifndef PLANE_SEGMENTER
#define PLANE_SEGMENTER
#include <iostream>
#include <exception>
#include <assert.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/visualization/image_viewer.h>
#include <pcl/filters/filter.h>
#include <pcl/common/common_headers.h>
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/core/core.hpp"
#include "SimpleConfig.h"
struct plane_data {
pcl::ModelCoefficients coeffs;
cv::Mat image;
};
class PlaneSegmenter{
public:
typedef pcl::PointXYZRGBA Point;
typedef pcl::PointCloud<Point> PointCloud;
typedef std::vector< cv::Vec4i > LineArray;
typedef std::vector< pcl::PointXYZ > LinePosArray;
PlaneSegmenter( const std::string & configFileName );
PlaneSegmenter(int maxNumPlanes=6, int minSize=50000,
bool optimize=false, float threshold=0.03,
int sacMethod=0);
//call this to actually run the segmentation algorithm.
//if the user wants to display an image of the lines and planes in 2d, then
//the user can input a pointer to an image viewer.
void segment(const PointCloud::ConstPtr &cloud,
std::vector< plane_data > & planes,
std::vector< LinePosArray > & linePositions,
pcl::visualization::ImageViewer * viewer=NULL );
//set the hough line parameters
void setHoughLinesBinary( float rho, float theta, int threshold,
int minLineLength, int maxLineGap);
void setHoughLinesIntensity( float rho, float theta, int threshold,
int minLineLength, int maxLineGap);
//set the camera intrinsics
void setCameraIntrinsics( float focus_x, float focus_y,
float origin_x, float origin_y );
//sets parameters for several openCV filters that are used inside
//the findLines function
void setFilterParams(int blur, int filterSize,
int intensityErosion, int lineDilation);
void setCannyParams( int binarySize, int binaryLowerThreshold,
int binaryUpperThreshold,
int intensitySize, int intensityLowerThreshold,
int intensityUpperThreshold );
private:
int maxPlaneNumber;
int minPlaneSize;
//these are the parameters for the filters which are applied to images.
int blurSize; //this controls the size of the blurring kernel
//used to blur the intensity image;
int filterSize; //this is the size of the kernel used in the
//morphological closing operation on the binary
//image.
int intensityErosionSize; //this is the size of the dilation on the intensity
//image.
int lineDilationSize; //This controls the amout by which the mask image
//is dilated to remove the edges of the plane
//from the canny edge detection consideration.
//these control the parameters for canny edge detection
int cannyIntensitySize, cannyBinarySize;
int cannyIntensityLowThreshold, cannyIntensityHighThreshold;
int cannyBinaryLowThreshold, cannyBinaryHighThreshold;
//these variables control the parameters of the HoughLines function.
float binary_rhoRes, binary_thetaRes, intensity_rhoRes, intensity_thetaRes;
int binary_threshold, binary_minLineLength, binary_maxLineGap ;
int intensity_threshold, intensity_minLineLength, intensity_maxLineGap ;
//these are the intrinsics of the camera, they must be set for the
//function to work. Not setting these values results in an assertion failure.
float fx, fy, u0, v0;
bool haveSetCamera;
pcl::SACSegmentation<Point> seg;
//this modifies the vector "larger" in place, and resizes it.
//This function assumes that both structures hold integer
//values that get larger.
inline void filterOutIndices( std::vector< int > & larger,
const std::vector<int> & remove );
//this takes a set of indices (validPoints) and sets the corresponding
//cells in a matrix to 255. The matrix should start out as a matrix of all
//zeros.
//This creates a binary image that can be easily used to threshold and
//find lines or features.
inline void cloudToMatBinary(const std::vector< int > & validPoints,
cv::Mat &mat );
inline void cloudToMatIntensity(
const std::vector< int > & validPoints,
cv::Mat &mat,
const PointCloud::ConstPtr & cloud);
//This takes an image (preferably a binary image) and performs the canny
//edge detection algorithm. Then a houghLine algorithm is run to extract lines
inline void findLines(const pcl::PointIndices::Ptr & inliers,
const PointCloud::ConstPtr & cloud,
std::vector< plane_data > & planes,
LineArray & planarLines,
LineArray & intensityLines,
pcl::visualization::ImageViewer * viewer );
//this takes the equation of a plane (Ax + By + Cz + D = 0) as coeffs,
//and the positions of the endpoints of lines in a picture.
//These endpoints are then projected onto the plane to convert the 2d
//line positions into 3d lines on the plane.
//the projection equations have been solved analytically.
inline void linesToPositions( const pcl::ModelCoefficients::Ptr & coeffs,
const LineArray & lines,
LinePosArray & linePositions );
inline void matrixLinesToPositions( const pcl::ModelCoefficients::Ptr & coeffs,
const LineArray & lines,
LinePosArray & linePositions );
inline void orderPoints();
};
#endif