LED虚拟拍摄-跟踪算法

LED虚拟拍摄-跟踪算法

图引用拍摄黑科技，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使用冲突,等等这些细节问题才是更麻烦的.