SLAM 中evo的原理（一）

最新推荐文章于 2024-12-26 10:55:33 发布

苏源流

最新推荐文章于 2024-12-26 10:55:33 发布

阅读量878

点赞数 1

分类专栏：视觉导航位姿测量视觉SLAM

位姿测量同时被 3 个专栏收录

73 篇文章

订阅专栏

视觉SLAM

61 篇文章

订阅专栏

视觉导航

50 篇文章

订阅专栏

本文介绍了一种在k维欧几里得空间中进行两组点刚性对准的算法，通过计算缩放、旋转和平移参数，实现点集间的最佳匹配，以最小化平方误差。适用于计算机视觉、机器人学等领域。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

"""
 RALIGN - Rigid alignment of two sets of points in k-dimensional
          Euclidean space.  Given two sets of points in
          correspondence, this function computes the scaling,
          rotation, and translation that define the transform TR
          that minimizes the sum of squared errors between TR(X)
          and its corresponding points in Y.  This routine takes
          O(n k^3)-time.
在k维欧几里得空间中两组点的刚性对准。给定两组对应的点，此函数计算定义转换tr的缩放、旋转和平移，以最小化tr（x）与其在y中对应点之间的平方误差之和。此例程需要O（n k^3）-时间。

 Inputs:
   X - a k x n matrix whose columns are points 
   Y - a k x n matrix whose columns are points that correspond to
       the points in X
 Outputs: 
   c, R, t - the scaling, rotation matrix, and translation vector
             defining the linear map TR as 
 
                       TR(x) = c * R * x + t

             such that the average norm of TR(X(:, i) - Y(:, i))
             is minimized.
"""

输入：

x-列为点的k x n矩阵

y-一个k x n矩阵，其列是对应于X中的点

输出：

C，R，T-缩放、旋转矩阵和平移向量将线性映射tr定义为

Tr（x）=c*r*x+t

使得tr（x（：，i）-y（：，i）的平均范数最小化。


"""
Copyright: Carlo Nicolini, 2013
Code adapted from the Mark Paskin Matlab version
from http://openslam.informatik.uni-freiburg.de/data/svn/tjtf/trunk/matlab/ralign.m 
版权所有：Carlo Nicolini，2013代码改编自Mark Paskin Matlab版本
来自http://openslam.informatik.uni-freiburg.de/data/svn/tjtf/trunk/matlab/ralign.m

"""

import numpy as np
def ralign(X,Y):
    m, n = X.shape

    mx = X.mean(1)
    my = Y.mean(1)
    Xc =  X - np.tile(mx, (n, 1)).T
    Yc =  Y - np.tile(my, (n, 1)).T

    sx = np.mean(np.sum(Xc*Xc, 0))
    sy = np.mean(np.sum(Yc*Yc, 0))

    Sxy = np.dot(Yc, Xc.T) / n

    U,D,V = np.linalg.svd(Sxy,full_matrices=True,compute_uv=True)
    V=V.T.copy()
    #print U,"\n\n",D,"\n\n",V
    r = np.rank(Sxy)
    d = np.linalg.det(Sxy)
    S = np.eye(m)
    if r > (m - 1):
        if ( np.det(Sxy) < 0 ):
            S[m, m] = -1;
        elif (r == m - 1):
            if (np.det(U) * np.det(V) < 0):
                S[m, m] = -1  
        else:
            R = np.eye(2)
            c = 1
            t = np.zeros(2)
            return R,c,t

    R = np.dot( np.dot(U, S ), V.T)

    c = np.trace(np.dot(np.diag(D), S)) / sx
    t = my - c * np.dot(R, mx)

    return R,c,t

# Run an example test
# We have 3 points in 3D. Every point is a column vector of this matrix A
A=np.array([[0.57215 ,  0.37512 ,  0.37551] ,[0.23318 ,  0.86846 ,  0.98642],[ 0.79969 ,  0.96778 ,  0.27493]])
# Deep copy A to get B
B=A.copy()
# and sum a translation on z axis (3rd row) of 10 units
B[2,:]=B[2,:]+10

# Reconstruct the transformation with ralign.ralign
R, c, t = ralign(A,B)
print "Rotation matrix=\n",R,"\nScaling coefficient=",c,"\nTranslation vector=",t

#运行示例测试
#我们有3个三维点，每个点都是这个矩阵A的列向量
A=np.数组（[[0.57215，0.37512，0.37551]，[0.23318，0.86846，0.98642]，[0.79969，0.96778，0.27493]）
#深拷贝A得到B
B.A.Copy.（）
#在10个单位的Z轴（第3行）上求和平移
B[2，：]=B[2，：]+10
#用ralign.ralign重新构造转换
R，C，T=RALIGN（A，B）
打印“旋转矩阵=\n”，R，“\n压缩系数=”，C，“\n转换向量=”，T