#include <normals.h>
Public Member Functions | |
cv::Vec4f | getNormal (int i, int j, int step, float depthThreshold) |
cv::Vec4f | getNormalLSQ (int i, int j, int step, float depthThreshold) |
cv::Vec4f | getNormalLSQAround (int i, int j, int step, float depthThreshold) |
cv::Vec4f | getNormalLTS (int i, int j, int step, float depthThreshold, float outlierThreshold, int maxIter) |
cv::Vec4f | getNormalLTSAround (int i, int j, int step, float depthThreshold, float outlierThreshold, int maxIter) |
Normals (cv::Mat &points, const sensor_msgs::CameraInfoConstPtr &cam_info, int normalType=NormalType::PCL, int neighborhood=4, float threshold=0.2, float outlierThreshold=0.02, int iter=3) | |
Normals (pcl::PointCloud< pcl::PointXYZ > &pointcloud, int normalType=NormalType::PCL, int neighborhood=4, float threshold=0.2, float outlierThreshold=0.02, int iter=3) | |
Static Public Member Functions | |
static Plane< float > | LeastSquaresPlane (std::vector< cv::Vec3f > &points) |
Public Attributes | |
sensor_msgs::CameraInfoConstPtr | m_cam_info |
cv::Mat | m_planes |
cv::Mat | m_points |
int | m_quantbins |
std::vector< cv::Vec3f > | m_quantVectors |
Private Member Functions | |
unsigned int | getQuantVector (std::vector< cv::Vec3f > &vec, cv::Vec3f &vector) |
void | initQuantVectors (int n_bins, std::vector< cv::Vec3f > &vec) |
void | nextStep (int step, int &x, int &y, int &plusX, int &plusY) |
but_plane_detector::Normals::Normals | ( | cv::Mat & | points, |
const sensor_msgs::CameraInfoConstPtr & | cam_info, | ||
int | normalType = NormalType::PCL , |
||
int | neighborhood = 4 , |
||
float | threshold = 0.2 , |
||
float | outlierThreshold = 0.02 , |
||
int | iter = 3 |
||
) |
Constructor - computes real point positions (in scene coordinates) and normals and initiates all variables
points | Input CV_16UC depth matrix (raw input from kinect) |
cam_info | Camera info message (ROS) |
normalType | Type of normal computation method (NormalType enum) |
neighborhood | Neighborhood from which normals are computed |
threshold | Threshold for depth difference outlier marking (if depth of neighbor is greater than this threshold, point is skipped) |
outlierThreshold | Outlier threshold for least trimmed squares regression (max error between point depth and proposed plane) |
iter | Maximum RANSAC iterations |
but_plane_detector::Normals::Normals | ( | pcl::PointCloud< pcl::PointXYZ > & | pointcloud, |
int | normalType = NormalType::PCL , |
||
int | neighborhood = 4 , |
||
float | threshold = 0.2 , |
||
float | outlierThreshold = 0.02 , |
||
int | iter = 3 |
||
) |
Constructor - computes real point positions (in scene coordinates) and normals and initiates all variables
pointcloud | Point cloud |
threshold | Threshold for depth difference outlier marking (if depth of neighbor is greater than this threshold, point is skipped) |
neighborhood | Neighborhood from which normals are computed |
Definition at line 282 of file normals.cpp.
cv::Vec4f but_plane_detector::Normals::getNormal | ( | int | i, |
int | j, | ||
int | step, | ||
float | depthThreshold | ||
) |
Function computes normal for point (i, j) using direct computation (mean of surrounding triangles)
i | row index |
j | column index |
step | Neighborhood to compute with |
depthThreshold | Threshold for depth difference outlier marking (if depth of neighbor is greater than this threshold, point is skipped) |
Definition at line 444 of file normals.cpp.
cv::Vec4f but_plane_detector::Normals::getNormalLSQ | ( | int | i, |
int | j, | ||
int | step, | ||
float | depthThreshold | ||
) |
Function computes normal for point (i, j) using least squares regression
i | row index |
j | column index |
step | Neighborhood to compute with |
depthThreshold | Threshold for depth difference outlier marking (if depth of neighbor is greater than this threshold, point is skipped) |
Definition at line 535 of file normals.cpp.
cv::Vec4f but_plane_detector::Normals::getNormalLSQAround | ( | int | i, |
int | j, | ||
int | step, | ||
float | depthThreshold | ||
) |
Function computes normal for point (i, j) using least squares regression with using only outer neighborhood ring
i | row index |
j | column index |
step | Neighborhood to compute with |
depthThreshold | Threshold for depth difference outlier marking (if depth of neighbor is greater than this threshold, point is skipped) |
Definition at line 578 of file normals.cpp.
cv::Vec4f but_plane_detector::Normals::getNormalLTS | ( | int | i, |
int | j, | ||
int | step, | ||
float | depthThreshold, | ||
float | outlierThreshold, | ||
int | maxIter | ||
) |
Function computes normal for point (i, j) using least trimmed squares regression
i | row index |
j | column index |
step | Neighborhood to compute with |
depthThreshold | Threshold for depth difference outlier marking (if depth of neighbor is greater than this threshold, point is skipped) |
outlierThreshold | Threshold for marking outliers in RANSAC (maximum difference from proposed plane) |
maxIter | Maximum RANSAC iterations |
Definition at line 629 of file normals.cpp.
cv::Vec4f but_plane_detector::Normals::getNormalLTSAround | ( | int | i, |
int | j, | ||
int | step, | ||
float | depthThreshold, | ||
float | outlierThreshold, | ||
int | maxIter | ||
) |
Function computes normal for point (i, j) using least trimmed squares regression with using only outer neighborhood ring
i | row index |
j | column index |
step | Neighborhood to compute with |
depthThreshold | Threshold for depth difference outlier marking (if depth of neighbor is greater than this threshold, point is skipped) |
outlierThreshold | Threshold for marking outliers in RANSAC (maximum difference from proposed plane) |
maxIter | Maximum RANSAC iterations |
Definition at line 699 of file normals.cpp.
unsigned int but_plane_detector::Normals::getQuantVector | ( | std::vector< cv::Vec3f > & | vec, |
cv::Vec3f & | vector | ||
) | [private] |
Helper function which returns bin index for given vector (not used, TODO)
vec | Vector of computed quantization vectors |
vector | Vector whos bin we are computing |
Definition at line 905 of file normals.cpp.
void but_plane_detector::Normals::initQuantVectors | ( | int | n_bins, |
std::vector< cv::Vec3f > & | vec | ||
) | [private] |
Helper function which initializes quantization vectors (not used, TODO)
n_bins | Number of quantization bins |
vec | Vector of computed quantization vectors |
Definition at line 885 of file normals.cpp.
Plane< float > but_plane_detector::Normals::LeastSquaresPlane | ( | std::vector< cv::Vec3f > & | points | ) | [static] |
Function computes plane using least squares regression from given point set
points | Vector of Vec3f points |
Definition at line 777 of file normals.cpp.
void but_plane_detector::Normals::nextStep | ( | int | step, |
int & | x, | ||
int & | y, | ||
int & | plusX, | ||
int & | plusY | ||
) | [private] |
Helper function for "Around" functions - sets next point on outer ring
step | Maximum distance from center (neighborhood) |
x | Current x offset |
y | Current y offset |
plusX | Next x shift |
plusY | Next y shift |
Definition at line 854 of file normals.cpp.
sensor_msgs::CameraInfoConstPtr but_plane_detector::Normals::m_cam_info |