激光雷达标定(坐标系转换)

树和猫 2024-04-11 原文

文章目录

1. 旋转矩阵

由于激光雷达获取的点云数据的坐标是相对于激光雷达坐标系的，为了使车最终得到的点云数据坐标是在车坐标系下的，我们需要对点云中每一个点的坐标进行坐标转换。
首先是需要对坐标系进行旋转变换，先以二维平面的单位向量坐标转换为例，假设两坐标系中的旋转矩阵为R，旋转角度为 $\theta$ ,点P在 $x_1oy_1$ 坐标(车坐标系)下的坐标为 $x_1,y_1)$ ；点P在 $x_2oy_2$ 坐标（激光雷达坐标系）下的坐标为 $x_2,y_2)$ ,已知点在激光雷达坐标系下的坐标 $x_2,y_2)$ ，可以由以下的坐标系的转换关系得到 $x_1,y_1)$ ：

旋转矩阵R:将点P在旋转后的坐标系下的坐标转换为旋转前的坐标系下的坐标

$\begin{bmatrix} x_1\\y_1 \end{bmatrix} =R\begin{bmatrix} x_2\\y_2 \end{bmatrix} \tag{1}$

图1 二维坐标系转换

根据旋转角度 $\theta$ ,由上图1可得 $x_1$ 和 $y_1$ 为：
$x_1=x_2cos\theta-y_2sin\theta\\ y_1=x_2sin\theta+y_2cos\theta \tag{2}$
由此可以得出以下的坐标转换矩阵等式：
$\begin{bmatrix} x_1\\y_1 \end{bmatrix} =\begin{bmatrix} cos\theta,-sin\theta\\ sin\theta,cos\theta\\ \end{bmatrix} \begin{bmatrix} x_2\\y_2 \end{bmatrix} \tag{3}$
旋转矩阵R即为：
$=\begin{bmatrix} cos\theta,-sin\theta\\ sin\theta,cos\theta\\ \end{bmatrix} \tag{4}$
由二维推广至三维，由右手定则可以想象在上图1中的O点处有一条垂直于XOY平面指向屏幕的的Z轴：

图2 航向角转换

那么上述的二维空间内的坐标旋转便可推广至三维空间中绕z轴的旋转，旋转角度 $\theta$ 便是欧拉角中的航向角（也称偏航角yaw），由于旋转前后z轴没有发生变换，上述公式（3）可以写为以下形式：
$\begin{bmatrix} x_1\\y_1\\z_1 \end{bmatrix} =\begin{bmatrix} cos\theta&-sin\theta&0\\ sin\theta&cos\theta&0\\ 0&0&1\\ \end{bmatrix} \begin{bmatrix} x_2\\y_2\\z_2 \end{bmatrix} \tag{5}$
由此可得航向角（也称偏航角yaw）旋转矩阵 $R_{yaw}$ 为：
$R_{yaw}=\begin{bmatrix} cos\theta&-sin\theta&0\\ sin\theta&cos\theta&0\\ 0&0&1\\ \end{bmatrix} \tag{6}$

图3 俯仰角转换

由上述公式（6）同理可以推导出绕坐标轴y轴旋转 $\beta$ （俯仰角pictch）的公式如下:
$\begin{bmatrix} x_1\\y_1\\z_1 \end{bmatrix} =\begin{bmatrix} cos\beta&0&sin\beta\\ 0&1&0\\ -sin\beta&0&cos\beta\\ \end{bmatrix} \begin{bmatrix} x_2\\y_2\\z_2 \end{bmatrix} \tag{7}$
由此可得俯仰角pictch旋转矩阵 $R_{pictch}$ 为：
$R_{pitch}=\begin{bmatrix} cos\beta&0&sin\beta\\ 0&1&0\\ -sin\beta&0&cos\beta\\ \end{bmatrix} \tag{8}$

图4 横滚角转换

也可以推导出绕坐标轴x轴旋转 $\gamma$ （横滚角roll）的公式如下:
$\begin{bmatrix} x_1\\y_1\\z_1 \end{bmatrix} =\begin{bmatrix} 1&0&0\\ 0&cos\gamma&-sin\gamma\\ 0&sin\gamma&cos\gamma\\ \end{bmatrix} \begin{bmatrix} x_2\\y_2\\z_2 \end{bmatrix} \tag{9}$
由此可得横滚角roll旋转矩阵 $R_{roll}$ 为：
$R_{roll}=\begin{bmatrix} 1&0&0\\ 0&cos\gamma&-sin\gamma\\ 0&sin\gamma&cos\gamma\\ \end{bmatrix} \tag{10}$
按照不同的顺序对坐标轴进行旋转可以得到不同的旋转矩阵R，R共有六种形式，分别为
$R=R_{yaw}R_{pitch}R_{roll} \ R=R_{yaw}R_{roll}R_{pitch}\\ R=R_{pitch}R_{yaw}R_{roll} \ R=R_{pitch}R_{yaw}R_{roll}\\ R=R_{roll}R_{pitch}R_{yaw} \ R=R_{roll}R_{yaw}R_{pitch} \tag{11}$
按照欧拉角的测量方式，每次旋转按照车底盘坐标系的坐标轴进行旋转（即外旋），外旋旋转矩阵是左乘矩阵，即按照X-Y-Z的顺序进行旋转的话，得到的旋转矩阵是 $R=R_ZR_YR_X$ ,通过测量的得到的欧拉角的角度值就可以计算出对应的旋转矩阵。

