技术标签： 自动驾驶
Kabsch算法【1】由W.Kabsch在1976年提出的，用于求解最优旋转，在分子生物学，特别是比较蛋白质的相似性方面有重要的应用。
文章【2】将其应用在传感器外参标定上，即对属于同一目标的两批三维点，通过Kabsh算法求得其旋转矩阵R。
设 P P P和 Q Q Q是同一个目标在不同坐标系下的两组点， p i p_i pi和 q i q_i qi是集合内的第 i i i个点。令两个坐标系的旋转为 R R R，平移为 T T T，那么Kabsch求解下述优化问题：
(1) m i n E = 1 N ∑ i = 1 N ∣ ∣ R p i − q i + T ∣ ∣ 2 2 minE = \frac{1}{N}\sum_{i=1}^{N}{||Rp_i-q_i+T||_2^2}\tag{1} minE=N1i=1∑N∣∣Rpi−qi+T∣∣22(1)
由(1)式，可以推得
(2) m i n N E = ∑ i = 1 N ∣ ∣ p i , − q i ∣ ∣ 2 2 = T r ( ( P ′ − Q ) T ( P ′ − Q ) ) minNE =\sum_{i=1}^{N}{||p_i^,-q_i||_2^2}=Tr((P'-Q)^T(P'-Q))\tag{2} minNE=i=1∑N∣∣pi,−qi∣∣22=Tr((P′−Q)T(P′−Q))(2)
其中 p i ′ = R p i p_i'=Rp_i pi′=Rpi， P ′ = R P P'=RP P′=RP， T r ( ) Tr() Tr()是求迹运算
根据迹的性质，有
T r ( ( P ′ − Q ) T ( P ′ − Q ) ) = T r ( P ′ T P ′ ) + T r ( Q T Q ) − 2 T r ( Q T P ′ ) Tr((P'-Q)^T(P'-Q))=Tr(P'^TP')+Tr(Q^TQ)-2Tr(Q^TP') Tr((P′−Q)T(P′−Q))=Tr(P′TP′)+Tr(QTQ)−2Tr(QTP′)
又因为 P ′ T P ′ = P T R T R P = P T P P'^TP'=P^TR^TRP=P^TP P′TP′=PTRTRP=PTP（旋转矩阵为正交矩阵），有
(3) N E = T r ( P T P ) + T r ( Q T Q ) − 2 T r ( Q T P ′ ) NE=Tr(P^TP)+Tr(Q^TQ)-2Tr(Q^TP')\tag{3} NE=Tr(PTP)+Tr(QTQ)−2Tr(QTP′)(3)
求(2)式的最小值相当于求(3)式内 T r ( Q T P ′ ) Tr(Q^TP') Tr(QTP′)的最大值
T r ( Q T P ′ ) = T r ( Q T R P ) = T r ( P Q T R ) Tr(Q^TP')=Tr(Q^TRP)=Tr(PQ^TR) Tr(QTP′)=Tr(QTRP)=Tr(PQTR)
对 P Q T PQ^T PQT做SVD分解，有
T r ( Q T P ′ ) = T r ( V Σ W T R ) = T r ( Σ W T R V ) = ∑ i = 1 3 σ i T i i Tr(Q^TP')=Tr(V\Sigma W^TR)=Tr(\Sigma W^TRV)=\sum_{i=1}^3{\sigma_iT_{ii}} Tr(QTP′)=Tr(VΣWTR)=Tr(ΣWTRV)=i=1∑3σiTii
其中 T = W T R V T=W^TRV T=WTRV
因为 R R R和 V V V都是正交矩阵，因此 T T T正交，即 ∣ T i i ∣ ≤ 1 |T_{ii}|\leq 1 ∣Tii∣≤1，因此
T r ( Q T P ′ ) ≤ ∑ i = 1 3 σ i Tr(Q^TP')\leq \sum_{i=1}^3{\sigma_i} Tr(QTP′)≤i=1∑3σi
即 W T R V = 1 W^TRV=1 WTRV=1为所求：
(4) R = W V T R=WV^T\tag{4} R=WVT(4)
这里需要注意的是 R R R必须是右手系下的旋转，因此(4)式修正为
(5) R = W [ 1 0 0 0 1 0 0 0 d ] V T R=W\left[ \begin {matrix}1&0&0 \\ 0&1&0 \\ 0&0&d \end {matrix} \right]V^T\tag{5} R=W⎣⎡10001000d⎦⎤VT(5)
其中 d = s i g n ( d e t ( P Q T ) ) d=sign(det(PQ^T)) d=sign(det(PQT))
根据上一节的讨论，可以得到Kabsch算法求解旋转矩阵R的步骤
【1】Kabsch W . A Solution of the Best Rotation to Relate Two Sets of Vectors[J]. Acta Crystallographica Section A Foundations of Crystallography, 1976, A32(5).
【2】Chai Z, Sun Y, Xiong Z. A Novel Method for LiDAR Camera Calibration by Plane Fitting[C]//2018 IEEE/ASME International Conference on Advanced Intelligent Mechatronics (AIM). IEEE, 2018: 286-291.
文章浏览阅读3.8k次，点赞9次，收藏28次。直接上一个工作中碰到的问题，另外一个系统开启多线程调用我这边的接口，然后我这边会开启多线程批量查询第三方接口并且返回给调用方。使用的是两三年前别人遗留下来的方法，放到线上后发现确实是可以正常取到结果，但是一旦调用，CPU占用就直接100%(部署环境是win server服务器)。因此查看了下相关的老代码并使用JProfiler查看发现是在某个while循环的时候有问题。具体项目代码就不贴了，类似于下面这段代码。while(flag) {//your code;}这里的flag._main函数使用while(1)循环cpu占用99
文章浏览阅读347次。idea shift f6 快捷键无效_idea shift +f6快捷键不生效
文章浏览阅读135次。Ecmacript 中没有DOM 和 BOM核心模块Node为JavaScript提供了很多服务器级别，这些API绝大多数都被包装到了一个具名和核心模块中了，例如文件操作的 fs 核心模块 ，http服务构建的http 模块 path 路径操作模块 os 操作系统信息模块// 用来获取机器信息的var os = require('os')// 用来操作路径的var path = require('path')// 获取当前机器的 CPU 信息console.log(os.cpus._node模块中有很多核心模块,以下不属于核心模块,使用时需下载的是
文章浏览阅读10w+次，点赞435次，收藏3.4k次。SPSS 22 下载安装过程7.6 方差分析与回归分析的SPSS实现7.6.1 SPSS软件概述1 SPSS版本与安装2 SPSS界面3 SPSS特点4 SPSS数据7.6.2 SPSS与方差分析1 单因素方差分析2 双因素方差分析7.6.3 SPSS与回归分析SPSS回归分析过程牙膏价格问题的回归分析_化工数学模型数据回归软件
文章浏览阅读7.5k次。如何利用hutool工具包实现邮件发送功能呢？1、首先引入hutool依赖<dependency> <groupId>cn.hutool</groupId> <artifactId>hutool-all</artifactId> <version>5.7.19</version></dependency>2、编写邮件发送工具类package com.pc.c..._hutool发送邮件
文章浏览阅读867次，点赞2次，收藏2次。docker安装elasticsearch，elasticsearch-head，kibana，ik分词器安装方式基本有两种，一种是pull的方式，一种是Dockerfile的方式，由于pull的方式pull下来后还需配置许多东西且不便于复用，个人比较喜欢使用Dockerfile的方式所有docker支持的镜像基本都在https://hub.docker.com/docker的官网上能找到合..._docker安装kibana连接elasticsearch并且elasticsearch有密码
文章浏览阅读1.3w次，点赞57次，收藏92次。整理 | 郑丽媛出品 | CSDN（ID：CSDNnews）近年来，随着机器学习的兴起，有一门编程语言逐渐变得火热——Python。得益于其针对机器学习提供了大量开源框架和第三方模块，内置..._beeware
文章浏览阅读7.9k次。//// ViewController.swift// Day_10_Timer//// Created by dongqiangfei on 2018/10/15.// Copyright 2018年 飞飞. All rights reserved.//import UIKitclass ViewController: UIViewController { ..._swift timer 暂停
文章浏览阅读986次，点赞2次，收藏2次。1.硬性等待让当前线程暂停执行，应用场景：代码执行速度太快了，但是UI元素没有立马加载出来，造成两者不同步，这时候就可以让代码等待一下，再去执行找元素的动作线程休眠，强制等待 Thread.sleep(long mills)package com.example.demo;import org.junit.jupiter.api.Test;import org.openqa.selenium.By;import org.openqa.selenium.firefox.Firefox.._元素三大等待
文章浏览阅读3k次，点赞4次，收藏14次。Java软件工程师职位分析_java岗位分析
文章浏览阅读2k次。Java：Unreachable code的解决方法_java unreachable code
文章浏览阅读1w次。1、html中设置标签data-*的值 标题 11111 222222、点击获取当前标签的data-url的值$('dd').on('click', function() { var urlVal = $(this).data('ur_如何根据data-*属性获取对应的标签对象