前言

通过上一篇文章，我们看到了kalibr做内参标定的第一步是做相机内参和畸变的初始化，我们接下来就要对这部分进行详细的讨论

提示：以下是本篇文章正文内容，下面案例可供参考

一、内参、畸变初始化调用部分

在前文说到过这块，这里就是初始化内参和畸变的地方
调用的函数定义在kalibr_camera_calibration中可以找到

这块我们可以看到在这个函数里，主要执行的是两个函数，一个是geometry.initializeIntrinsics(observations)，这个函数可以在相机模型里找到对应的定义。
另一个是calibrateIntrinsics，这个函数主要用来标定，函数定义在kalibr_camera_calibration中的CameraIntializers.py中。

二、initializeIntrinsics初始化内参

这里是用来初始化内参的，我们来看小孔成像模型的这个成员函数。

/// \brief initialize the intrinsics based on one view of a gridded calibration target
/// These functions are based on functions from Lionel Heng and the excellent camodocal
/// https://github.com/hengli/camodocal
//this algorithm can be used with high distortion lenses
template<typename DISTORTION_T>
bool PinholeProjection<DISTORTION_T>::initializeIntrinsics(const std::vector<GridCalibrationTargetObservation> &observations) {

  SM_DEFINE_EXCEPTION(Exception, std::runtime_error);
  SM_ASSERT_TRUE(Exception, observations.size() != 0, "Need min. one observation");

  // First, initialize the image center at the center of the image.
  _cu = (observations[0].imCols() - 1.0) / 2.0;
  _cv = (observations[0].imRows() - 1.0) / 2.0;
  _ru = observations[0].imCols();
  _rv = observations[0].imRows();
  _distortion.clear();

  //process all images
  size_t nImages = observations.size();

  // Initialize focal length
  // C. Hughes, P. Denny, M. Glavin, and E. Jones,
  // Equidistant Fish-Eye Calibration and Rectification by Vanishing Point
  // Extraction, PAMI 2010
  // Find circles from rows of chessboard corners, and for each pair
  // of circles, find vanishing points: v1 and v2.
  // f = ||v1 - v2|| / PI;
  std::vector<double> f_guesses;

  for (size_t i=0; i<nImages; ++i) {
    const GridCalibrationTargetObservation& obs = observations.at(i);
    SM_ASSERT_TRUE(Exception, obs.target(), "The GridCalibrationTargetObservation has no target object");
    const GridCalibrationTargetBase & target = *obs.target();

    std::vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d>> center(target.rows());
    double radius[target.rows()];
    bool skipImage=false;

    for (size_t r=0; r<target.rows(); ++r) {
      std::vector<cv::Point2d> circle;
      for (size_t c=0; c<target.cols(); ++c) {
        Eigen::Vector2d imagePoint;
        Eigen::Vector3d gridPoint;

        if (obs.imageGridPoint(r, c, imagePoint))
          circle.push_back(cv::Point2f(imagePoint[0], imagePoint[1]));
        else
          //skip this image if the board view is not complete
          skipImage=true;
      }
      PinholeHelpers::fitCircle(circle, center[r](0), center[r](1), radius[r]);
    }

    if(skipImage)
      continue;

    for (size_t j=0; j<target.rows(); ++j)
    {
      for (size_t k=j+1; k<target.cols(); ++k)
      {
        // find distance between pair of vanishing points which
        // correspond to intersection points of 2 circles
        std::vector < cv::Point2d > ipts;
        ipts = PinholeHelpers::intersectCircles(center[j](0), center[j](1),
                                                radius[j], center[k](0), center[k](1), radius[k]);
        if (ipts.size()<2)
          continue;

         double f_guess = cv::norm(ipts.at(0) - ipts.at(1)) / M_PI;
         f_guesses.push_back(f_guess);
      }
    }
  }
  if(f_guesses.empty()) {
    const char* manual_input = std::getenv("KALIBR_MANUAL_FOCAL_LENGTH_INIT");
    if(manual_input != nullptr) {
      double input_guess;
      std::cout << "Initialization of focal length failed. Provide manual initialization: " << std::endl;
      std::cin >> input_guess;
      SM_ASSERT_GT(std::runtime_error, input_guess, 0.0, 
                "Focal length needs to be positive.");
      std::cout << "Initializing focal length to " << input_guess << std::endl;
      f_guesses.push_back(input_guess);
    } else {
      std::cout << "Initialization of focal length failed. You can enable"
        << " manual input by setting 'KALIBR_MANUAL_FOCAL_LENGTH_INIT'." << std::endl;
      return false;
    }
  }
  // Get the median of the guesses if available.
  double f0 = PinholeHelpers::medianOfVectorElements(f_guesses);

  //set the estimate
  _fu = f0;
  _fv = f0;
  updateTemporaries();

  return true;
}

