Ciclop开源3D扫描仪软件---Horus源码分析之laser_segmentation.py

/****************************************************************************/
 *
 *                  (c)    光明工作室  2017-2037  COPYRIGHT
 *
 *   光明工作室团队成员大部分来自全国著名985、211工程院校。具有丰富的工程实践经验，
 *本工作室热忱欢迎大家的光临。工作室长期承接嵌入式开发、PCB设计、算法仿真等软硬件设计。
 *
 *
 *1）基于C8051、AVR、MSP430单片机开发。
 *2）基于STM32F103、STM32F407等ARM处理器开发。（IIC、SPI、485、WIFI等相关设计）
 *3）基于C6678、DM388等DSP处理器开发。（视频、网络、通信协议相关设计）
 *4）基于QT、C#软件开发。
 *5）基于OPENCV、OPENGL图像处理算法开发。（基于LINUX、WINDOWS、MATLAB等）
 *6）无人机飞控、地面站程序开发。（大疆、PIX、 qgroundcontrol、missionplanner、MAVLINK）
 *7) ROS机器人操作系统下相关开发。
 *8）LINUX、UCOSII、VXWORKS操作系统开发。
 *
 *
 *                                                 联系方式：
 *                                                 QQ:2468851091 call：18163325140
 *                                                 Email:[e-mail]
 *

/ ****************************************************************************/

