PCL-利用相机移动模拟小车移动
前言
通过相机模拟小车的第一视角，这就需要相机模拟小车的运动。
小车数据
//点的位置
struct geometry_msgs_Point
{
double x;
double y;
double z;
};
//四元数
struct geometry_msgs_Quaternion
{
double x;
double y;
double z;
double w;
};
坐标x,y,z比较直接使用就可以，这里的问题在于四元数，之前从来没有接触过，于是我花了两天学习了一下四元数的相关内容，大致了解了其是如何表示旋转的，真的是非常巧妙。
根据学到的四元数,我们有单位四元数如下（表示绕向量u旋转θ度）
q
=
[
cos
(
1
2
θ
)
,
sin
(
1
2
θ
)
u
]
q = [\cos(\frac{1}{2}\theta) , \sin(\frac{1}{2}\theta)\bold{u}]
q=[cos(21θ),sin(21θ)u]
而u是一个三维单位向量，在这里可以用x,y,z表示，即
q
=
[
cos
(
1
2
θ
)
,
sin
(
1
2
θ
)
x
,
sin
(
1
2
θ
)
y
,
sin
(
1
2
θ
)
z
]
q = [\cos(\frac{1}{2}\theta) , \sin(\frac{1}{2}\theta)\bold{x},\sin(\frac{1}{2}\theta)\bold{y},\sin(\frac{1}{2}\theta)\bold{z}]
q=[cos(21θ),sin(21θ)x,sin(21θ)y,sin(21θ)z]
对应上面的四元数的话，也就是
x
=
sin
(
1
2
θ
)
x
y
=
sin
(
1
2
θ
)
y
z
=
sin
(
1
2
θ
)
z
w
=
cos
(
1
2
θ
)
x = \sin(\frac{1}{2}\theta)\bold{x}\\ y = \sin(\frac{1}{2}\theta)\bold{y}\\ z = \sin(\frac{1}{2}\theta)\bold{z} \\ w = \cos(\frac{1}{2}\theta)
x=sin(21θ)xy=sin(21θ)yz=sin(21θ)zw=cos(21θ)
根据提供的四元数
q
=
[
w
,
u
]
q = [w,\bold{u}]
q=[w,u]
如果想要获得旋转角度，只需要
θ
2
=
arccos
(
w
)
\frac{\theta}{2} = \arccos (w)
2θ=arccos(w)
如果想要获得旋转轴,这里用u的x分量举例
x
=
x
sin
(
1
2
θ
)
\bold{x} = \frac{x}{\sin(\frac{1}{2}\theta)}
x=sin(21θ)x
弄明白变量的含义之后，就可以根据相机的方式变化角度了。
调整相机
PCL的可视化是建立在vtk基础之上的，其相机是使用的vtk的相机，而相机的原理如上图所示。
其参数如下：
| 类型 | 参数名 | 意义 |
|---|---|---|
| double | focal[3] | 焦点的坐标，默认焦点位于世界坐标系原点 |
| double | pos[3] | 相机的位置 |
| double | view[3] | 相机的朝上方向向量 |
| double | clip[2] | 裁剪平面深度(0是前裁剪平面，1是后裁剪平面) |
| double | fovy | 视角的弧度大小 |
| double | window_size[2] | 屏幕上窗口的实际大小 |
| double | window_pos[2] | 屏幕上窗口的实际位置 |
也就是说，通过相机位置和焦点位置可以决定其绕z轴的旋转，而通过对朝上方向的调整，可以实现在x轴或y轴的旋转。
调整相机位置的函数
void pcl::visualization::PCLVisualizer::setCameraPosition ( double pos_x,
double pos_y,
double pos_z,
double view_x,
double view_y,
double view_z,
double up_x,
double up_y,
double up_z,
int viewport = 0
)
参数表
| 参数名 | 描述 |
|---|---|
| pos_x | 相机位置的x坐标 |
| pos_y | 相机位置的y坐标 |
| pos_z | 相机位置的z坐标 |
| view_x | 相机焦点的x分量 |
| view_y | 相机焦点的y分量 |
| view_z | 相机焦点的z分量 |
| up_x | 相机的上方向向量的 x 分量 |
| up_y | 相机的上方向向量的 y 分量 |
| up_z | 相机的上方向向量的 z 分量 |
| viewport | 修改相机的viewport，如果为0则修改所有相机，默认为1 |
构造相机的坐标轴
这就需要按照小车的坐标轴来考虑，小车的x轴就是小车的视角方向，因此我们可以将相机的x轴定义为相机的投影方向，相机的z轴即相机的朝上方向，这也等同于小车的z轴，而最后的y轴，垂直于x轴和z轴，也等同于小车的y轴。这样，就将相机的坐标轴定义好了。
但实际上，相机只有两个向量，所以要模拟三维旋转还需要构思。绕x轴旋转，就是令z轴的方向向量绕x轴进行旋转。绕z轴旋转，就是令x轴的方向向量绕z轴进行旋转。而绕y轴旋转较为特殊，需要令z轴和x轴的方向向量同时绕y轴进行旋转。
实现
相机的坐标直接使用小车的坐标即可。
camera_position[0] = b.x();
camera_position[1] = b.y();
camera_position[2] = b.z();
相机的焦点和相机坐标会构成相机的x轴向量，用于旋转，因此，应当先求出相机的x轴向量，再将其旋转，再与相机位置向量相加，就得到旋转后焦点应在的位置。
axis_x_direction.x() = focus_position[0] - camera_position[0];
axis_x_direction.y() = focus_position[1] - camera_position[1];
axis_x_direction.z() = focus_position[2] - camera_position[2];
// 利用Eigen将四元数转换为旋转矩阵，利用旋转矩阵对x轴向量进行旋转
axis_x_direction = A.toRotationMatrix() * axis_x_direction;
// 利用向量加法获得旋转后焦点的位置
focus_position[0] = camera_position[0] + axis_x_direction.x();
focus_position[1] = camera_position[1] + axis_x_direction.y();
focus_position[2] = camera_position[2] + axis_x_direction.z();
相机的上方向的方向向量其实就相当于相机的z轴向量，因此直接对其进行旋转，再将旋转后的结果赋值回去即可。
// 构造向上方向向量
axis_z_direction.x() = view_position[0] - camera_position[0];
axis_z_direction.y() = view_position[1] - camera_position[1];
axis_z_direction.z() = view_position[2] - camera_position[2];
// 旋转z轴向量
axis_z_direction = A.toRotationMatrix() * axis_z_direction;
// 将旋转向量赋值回去
view_position[0] = axis_z_direction.x();
view_position[1] = axis_z_direction.y();
view_position[2] = axis_z_direction.z();
最终根据生成的这些向量和坐标，设置相机参数
viewer -> setCameraPosition(camera_position[0],camera_position[1],camera_position[2],
focus_position[0],focus_position[1],focus_position[2],
view_position[0],view_position[1],view_position[2],0
);
成果展示
存在的问题（待解决）
由于相机旋转幅度过大，旋转速度过快，导致出现抖动剧烈，并且有画面闪动的情况。
解决的猜想
以后如果有时间再来解决这个问题。
1. 四元数插值
利用四元数差值，让旋转更加平滑，从而解决抖动的问题。
2. 利用视频防抖相关算法进行防抖
由于从未接触过相关领域，可能会花费一些时间来学习和应用。但感觉这个也是最有可能实现防抖的方法。
3.利用睡眠降低帧率
感觉应该不会起什么作用，但作为一个点子先留在这。