这个成员函数就很良心了，不仅仅有注释，而且把他从哪里抄来的，对应的论文是什么都写上去了。
https://github.com/hengli/camodocal 这个源码其实就很好看一些了，里面写的结构也比较简单，写了张正友标定法用来做普通工业镜头的标定的方法和鱼眼相机初始化参数的方法，我先来说kalibr这里的，这段函数是用来初始化畸变比较大的鱼眼相机或广角相机。方法就是提取标定板每一行的角点，然后拟合一个圆，与其他行拟合的圆有两个交点，f=两点之间的直线长度/pi，用这种方法来初始化。

下面我会把他抄的那个的源码中关于张正友标定法的部分贴上,后面看有时间专门针对这块补充一下原理

void PinholeCamera::estimateIntrinsics(const cv::Size& boardSize,
                                  const std::vector< std::vector<cv::Point3f> >& objectPoints,
                                  const std::vector< std::vector<cv::Point2f> >& imagePoints)
{
    // Z. Zhang, A Flexible New Technique for Camera Calibration, PAMI 2000

    Parameters params = getParameters();

    params.k1() = 0.0;
    params.k2() = 0.0;
    params.p1() = 0.0;
    params.p2() = 0.0;

    double cx = params.imageWidth() / 2.0;
    double cy = params.imageHeight() / 2.0;
    params.cx() = cx;
    params.cy() = cy;

    size_t nImages = imagePoints.size();

    cv::Mat A(nImages * 2, 2, CV_64F);
    cv::Mat b(nImages * 2, 1, CV_64F);

    for (size_t i = 0; i < nImages; ++i)
    {
        const std::vector<cv::Point3f>& oPoints = objectPoints.at(i);

        std::vector<cv::Point2f> M(oPoints.size());
        for (size_t j = 0; j < M.size(); ++j)
        {
            M.at(j) = cv::Point2f(oPoints.at(j).x, oPoints.at(j).y);
        }

        cv::Mat H = cv::findHomography(M, imagePoints.at(i));

        H.at<double>(0,0) -= H.at<double>(2,0) * cx;
        H.at<double>(0,1) -= H.at<double>(2,1) * cx;
        H.at<double>(0,2) -= H.at<double>(2,2) * cx;
        H.at<double>(1,0) -= H.at<double>(2,0) * cy;
        H.at<double>(1,1) -= H.at<double>(2,1) * cy;
        H.at<double>(1,2) -= H.at<double>(2,2) * cy;

        double h[3], v[3], d1[3], d2[3];
        double n[4] = {0,0,0,0};

        for (int j = 0; j < 3; ++j)
        {
            double t0 = H.at<double>(j,0);
            double t1 = H.at<double>(j,1);
            h[j] = t0; v[j] = t1;
            d1[j] = (t0 + t1) * 0.5;
            d2[j] = (t0 - t1) * 0.5;
            n[0] += t0 * t0; n[1] += t1 * t1;
            n[2] += d1[j] * d1[j]; n[3] += d2[j] * d2[j];
        }

        for (int j = 0; j < 4; ++j)
        {
            n[j] = 1.0 / sqrt(n[j]);
        }

        for (int j = 0; j < 3; ++j)
        {
            h[j] *= n[0]; v[j] *= n[1];
            d1[j] *= n[2]; d2[j] *= n[3];
        }

        A.at<double>(i * 2, 0) = h[0] * v[0];
        A.at<double>(i * 2, 1) = h[1] * v[1];
        A.at<double>(i * 2 + 1, 0) = d1[0] * d2[0];
        A.at<double>(i * 2 + 1, 1) = d1[1] * d2[1];
        b.at<double>(i * 2, 0) = -h[2] * v[2];
        b.at<double>(i * 2 + 1, 0) = -d1[2] * d2[2];
    }

    cv::Mat f(2, 1, CV_64F);
    cv::solve(A, b, f, cv::DECOMP_NORMAL | cv::DECOMP_LU);

    params.fx() = sqrt(fabs(1.0 / f.at<double>(0)));
    params.fy() = sqrt(fabs(1.0 / f.at<double>(1)));

    setParameters(params);
}

三、calibrateIntrinsics标定内参和畸变

