通过权重以及限制车辆高度来用uwb对车辆进行定位

该项目主要用来计算多相机的车辆定位算法，主要代码在pose_estimation_3d2d.cpp， 运行命令为./pose_estimation_3d2d， 该项目数据为华润装载机采集的数据，使用的相机为前视相机与后视相机，通过前视相机采集的 反光标志特征点像素坐标以及后视相机采集的特征点像素坐标，然后与地图中的3d点坐标投影到2d 中的投影坐标进行求差，迭代优化使差最小化； 其中的难点在于，不同的相机内参以及相机到车体的外参不一样，所以导致高斯牛顿法的雅克比矩阵需要 重新求解； 解决思路： 原有解决方法为，将雅克比矩阵分为两部分，一部分为相机坐标系下的坐标（X、Y、Z）转化为像素坐标（U、V），然后由U、V分别对x、y、z求偏导得到的2行3列的雅克比矩阵； 另一部分为世界坐标系的点转换到相机坐标系，计算旋转之后点的坐标相对于（旋转、平移）的导数，扰动量为旋转平移矩阵，即旋转平移矩阵的稍微变化；即可以得到3行6列的雅克比矩阵； 将这两部分雅克比矩阵相乘即可以得到2行6列的雅克比矩阵； 创新点： 将上述方法中的基准坐标系为相机坐标系转换为整车坐标系， 从而将世界坐标系转换到整车坐标系的坐标相对于（旋转、平移）的导数的雅克比矩阵不变， 从而只需要修改第一部分，不过由于从整车坐标系转换为像素坐标，还需要为K*RT*P，所以需要将 外参矩阵进行计算从而来进行求偏导得到雅克比矩阵，具体计算在代码中；