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) * Tcg = Rcg*Tcij - Tgij
\]

其中Rgij表示标定记录中二次记录之间追踪器空间下追踪器之间的旋转,根据记录是已知量.I表示3x3的单位矩阵,是固定量.Tcg表示追踪器空间下,摄像机相对追踪器的位移偏移,也就是我们要求解的值.Rcg表示追踪器空间下,摄像机相对追踪器的旋转偏移.Tcij 表示标定记录中二次记录之间摄像机空间下标定板之间的旋转,根据记录是已知量.Tgij 表示标定记录中二次记录之间追踪器空间下追踪器之间的位移,根据记录是已知量.假定Tracl与现实中的缩放参数是Scale,那么可以重组为如下等式.

\[(Rgij - I) * Tcg - Rcg*Tcij*Scale = -Tgij
\]

原来矩阵算式如下.

\[\begin{bmatrix}
A_{00}&A_{01}&A_{02}\\
A_{10}&A_{11}&A_{12}\\
A_{20}&A_{21}&A_{22}\\
\end{bmatrix} * \begin{bmatrix}
T_{0}\\
T_{1}\\
T_{2}\\
\end{bmatrix} = \begin{bmatrix}
B_{0}\\
B_{1}\\
B_{2}\\
\end{bmatrix}
\]

现在变换后,矩阵算式如下.

\[\begin{bmatrix}
A_{00}&A_{01}&A_{02}&C_{03}\\
A_{10}&A_{11}&A_{12}&C_{13}\\
A_{20}&A_{21}&A_{22}&C_{23}\\
\end{bmatrix} * \begin{bmatrix}
T_{0}\\
T_{1}\\
T_{2}\\
S_{3}\\
\end{bmatrix} = \begin{bmatrix}
B_{0}\\
B_{1}\\
B_{2}\\
\end{bmatrix}
\]

其中新变换里的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使用冲突,等等这些细节问题才是更麻烦的.