00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #pragma once
00029 #ifndef BUT_SEG_UTILS_FILTERING_H
00030 #define BUT_SEG_UTILS_FILTERING_H
00031
00032
00038
00039 #include <opencv2/highgui/highgui.hpp>
00040 #include <opencv2/imgproc/imgproc_c.h>
00041
00042
00043 #include <sensor_msgs/CameraInfo.h>
00044
00045
00046 #include "normals.h"
00047
00048 namespace but_plane_detector
00049 {
00050 #define NORMAL_COMPUTATION_SIZE 5
00051 #define NORMAL_COMPUTATION_TYPE NormalType::PCL
00052
00053 #define MIN_DISTANCE 0.1
00054 #define MAX_DISTANCE 3.1
00055 #define RANSACMAX 1000
00056
00060 class WatershedType
00061 {
00062 public:
00066 static const int DepthDiff = 1;
00067
00071 static const int NormalDiff = 2;
00072
00076 static const int Combined = 4;
00077
00081 static const int PredictorDiff = 8;
00082 };
00083
00087 class Regions
00088 {
00089 public:
00095 Regions(Normals *normals = NULL);
00096
00100 ~Regions();
00101
00114 void watershedRegions(cv::Mat &src, const sensor_msgs::CameraInfoConstPtr& cam_info, int type = WatershedType::Combined, int seedThreshold = 1, float alpha = 0.0, float beta = 0.0, float gamma = 0.0);
00115
00128 void independentTileRegions(cv::Mat &src, const sensor_msgs::CameraInfoConstPtr& cam_info, float minimumPlaneDistance = 0.01, float minimumTriDistance= 0.02, float minimumFloodDistance = 0.05, int tileSize = 30, int smallestTriangleArea = 100, int minRegionSize = 10, int minCandidates = 10);
00129
00134 bool computeStatistics(float threshold);
00135
00144 static void gradientPlanePredictor(cv::Mat &src, cv::Mat& dst, float differenceThreshold = 0.02, int size = 2, int sobelSize = 5);
00145
00153 static void gradientNormalDifference(cv::Mat &src, cv::Mat& dst, int size, float differenceThreshold);
00154
00162 static void gradientDepthDifference(cv::Mat &src, cv::Mat& dst, int size, float differenceThreshold);
00163
00169 cv::Mat m_regionMatrix;
00170
00176 cv::Mat m_planes;
00177
00183 cv::Mat m_stddeviation;
00184
00190 Normals *m_normals;
00191
00192 private:
00205 int floodFillTile(cv::Mat &tile, cv::Mat&tileMask, cv::Mat&pointMask, cv::Vec3f *points, int index, Plane<float> &plane, int ioffset, int joffset, float min_planeDistance = 0.02);
00206
00216 void getLeastSquaresAndBorder(Plane<float> &plane, cv::Mat &tile, cv::Mat&tileMask, int ioffset, int joffset, int index);
00217
00230 void fillEverything(Plane<float> &plane, cv::Mat & depth, cv::Mat & mask, cv::Mat & points, int i_tile, int j_tile, int tileSize, int index, float min_planeDistance = 0.05);
00231 };
00232 }
00233 #endif // BUT_SEG_UTILS_FILTERING_H