SLAM 中evo的原理(一)

本文介绍了一种在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

 

### SLAM 轨迹生成与处理方法 SLAM(Simultaneous Localization and Mapping,同步定位与地图构建)的核心目标之是通过传感器数据估计设备的位置和姿态,并同时创建环境的地图。轨迹生成与处理作为SLAM的重要组成部分,在实际应用中具有重要意义。 #### 、轨迹生成原理SLAM系统中,轨迹通常由位姿序列组成,表示相机或机器人随时间变化的空间位置和方向。对于不同的SLAM变体(单目、双目、RGB-D),其轨迹生成方式有所不同: 1. **单目ORB-SLAM2** 单目ORB-SLAM2利用图像特征点匹配和三角化技术计算相对运动[^1]。由于缺乏尺度信息,单目系统的轨迹通常是无尺度的,即仅能提供比例关系而非真实距离。为了获得绝对尺度,可以引入外部约束(如IMU数据或已知地标尺寸)。最终生成的轨迹可以通过evo工具进行评估,验证其准确性。 2. **双目ORB-SLAM2** 双目摄像头提供了额外的深度信息,使得轨迹可以直接以真实世界单位表达。双目ORB-SLAM2基于立体视觉原理,通过两幅图像间的视差计算三维坐标[^3]。这种配置显著提高了轨迹精度,尤其是在动态场景下表现更优。 3. **RGB-D ORB-SLAM2** RGB-D SLAM结合彩色图像和深度图,进步增强了对复杂环境的理解能力。该版本能够直接读取深度传感器的数据,从而简化了深度估算过程[^2]。这不仅提升了轨迹质量,还加快了地图构建速度。 --- #### 二、轨迹处理流程 旦完成轨迹生成,后续需对其进行优化和分析,以下是常见步骤: 1. **滤波平滑** 使用卡尔曼滤波器或其他统计模型去除噪声干扰,使轨迹更加平稳自然。例如,EKF (Extended Kalman Filter) 常被用于实时调整预测值与测量值之间的偏差。 2. **重投影误差校正** 对于每帧的关键点重新投射到原始图像上比较理论位置与实际检测位置差异大小,进而微调当前节点的姿态参数直至满足收敛条件为止。 3. **全局致性改进** 应用Bundle Adjustment算法整体优化整个路径上的所有变量集合{Pi, Xi},其中P代表相机外参矩阵而X则对应空间点坐标的向量形式。这样做的目的是最小化总的几何残差函数F(P,X)=Σ||pi−h(xi,P)||² ,达到最佳拟合效果。 4. **可视化展示** 利用Matplotlib或者Mayavi库绘制二维/三维图形直观呈现移动路线走向以及周围结构布局情况;另外也可以借助rviz插件实现在ROS框架下的交互式观察体验。 --- ```python import numpy as np from scipy.optimize import least_squares def bundle_adjustment(params, points_2d, camera_matrix): """ Perform Bundle Adjustment to refine trajectory. Args: params (array): Initial guess of parameters including poses and landmarks. points_2d (list): List of observed 2D points across frames. camera_matrix (matrix): Calibration matrix of the camera. Returns: array: Optimized set of parameters after adjustment. """ optimized_params = least_squares(fun=compute_residuals, x0=params, args=(points_2d, camera_matrix)) return optimized_params.x def compute_residuals(params, points_2d, camera_matrix): residuals = [] for i in range(len(points_2d)): projected_point = project_to_image(params[i], camera_matrix) residual = points_2d[i] - projected_point residuals.append(residual.flatten()) return np.concatenate(residuals) def project_to_image(pose, cam_mat): R, t = pose[:9].reshape(3, 3), pose[9:].reshape(-1, 1) P = np.hstack([R, t]) homog_coord = np.array([[0, 0, 0, 1]]).T img_coords = cam_mat @ P @ homog_coord return img_coords[:-1] / img_coords[-1] # Example usage params_initial = ... # Define initial guesses here camera_intrinsics = ... # Provide calibration data observed_points = [...] # Collect all detected keypoints over time optimized_result = bundle_adjustment(params_initial, observed_points, camera_intrinsics) print("Optimized Parameters:", optimized_result) ``` --- #### §相关问题§ 1. 如何选择合适的滤波器来减少SLAM轨迹中的抖动? 2. 在不同光照条件下,如何提高ORB-SLAM2提取特征点的鲁棒性? 3. 是否存在其他替代方案代替传统的BA算法来进行大规模轨迹优化? 4. 怎样将多传感器融合技术融入现有的SLAM架构当中提升性能? 5. 面对长时间运行的任务,怎样防止累积漂移现象影响最终结果?
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值