LED虚拟拍摄-跟踪算法
标定流程
上面是一台Track设备,现精度比较高的主要是Redspy,Mosys,一般影视用这二种,其底层技术参考SMAL单目+惯性传感器(IMU),因为需要稳定精准的结果,实现上会贴红外反光片,使用红外相机得到这些贴片对应的稳定特征点用于建图(红外相机受外部亮度影响较小,在红外相机里,红外反光贴片相对周边会非常明亮,就像星空里发光的星星一样),也因为单目的原因,传感器本身的尺度与现实其实不对应,需要额外确保传感器本身的尺度与现实对应.
Track跟踪,主要是如下几步,求相机内参,相机与Track相对变换,Track空间与LED空间的相对变换,现在的标定一般来说,都是三步一起解决的.
标定内参,主要还是类似张正友标定法,得到在不同相机姿态态,得到一系列角点的2D-3D关系,其角点的选取可以来自棋盘格,这种方式限制大,需要拍摄全,对相机方向与位置限定了,影响后续手眼标定的精度,所以现在更多选择是的Aruco码,Aruco角点是有索引的,这样就方便找到明确的3D位置,相机只需要拍摄一部分aruco码就可以了,不会对相机距离与方向有限制,不过Aruco角点精度可能比不上圆形块,开了亚像素也不太行,于是像hecoos这样的,会利用视频流打圆形块,根据一系列帧出现/没出现计算出唯一索引,也能确定唯一位置,但是这种方式标定就会比较费时.
得到一系列3D-2D的角点后,代入cv::calibrateCamera求出就可以了,也得到每帧数据相机的变换,一般在记录图片时,也会记录Track的变换,这样就得到一组相机与Track的变换,如果根据这组数据,得到相机与Track相对变换了,这其实就是机器人常见的手眼标定的问题.
其手眼标定(本文限定眼在手上)的算法利用二个坐标系的钢体变化,构建一个AX=XB的问题去求解,先求得相记与Track的变换矩阵,然后得到Track坐标系与LED坐标系的变换,把结果代入记录数组,计算重投影的误差来判断结果的好坏.
实际过程来说,一般是取九张不同姿态的图,然后先算内参,再用手眼标定算相机与Track相对变换,但是直接九张图效果一般不太好,会加上RANSAC算法,选择其中3-5张结合结果,使用SVD得到最优Track空间与LED空间的相对变换,最后根据这二个结果代入之前记录计算重投影拿到最优结果.
手眼标定算法改进
这里有个问题,前面提到因为单目的原因,传感器本身的尺度与现实其实不对应,意思Track给出的数据是1m,但是在现实中可能对应10cm,10m,3m这些位移,OpneCV本身手眼标定并不能计算这种与现实的尺度不匹配的数据,现场一般会利用Redspy/Mosys硬件本身的功能纠正,但是使用比较麻烦,比如已经绑在摄像机上需要重新取下,人工取尺输入现实数据移动,精度就和人扯上关系了,和人扯上关系,就容易出问题,能不能改了?其实是可以,还记的当时那段时间刚好新冠被封锁在租的小区里,就仔细看了下opencv里的手眼标定的Tsai算法.
改进的主要就是求得位移的过程,先来看下opencv原手眼标定里求位移相关逻辑.
// 解决追踪器与摄像机使用不同尺寸问题. Mat A(3 * K, 3, CV_64FC1); // Will store: Pcij - Pgij Mat B(3 * K, 1, CV_64FC1); idx = 0; for (size_t i = 0; i < Hg.size(); i++) { for (size_t j = i+1; j < Hg.size(); j++, idx++) { //Defines coordinate transformation from Gi to Gj //Hgi is from Gi (gripper) to RW (robot base) //Hgj is from Gj (gripper) to RW (robot base) Mat Hgij = vec_Hgij[static_cast<size_t>(idx)]; //Defines coordinate transformation from Ci to Cj //Hci is from CW (calibration target) to Ci (camera) //Hcj is from CW (calibration target) to Cj (camera) Mat Hcij = vec_Hcij[static_cast<size_t>(idx)]; //Left-hand side: (Rgij - I) Mat diff = Hgij(Rect(0,0,3,3)) - Mat::eye(3,3,CV_64FC1); diff.copyTo(A(Rect(0, idx*3, 3, 3))); //Right-hand side: Rcg*Tcij - Tgij diff = Rcg*Hcij(Rect(3, 0, 1, 3)) - Hgij(Rect(3, 0, 1, 3)); diff.copyTo(B(Rect(0, idx*3, 1, 3))); } } Mat Tcg; //Translation from camera to gripper is obtained from the set of equations: // (Rgij - I) * Tcg = Rcg*Tcij - Tgij (eq 15) solve(A, B, Tcg, DECOMP_SVD); 利用等式
其中Rgij表示标定记录中二次记录之间追踪器空间下追踪器之间的旋转,根据记录是已知量.I表示3x3的单位矩阵,是固定量.Tcg表示追踪器空间下,摄像机相对追踪器的位移偏移,也就是我们要求解的值.Rcg表示追踪器空间下,摄像机相对追踪器的旋转偏移.Tcij 表示标定记录中二次记录之间摄像机空间下标定板之间的旋转,根据记录是已知量.Tgij 表示标定记录中二次记录之间追踪器空间下追踪器之间的位移,根据记录是已知量.假定Tracl与现实中的缩放参数是Scale,那么可以重组为如下等式.
原来矩阵算式如下.
现在变换后,矩阵算式如下.
其中新变换里的C表示- Rcg*Tcij,这样就把缩放参数带入要求解的[T0,T1,T2,S3]中的S3里面,新的代码如下.
// 解决追踪器与摄像机使用不同尺寸问题. Mat TA(3 * K, 4, CV_64FC1); // Will store: Pcij - Pgij Mat TB(3 * K, 1, CV_64FC1); idx = 0; for (size_t i = 0; i < Hg.size(); i++) { for (size_t j = i + 1; j < Hg.size(); j++, idx++) { // Defines coordinate transformation from Gi to Gj // Hgi is from Gi (gripper) to RW (robot base) // Hgj is from Gj (gripper) to RW (robot base) Mat Hgij = vec_Hgij[static_cast<size_t>(idx)]; // Defines coordinate transformation from Ci to Cj // Hci is from CW (calibration target) to Ci (camera) // Hcj is from CW (calibration target) to Cj (camera) Mat Hcij = vec_Hcij[static_cast<size_t>(idx)]; // Left-hand side:3x3_(Rgij - I) 3x1_(Rcg*Tcij) Mat ldiff1 = Hgij(Rect(0, 0, 3, 3)) - Mat::eye(3, 3, CV_64FC1); ldiff1.copyTo(TA(Rect(0, idx * 3, 3, 3))); Mat ldiff2 = -(Rcg * Hcij(Rect(3, 0, 1, 3))); ldiff2.copyTo(TA(Rect(3, idx * 3, 1, 3))); // Right-hand side: -Tgij Mat diff = -Hgij(Rect(3, 0, 1, 3)); diff.copyTo(TB(Rect(0, idx * 3, 1, 3))); } } Mat Tcg; solve(TA, TB, Tcg, DECOMP_SVD); 其求得的4*1前面三个表示Tcg(摄像机相对追踪器的位移),最后数据表示现实世界相对追踪器坐标系下缩放是scale.
这样后,应该得到类似一个如下结果.
struct ECameraTrack { // 追踪器坐标系如何变换成标定板坐标系 Eigen::Matrix4d base2target = {}; // 保存摄像机相对track的姿态 Eigen::Matrix4d camera2track = {}; // 追踪器坐标系相对标定板的位移缩放 double scale = 1.0; }; 图优化标定结果
上面处理后,一般来说,结果平均在8个像素误差左右,用于VP肯定是够了,VP一般是扩展FOV的,但是如果想用于XR拍摄,这个精度就可能会不够,还能不能计算更精准,先看下优化后的效果.
可以看到平均4个像素误差优化到1个左右,实际实现情况大的LED幕墙,原结果平均8个像素(还不稳定)可以优化到稳定的平均3个像素左右误差,精度够XR用了.
那段时间正好在看同事推荐的视觉SLAM十四讲,学习图优化相关框架g2o,可以直接使用测量值比较误差求解或是优化参数,简单总结图优化的步骤,一是确定需要优化的变量,二是确定变量到观测量的计算过程,通过这个过程得到计算结果与观测量比较得到误差,第三步是构建变量与计算过程与观测量误差的图.
前面二步对应图优化框架g2o里的二个概念,分别是顶点和边,其顶点对应的就是需要求解的变量,而边就是由顶点构建计算过程,得到结果并与测量值确定误差,第三步优化过程就是构建顶点,边与测量值的图.
相对常规解法,图优化是直接根据测量值优化结果,只需要构建一个参数能正确和测量值比较误差的模型就行,相比手眼标定算法,需要理解钢体关系多帧间几个变化的相等性到构建AX=XB的处理来说,直接由结果去代入重投影比较误差,然后构建模型会非常容易理解,在3D视觉中,有非常多的这种需求,知道变量,知道变量导致的结果误差,但是不知道怎么求解,其图优化就可以求解这种情况,所以在3D视觉中,其图优化使用非常普遍.
如下有些代码,其变换关系不理解会比较乱,为了方便理解,简单介绍一下结构与命名,比如target说的是屏幕坐标系,camera表示摄像机在屏幕坐标系下的运动,base是追踪器坐标系,track表示追踪器在追踪器坐标系下的运动,这样如base2target表示追踪器坐标系转到屏幕坐标系下的变换,而camera2track表示camera相对track在追踪器坐标系下相对变换,camera2target表示摄像机在屏幕坐标系下的变换,target2camera表示camera2target的逆变换,可以理解成屏幕在摄像机下的变换,track2base表示追踪器在追踪器坐标系下的变换.
如前面介绍图优化的过程,先需要确定求解的值,在这就是camera2track(也就是原手眼标定求解的值),base2target用于把追踪器坐标系转化到屏幕坐标系下,需要注意的是,这里的track给的位移与现实中的位移没有对应,有个缩放关系,在这我们也需要求解这个Scale值,这样就有三个值需要求解,然后假定标定过程使用的Track变换相对真实值有细微误差,针对每个Track记录变换也当做一个需要优化的值,这个处理有兴趣的可以详细参考基于重投影误差最小化的手眼标定,能正确的处理Track异常记录,经实际测试,对于结果的正确性会有很大提高.
需要的结果是二个变换+一个缩放,和中间Track记录变换的优化,变换对应的变量直接使用g2o内置的VertexSE3,截取重要代码如下,有兴趣可以自己去看g2o里的源码,而缩放只需要简单封装一个double就行,简单来说,顶点最主要的是实现方法oplusImpl,告诉顶点如果更新参数.
// 内置钢体变换顶点 class G2O_TYPES_SLAM3D_API VertexSE3 : public BaseVertex<6, Isometry3> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW; virtual void setToOriginImpl() { _estimate = Isometry3::Identity(); } virtual void oplusImpl(const double* update) { Eigen::Map<const Vector6> v(update); Isometry3 increment = internal::fromVectorMQT(v); _estimate = _estimate * increment; } } // 封装缩放的顶点 class ScaleVertex : public g2o::BaseVertex<1, double> { public: ScaleVertex() {} virtual void setToOriginImpl() { _estimate = 1.0; } virtual void oplusImpl(const double* update) { _estimate += update[0]; } virtual bool read(std::istream& is) { is >> _estimate; return true; } virtual bool write(std::ostream& os) const { os << _estimate; return true; } }; 确定顶点后,然后就是确定边,就如前面所说,边是确定如何优化顶点与测量值的误差变小的,这里使用重投影确定角点UV与测量UV的误差,角点的三维位置与测量UV分别由建立屏幕坐标系时确定以及OPENCV查找角点得到,边的computeError记录了如何把角点的三维位置转化到摄像机下的位置,并得到UV,然后与测量UV比较的过程.
class ProjectionHandEdge : public g2o::BaseUnaryEdge<2, Eigen::Vector2d, g2o::VertexSE3> { EIGEN_MAKE_ALIGNED_OPERATOR_NEW public: ProjectionHandEdge() : g2o::BaseUnaryEdge<2, Eigen::Vector2d, g2o::VertexSE3>() { resizeParameters(1); installParameter(lensModelPar, 0); } virtual bool read(std::istream& is) { return false; } virtual bool write(std::ostream& os) const { return false; }; virtual void computeError() override; g2o::Vector3 point = {}; protected: LensModelParameter* lensModelPar = nullptr; }; void ProjectionHandEdge::computeError() { const g2o::VertexSE3* target2cameraVec = dynamic_cast<const g2o::VertexSE3*>(_vertices[0]); Eigen::Isometry3d target2camera = target2cameraVec->estimate(); // 点在相机下位置 g2o::Vector3 cameraPos = target2camera * point; // UV g2o::Vector2 cuv = lensModelPar->map(cameraPos); // 误差由观测值减预测值 _error = measurement() - cuv; } // 使用小孔相机模型确定3维顶点的投影平面 g2o::Vector2 lensMap(const LensModel& lensModel, const g2o::Vector3& pos, double scale) { double x = pos[0] / pos[2]; double y = pos[1] / pos[2]; double fx = lensModel.focalLength.x; double fy = lensModel.focalLength.y; double cx = lensModel.focalCenter.x; double cy = lensModel.focalCenter.y; double k1 = lensModel.K1; double k2 = lensModel.K2; double k3 = lensModel.K3; double p1 = lensModel.P1; double p2 = lensModel.P2; // 径向畸变 double r2 = x * x + y * y; double r4 = r2 * r2; double r6 = r4 * r2; // 切向畸变 double a1 = 2 * x * y; double a2 = r2 + 2 * x * x; double a3 = r2 + 2 * y * y; double cdist = 1.0 + k1 * r2 + k2 * r4 + k3 * r6; x = x * cdist + p1 * a1 + p2 * a2; y = y * cdist + p1 * a3 + p2 * a1; // UV double u = fx * x + cx; double v = fy * y + cy; return g2o::Vector2(u * scale, v * scale); } 每次记录对应的Track变换优化对应设计如下,已经每次记录track2base(track在Track坐标系下的变换),和根据需要求解的变量camera2track,base2target,scale以及摄像机在屏幕坐标系下的变换,得到求解出来的track2base,根据测试出来的track2base,比较误差.
// track姿态优化 class HandEyeEdge : public g2o::BaseMultiEdge<6, Eigen::Isometry3d> { EIGEN_MAKE_ALIGNED_OPERATOR_NEW public: HandEyeEdge(); virtual bool read(std::istream& is); virtual bool write(std::ostream& os) const; virtual void computeError() override; }; HandEyeEdge::HandEyeEdge() { resize(4); } bool HandEyeEdge::read(std::istream& is) { Vector7 est = {}; bool state = internal::readVector(is, est); _measurement = internal::fromVectorQT(est); return readInformationMatrix(is); } bool HandEyeEdge::write(std::ostream& os) const { internal::writeVector(os, internal::toVectorQT(measurement())); return writeInformationMatrix(os); } Eigen::Isometry3d getScaleIsometry3d(const Eigen::Isometry3d& src, double scale) { Eigen::Isometry3d result = src; result.translation() = scale * src.translation(); return result; }; void HandEyeEdge::computeError() { // 摄像机相对Track const g2o::VertexSE3* camera2trackVec = dynamic_cast<const g2o::VertexSE3*>(_vertices[0]); // 追踪器坐标系变换到角点坐标系 const g2o::VertexSE3* base2targetVec = dynamic_cast<const g2o::VertexSE3*>(_vertices[1]); // 角点 const g2o::VertexSE3* target2cameraVec = dynamic_cast<const g2o::VertexSE3*>(_vertices[2]); // 缩放 const ScaleVertex* scaleVec = dynamic_cast<const ScaleVertex*>(_vertices[3]); // 当前值 Eigen::Isometry3d camera2track = camera2trackVec->estimate(); Eigen::Isometry3d base2target = base2targetVec->estimate(); Eigen::Isometry3d target2camera = target2cameraVec->estimate(); double scale = scaleVec->estimate(); // 优化量计算得到的Track Eigen::Isometry3d base2track = camera2track * target2camera * base2target; // 转化成世界尺度下 Eigen::Isometry3d measurementScale = getScaleIsometry3d(_measurement, scale); // 比较计算的Track与测量的Track姿态 Eigen::Isometry3d delta = measurementScale * base2track; _error = g2o::internal::toVectorMQT(delta); } 同上面重投影边一样,在computeError把根据这些结果求得的track变换与测量的track变换比较误差,最后我们把所有已知数据与测量值组成图如下.
CameraTrack HandEyeOptimizer::compute(const HandEyeParamet& handEyeParamet, const LensModel& lensModel, const CameraTrack& cameraTrack) { handEyePar = handEyeParamet; eigen::ECameraTrack camtrack = {}; eigen::toCameraTrack(cameraTrack, camtrack); Eigen::Isometry3d camera2track = Eigen::Isometry3d::Identity(); Eigen::Isometry3d base2target = Eigen::Isometry3d::Identity(); camera2track.matrix() = camtrack.camera2track; base2target.matrix() = camtrack.base2target; // 设置HandEyeEdge的变化率 Eigen::MatrixXd handeyeInf = Eigen::MatrixXd::Identity(6, 6); // 移动部分 handeyeInf.topLeftCorner(3, 3) *= 0.01; // 旋转部分 handeyeInf.bottomRightCorner(3, 3) *= 1.0; // 优化器 SparseOptimizer optimizer; using LinearSolver = g2o::LinearSolverDense<g2o::BlockSolverX::PoseMatrixType>; OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( std::make_unique<g2o::BlockSolverX>( std::make_unique<LinearSolver>())); // 设置优化方法 optimizer.setAlgorithm(solver); // 设置镜头参数 LensModelParameter* lensPar = new LensModelParameter(lensModel); lensPar->uvScale = handEyeParamet.uvScale; lensPar->setId(0); optimizer.addParameter(lensPar); // 优化量camera2Track VertexSE3* c2tVec = new VertexSE3(); c2tVec->setEstimate(camera2track); c2tVec->setId(0); // c2tVec->setFixed(true); optimizer.addVertex(c2tVec); // 优化量base2target VertexSE3* b2tVec = new VertexSE3(); b2tVec->setEstimate(base2target); b2tVec->setId(1); // b2tVec->setFixed(true); optimizer.addVertex(b2tVec); ScaleVertex* scaleVec = new ScaleVertex(); scaleVec->setEstimate(cameraTrack.scale); scaleVec->setId(2); scaleVec->setFixed(handEyePar.bFixScale); optimizer.addVertex(scaleVec); // 每条记录(包含摄像机变换,Track变换,角点UV与三维位置) for (const TrackCorners& trackCorner : trackCorners) { aoce::Mat4x4d trackPose = trackCorner.trackPose; aoce::Mat4x4d cameraPose = trackCorner.cameraPose; Eigen::Isometry3d track2base = Eigen::Isometry3d::Identity(); track2base.matrix() = eigen::toMat(trackPose); // track转化成真实世界坐标系中 Eigen::Isometry3d track2baseScale = getScaleIsometry3d(track2base, cameraTrack.scale); // 角点相对摄像机坐标系的转换 Eigen::Isometry3d camera2target = Eigen::Isometry3d::Identity(); if (trackCorner.cameraPose.valid()) { camera2target.matrix() = eigen::toMat(trackCorner.cameraPose); } else { camera2target = base2target * track2baseScale * camera2track; } Eigen::Isometry3d target2camera = camera2target.inverse(); g2o::VertexSE3* t2cVec = new g2o::VertexSE3(); t2cVec->setEstimate(target2camera); t2cVec->setId(optimizer.vertices().size()); // t2cVec->setFixed(true); optimizer.addVertex(t2cVec); // 设置Track姿态边 HandEyeEdge* handEyeEdge = new HandEyeEdge(); handEyeEdge->vertices()[0] = c2tVec; handEyeEdge->vertices()[1] = b2tVec; handEyeEdge->vertices()[2] = t2cVec; handEyeEdge->vertices()[3] = scaleVec; // 比较实测与观测的Track数据 handEyeEdge->setMeasurement(track2base); handEyeEdge->setInformation(handeyeInf); handEyeEdge->setId(optimizer.edges().size()); if (handEyePar.robustHandEye) { g2o::RobustKernelHuber* kerner = new g2o::RobustKernelHuber(); kerner->setDelta(handEyePar.handEyeDelta); handEyeEdge->setRobustKernel(kerner); } optimizer.addEdge(handEyeEdge); // 经测试,UV使用的粒度小时,精度会提升 vec2d sizeInv = { handEyeParamet.uvScale / trackCorner.pointCorners.imageSize.x, handEyeParamet.uvScale / trackCorner.pointCorners.imageSize.y}; for (int32_t i = 0; i < trackCorner.pointCorners.count; i++) { vec2f corner = *(trackCorner.pointCorners.corners + i); vec3f point = *(trackCorner.pointCorners.points + i); // 设置投影边 ProjectionHandEdge* proEdge = new ProjectionHandEdge(); proEdge->setMeasurement({corner.x * sizeInv.x, corner.y * sizeInv.y}); proEdge->setInformation(Eigen::Matrix2d::Identity() * 0.01); proEdge->vertices()[0] = t2cVec; proEdge->setParameterId(0, 0); proEdge->setId(optimizer.edges().size()); proEdge->point = {point.x, point.y, point.z}; if (handEyePar.projectionHand) { g2o::RobustKernelHuber* kerner = new g2o::RobustKernelHuber(); kerner->setDelta(handEyePar.projectionHand); proEdge->setRobustKernel(kerner); } optimizer.addEdge(proEdge); } } // 执行优化 bool bInit = optimizer.initializeOptimization(); if (!bInit) { logMessage(LogLevel::warn, "ZoomScaleOptimizer::computePoseZoom init optimizer failed"); return cameraTrack; } // optimizer.setVerbose(true); optimizer.optimize(10); // 检测 camtrack.camera2track = c2tVec->estimate().matrix(); camtrack.base2target = b2tVec->estimate().matrix(); camtrack.scale = scaleVec->estimate(); CameraTrack result = {}; eigen::toCameraTrack(camtrack, result); return result; } 整个过程经过优化器多次迭代就能得到更优结果,相对于原始手眼标定的结果来说,优点不少,原始的数据越多,其直接组合一起计算结果很差,只能从各种组合计算结果使用重投影确定最优值,一般最多只选择其中不大于五条记录会是一个最好的结果,大部分记录被排除计算,数据量比较少的情况下,又不能保证结果在10个像素以内,使用图优化的方法,在记录少的情况就能得到非常优秀的结果,在记录多的情况下,更能保证更多的数据得到平均最优的结果,代入以前记录的各组记录数据,在图优化的情况下,能保证所有结果都能得到更好的结果.
变焦标定
在标定Track之后,知道镜头在某个焦段的内外参,扩展到变焦镜头,当镜头的zoom变化后,内参如何变化?
一般相机内参fx/fy,cx/cy,考虑畸变系数K1,K2,P1,P2,K3的畸变模型,与zoom有关的是fx/fy,cx/cy影响不大,畸变P1,P2现在相机在工艺上上,值非常小,对畸变的影响非常小,K3影响也不太大,故需要考虑的fx/fy,K1,K2这四个参数,考虑fx/fy比值固定,只需要考虑fx,k1,k2这三个参数在变焦镜头的zoom变化后,如何变化?
相机zoom变化后,内参不做变化,可以看到匹配的特征误差越来越大,也就是反投影的误差变大,误差优化参数,还得是图优化.
相比上一个手眼标定的图优化过程的点边模型,这个模型会简单不少,假定fx,k1,k2是zoom变化的曲线(ax^2+bx+c=y)变化,最后结果就是求得fx,k1,k2对应的a,b,c的值.对应有二种思路,一种是根据每个zoom下的图,反投影图优化得到fx,k1,k2,最后再拟合多个fx,k1,k2各自的曲线,第二种是直接把各自曲线所有zoom下的图然后反投影图优化一起优化,现二种最终结果相关不大,在定焦三个误差内,变焦后各zoom平均在10个像素,后面也还需要持续优化.
这里列一种处理的部分代码.
void ZoomCurveEdge::computeError() { const CurveVertex* fxVer = dynamic_cast<const CurveVertex*>(_vertices[0]); const CurveVertex* k1Ver = dynamic_cast<const CurveVertex*>(_vertices[1]); const CurveVertex* k2Ver = dynamic_cast<const CurveVertex*>(_vertices[2]); const ScaleVertex* scaleVer = dynamic_cast<const ScaleVertex*>(_vertices[3]); const g2o::Vector3 fxCurve = fxVer->estimate(); const g2o::Vector3 k1Curve = k1Ver->estimate(); const g2o::Vector3 k2Curve = k2Ver->estimate(); const double sacle = scaleVer->estimate(); LensModel lensModel = lensModelPar->lensModel; double aspectRatio = lensModel.focalLength.y / lensModel.focalLength.x; double fx = getCurveVal(fxCurve, sacle); lensModel.focalLength.x = fx; lensModel.focalLength.y = fx * aspectRatio; lensModel.K1 = getCurveVal(k1Curve, sacle); lensModel.K2 = getCurveVal(k2Curve, sacle); g2o::Vector2 estValue = lensMap(lensModel, point); // 误差由观测值减预测值 _error = measurement() - estValue; } bool ZoomLensOptimizer::compute(const ZoomOptParamet& paramet) { optParamet = paramet; // 开始优化 SparseOptimizer optimizer; // LinearSolverDense LinearSolverEigen LinearSolverPCG using LinearSolver = g2o::LinearSolverEigen<g2o::BlockSolverX::PoseMatrixType>; // OptimizationAlgorithmGaussNewton OptimizationAlgorithmLevenberg // OptimizationAlgorithmDogleg OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( std::make_unique<g2o::BlockSolverX>( std::make_unique<LinearSolver>())); // 设置优化方法 optimizer.setAlgorithm(solver); // 设置参数 LensModelParameter* lensPar = new LensModelParameter(lensModel); lensPar->setId(0); optimizer.addParameter(lensPar); // 曲线 CurveVertex* fxCurve = new CurveVertex(); fxCurve->setEstimate({0, 0, lensModel.focalLength.x}); fxCurve->setId(0); optimizer.addVertex(fxCurve); CurveVertex* k1Curve = new CurveVertex(); k1Curve->setEstimate({0, 0, lensModel.K1}); k1Curve->setId(1); k1Curve->setFixed(optParamet.fixDistort); optimizer.addVertex(k1Curve); CurveVertex* k2Curve = new CurveVertex(); k2Curve->setEstimate({0, 0, lensModel.K2}); k2Curve->setId(2); k2Curve->setFixed(true); optimizer.addVertex(k2Curve); std::vector<ScaleVertex*> fxList; // 添加点与边 for (const CameraZoom& poseZoom : poseZooms) { Eigen::Isometry3d target2camera = Eigen::Isometry3d::Identity(); target2camera.matrix() = eigen::toMat(invPoseMat(poseZoom.cameraPose)); // 假定变焦环的数据并不严谨(推理不出来,只能固定) ScaleVertex* scaleValue = new ScaleVertex(); scaleValue->setId(optimizer.vertices().size()); scaleValue->setEstimate(poseZoom.zoomScale); scaleValue->setFixed(true); optimizer.addVertex(scaleValue); // vec2d sizeInv = {1.0 / poseZoom.pointCorners.imageSize.x, 1.0 / poseZoom.pointCorners.imageSize.y}; // 反投影 for (int32_t i = 0; i < poseZoom.pointCorners.count; i++) { vec2f corner = *(poseZoom.pointCorners.corners + i); vec3f point = *(poseZoom.pointCorners.points + i); // 点在相机下位置 g2o::Vector3 cornerPos = {point.x, point.y, point.z}; g2o::Vector3 cameraPos = target2camera * cornerPos; // 设置边 ZoomCurveEdge* zoomLensEdge = new ZoomCurveEdge(); zoomLensEdge->vertices()[0] = fxCurve; zoomLensEdge->vertices()[1] = k1Curve; zoomLensEdge->vertices()[2] = k2Curve; zoomLensEdge->vertices()[3] = scaleValue; zoomLensEdge->setMeasurement( {corner.x * sizeInv.x, corner.y * sizeInv.y}); zoomLensEdge->setInformation(Eigen::Matrix2d::Identity()); zoomLensEdge->setParameterId(0, 0); zoomLensEdge->setId(optimizer.edges().size()); zoomLensEdge->point = cameraPos; if (true) { g2o::RobustKernelHuber* kerner = new g2o::RobustKernelHuber(); kerner->setDelta(1.0); zoomLensEdge->setRobustKernel(kerner); } optimizer.addEdge(zoomLensEdge); } } // 执行优化 bool bInit = optimizer.initializeOptimization(); optimizer.optimize(10); fxPar = fxCurve->estimate(); k1Par = k1Curve->estimate(); k2Par = k2Curve->estimate(); // 计算误差 LensModel slensModel = lensModel; for (CameraZoom& poseZoom : poseZooms) { Eigen::Vector2d offset = {0, 0}; if (getLensModel(poseZoom.zoomScale, slensModel)) { Mat4x4d target2camera = poseZoom.cameraPose.inverse(); offset = projectOffset(target2camera, slensModel, poseZoom.pointCorners); } } return true; } 这个结果还需优化,在写到这里的时候仔细想了下,主要可能有几点,一是fx/fy在变焦下比值是否有相对大的变化,二是上述曲线是否有更科学的模型,三是现k2代入后,结果大部分结果更差,导致k2现在用固定的,但是焦距变化比较大后,K2的变化还是比较明显的,后面有机会再想想改进吧.
整个跟踪相关的算法差不多就是这个样子,其实算法占的比例并不大,主要是工程上的各种问题,如各种相机采集(Decklink,MF...),Redspy,Mosys硬件接入,以及给的是欧拉角,如何确定顺序,追踪数据如何通过LiveLink数据发送到UE,不同空间坐标系的变换,FBXMesh的导入与导出,各个模块如何有序的组合,动态链接库与UE使用冲突,等等这些细节问题才是更麻烦的.
![洛谷 P11345 [KTSC 2023 R2] 基地简化 题解](http://www.itfaba.com/wp-content/themes/kemi/timthumb.php?src=http://www.itfaba.com/wp-content/themes/kemi/img/random/3.jpg&w=218&h=124&zc=1)
