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 time
import numpy as np

from horus import Singleton
from horus.engine.calibration.calibration import Calibration, CalibrationCancel


class 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")


@Singleton
class 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 time
import numpy as np

from horus import Singleton
from horus.engine.calibration.calibration import Calibration, CalibrationCancel


class 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")


@Singleton
class 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()