Ciclop开源3D扫描仪软件---Horus源码分析之src\horus\engine\calibration\combo_calibration.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:2468851091@qq.com
 *

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

# -*- coding: utf-8 -*-
# This file is part of the Horus Project

__author__ = 'Jes煤s Arroyo Torrens <jesus.arroyo@bq.com>'
__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 np

from horus import Singleton
from horus.engine.calibration.calibration import CalibrationCancel
from horus.engine.calibration.moving_calibration import MovingCalibration
from horus.engine.calibration import laser_triangulation, platform_extrinsics

import logging
logger = logging.getLogger(__name__)

class ComboCalibrationError(Exception):

def __init__(self):
Exception.__init__(self, "ComboCalibrationError")

@Singleton
class ComboCalibration(MovingCalibration):

"""Combine Laser Triangulation and Platform Extrinsics calibration in one"""

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._point_cloud = [None, None]
self.x = []
self.y = []
self.z = []

def _capture(self, angle):
image = self.image_capture.capture_pattern()
pose = self.image_detection.detect_pose(image)
plane = self.image_detection.detect_pattern_plane(pose)
if plane is not None:
distance, normal, corners = plane

# Laser triangulation
if (angle > 65 and angle < 115):
self.image_capture.flush_laser()
self.image_capture.flush_laser()
for i in xrange(2):
image = self.image_capture.capture_laser(i)
image = self.image_detection.pattern_mask(image, corners)
self.image = image
points_2d, _ = self.laser_segmentation.compute_2d_points(image)
point_3d = self.point_cloud_generation.compute_camera_point_cloud(
points_2d, distance, normal)
if self._point_cloud[i] is None:
self._point_cloud[i] = point_3d.T
else:
self._point_cloud[i] = np.concatenate(
(self._point_cloud[i], point_3d.T))

# Platform extrinsics
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

# Laser triangulation
# Save point clouds
for i in xrange(2):
laser_triangulation.save_point_cloud('PC' + str(i) + '.ply', self._point_cloud[i])

self.distance = [None, None]
self.normal = [None, None]
self.std = [None, None]

# Compute planes
for i in xrange(2):
if self._is_calibrating:
plane = laser_triangulation.compute_plane(i, self._point_cloud[i])
self.distance[i], self.normal[i], self.std[i] = plane

# Platform extrinsics
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 = platform_extrinsics.fit_plane(points)
if normal[1] > 0:
normal = -normal
# Fitting a circle inside the plane
center, self.R, circle = platform_extrinsics.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))

# Return response
result = True
if self._is_calibrating:
if self.t is not None and \
np.linalg.norm(self.t - platform_extrinsics.estimated_t) < 100:
response_platform_extrinsics = (
self.R, self.t, center, point, normal, [self.x, self.y, self.z], circle)
else:
result = False

if self.std[0] < 1.0 and self.std[1] < 1.0 and \
self.normal[0] is not None and self.normal[1] is not None:
response_laser_triangulation = ((self.distance[0], self.normal[0], self.std[0]),
(self.distance[1], self.normal[1], self.std[1]))
else:
result = False

if result:
response = (True, (response_platform_extrinsics, response_laser_triangulation))
else:
response = (False, ComboCalibrationError())
else:
response = (False, CalibrationCancel())

self._is_calibrating = False
self.image = None

return response

def accept(self):
for i in xrange(2):
self.calibration_data.laser_planes[i].distance = self.distance[i]
self.calibration_data.laser_planes[i].normal = self.normal[i]
self.calibration_data.platform_rotation = self.R
self.calibration_data.platform_translation = self.t