外旋（左乘）：每次旋转绕固定轴旋转
内旋（右乘）：每次旋转绕自身旋转后的轴旋转

2. 平移矩阵

已知激光雷达相对于车坐标原点的三维坐标x,y,z，可以得到激光雷达与车坐标系的平移矩阵为：
$T=\begin{bmatrix} x\\y\\z \end{bmatrix} \tag{12}$

图5 三维坐标系转换

3. 坐标系的转换

根据上述计算得到的旋转矩阵R和平移矩阵T。设车坐标下点的坐标为 $x_c,y_c,z_c)$ 激光雷达下点的坐标为 $x_l,y_l,z_l)$ 即最终的变换形式为：
$\begin{bmatrix} x_c \\ y_c\\ z_c \end{bmatrix} =R\begin{bmatrix} x_l \\ y_l\\ z_l \end{bmatrix} +T \tag{13}$

4. 坐标转换代码

构造旋转平移矩阵对点云中的点进行转换

//坐标变换将激光雷达坐标系下的点转换到小车坐标系下
void MainWindow::changePoint(pcl::PointCloud<pcl::PointXYZ>::Ptr lidarCloud,
                 pcl::PointCloud<pcl::PointXYZ>::Ptr carCloud,
                 double yaw,double pitch,double roll,double x,double y,double z)
{
    Eigen::Matrix4f transform=Eigen::Matrix4f::Identity();
    Eigen::Matrix4f transformYaw;
    Eigen::Matrix4f transformPitch;
    Eigen::Matrix4f transformRoll;
    
    //航向角
    transformYaw<<cos(yaw),-sin(yaw),0,0,\
            sin(yaw),cos(yaw),0,0,\
            0,0,1,0,\
            0,0,0,1;
    //俯仰角
    transformPitch<<cos(pitch),0,sin(pitch),0,\
            0,1,0,0,\
            -sin(pitch),0,cos(pitch),0,\
            0,0,0,1;
    //横滚角
    transformRoll<<1,0,0,0,\
            0,cos(roll),-sin(roll),0,\
            0,sin(roll),cos(roll),0,\
            0,0,0,1;
            
    //旋转矩阵
    transform=transformRoll*transformPitch*transformYaw;
    
    //平移矩阵
    transform(0,3)=x;
    transform(1,3)=y;
    transform(2,3)=z;
    
    //坐标转换
    pcl::transformPointCloud(*lidarCloud,*carCloud,transform);
}

激光雷达激光 span class style 线性代数矩阵算法

有关激光雷达标定(坐标系转换)的更多相关文章