# -*- coding: utf-8 -*-# This file is part of the Horus Project__author__ = 'Jes煤s Arroyo Torrens <[e-mail]>'__copyright__ = 'Copyright (C) 2014-2016 Mundo Reader S.L.'__license__ = 'GNU General Public License v2 http://www.gnu.org/licenses/gpl2.html'import cv2import mathimport numpy as npimport scipy.ndimagefrom horus import Singletonfrom horus.engine.calibration.calibration_data import CalibrationDatafrom horus.engine.algorithms.point_cloud_roi import PointCloudROI@Singletonclass LaserSegmentation(object):    def __init__(self):        self.calibration_data = CalibrationData()        self.point_cloud_roi = PointCloudROI()        self.red_channel = 'R (RGB)'        self.threshold_enable = False        self.threshold_value = 0        self.blur_enable = False        self.blur_value = 0        self.window_enable = False        self.window_value = 0        self.refinement_method = 'SGF'    def set_red_channel(self, value):        self.red_channel = value    def set_threshold_enable(self, value):        self.threshold_enable = value    def set_threshold_value(self, value):        self.threshold_value = value    def set_blur_enable(self, value):        self.blur_enable = value    def set_blur_value(self, value):        self.blur_value = 2 * value + 1    def set_window_enable(self, value):        self.window_enable = value    def set_window_value(self, value):        self.window_value = value    def set_refinement_method(self, value):        self.refinement_method = value    def compute_2d_points(self, image):        if image is not None:            image = self.compute_line_segmentation(image)            # Peak detection: center of mass            s = image.sum(axis=1)            v = np.where(s > 0)[0]            u = (self.calibration_data.weight_matrix * image).sum(axis=1)[v] / s[v]            if self.refinement_method == 'SGF':                # Segmented gaussian filter                u, v = self._sgf(u, v, s)            elif self.refinement_method == 'RANSAC':                # Random sample consensus                u, v = self._ransac(u, v)            return (u, v), image    def compute_hough_lines(self, image):        if image is not None:            image = self.compute_line_segmentation(image)            lines = cv2.HoughLines(image, 1, np.pi / 180, 120)            # if lines is not None:            #   rho, theta = lines[0][0]            #   ## Calculate coordinates            #   u1 = rho / np.cos(theta)            #   u2 = u1 - height * np.tan(theta)            return lines    def compute_line_segmentation(self, image, roi_mask=False):        if image is not None:            # Apply ROI mask            if roi_mask:                image = self.point_cloud_roi.mask_image(image)            # Obtain red channel            image = self._obtain_red_channel(image)            if image is not None:                # Threshold image                image = self._threshold_image(image)                # Window mask                image = self._window_mask(image)            return image    def _obtain_red_channel(self, image):        ret = None        if self.red_channel == 'R (RGB)':            ret = cv2.split(image)[0]        elif self.red_channel == 'Cr (YCrCb)':            ret = cv2.split(cv2.cvtColor(image, cv2.COLOR_RGB2YCR_CB))[1]        elif self.red_channel == 'U (YUV)':            ret = cv2.split(cv2.cvtColor(image, cv2.COLOR_RGB2YUV))[1]        return ret    def _threshold_image(self, image):        if self.threshold_enable:            image = cv2.threshold(                image, self.threshold_value, 255, cv2.THRESH_TOZERO)[1]            if self.blur_enable:                image = cv2.blur(image, (self.blur_value, self.blur_value))            image = cv2.threshold(                image, self.threshold_value, 255, cv2.THRESH_TOZERO)[1]        return image    def _window_mask(self, image):        if self.window_enable:            peak = image.argmax(axis=1)            _min = peak - self.window_value            _max = peak + self.window_value + 1            mask = np.zeros_like(image)            for i in xrange(self.calibration_data.height):                mask[i, _min[i]:_max[i]] = 255            # Apply mask            image = cv2.bitwise_and(image, mask)        return image    # Segmented gaussian filter    def _sgf(self, u, v, s):        if len(u) > 1:            i = 0            sigma = 2.0            f = np.array([])            segments = [s[_r] for _r in np.ma.clump_unmasked(np.ma.masked_equal(s, 0))]            # Detect stripe segments            for segment in segments:                j = len(segment)                # Apply gaussian filter                fseg = scipy.ndimage.gaussian_filter(u[i:i + j], sigma=sigma)                f = np.concatenate((f, fseg))                i += j            return f, v        else:            return u, v    # RANSAC implementation: https://github.com/ahojnnes/numpy-snippets/blob/master/ransac.py    def _ransac(self, u, v):        if len(u) > 1:            data = np.vstack((v.ravel(), u.ravel())).T            dr, thetar = self.ransac(data, self.LinearLeastSquares2D(), 2, 2)            u = (dr - v * math.sin(thetar)) / math.cos(thetar)        return u, v    class LinearLeastSquares2D(object):        '''        2D linear least squares using the hesse normal form:            d = x*sin(theta) + y*cos(theta)        which allows you to have vertical lines.        '''        def fit(self, data):            data_mean = data.mean(axis=0)            x0, y0 = data_mean            if data.shape[0] > 2:  # over determined                u, v, w = np.linalg.svd(data - data_mean)                vec = w[0]                theta = math.atan2(vec[0], vec[1])            elif data.shape[0] == 2:  # well determined                theta = math.atan2(data[1, 0] - data[0, 0], data[1, 1] - data[0, 1])            theta = (theta + math.pi * 5 / 2) % (2 * math.pi)            d = x0 * math.sin(theta) + y0 * math.cos(theta)            return d, theta        def residuals(self, model, data):            d, theta = model            dfit = data[:, 0] * math.sin(theta) + data[:, 1] * math.cos(theta)            return np.abs(d - dfit)        def is_degenerate(self, sample):            return False    def ransac(self, data, model_class, min_samples, threshold, max_trials=100):        '''        Fits a model to data with the RANSAC algorithm.        :param data: numpy.ndarray            data set to which the model is fitted, must be of shape NxD where            N is the number of data points and D the dimensionality of the data        :param model_class: object            object with the following methods implemented:             * fit(data): return the computed model             * residuals(model, data): return residuals for each data point             * is_degenerate(sample): return boolean value if sample choice is                degenerate            see LinearLeastSquares2D class for a sample implementation        :param min_samples: int            the minimum number of data points to fit a model        :param threshold: int or float            maximum distance for a data point to count as an inlier        :param max_trials: int, optional            maximum number of iterations for random sample selection, default 100        :returns: tuple            best model returned by model_class.fit, best inlier indices        '''        best_model = None        best_inlier_num = 0        best_inliers = None        data_idx = np.arange(data.shape[0])        for _ in xrange(max_trials):            sample = data[np.random.randint(0, data.shape[0], 2)]            if model_class.is_degenerate(sample):                continue            sample_model = model_class.fit(sample)            sample_model_residua = model_class.residuals(sample_model, data)            sample_model_inliers = data_idx[sample_model_residua < threshold]            inlier_num = sample_model_inliers.shape[0]            if inlier_num > best_inlier_num:                best_inlier_num = inlier_num                best_inliers = sample_model_inliers        if best_inliers is not None:            best_model = model_class.fit(data[best_inliers])        return best_model

