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: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_data import CalibrationData


@Singleton
class 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 <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_data import CalibrationData




@Singleton
class 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矩阵