最后，我们来看一下另外一个用来标定内参和畸变的函数，这个函数主要是用光束平差法去求解内参和畸变的过程，我对光束平差法了解的不是很全面，这里暂时放一放，先略过，后面再来补充

def calibrateIntrinsics(cam_geometry, obslist, distortionActive=True, intrinsicsActive=True):
    #verbose output
    if sm.getLoggingLevel()==sm.LoggingLevel.Debug:
        d = cam_geometry.geometry.projection().distortion().getParameters().flatten()
        p = cam_geometry.geometry.projection().getParameters().flatten()
        sm.logDebug("calibrateIntrinsics: intrinsics guess: {0}".format(p))
        sm.logDebug("calibrateIntrinsics: distortion guess: {0}".format(d))
    
    ############################################
    ## solve the bundle adjustment
    ############################################
    problem = aopt.OptimizationProblem()
    
    #add camera dvs
    cam_geometry.setDvActiveStatus(intrinsicsActive, distortionActive, False)
    problem.addDesignVariable(cam_geometry.dv.distortionDesignVariable())
    problem.addDesignVariable(cam_geometry.dv.projectionDesignVariable())
    problem.addDesignVariable(cam_geometry.dv.shutterDesignVariable())
    
    #corner uncertainty
    cornerUncertainty = 1.0
    R = np.eye(2) * cornerUncertainty * cornerUncertainty
    invR = np.linalg.inv(R)
    
    #get the image and target points corresponding to the frame
    target = cam_geometry.ctarget.detector.target()
    
    #target pose dv for all target views (=T_camL_w)
    reprojectionErrors = [];    
    sm.logDebug("calibrateIntrinsics: adding camera error terms for {0} calibration targets".format(len(obslist)))
    target_pose_dvs=list()
    for obs in obslist: 
        success, T_t_c = cam_geometry.geometry.estimateTransformation(obs)
        target_pose_dv = addPoseDesignVariable(problem, T_t_c)
        target_pose_dvs.append(target_pose_dv)
        
        T_cam_w = target_pose_dv.toExpression().inverse()
    
        ## add error terms
        for i in range(0, target.size()):
            p_target = aopt.HomogeneousExpression(sm.toHomogeneous(target.point(i)));
            valid, y = obs.imagePoint(i)
            if valid:
                rerr = cam_geometry.model.reprojectionError(y, invR, T_cam_w * p_target, cam_geometry.dv)
                problem.addErrorTerm(rerr)
                reprojectionErrors.append(rerr)
                                                    
    sm.logDebug("calibrateIntrinsics: added {0} camera error terms".format(len(reprojectionErrors)))
    
    ############################################
    ## solve
    ############################################       
    options = aopt.Optimizer2Options()
    options.verbose = True if sm.getLoggingLevel()==sm.LoggingLevel.Debug else False
    options.nThreads = 4
    options.convergenceDeltaX = 1e-3
    options.convergenceDeltaJ = 1
    options.maxIterations = 200
    options.trustRegionPolicy = aopt.LevenbergMarquardtTrustRegionPolicy(10)

    optimizer = aopt.Optimizer2(options)
    optimizer.setProblem(problem)

    #verbose output
    if sm.getLoggingLevel()==sm.LoggingLevel.Debug:
        sm.logDebug("Before optimization:")
        e2 = np.array([ e.evaluateError() for e in reprojectionErrors ])
        sm.logDebug( " Reprojection error squarred (camL):  mean {0}, median {1}, std: {2}".format(np.mean(e2), np.median(e2), np.std(e2) ) )
    
    #run intrinsic calibration
    try: 
        retval = optimizer.optimize()
        if retval.linearSolverFailure:
            sm.logError("calibrateIntrinsics: Optimization failed!")
        success = not retval.linearSolverFailure

    except:
        sm.logError("calibrateIntrinsics: Optimization failed!")
        success = False
    
    #verbose output
    if sm.getLoggingLevel()==sm.LoggingLevel.Debug:
        d = cam_geometry.geometry.projection().distortion().getParameters().flatten()
        p = cam_geometry.geometry.projection().getParameters().flatten()
        sm.logDebug("calibrateIntrinsics: guess for intrinsics cam: {0}".format(p))
        sm.logDebug("calibrateIntrinsics: guess for distortion cam: {0}".format(d))
    
    return success

四、小结

可能看到这里有的朋友就会问了，这个函数主要是做内参和畸变的初始化，为什么在里面用了光束平差法直接把解给求出来了，我想应该是为了主流程那边后续要做离群点的筛选，因此就先求解，然后利用重头影误差把离群点筛掉，使结果更准确