PnP解算 --- 被一个整数的bug

从 4 个角点算倾角，以及一个被"整数"吃掉的 bug

一、为什么板子要算倾角

我们检测装甲板的时候，光知道"它在哪、是哪一类"是不够的，还得知道它歪了多少度，下一步才好决定打不打。

比如倾角太大就不打了，或者说打的风险太大了。不只是打装甲板，像无人机自主降落、机械臂抓取这些，都得先识别出位姿才能做下一步判断。

这一章用不到上一篇标定的相机内参，我先把前面几篇的关系捋一下，免得混。

标定是把相机的参数提前测出来。前面说过，相机跟外界打交道，靠的就是这些参数搭起来的桥。桥搭好了，后面不管做什么识别都会更准。

ROS2 就简单了，它只负责把算完的结果（或者视频图像）发到话题上，谁需要谁去订阅读。

倾角算出来呢，是给云台这些下位机做后续控制的。

这里有个地方我自己一开始也绕进去了：算倾角不需要内参，但 PnP 解算必须要内参。PnP 就是靠内参才能求出外参（板子相对相机的位置和姿态）。内参标定好放在那儿不动，等到做 PnP 的时候直接拿来用。这一章不碰它，不代表它没用。

二、4 个角点的顺序不能乱

前面说过，关键点的顺序是：

左上 → 左下 → 右下 → 右上
left_top, left_bottom, right_bottom, right_top

这个顺序必须和前面函数输出的一致，不然数据就对不上。顺序错了，后面算出来的一切都是错的。

三、角度是这么算出来的

我们取左边灯条的两个端点，来算它的方向角度：

dx = x₂ − x₁ dy = y₂ − y₁
angle = atan2(dx, dy)

我们默认"竖直的时候是 0 度"当基准。 所以板子立正的时候，dx 差不多是 0，dy 是大于 0 的，算出来 angle 约等于 0，很直观。

要是反过来写成 atan2(dy, dx) 呢？竖直的时候算出来是 90 度。这没毛病，也是事实，但你观察数据变化的时候就很难受了。我们习惯拿 0 当标准，数据往哪边偏一眼就看出来。

dx 和 dy 都是下面的点减上面的点。你想想三角形的角度关系就明白了：左边灯条的上端往左歪的时候，灯条和竖直方向夹的那个锐角，就是我们要的角度。用 tan 求出比例，再转成角度打印出来。

其实左右两个灯条是对称的，取哪一个都行。

四、代码就这几行，顺便聊聊 auto

位置在 02-onnx-inference/draw_refactored.cpp 的 processFrame 函数里：

// 4 个角点（除以 scale 还原到原图坐标）
auto left_top = cv::Point2f(dets[idx].kpts[0]/scale, dets[idx].kpts[1]/scale);
auto left_bottom = cv::Point2f(dets[idx].kpts[2]/scale, dets[idx].kpts[3]/scale);
// right_bottom / right_top 同理

auto dx = left_bottom.x – left_top.x;
auto dy = left_bottom.y – left_top.y;
auto angle_rad = atan2(dx, dy);
auto angle_deg = angle_rad * 180 / CV_PI;

这个 auto 我也是从同学和别人的代码里学来的。一开始用着挺爽的，根本不用管类型，auto auto auto 完事。

但你写了 auto，你就不知道这个参数到底是什么类型了，这是它最大的坏处。我这次函数写得多，参数类型又乱，排查的时候特别难受。所以我不太建议全都用 auto，C++ 本来就是强类型语言，类型本身就是信息。

还有 atan2 出来的是弧度，我们看数据统一用角度，所以要乘 180 / CV_PI 转一下。

五、先拿一张图片试错，别直接跑视频

我是先把一张图片跑通的。

因为视频说白了就是循环播放的图片，图片跑通了，套上循环就是视频。前提是你要分清楚：哪些东西得放在循环里反复用，哪些只要在循环外面打开一次就够了（比如模型、视频对象）。

而且代价差太多了。跑一张图片几秒钟，跑一整段视频好久。要是一上来就直接跑视频，报错了还贼麻烦。所以先用图片把范围缩小，看看到底是哪儿的问题。

