# 任务6_PNP

**Repository Path**: MrDreamQ/task-6--pnp

## Basic Information

- **Project Name**: 任务6_PNP
- **Description**: 任务6——姿态解算
- **Primary Language**: C++
- **License**: Not specified
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 0
- **Forks**: 0
- **Created**: 2021-09-30
- **Last Updated**: 2021-10-15

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

# 任务6_PNP

参考与学习的[博客1](https://blog.csdn.net/qq_40501580/article/details/101794629)，[博客2](https://www.cnblogs.com/singlex/p/RotateMatrix2Euler.html)

## 项目介绍

识别出装甲板的四个角点后，本人用sovlePnP函数得到旋转向量rVec和平移向量tVec，再用Rodrigues函数将旋转向量rVec转为等价旋转矩阵rVec。再根据欧拉角和欧式距离的求解公式，得出了装甲板1的位姿。



### 设定世界坐标系

在程序里，本人的装甲板角点的像素坐标系坐标存储顺序如图：

![设定世界坐标系](https://s3.bmp.ovh/imgs/2021/10/738d2ba4b09263ce.png)

因此，装甲板的世界坐标系设定代码如下：

```c++
void PNP::set_objpoint(double length, double width)
{
    _half_length = length / 2.0;
    _half_width = width / 2.0;
    _obj = vector<Point3f>{
        Point3f(_half_length, _half_width, 0),
        Point3f(_half_length, -_half_width, 0),
        Point3f(-_half_length, _half_width, 0),
        Point3f(-_half_length, -_half_width, 0)};
}

```



### 姿态解算——欧拉角（旋转角、姿态角？）

程序的类里定义了姿态解算的方法`vector<double> p4psolution(const vector<Point2f> objectPoints)` 。

其中，先用`solvePnP(_obj, objectPoints, _camera_matrix, _distcoeffs, _rVec, _tVec)`解算出旋转向量_rVec 和平移向量_tVec ；再用`Rodrigues(_rVec, _rVec)`得到旋转矩阵_rVec ，再由下列公式：

![](https://s3.bmp.ovh/imgs/2021/10/b393149449a874f5.png)

分别得到Z轴，Y轴，X轴的旋转角，即roll, yaw, pitch，也叫翻滚角，偏航角，俯仰角。

对应代码如下：

```c++
double rollErr = atan2(_rVec.at<double>(1, 0), _rVec.at<double>(0, 0)) / CV_PI * 180 + 180; //z轴旋转角roll

double yawErr = atan2(-_rVec.at<double>(2, 0), sqrt(pow(_rVec.at<double>(2, 0), 2) +  
pow(_rVec.at<double>(2, 2), 2))) / CV_PI * 180; //y轴旋转角yaw

double pitchErr = atan2(_rVec.at<double>(2,1), _rVec.at<double>(2,2)) / CV_PI * 180; //x轴旋转角pitch
```



### 姿态解算——测距

根据平移向量的各项元素，利用三维空间的欧式距离测算公式，即可得到距离，公式如下：

![](https://s3.bmp.ovh/imgs/2021/10/0460e60ed78e851a.png)

对应的代码如下：

```c++
double euclidean_distance = sqrt(_tVec.at<double>(0, 0) * _tVec.at<double>(0, 0) +
_tVec.at<double>(1, 0) * _tVec.at<double>(1, 0) + _tVec.at<double>(2, 0) * _tVec.at<double>(2, 0) ) //计算三维空间下的欧式距离
```



得到旋转角与距离，在方法中将其存储进容器中并返回。



## 使用说明

### 运行任务6可执行文件

需新建build文件夹，并重新编译

在此项目文件夹的终端，运行如下指令：

```bash
$ mkdir build

$ cd build/

$ cmake ..

$ make

$ ./task6 <装甲板_1视频文件路径>
```

即可查看完成情况（需附上装甲板\_1视频文件的路径）

运行后，终端会输出各轴旋转角及装甲板距离距离，输出视频也有相应的数据文本输出，如图：

![](https://s3.bmp.ovh/imgs/2021/10/758773350447dfcd.png)



### 实例化对象的接口设置

与任务1和任务2的程序一样，该任务程序里有多种构造函数重载：

```c++
PNP::PNP()
{
    _is_recorded = false;
    _color = RED;
}
PNP::PNP(bool is_recorded=false)
{
    _is_recorded = is_recorded;
    _color = RED;
}
PNP::PNP(bool is_recorded = false, int color = RED)
{
    _is_recorded = is_recorded;
    _color = color;
}
```

其中，参数1为视频写入判断参数，参数2为视频中灯条的颜色**（会影响图像预处理阶段中红蓝色通道增强过程）**。

在`main.cpp`创建对象时，可选择多种创建方式。

如默认的创建对象方式为：

```c++
PNP pnp(false);
```

则运行程序后不会写入并保存效果视频，处理默认灯条颜色——红光。

若创建对象的方式为：

```c++
PNP pnp(true,BLUE);
```

则运行程序后会写入视频，并在build文件夹下保存名为"代码效果.avi"，编码格式为MPG2的视频文件，处理的灯条颜色为蓝光。



## 注意事项

<font color=RED>注意：视频播放时，按下空格键使视频暂停，暂停后按下除esc键的任意键使视频继续，暂停后按下esc键退出</font>，该逻辑代码如下：

```c++
		if (waitKey(30) == ' ') //按下空格暂停
            {
                if (waitKey(0) == 27) //暂停后按下esc键退出
                {
                    break;
                }
            }
```



调用任务6可执行文件时，要附上装甲板\_1的视频文件路径，该项目文件夹下已包含有，也可附上其他的装甲板\_1路径。



#### 另附

**世界坐标系坐标与像素坐标的顺序一一对应**。虽然一般为顺时针或逆时针的顺序去设定世界坐标系，但本人的算法是先找出轮廓的上下角点并存储进light_point容器里，所以世界坐标系才以第一、四、二、三象限的顺序设定（如上文的“设定世界坐标系”图所示）。

要想按顺时针或逆时针的顺序设定读取，本人的想法是另定义一个相同类型的容器，以顺时针或逆时针的顺序将light_point对应的点的序号存入新定义的容器中，然后世界坐标系便能以顺时针或逆时针的顺序设定。