ruby-on-rails - 在 Rails 中将文件大小字符串转换为等效千字节 - 2
我的目标是转换表单输入，例如“100兆字节”或“1GB”，并将其转换为我可以存储在数据库中的文件大小(以千字节为单位)。目前，我有这个:defquota_convert@regex=/([0-9]+)(.*)s/@sizes=%w{kilobytemegabytegigabyte}m=self.quota.match(@regex)if@sizes.include?m[2]eval("self.quota=#{m[1]}.#{m[2]}")endend这有效，但前提是输入是倍数(“gigabytes”，而不是“gigabyte”)并且由于使用了eval看起来疯狂不安全。所以，功能正常，
ruby - 使用 ruby 将 HTML 转换为纯文本并维护结构/格式 - 2
我想将html转换为纯文本。不过，我不想只删除标签，我想智能地保留尽可能多的格式。为插入换行符标签，检测段落并格式化它们等。输入非常简单，通常是格式良好的html(不是整个文档，只是一堆内容，通常没有anchor或图像)。我可以将几个正则表达式放在一起，让我达到80%，但我认为可能有一些现有的解决方案更智能。最佳答案首先，不要尝试为此使用正则表达式。很有可能你会想出一个脆弱/脆弱的解决方案，它会随着HTML的变化而崩溃，或者很难管理和维护。您可以使用Nokogiri快速解析HTML并提取文本:require'nokogiri'h
ruby - 将数组的内容转换为 int - 2
我需要读入一个包含数字列表的文件。此代码读取文件并将其放入二维数组中。现在我需要获取数组中所有数字的平均值，但我需要将数组的内容更改为int。有什么想法可以将to_i方法放在哪里吗？ClassTerraindefinitializefile_name@input=IO.readlines(file_name)#readinfile@size=@input[0].to_i@land=[@size]x=1whilex 最佳答案只需将数组映射为整数:@land边注如果你想得到一条线的平均值，你可以这样做:values=@input[x]
ruby - 将散列转换为嵌套散列 - 2
这道题是thisquestion的逆题.给定一个散列，每个键都有一个数组，例如{[:a,:b,:c]=>1,[:a,:b,:d]=>2,[:a,:e]=>3,[:f]=>4,}将其转换为嵌套哈希的最佳方法是什么{:a=>{:b=>{:c=>1,:d=>2},:e=>3,},:f=>4,} 最佳答案这是一个迭代的解决方案，递归的解决方案留给读者作为练习:defconvert(h={})ret={}h.eachdo|k,v|node=retk[0..-2].each{|x|node[x]||={};node=node[x]}node[
ruby-on-rails - Ruby url 到 html 链接转换 - 2
我正在使用Rails构建一个简单的聊天应用程序。当用户输入url时，我希望将其输出为html链接(即“url”)。我想知道在Ruby中是否有任何库或众所周知的方法可以做到这一点。如果没有，我有一些不错的正则表达式示例代码可以使用... 最佳答案查看auto_linkRails提供的辅助方法。这会将所有URL和电子邮件地址变成可点击的链接(htmlanchor标记)。这是文档中的代码示例。auto_link("Gotohttp://www.rubyonrails.organdsayhellotodavid@loudthinking.
ruby-on-rails - 使用 ruby 将多个实例变量转换为散列的更好方法？ - 2
我收到格式为的回复#我需要将其转换为哈希值(针对活跃商家)。目前我正在遍历变量并执行此操作:response.instance_variables.eachdo|r|my_hash.merge!(r.to_s.delete("@").intern=>response.instance_eval(r.to_s.delete("@")))end这有效，它将生成{:first="charlie",:last=>"kelly"},但它似乎有点hacky和不稳定。有更好的方法吗？编辑:我刚刚意识到我可以使用instance_variable_get作为该等式的第二部分，但这仍然是主要问题。
python ffmpeg 使用 pyav 转换一组图像到视频 - 2
2022/8/4更新支持加入水印水印必须包含透明图像，并且水印图像大小要等于原图像的大小pythonconvert_image_to_video.py-f30-mwatermark.pngim_dirout.mkv2022/6/21更新让命令行参数更加易用新的命令行使用方法pythonconvert_image_to_video.py-f30im_dirout.mkvFFMPEG命令行转换一组JPG图像到视频时，是将这组图像视为MJPG流。我需要转换一组PNG图像到视频，FFMPEG就不认了。pyav内置了ffmpeg库，不需要系统带有ffmpeg工具因此我使用ffmpeg的python包装p
ruby-on-rails - 将字符串转换为 ruby-on-rails 中的函数 - 2
我需要一个通过输入字符串进行计算的方法，像这样function="(a/b)*100"a=25b=50function.something>>50有什么方法吗？最佳答案您可以使用instance_eval:function="(a/b)*100"a=25.0b=50instance_evalfunction#=>50.0请注意，使用eval本质上是不安全的，尤其是当您使用外部输入时，因为它可能包含注入(inject)的恶意代码。另请注意，a设置为25.0而不是25，因为如果它是整数a/b将导致0(整数)。
ruby-on-rails - 将 ruby 数组转换为整齐的列字符串？ - 2
我是ruby的新手，我正在尝试制作一个程序来自动格式化给定的字符串和数组。我试图弄清楚的一种自动格式化功能是一种用于数组的功能。假设我有一个如下例所示的数组myArray=["a","b","c"]我想把它变成一个列化的字符串，这样putsmyString就会给出`1)a``2)b``3)c`我该怎么做呢？我能找到的最接近的东西是使用.each这不是我想要的，我不能让每一行都有一个单独的条目。这一切都必须是一个带有换行符的字符串。任何帮助将不胜感激，提前致谢最佳答案您可以使用.map与.with_index:myArray=
ruby - 如何根据长度将路径数组转换为嵌套数组或散列 - 2
我需要根据字符串路径的长度将字符串路径数组转换为符号、哈希和数组的数组给定以下数组:array=["info","services","about/company","about/history/part1","about/history/part2"]我想生成以下输出，对不同级别进行分组，根据级别的结构混合使用符号和对象。产生以下输出:[:info,:services,about:[:company,history:[:part1,:part2]]]#altsyntax[:info,:services,{:about=>[:company,{:history=>[:part1,:pa