Ciclop开源3D扫描仪软件---Horus源码分析之src\horus\engine\calibration\platform_extrinsics.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 numpy as npfrom scipy import optimizefrom horus import Singletonfrom horus.engine.calibration.calibration import CalibrationCancelfrom horus.engine.calibration.moving_calibration import MovingCalibrationimport logginglogger = logging.getLogger(__name__)class PlatformExtrinsicsError(Exception):    def __init__(self):        Exception.__init__(self, "PlatformExtrinsicsError")estimated_t = [-5, 90, 320]@Singletonclass PlatformExtrinsics(MovingCalibration):    """Platform extrinsics algorithm:            - Rotation matrix            - Translation vector    """    def __init__(self):        self.image = None        self.has_image = False        MovingCalibration.__init__(self)    def _initialize(self):        self.image = None        self.has_image = True        self.image_capture.stream = False        self.x = []        self.y = []        self.z = []    def _capture(self, angle):        image = self.image_capture.capture_pattern()        pose = self.image_detection.detect_pose(image)        if pose is not None:            plane = self.image_detection.detect_pattern_plane(pose)            if plane is not None:                distance, normal, corners = plane                self.image = self.image_detection.draw_pattern(image, corners)                if corners is not None:                    origin = corners[self.pattern.columns * (self.pattern.rows - 1)][0]                    origin = np.array([[origin[0]], [origin[1]]])                    t = self.point_cloud_generation.compute_camera_point_cloud(                        origin, distance, normal)                    if t is not None:                        self.x += [t[0][0]]                        self.y += [t[1][0]]                        self.z += [t[2][0]]        else:            self.image = image    def _calibrate(self):        self.has_image = False        self.image_capture.stream = True        self.t = None        self.x = np.array(self.x)        self.y = np.array(self.y)        self.z = np.array(self.z)        points = zip(self.x, self.y, self.z)        if len(points) > 4:            # Fitting a plane            point, normal = fit_plane(points)            if normal[1] > 0:                normal = -normal            # Fitting a circle inside the plane            center, self.R, circle = fit_circle(point, normal, points)            # Get real origin            self.t = center - self.pattern.origin_distance * np.array(normal)            logger.info("Platform calibration ")            logger.info(" Translation: " + str(self.t))            logger.info(" Rotation: " + str(self.R).replace('\n', ''))            logger.info(" Normal: " + str(normal))        if self._is_calibrating and self.t is not None and \           np.linalg.norm(self.t - estimated_t) < 100:            response = (True, (self.R, self.t, center, point, normal,                        [self.x, self.y, self.z], circle))        else:            if self._is_calibrating:                response = (False, PlatformExtrinsicsError())            else:                response = (False, CalibrationCancel())        self._is_calibrating = False        self.image = None        return response    def accept(self):        self.calibration_data.platform_rotation = self.R        self.calibration_data.platform_translation = self.t    def set_estimated_size(self, estimated_size):        global estimated_t        estimated_t = estimated_sizedef distance2plane(p0, n0, p):    return np.dot(np.array(n0), np.array(p) - np.array(p0))def residuals_plane(parameters, data_point):    px, py, pz, theta, phi = parameters    nx, ny, nz = np.sin(theta) * np.cos(phi), np.sin(theta) * np.sin(phi), np.cos(theta)    distances = [distance2plane(        [px, py, pz], [nx, ny, nz], [x, y, z]) for x, y, z in data_point]    return distancesdef fit_plane(data):    estimate = [0, 0, 0, 0, 0]  # px,py,pz and zeta, phi    # you may automize this by using the center of mass data    # note that the normal vector is given in polar coordinates    best_fit_values, ier = optimize.leastsq(residuals_plane, estimate, args=(data))    xF, yF, zF, tF, pF = best_fit_values    # point  = [xF,yF,zF]    point = data[0]    normal = -np.array([np.sin(tF) * np.cos(pF), np.sin(tF) * np.sin(pF), np.cos(tF)])    return point, normaldef residuals_circle(parameters, points, s, r, point):    r_, s_, Ri = parameters    plane_point = s_ * s + r_ * r + np.array(point)    distance = [np.linalg.norm(plane_point - np.array([x, y, z])) for x, y, z in points]    res = [(Ri - dist) for dist in distance]    return resdef fit_circle(point, normal, points):    # creating two inplane vectors    # assuming that normal not parallel x!    s = np.cross(np.array([1, 0, 0]), np.array(normal))    s = s / np.linalg.norm(s)    r = np.cross(np.array(normal), s)    r = r / np.linalg.norm(r)  # should be normalized already, but anyhow    # Define rotation    R = np.array([s, r, normal]).T    estimate_circle = [0, 0, 0]  # px,py,pz and zeta, phi    best_circle_fit_values, ier = optimize.leastsq(        residuals_circle, estimate_circle, args=(points, s, r, point))    rF, sF, RiF = best_circle_fit_values    # Synthetic Data    center_point = sF * s + rF * r + np.array(point)    synthetic = [list(center_point + RiF * np.cos(phi) * r + RiF * np.sin(phi) * s)                 for phi in np.linspace(0, 2 * np.pi, 50)]    [cxTupel, cyTupel, czTupel] = [x for x in zip(*synthetic)]    return center_point, R, [cxTupel, cyTupel, czTupel]