# -*- coding: utf-8 -*-# This file is part of the Horus Project__author__ = 'Jes煤s Arroyo Torrens <[e-mail]>'__copyright__ = 'Copyright (C) 2014-2016 Mundo Reader S.L.'__license__ = 'GNU General Public License v2 http://www.gnu.org/licenses/gpl2.html'import cv2import mathimport numpy as npimport scipy.ndimagefrom horus import Singletonfrom horus.engine.calibration.calibration_data import CalibrationData##这是一个数据标定文件from horus.engine.algorithms.point_cloud_roi import PointCloudROI##感兴趣区域的点云数据@Singletonclass LaserSegmentation(object):    def __init__(self):        self.calibration_data = CalibrationData()        self.point_cloud_roi = PointCloudROI()##对应整个工程中一个文件Point_cloud_roi.py，主要对激光照射的区域进行点云重建。        self.red_channel = 'R (RGB)'##我的理解是激光束为红色，所以对红色通道开启。        self.threshold_enable = False        self.threshold_value = 0        self.blur_enable = False        self.blur_value = 0        self.window_enable = False        self.window_value = 0        self.refinement_method = 'SGF'##seminiferous growth factor (SGF)增强现实框架，一种增强方式。    def set_red_channel(self, value):        self.red_channel = value    def set_threshold_enable(self, value):        self.threshold_enable = value    def set_threshold_value(self, value):        self.threshold_value = value    def set_blur_enable(self, value):        self.blur_enable = value    def set_blur_value(self, value):        self.blur_value = 2 * value + 1    def set_window_enable(self, value):        self.window_enable = value    def set_window_value(self, value):        self.window_value = value    def set_refinement_method(self, value):        self.refinement_method = value    def compute_2d_points(self, image):##计算2D点云        if image is not None:##如果图片非空            image = self.compute_line_segmentation(image)            # Peak detection: center of mass            s = image.sum(axis=1)            v = np.where(s > 0)[0]            u = (self.calibration_data.weight_matrix * image).sum(axis=1)[v] / s[v]            if self.refinement_method == 'SGF':                # Segmented gaussian filter                u, v = self._sgf(u, v, s)            elif self.refinement_method == 'RANSAC':                # Random sample consensus                u, v = self._ransac(u, v)            return (u, v), image    def compute_hough_lines(self, image):        if image is not None:            image = self.compute_line_segmentation(image)            lines = cv2.HoughLines(image, 1, np.pi / 180, 120)            #函数HoughLines的原型为：#void HoughLines(InputArray image,OutputArray lines, double rho, double theta, int threshold, double srn=0,double stn=0 )#image为输入图像，要求是单通道的二值图像#lines为输出直线向量，两个元素的向量（ρ，θ）代表一条直线，ρ是从原点（图像的左上角）的距离，θ是直线的角度（单位是弧度），0表示垂直线，π/2表示水平线#rho为距离分辨率#theta为角度分辨率#threshold为阈值，在步骤2中，只有大于该值的点才有可能被当作极大值，即至少有多少条正弦曲线交于一点才被认为是直线#srn和stn在多尺度霍夫变换的时候才会使用，在这里我们只研究经典霍夫变换             # if lines is not None:            #   rho, theta = lines[0][0]            #   ## Calculate coordinates            #   u1 = rho / np.cos(theta)            #   u2 = u1 - height * np.tan(theta)            return lines    def compute_line_segmentation(self, image, roi_mask=False):        if image is not None:            # Apply ROI mask            if roi_mask:                image = self.point_cloud_roi.mask_image(image)##这是一个很重要的掩膜算法，就像是用一个                #蒙皮与线色的激光线相与，可以把除激光线以外的图形挡住，只提取红色激光ROI区域。            # Obtain red channel            image = self._obtain_red_channel(image)##获得红色通道图形。见下面函数。。。。            if image is not None:                # Threshold image                image = self._threshold_image(image)                # Window mask                image = self._window_mask(image)            return image    def _obtain_red_channel(self, image):        ret = None        if self.red_channel == 'R (RGB)':##通道分解 http://blog.csdn.net/eric_pycv/article/details/72887758            ret = cv2.split(image)[0]        elif self.red_channel == 'Cr (YCrCb)':##不同的图形空间的通道分解。            ret = cv2.split(cv2.cvtColor(image, cv2.COLOR_RGB2YCR_CB))[1]        elif self.red_channel == 'U (YUV)':##不同的图形空间的通道分解。            ret = cv2.split(cv2.cvtColor(image, cv2.COLOR_RGB2YUV))[1]        return ret    def _threshold_image(self, image):        if self.threshold_enable:            image = cv2.threshold(                image, self.threshold_value, 255, cv2.THRESH_TOZERO)[1]            if self.blur_enable:                image = cv2.blur(image, (self.blur_value, self.blur_value))            image = cv2.threshold(                image, self.threshold_value, 255, cv2.THRESH_TOZERO)[1]        return image        #cv2.threshold() #这个函数有四个参数，第一个原图像，第二个进行分类的阈值，第三个是高于（低于）阈值时赋予的新值，第四个是一个方法选择参数，常用的有： # cv2.THRESH_BINARY（黑白二值） # cv2.THRESH_BINARY_INV（黑白二值反转） # cv2.THRESH_TRUNC （得到的图像为多像素值） # cv2.THRESH_TOZERO # cv2.THRESH_TOZERO_INV #该函数有两个返回值，第一个retVal（得到的阈值值（在后面一个方法中会用到）），第二个就是阈值化后的图像。 #http://blog.csdn.net/on2way/article/details/46812121    def _window_mask(self, image):        if self.window_enable:            peak = image.argmax(axis=1)            _min = peak - self.window_value            _max = peak + self.window_value + 1            mask = np.zeros_like(image)            for i in xrange(self.calibration_data.height):                mask[i, _min[i]:_max[i]] = 255            # Apply mask            image = cv2.bitwise_and(image, mask)        return image    # Segmented gaussian filter    def _sgf(self, u, v, s):        if len(u) > 1:            i = 0            sigma = 2.0            f = np.array([])            segments = [s[_r] for _r in np.ma.clump_unmasked(np.ma.masked_equal(s, 0))]            # Detect stripe segments            for segment in segments:                j = len(segment)                # Apply gaussian filter                fseg = scipy.ndimage.gaussian_filter(u[i:i + j], sigma=sigma)                f = np.concatenate((f, fseg))                i += j            return f, v        else:            return u, v    # RANSAC implementation: https://github.com/ahojnnes/numpy-snippets/blob/master/ransac.py    def _ransac(self, u, v):        if len(u) > 1:            data = np.vstack((v.ravel(), u.ravel())).T            dr, thetar = self.ransac(data, self.LinearLeastSquares2D(), 2, 2)            u = (dr - v * math.sin(thetar)) / math.cos(thetar)        return u, v    class LinearLeastSquares2D(object):        '''        2D linear least squares using the hesse normal form:            d = x*sin(theta) + y*cos(theta)        which allows you to have vertical lines.        '''        def fit(self, data):            data_mean = data.mean(axis=0)            x0, y0 = data_mean            if data.shape[0] > 2:  # over determined                u, v, w = np.linalg.svd(data - data_mean)                vec = w[0]                theta = math.atan2(vec[0], vec[1])            elif data.shape[0] == 2:  # well determined                theta = math.atan2(data[1, 0] - data[0, 0], data[1, 1] - data[0, 1])            theta = (theta + math.pi * 5 / 2) % (2 * math.pi)            d = x0 * math.sin(theta) + y0 * math.cos(theta)            return d, theta        def residuals(self, model, data):            d, theta = model            dfit = data[:, 0] * math.sin(theta) + data[:, 1] * math.cos(theta)            return np.abs(d - dfit)        def is_degenerate(self, sample):            return False# RANSAC是“RANdom SAmple Consensus（随机抽样一致）”的缩写。#它可以从一组包含“局外点”的观测数据集中，通过迭代方式估计数学模型的参数。#它是一种不确定的算法——它有一定的概率得出一个合理的结果；为了提高概率必须提高迭代次数。# 该算法最早由Fischler和Bolles于1981年提出?#我的理解是，利用激光线会生成很多线状点云，但是也会有很多离散的点，那么利用RANSAC这个算法#把线状点云集中在一条直线上。滤除掉那些离散的点。请参考如下链接。#https://www.cnblogs.com/xrwang/archive/2011/03/09/ransac-1.html    def ransac(self, data, model_class, min_samples, threshold, max_trials=100):        '''        Fits a model to data with the RANSAC algorithm.        :param data: numpy.ndarray            data set to which the model is fitted, must be of shape NxD where            N is the number of data points and D the dimensionality of the data        :param model_class: object            object with the following methods implemented:             * fit(data): return the computed model             * residuals(model, data): return residuals for each data point             * is_degenerate(sample): return boolean value if sample choice is                degenerate            see LinearLeastSquares2D class for a sample implementation        :param min_samples: int            the minimum number of data points to fit a model        :param threshold: int or float            maximum distance for a data point to count as an inlier        :param max_trials: int, optional            maximum number of iterations for random sample selection, default 100        :returns: tuple            best model returned by model_class.fit, best inlier indices        '''        best_model = None        best_inlier_num = 0        best_inliers = None        data_idx = np.arange(data.shape[0])        for _ in xrange(max_trials):            sample = data[np.random.randint(0, data.shape[0], 2)]            if model_class.is_degenerate(sample):                continue            sample_model = model_class.fit(sample)            sample_model_residua = model_class.residuals(sample_model, data)            sample_model_inliers = data_idx[sample_model_residua < threshold]            inlier_num = sample_model_inliers.shape[0]            if inlier_num > best_inlier_num:                best_inlier_num = inlier_num                best_inliers = sample_model_inliers        if best_inliers is not None:            best_model = model_class.fit(data[best_inliers])        return best_model