Ciclop开源3D扫描仪软件---Horus源码分析之src\horus\engine\calibration\autocheck.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 timeimport numpy as npfrom horus import Singletonfrom horus.engine.calibration.calibration import Calibration, CalibrationCancelclass PatternNotDetected(Exception):    def __init__(self):        Exception.__init__(self, "Pattern Not Detected")class WrongMotorDirection(Exception):    def __init__(self):        Exception.__init__(self, "Wrong Motor Direction")class LaserNotDetected(Exception):    def __init__(self):        Exception.__init__(self, "Laser Not Detected")class WrongLaserPosition(Exception):    def __init__(self):        Exception.__init__(self, "Wrong Laser Position")@Singletonclass Autocheck(Calibration):    """Auto check algorithm:            - Check pattern detection            - Check motor direction            - Check lasers    """    def __init__(self):        self.image = None        Calibration.__init__(self)    def _start(self):        if self.driver.is_connected:            ret = False            response = None            self.image = None            self._is_calibrating = True            self.image_capture.stream = False            # Setup scanner            self.driver.board.lasers_off()            self.driver.board.motor_enable()            self.driver.board.motor_reset_origin()            self.driver.board.motor_speed(200)            self.driver.board.motor_acceleration(200)            # Perform autocheck            try:                self.check_pattern_and_motor()                self.check_lasers()                ret = True            except Exception as exception:                response = exception            finally:                self._is_calibrating = False                self.image_capture.stream = True                self.driver.board.lasers_off()                self.driver.board.motor_disable()                if self._progress_callback is not None:                    self._progress_callback(100)                if self._after_callback is not None:                    self._after_callback((ret, response))                self.image = None    def check_pattern_and_motor(self):        scan_step = 30        patterns_detected = {}        patterns_sorted = {}        if self._progress_callback is not None:            self._progress_callback(0)        # Capture data        for i in xrange(0, 360, scan_step):            if not self._is_calibrating:                raise CalibrationCancel()            image = self.image_capture.capture_pattern()            pose = self.image_detection.detect_pose(image)            if pose is not None:                self.image = self.image_detection.draw_pattern(image, pose[2])                patterns_detected[i] = pose[0].T[2][0]            else:                self.image = self.image_detection.detect_pattern(image)            if self._progress_callback is not None:                self._progress_callback(i / 3.6)            self.driver.board.motor_move(scan_step)        # Check pattern detection        if len(patterns_detected) == 0:            raise PatternNotDetected()        # Check motor direction        max_x = max(patterns_detected.values())        max_i = [key for key, value in patterns_detected.items() if value == max_x][0]        min_v = max_x        for i in xrange(max_i, max_i + 360, scan_step):            if i % 360 in patterns_detected:                v = patterns_detected[i % 360]                patterns_sorted[i] = v                if v <= min_v:                    min_v = v                else:                    raise WrongMotorDirection()        # Move to nearest position        x = np.array(patterns_sorted.keys())        y = np.array(patterns_sorted.values())        A = np.vstack([x, np.ones(len(x))]).T        m, c = np.linalg.lstsq(A, y)[0]        pos = -c / m % 360        if pos > 180:            pos = pos - 360        self.driver.board.motor_move(pos)    def check_lasers(self):        image = self.image_capture.capture_pattern()        corners = self.image_detection.detect_corners(image)        self.image_capture.flush_laser()        for i in xrange(2):            if not self._is_calibrating:                raise CalibrationCancel()            image = self.image_capture.capture_laser(i)            image = self.image_detection.pattern_mask(image, corners)            lines = self.laser_segmentation.compute_hough_lines(image)            if lines is None:                raise LaserNotDetected()

# -*- 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 timeimport numpy as npfrom horus import Singletonfrom horus.engine.calibration.calibration import Calibration, CalibrationCancelclass PatternNotDetected(Exception):##这里检查相关的模式    def __init__(self):        Exception.__init__(self, "Pattern Not Detected")class WrongMotorDirection(Exception):    def __init__(self):        Exception.__init__(self, "Wrong Motor Direction")##如果电机存在错误的旋转方向，打印此消息。class LaserNotDetected(Exception):    def __init__(self):        Exception.__init__(self, "Laser Not Detected")##如果没有检测到激光器，则输出这个异常。class WrongLaserPosition(Exception):    def __init__(self):        Exception.__init__(self, "Wrong Laser Position")@Singletonclass Autocheck(Calibration):    """Auto check algorithm:            - Check pattern detection            - Check motor direction            - Check lasers    """    def __init__(self):        self.image = None        Calibration.__init__(self)    def _start(self):        if self.driver.is_connected:            ret = False            response = None            self.image = None            self._is_calibrating = True            self.image_capture.stream = False            # Setup scanner            self.driver.board.lasers_off()##关闭激光器            self.driver.board.motor_enable()##使能电机            self.driver.board.motor_reset_origin()            self.driver.board.motor_speed(200)##设置电机速度，200具体是什么单位要看SPEED            self.driver.board.motor_acceleration(200)##设置电机回速度            # Perform autocheck            try:                self.check_pattern_and_motor()                self.check_lasers()                ret = True            except Exception as exception:                response = exception            finally:                self._is_calibrating = False                self.image_capture.stream = True                self.driver.board.lasers_off()                self.driver.board.motor_disable()                if self._progress_callback is not None:                    self._progress_callback(100)                if self._after_callback is not None:                    self._after_callback((ret, response))                self.image = None    def check_pattern_and_motor(self):        scan_step = 30##步进速度，30个单位一步        patterns_detected = {}        patterns_sorted = {}        if self._progress_callback is not None:            self._progress_callback(0)        # Capture data        for i in xrange(0, 360, scan_step):#从0到360度旋转，每步30个单位。            if not self._is_calibrating:                raise CalibrationCancel()            image = self.image_capture.capture_pattern()            pose = self.image_detection.detect_pose(image)            if pose is not None:                self.image = self.image_detection.draw_pattern(image, pose[2])                patterns_detected[i] = pose[0].T[2][0]            else:                self.image = self.image_detection.detect_pattern(image)            if self._progress_callback is not None:                self._progress_callback(i / 3.6)            self.driver.board.motor_move(scan_step)        # Check pattern detection        if len(patterns_detected) == 0:            raise PatternNotDetected()        # Check motor direction        max_x = max(patterns_detected.values())        max_i = [key for key, value in patterns_detected.items() if value == max_x][0]        min_v = max_x        for i in xrange(max_i, max_i + 360, scan_step):            if i % 360 in patterns_detected:                v = patterns_detected[i % 360]                patterns_sorted[i] = v                if v <= min_v:                    min_v = v                else:                    raise WrongMotorDirection()        # Move to nearest position        x = np.array(patterns_sorted.keys())        y = np.array(patterns_sorted.values())        A = np.vstack([x, np.ones(len(x))]).T        m, c = np.linalg.lstsq(A, y)[0]        pos = -c / m % 360        if pos > 180:            pos = pos - 360        self.driver.board.motor_move(pos)    def check_lasers(self):        image = self.image_capture.capture_pattern()        corners = self.image_detection.detect_corners(image)        self.image_capture.flush_laser()        for i in xrange(2):            if not self._is_calibrating:                raise CalibrationCancel()            image = self.image_capture.capture_laser(i)            image = self.image_detection.pattern_mask(image, corners)            lines = self.laser_segmentation.compute_hough_lines(image)            if lines is None:                raise LaserNotDetected()