Ciclop开源3D扫描仪软件---Horus源码分析之point_cloud_generation.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 horus import Singletonfrom horus.engine.calibration.calibration_data import CalibrationData@Singletonclass PointCloudGeneration(object):    def __init__(self):        self.calibration_data = CalibrationData()    def compute_point_cloud(self, theta, points_2d, index):        # Load calibration values        R = np.matrix(self.calibration_data.platform_rotation)        t = np.matrix(self.calibration_data.platform_translation).T        # Compute platform transformation        Xwo = self.compute_platform_point_cloud(points_2d, R, t, index)        # Rotate to world coordinates        c, s = np.cos(-theta), np.sin(-theta)        Rz = np.matrix([[c, -s, 0], [s, c, 0], [0, 0, 1]])        Xw = Rz * Xwo        # Return point cloud        if Xw.size > 0:            return np.array(Xw)        else:            return None    def compute_platform_point_cloud(self, points_2d, R, t, index):        # Load calibration values        n = self.calibration_data.laser_planes[index].normal        d = self.calibration_data.laser_planes[index].distance        # Camera system        Xc = self.compute_camera_point_cloud(points_2d, d, n)        # Compute platform transformation        return R.T * Xc - R.T * t    def compute_camera_point_cloud(self, points_2d, d, n):        # Load calibration values        fx = self.calibration_data.camera_matrix[0][0]        fy = self.calibration_data.camera_matrix[1][1]        cx = self.calibration_data.camera_matrix[0][2]        cy = self.calibration_data.camera_matrix[1][2]        # Compute projection point        u, v = points_2d        x = np.concatenate(((u - cx) / fx, (v - cy) / fy, np.ones(len(u)))).reshape(3, len(u))        # Compute laser intersection        return d / np.dot(n, x) * x

# -*- 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 horus import Singletonfrom horus.engine.calibration.calibration_data import CalibrationData@Singletonclass PointCloudGeneration(object):    def __init__(self):        self.calibration_data = CalibrationData() #从世界坐标系到相机坐标系的转换，需要矩阵[R|t]，其中R是旋转矩阵，t是位移向量。 #如果世界坐标系为X，相机坐标系对应坐标为X‘，那么X' = [R|t]*X。 #从相机坐标系到理想屏幕坐标系的变换就需要内参数矩阵C。那么理想屏幕坐标系L = C*[R|t]*X? #如何获得[R|t]，大致是已知模板上的几个关键点在世界坐标系的坐标即X已知? #然后在摄像头捕获的帧里获得模板上对应点在屏幕坐标系的坐标即L已知， #通过求解线性方程组得到[R|t]的初值，再利用非线性最小二乘法迭代求得最优变换矩阵[R|t]。#大多数情况下，背景是二维平面，识别的物体也是二维平面#对于ARToolkit，识别的Targets就是平面的（但是这种方法鲁棒性不好）?#如果内参数矩阵是已知的，那么知道4个或者更多共面不共线的点就可以计算出相机的姿态。    def compute_point_cloud(self, theta, points_2d, index):        # Load calibration values        R = np.matrix(self.calibration_data.platform_rotation)        t = np.matrix(self.calibration_data.platform_translation).T        # Compute platform transformation        Xwo = self.compute_platform_point_cloud(points_2d, R, t, index)        # Rotate to world coordinates        c, s = np.cos(-theta), np.sin(-theta)        Rz = np.matrix([[c, -s, 0], [s, c, 0], [0, 0, 1]])        Xw = Rz * Xwo        # Return point cloud        if Xw.size > 0:            return np.array(Xw)        else:            return None    def compute_platform_point_cloud(self, points_2d, R, t, index):##这是一个从相机坐标系到理想屏幕坐标系的变换        # Load calibration values        n = self.calibration_data.laser_planes[index].normal        d = self.calibration_data.laser_planes[index].distance        # Camera system        Xc = self.compute_camera_point_cloud(points_2d, d, n)        # Compute platform transformation        return R.T * Xc - R.T * t    def compute_camera_point_cloud(self, points_2d, d, n):        # Load calibration values   装载标定值        fx = self.calibration_data.camera_matrix[0][0]        fy = self.calibration_data.camera_matrix[1][1]        cx = self.calibration_data.camera_matrix[0][2]        cy = self.calibration_data.camera_matrix[1][2]        # Compute projection point        u, v = points_2d        x = np.concatenate(((u - cx) / fx, (v - cy) / fy, np.ones(len(u)))).reshape(3, len(u))        # Compute laser intersection 计算激光相交        return d / np.dot(n, x) * x############这是很重要的激光相交的数据。

#可以在调试中打印。#数组拼接方法三#思路：numpy提供了numpy.concatenate((a1,a2,...), axis=0)函数。能够一次完成多个

#数组的拼接。其中a1,a2,...是数组类型的参数#示例3：#>>> a=np.array([1,2,3])#>>> b=np.array([11,22,33])#>>> c=np.array([44,55,66])#>>> np.concatenate((a,b,c),axis=0)  # 默认情况下，axis=0可以不写#array([ 1,  2,  3, 11, 22, 33, 44, 55, 66]) #对于一维数组拼接，axis的值不影响最后的结果#>>> np.ones(5)#array([ 1.,  1.,  1.,  1.,  1.])#image.reshape((-1, 6))#array([[1, 2, 3, 4, 5, 6],#     [1, 1, 1, 1, 1, 1]])#dot函数是np中的矩阵乘法，#x.dot(y) 等价于 np.dot(x,y)#x是m*n 矩阵 ，y是n*m矩阵#则x.dot(y) 得到m*m矩阵