zoukankan      html  css  js  c++  java
  • 立体像对空间前方交会-共线方程求解法(python实现)

    一、原理

     

    二、步骤

    a.用各自像片的角元素计算出左右像片的旋转矩阵R1和R2。

    b.有同名像点列出共线方程。

    c.将方程写为未知数的线性方程形式,计算线性系数。

    d.写出误差方程,系数矩阵与常数项。

    e.计算未知点的最小二乘解。

    f.重复以上步骤完成所有点的地面坐标的计算。

    三、示例代码

    # -*- coding: utf-8 -*-
    """
    Created on Mon Nov 25 09:38:08 2019
    
    @author: L JL
    """
    import numpy as np
    import math as m
    
    def r_mat(f,w,k):
       Rf = np.mat([[m.cos(f), 0, -m.sin(f)],
                  [0,         1,          0],
                  [m.sin(f), 0,  m.cos(f)]])
    
       Rw = np.mat([[1,         0,          0],
                  [0, m.cos(w), -m.sin(w)],
                  [0, m.sin(w),  m.cos(w)]])
        
       Rk = np.mat([[m.cos(k), -m.sin(k), 0],
                  [m.sin(k),  m.cos(k), 0],
                  [0,         0,          1]])
        
       R = Rf*Rw*Rk 
        
       return R
    
    def l_mat(In,R,coor):
        l = np.mat(np.zeros((2,3)))
        f = In[0,2]
        xo = In[0,0]
        yo = In[0,1]
        x = coor[0]
        y = coor[1]
        
        l[0,0] = f*R[0,0] + (x-xo)*R[0,2]
        l[0,1] = f*R[1,0] + (x-xo)*R[1,2]
        l[0,2] = f*R[2,0] + (x-xo)*R[2,2]
        l[1,0] = f*R[0,1] + (y-yo)*R[0,2]
        l[1,1] = f*R[1,1] + (y-yo)*R[1,2]
        l[1,2] = f*R[2,1] + (y-yo)*R[2,2]
        
        return l
        
    def l_approximate(In,R,coor,Ex):
        l_app = np.mat(np.zeros((2,1)))
        f = In[0,2]
        xo = In[0,0]
        yo = In[0,1]
        x = coor[0]
        y = coor[1]
        Xs = Ex[0,0]
        Ys = Ex[1,0]
        Zs = Ex[2,0]
        
        l_app[0,0] = (f*R[0,0]*Xs + f*R[1,0]*Ys + f*R[2,0]*Zs
             + (x-xo)*R[0,2]*Xs + (x-xo)*R[1,2]*Ys + (x-xo)*R[2,2]*Zs)
        l_app[1,0] = (f*R[0,1]*Xs + f*R[1,1]*Ys + f*R[2,1]*Zs
             + (y-yo)*R[0,2]*Xs + (y-yo)*R[1,2]*Ys + (y-yo)*R[2,2]*Zs)
        
        return l_app
    
    
    #main
    left_HomonymousImagePoints = [0.153,91.798]    
    right_HomonymousImagePoints = [-78.672,89.122]
        
    left_In = np.mat([0,0,152.91])
    left_Ex = np.mat([[970302.448784],
               [-1138644.971216],
               [3154.584941],
               [0.010425],
               [-0.012437],
               [0.003380]])     
    right_In = np.mat([0,0,152.91])
    right_Ex = np.mat([[971265.303768],
                [-1138634.245942],
                [3154.784258],
                [0.008870],
                [-0.005062],
                [-0.008703]])
        
    R_L = np.mat(np.zeros((3,3)))
    R_R = np.mat(np.zeros((3,3)))
    L = np.mat(np.zeros((4,3)))
    L_app = np.mat(np.zeros((4,1)))
    
    R_L = r_mat(left_Ex[3,0],left_Ex[4,0],left_Ex[5,0])
    R_R = r_mat(right_Ex[3,0],right_Ex[4,0],right_Ex[5,0])
    L[0:2,:] = l_mat(left_In,R_L,left_HomonymousImagePoints)
    L[2:4,:] = l_mat(right_In,R_R,right_HomonymousImagePoints) 
    L_app[0:2,0] = l_approximate(left_In,R_L,left_HomonymousImagePoints,left_Ex)
    L_app[2:4,0] = l_approximate(right_In,R_R,right_HomonymousImagePoints,right_Ex)
    
    GPCoordinates = np.mat(np.zeros((3,1)))
    
    GPCoordinates = (L.T * L).I * L.T * L_app
    
    print("左影像同名点:",left_HomonymousImagePoints)
    print("左影像同名点:",right_HomonymousImagePoints)
    print("地面点坐标:
             X=%f,
             Y=%f,
             Z=%f"
          %(GPCoordinates[0,0],GPCoordinates[1,0],GPCoordinates[2,0]))
  • 相关阅读:
    空指针的问题,感觉自己很傻
    在运行微服务架构的时候报错error creating bean h name 'advisor'.. Unsatisfied dependency..constructor argument with index 0...
    hibernate+oracle+主键varchar2类型,增加序列策略注解失败
    hibernate的报错异常
    7777端口的问题
    soapUI模拟发送json数据时,遇到的中文编码问题
    三、数组的使用
    四、内存中的数组
    一、初步认识数组
    二、数组的初始化
  • 原文地址:https://www.cnblogs.com/liqinglong/p/11985423.html
Copyright © 2011-2022 走看看