- 激光电机外参标定完成
- 相机之间外外参/激光相机Base外参的标定按下图进行
- 实验室人员多,计划0113早晨进行数据采集
-
Colmap中的Image位姿表示为 world in image[有必要时需要进行转置]
-
Eigen中的 q.coeffs() 输出顺序 q.coeffs = q.x q.y q.z q.w
-
从四元数恢复旋转矩阵的方法
- 数据采集[Done!]
600+张运动相机的图像+ 5*5张相机系统图像 + 5个地点的激光扫描[如上图]
-
图像COLMAP运行[使用COLMAP估计的内参进行建图]
所选用的相机模型 [fx,fy,cx,cy,k1,k2]
一共注册了632张image + 152998 个特征点
- 下一步: 使用AprilSLAM恢复相机的pose以及Tag的世界坐标[这一步可能需要采集的图像序列中对AprilTag的观测不间断]
- 实际测试表明AprilSlam对apriltag的检测效果不佳,而且由于是SLAM,不允许间断,这样对整个拍摄要求还是有点高的
- 由于COLMAP是对无序的图像进行重建,本着这个思路,对AprilTag的重建也是对无序图像进行重建
- Step1.遍历所有的图像进行AprilTag的检测
- Step2.根据AprilTag进行图像的匹配
- Step3.根据Apriltag提供的位姿进行粗略的地图构建
- Step4.全局做一个优化
apriltag_detect_slam.python[Done]
- 对所有输入的图像进行Apriltag检测
- 输入: 图像文件 + 相机参数文件[内参+畸变]
- 输出结果到txt: image_path + tag_id + tag_pose
/home/qk/Desktop/2Dlaser/calib_co_april/move_cam/raw/09_31_31_992.png
tag_num 3
tag_id 14
-0.0038321618801131496 0.9894535194577134 0.14480002544222412 0.4803617395013612
-0.9565231873997324 -0.045855759129169504 0.28802888973390084 0.06830523619435858
0.29163111374128214 -0.13740080853994896 0.9466109609076939 1.4199594244632063
0.0 0.0 0.0 1.0
tag_id 15
-0.3516640464714722 0.09458340286451579 0.9313358031998334 2.6271618006169435
-0.26943803226102836 -0.9630096804000527 -0.003937286768015219 -0.7603740918266348
0.8965129922040257 -0.2523218883853663 0.3641402469530489 3.0052332688109358
0.0 0.0 0.0 1.0
tag_id 20
0.25414559394286396 -0.8537185426340415 -0.45450485920640854 -0.7139860305536779
0.8318326773859872 -0.04680360339189149 0.553049563368777 0.3006400820030495
-0.4934211324135694 -0.5186271036808073 0.6982560514706392 2.2288445810225315
0.0 0.0 0.0 1.0
文件格式如上图所示
-
基于AprilTag的地图粗略恢复
-
相同图像Colmap建图
-
Apriltag约束的优化,还需要进一步测试
这是优化后的结果(.....看起来有问题 )
-
0115的apriltag建图有问题的原因:colmap中存储的地图pose都是world to image,而之前存储的格式为image to world,修复后地图的显示正常
-
colamp
- apriltag
- 重新采集了简单运动的数据进行测试
1.采集的图像
COLMAP Result:
apriltag轨迹恢复的结果
[优化还存在一点问题]
[TODO修复apriltag的优化问题 - > DONE]
- Apriltag优化构建完成
[2021-01-18 TODO 重新采集数据进行实验]
-
代码整理
- apriltag_detect_slam.py [从raw image中提取每帧image-pose & corners]
- apriltag_pose_estimate.cpp [从上一步中的txt输出,恢复pose并进行优化]
-
tag优化结果
绿色为原始位姿,红色为优化后的tag位姿
-
将tag的原始结果和优化结果放到激光点云地图中进行验证
-
原始结果
-
优化结果
[优化后的tag Pose反而变差了????]
-
-
优化前后图像位姿的变化
从图像的轨迹看来,好像也是optim之后的效果反倒是不如之前的了??????
-
解决方法 : 鲁棒核函数
-
[TODO]特征约束和AprilTag约束联合优化
修改COLMAP code 加入AprilTag约束
-
AprilTag轨迹生成geos.txt,使用colmap自带提供的model aligner进行对齐
-
联合优化
-
在colmap的优化框架中加入apriltag的约束项
-
apriltag约束构建时:
-
image位姿即colmap中image的位姿
-
image观测从apriltag txt中获得
[目前存在问题,这个轨迹对齐的误差有点大......应该是存在一些错误的地方(包括)]
-
-
-
使用了另一组更为简单的运动做测试发现对齐误差小很多
[数据采集的时候对标定板的观测还是要控制距离,由于定焦头的缘故, 远处的像素分辨率可能有所影响??????]
-
用evo_traj查看轨迹
[?????y轴相差很大!?]
-
另一组比较复杂的简图结果对比
- 未进行align的地图
kaoshi-
不同参数配置的align结果
-
关于Align的一些想法:
- Align的质量其实决定了error的初值,如果align的质量不好,对apriltag的角点观测可能会有比较大的影响
- 当第二组实验中,apriltag观测较远的时候,观测差异较大,下一次建图用的数据如果想保证较好的观测效果,可能要尽量使tag的距离保持始终
- 复习考试
- 复习考试
- [TODO] 重新采集一组数据,保证标定板的距离
- 输入:带有AprilTag的Image序列
- [apriltag_detect_slam.py] => raw.txt[标定板检测信息]
- [运行colmap进行稀疏重建] => sparse_raw[稀疏重建模型]
-
以simpletest数据作为测试数据,先把apriltag和BA的优化调通
-
目前存在的问题主要是在轨迹的对齐上,colmap提供了对所有图像和世界点进行相似变换操作的接口[Transform],也提供了Align的接口[Align]
- 联合优化的前提是april和colmap的坐标系有一致性
-
初始轨迹
Align结果 [colmap的align在rotation上存在很多错误!]
- 最终测试 = colmap的位姿Align + 用第一帧的pose进行Align
- 四门考试
-
Align脚本的整理
-
由于单纯的evo align 和 colmap的align效果都不佳,最终整合了colmap和evo的align,最终获得一个较好的效果
-
align中的注意点：1.最终由于优化时，image的第一帧将被固定fix，因此在align完成后额外增加一步第一帧的align
-
-
Apriltag与colmap的框架整合
- 大体框架完成
- 存在一个小问题:[Add Apriltag problem to Sum_Problem的时候发现参数传递存在问题....需要下一步解决]
-
原始的colmap的BA报告
-
加入Apriltag之后的BA报告
新增参差项 784 = 49 * 2 * 4 * 2 [ 49张图片每张图片观测2个tag,每个tag有4个角点加入到BA中,每个角点重投影残差为uv 2 项]
新增变量数 21 = 7 * 3[tag板子4变量q和3变量t] ?? 可是为什么是3呢?????
-
优化后的质量评估 [再思考一下]
更复杂数据的测试[0118-big-test 142张图 很多块板子]
-
BA
Bundle adjustment report ------------------------ Residuals : 709332 Parameters : 140297 Iterations : 19 Time : 12.9248 [s] Initial cost : 0.6203 [px] Final cost : 0.620275 [px] Termination : Convergence -
AprilTag + BA
Bundle adjustment report ------------------------ Residuals : 711588 Parameters : 140374 Iterations : 77 Time : 26.1174 [s] Initial cost : 4.68002 [px] Final cost : 3.66474 [px] -
轨迹对比
-
虽然加了AprilTag后,平均cost到了3.66,但是从轨迹来看依然约束住了,这个误差的增大应该是由于这组数据有很多apriltag的观测距离很远,导致误差很大的缘故.需要重新进行数据的采集
-
-
重新采集了一组数据
-
修复了一些代码中隐藏的一些Bug
-
新采集数据的align结果
-
长哥论文最终检查
-
对重新采集的数据进行重建
-
纯Apriltag恢复pose的结果
-
纯Colmap进行Image位姿估计的个结果
其中colmap见图使用外部穿入的参数
-
发现之前用evo_traj话轨迹的时候正好差了一个逆,存在bug,已修复
-
修复了各种各样的小问题
-
使用colmap进行的稠密重建
-
优化结果
红色为优化后,黄色为优化前
-
下一步,将相机参数从五个自由pose修改为 1 个自由pose + 4个相对pose
目前的相机pose结果
- april + BA
cam1 pose 0 = -0.0485857 0.135723 -0.244541 -0.321114 -0.0147088 0.946694 0.0209748 cam1 pose 1 = 0.170582 0.140898 -0.452368 0.66587 0.0126056 -0.745672 -0.0207909 cam1 pose 2 = -0.0703004 0.123238 0.423734 -0.251402 -0.0167631 0.967491 0.0218601 cam1 pose 3 = 0.923686 0.131545 0.303027 0.559366 0.0155254 -0.828466 -0.0226515 cam1 pose 4 = -0.514409 0.115909 0.750253 0.0494209 -0.0155203 0.998479 0.0188959 cam1 to cam 1 pose = -6.93889e-18 2.77556e-17 0 1 -3.46945e-18 4.16334e-17 0 cam1 to cam 2 pose = 0.156436 -0.000868648 -0.119435 0.805836 -0.00232152 0.592134 -0.00039844 cam1 to cam 3 pose = 0.0946539 -0.000862442 -0.304277 0.308983 -0.0097798 0.951017 -0.000313702 cam1 to cam 4 pose = -0.097055 0.00276807 -0.303732 -0.296674 0.00259925 0.954955 0.00628071 cam1 to cam 5 pose = -0.159647 0.00132047 -0.116511 0.80402 -0.00537274 -0.594577 -0.00136271- 五个相机的相对关系因此确定,接下来要确定的就是 cam1主相机和 激光雷达点云 之间的 外参关系
-
激光 - Cam1 外参标定
使用ICP将视觉获得的AprilTag与激光采集的点云进行ICP,获得Cam1与激光之间的外参变换
-
点云着色与遮挡
在获得外参后,获得最终的点云着色结果如上二图所示
-
单帧Mesh生成与纹理贴图
- 借助Meshlab的泊松重建恢复场景mesh,借助mvs-texturing对mesh进行场景纹理贴图
-
多帧融合与点云生成
-
由于单帧视角有限,进行多帧的融合与贴图
- 多帧ICP配准
- 多帧重建结果
-
