slam algorithm

wu_xinjun

2022-03-09 7e23498bb27c683b74b62f1495bc610ccc04bad2

slam algorithm

 .vscode/launch.json

@@ -5,6 +5,22 @@
    "version": "0.2.0",
    "configurations": [
        {
            "name": "test_slam",
            "type": "python",
            "request": "launch",
            "python": "${workspaceFolder}/env/aerial_deploy/python.exe",
            "program": "${workspaceFolder}/src/mannul_slam.py",
            "console": "integratedTerminal",
            "args": [
                "T:\\Data\\projects\\huaneng\\00_Docs\\slamtest",
                "0",
                "60",
                "auto",
                "test60120"
            ],
            "justMyCode": true
        },
        {
            "name": "Client_test",
            "type": "python",
            "request": "launch",

 TEST_Slam.bat

New file
@@ -0,0 +1,31 @@
@echo off

@REM change to the filepath
cd %~dp0
echo change to path: %cd%

set aerialpython=%cd%/env/aerial_deploy/python.exe
set envsetting=%cd%/env/SettingEnv.bat

@REM Setting the env
if not exist %aerialpython% (
    call %envsetting% 
)

set folder=T:\\Data\\projects\\huaneng\\00_Docs\\slamtest
set alpha=0
set beta=60
set gamma=auto
set savename=test60120
set PLYfile=./test60120.ply

cd %~dp0
echo change to path: %cd%

%aerialpython% ./src/mannul_slam.py %folder% %alpha% %beta% %gamma% %savename%


@REM -------------- Usage: plot_ply.py [PLYfile]"
%aerialpython% ./src/utils/plot_ply.py %PLYfile%



 TEST_ply_show .bat

@@ -15,7 +15,7 @@
cd %~dp0
echo change to path: %cd%

set PLYfile=./Manual_Microwave_test/pod1-2022-02-22-15-44-51/28344.ply
set PLYfile=./test60120.ply
@REM -------------- Usage: plot_ply.py [PLYfile]"
%aerialpython% ./src/utils/plot_ply.py %PLYfile%
pause

 src/mannul_slam.py

New file
@@ -0,0 +1,73 @@
import os,sys
import numpy as np
from slam.slam import compose_ply
from utils import read_ply,write_ply,file_fliter

def run(folder,alpha,beta,gamma,savename):
    
    # literate the folder
    dir_List_ = os.listdir(folder)
    sort_lambda = lambda x:int(x.split("°")[0])
    dir_List_.sort(key=sort_lambda)
    
    dir_List=[]
    for k in dir_List_:
        dir_List.append(k[:-1])


    angle = []
    index = 0 
    for i,degree in enumerate([alpha,beta,gamma]):
        if degree == "auto":
            degree = dir_List
            index = i
        angle.append(degree)

    data = []
    
    # iterate the auto angle degree
    for an in angle[index]:
        ply_with_mutiangle = []
        for z in angle:
            if isinstance(z,list):
                z = an
            ply_with_mutiangle.append(z)
        ply_name_list = file_fliter(os.path.join(folder,an+"°"), "ply",lambda x:int(x.split(".")[0]))
        # select the first ply file
        ply = read_ply(os.path.join(folder,an+"°",ply_name_list[0]))
        ply_with_mutiangle.append(ply)
        data.append(ply_with_mutiangle)

    # compose muti ply files to one ply file
    cloudpoint = compose_ply(data)

    # write the file
    write_ply(cloudpoint,".", savename)






if __name__ == "__main__":
    def help():
        print("Usage: mannul_slam.py [folder] [alpha] [beta] [gamma] [savename]")
        print("EXAMPLE: mannul_slam.py T:\\Data\\chuizhi60°_shuiping120° 0 60 auto test60120")
        exit(1)

    if len(sys.argv) != 6:
        help()
    try:
        folder = str(sys.argv[1])
        alpha = str(sys.argv[2])
        beta = str(sys.argv[3])
        gamma = str(sys.argv[4])
        savename = str(sys.argv[5])

    except Exception as e:
        print(e)
        help()

    run(folder,alpha,beta,gamma,savename)
    print("Done")


 src/slam/slam.py

New file
@@ -0,0 +1,85 @@
import numpy as np
import os, sys
# add python path of src to sys.path
src_path = os.path.join(__file__, *(['..'] * 2))
src_path = os.path.abspath(src_path)
sys.path.insert(0, src_path)

from utils.tools import read_ply



# 毫米波坐标系为右手坐标系。
# 毫米波设备分别绕xyz轴的旋转角度分别为 alpha, beta, gamma

# 左乘旋转矩阵分别为 Rx_alpha, Ry_beta, Rz_gamma

# Rx_alpha = [[1,0,0],[0,np.cos(alpha),np.sin(alpha)],[0,-np.sin(alpha),np.cos(alpha)]]
# Ry_beta = [[np.cos(beta),0,-np.sin(beta)],[0,1,0],[np.sin(beta),0,np.cos(beta)]]
# Rz_gamma = [[np.cos(gamma),np.sin(gamma),0],[-np.sin(gamma),np.cos(gamma),0],[0,0,1]]


def get_rotation_matrix(alpha_degree,beta_degree,gamma_degree,verbose=0):
    """generate a rotation matrix from the specific angel
    """
    rad_list = []
    for i, angle in enumerate([alpha_degree,beta_degree,gamma_degree]):

        if np.abs(float(angle))>360:
            raise ValueError(f"incorrect angle value: value in [-360,360] is excepted, but {angle} is offered.")
        rad_list.append(np.pi * float(angle) / 180)

    alpha = rad_list[0]
    Rx_alpha = np.matrix([[1,0,0],
                        [0,np.cos(rad_list[0]),np.sin(alpha)],
                        [0,-np.sin(alpha),np.cos(alpha)]])

    beta = rad_list[1]
    Ry_beta = np.matrix([[np.cos(beta),0,-np.sin(beta)],
                        [0,1,0],
                        [np.sin(beta),0,np.cos(beta)]])

    gamma = rad_list[2]
    Rz_gamma = np.matrix([[np.cos(gamma),np.sin(gamma),0],
                        [-np.sin(gamma),np.cos(gamma),0],
                        [0,0,1]])
         
    rotation_matrix = Rx_alpha * Ry_beta * Rz_gamma

    if verbose == 1:
        print(f"\n{alpha_degree}, rad {alpha}, Rx_alpha \n", Rx_alpha)
        print(f"\n{beta_degree}, {beta}, Ry_beta \n", Ry_beta)
        print(f"\n{gamma_degree}, {gamma}, Rz_gamma\n",Rz_gamma)
        print("\n rotation matrix\n",rotation_matrix)
    else:
        pass

    return rotation_matrix

def compose_ply(muti_ply_with_angle: list):

    ply_nums = len(muti_ply_with_angle)
    composed_ply_array = None

    for i, ply_with_angle in enumerate( muti_ply_with_angle):
        angle = ply_with_angle[:-1]
        ply = ply_with_angle[-1]

        if isinstance(ply,str):
            ply = read_ply(ply)
        
        rotation_matrix = get_rotation_matrix(angle[0],angle[1],angle[2])

        # z = (X * Y^T)^T = (Y * X^T)
        # np.matmul(rotation_matrix,ply.transpose()).transpose() == np.matmul(ply,rotation_matrix)
        rotated_ply = np.matmul(ply,rotation_matrix)
        if i == 0:
            composed_ply_array = np.array(rotated_ply)
        else:
            temp = np.array(rotated_ply)
            composed_ply_array = np.concatenate((composed_ply_array,temp),axis= 0 )
    
    composed_ply_array = composed_ply_array.reshape(-1,3)
    return composed_ply_array



 src/utils/slam.py

File was deleted

 src/utils/tools.py

@@ -1,5 +1,6 @@
import os, sys
import numpy as np
import time

def file_fliter(folder:str, file_ext:str, sort_format):

@@ -38,6 +39,31 @@
    return ply_array


def write_ply(data:np.array,folder:str,name:str):
    file = os.path.join(folder,name+".ply")
    with open (file,'w') as f:
        f.write("ply\n")
        f.write("format ascii 1.0\n")
        f.write("comment write_ply generated\n")
        f.write("element vertex %d\n" % data.shape[0])
        f.write("property float x\n")
        f.write("property float y\n")
        f.write("property float z\n")
        f.write("element face 0\n")
        f.write("property list uchar int vertex_indices\n")
        f.write("end_header\n")
        n_p = 0

        for p in data:
            x = p[0]
            y = p[1]
            z = p[2]
            f.write("{:.5f} {:.5f} {:.5f}\n".format(x, y, z))
            n_p += 1
        print(time.asctime(),f"Frame {name}.ply have been saved with {n_p} points!")



if __name__ == "__main__":
    ply_array = read_ply('src\\microwave\\demo\\demoData\\2522.ply')
    

 test60120.ply

New file
Diff too large