六、打开 CSV，我人傻了

跑了之后才知道有问题。打开 CSV 一看，有一堆全是 0。

很多 0。而且最要命的是，0 和 1 之间是空白的，一个其他数据都没有。

我当时就猜到可能是 int 类型没改成 float，而且是 point 的问题。

为什么这么想？因为计算机算数是不会那么凑巧的。视频里的画面一直在动，理论上总该有一点倾角。如果要是一段视频里只有极个别几个 0，那能解释成"板子那会儿确实立正了"，正常。但大多数都是 0，那就不是巧合了。

更关键的是"0 到 1 度之间整段空白"这件事——一个连续变化的量，不可能刚好在 0 和 1 之间一个数都不出现。这个我没法解释。

七、往回查：Point 把小数吞掉了

顺着前面 auto 那个坏处，我回到代码里一路往回查，看到计算的时候全是 Point，输出就是整数 int，小数部分全被吞掉了。cv::Point 就是 Point_<int>。

这时候才发现，前面那一堆 auto 根本看不出问题，只能盯着等号右边看。查下去才知道，后面的数据全被污染了。

怎么证明是它？我把 CSV 里的角度拿去做反推。

如果真是整数截断，那每一个角度都应该能由两个整数算出来。我挑了几个高频的角度值去试：

8 个全中，误差都在 0.00001 度以内。

这就实锤了：dx 和 dy 都是整数，那角度怎么可能不是被算出来的离散值。

八、我以为改好了，结果白高兴一场

我把 cv::Point 改成了 cv::Point2i，重新跑了一遍，然后看 CSV——一点变化都没有。

我第一反应是不是没保存。重新编译、重新运行，再看结果还是没变。然后去查了一下才知道：

Point2i 和 Point 是同一个东西，都是 int 类型，只是一个别名。

笑到我了。Point2f 才是浮点类型。

九、改完之后，数据变成什么样

不同取值的个数从 359 涨到 10774，数据一下子细了很多。

int 类型没有小数点，float 有，改完之后一眼就能看出来之前是量化（截断）出的问题。这个结论是从数据变化里看出来的，不是靠猜。

十、这件事我学到了什么

看到数据里有"空白"，就可以顺着往下想：是不是丢了东西？丢的是什么？在哪儿丢的？什么时候丢的？是代码里的问题，还是别的原因？是逻辑错了吗——逻辑没错。那是数据堆积了吗——是。

然后从报错的地方，回到打印数据的那一处，重新看数据是怎么处理的。

这时候 auto 就有点烦了，排查数据类型的时候特别麻烦，你从变量名上根本看不出它是什么类型。

还有个教训是：程序能跑通，不代表没问题。要看图、看警告、看数据。程序就算上线了也只是测试版本，你永远不知道哪儿还藏着问题。程序能跑但是结果不对，这个才是最恐怖的。

后来我给自己定了一套"数据体检"，专门用来看这种不报错但结果不对的问题，四句话：

这四句话不用改代码，打开 CSV 十秒就能看完。

十一、想自己复现一下

这个 bug 是可以复现的，而且只要改一个类型：

# 1. 把 draw_refactored.cpp 第 214~217 行的 cv::Point2f 改成 cv::Point（或 Point2i，等价）
# 2. 编译（记得核对二进制时间戳，必须比源码新）
bash build.sh

# 3. 跑同一个视频（约 4 分钟）
./draw_refactored best.onnx test.mp4

# 4. 看分布
awk -F, 'NR>1{n++; a=$7+0; if(a<0)a=-a;
if(a==0)b0++; else if(a<1)b1++} END{
printf "总数 %d | 恰好0: %d (%.2f%%) | 0~1度: %d (%.2f%%)\\n", n,b0,100*b0/n,b1,100*b1/n}' angles.csv

跑出来你会看到 0~1度: 0 (0.00%)，而"恰好 0"那一项突然冒出一千多条。

复现完记得改回 Point2f 再编译一次，不然后面全是整数角度。

十二、这一章我踩的坑

下一篇：让 NMS 把分组信息暴露出来，做到一个目标只留一条观测，再加一道几何二次验证。