Full Calibration Edge Issue

[goktugyildirim4d]

Hello, my aim is to optimize extrinsic, and intrinsic camera parameters and radial distortion coefficients of the camera. I could not find any edge in the g2o as default. So, I'm trying to implement the edge from scratch. I used a linear update step for all vertices except for the camera pose which is an exponential map. Even if it's converged, it takes lots of time. Do you have any suggestions for parameterization or optimization algorithms to improve the time consumption? Also, If I combined intrinsic vertices and distortion vertex, it makes algorithm faster? Maybe I can change the BaseFixedSizedEdge. I guess this issue belongs in a g2o forum. Sorry about that.

My optimizer:

    g2o::SparseOptimizer optimizer;

    // Initialize the solver and optimizer
    typedef g2o::BlockSolverX BlockSolverType;
    typedef g2o::LinearSolverCholmod<BlockSolverType::PoseMatrixType> LinearSolverType;
    std::unique_ptr<BlockSolverType::LinearSolverType> linearSolver (new LinearSolverType());
    std::unique_ptr<BlockSolverType> solver_ptr (new BlockSolverType(std::move(linearSolver)));
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(std::move(solver_ptr));
    solver->setUserLambdaInit(0.01);
    solver->setMaxTrialsAfterFailure(100);

    optimizer.setAlgorithm(solver);

Here is my Edge implementation:

class EdgeProjectFullCalibration : public g2o::BaseFixedSizedEdge<2, Eigen::Vector2d,
                                                                  VertexPrincipalPoint,   // Vertex0 2D
                                                                  VertexFocal2D,          // Vertex1 2D
                                                                  VertexRadialDistortion, // Vertex2 3D
                                                                  g2o::VertexSE3Expmap>   // Vertex3 6D
                                                                                          // Total   13D
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
  EdgeProjectFullCalibration(const Eigen::Vector3d& Xw):mXw(Xw) {}

  virtual void computeError()
  {
    VertexPrincipalPoint* vPrincipalPoint = static_cast<VertexPrincipalPoint*>(_vertices[0]);
    double cx = vPrincipalPoint->estimate()(0); double cy = vPrincipalPoint->estimate()(1);

    VertexFocal2D* vFocal2D = static_cast<VertexFocal2D*>(_vertices[1]);
    double fx = vFocal2D->estimate()(0); double fy = vFocal2D->estimate()(1);

    VertexRadialDistortion* vRadialDistortion = static_cast<VertexRadialDistortion*>(_vertices[2]);
    double k1 = vRadialDistortion->estimate()(0); double k2 = vRadialDistortion->estimate()(1); double k3 = vRadialDistortion->estimate()(2);

    g2o::VertexSE3Expmap* vPose = static_cast<g2o::VertexSE3Expmap*>(_vertices[3]);
    Eigen::Isometry3d Tcw = vPose->estimate();

    Eigen::Vector3d Xc = Tcw * mXw;
    double X = Xc(0); double Y = Xc(1); double Z = Xc(2);
    double r2 = (X*X + Y*Y) / (Z*Z); double r4 = r2 * r2; double r6 = r4 * r2;
    double ud = (X/Z) * fx + (X/Z) * fx * k1 * r2 + (X/Z) * fx * k2 * r4 + (X/Z) * fx * k3 * r6 + cx;
    double vd = (Y/Z) * fy + (Y/Z) * fy * k1 * r2 + (Y/Z) * fy * k2 * r4 + (Y/Z) * fy * k3 * r6 + cy;
    Eigen::Vector2d projection = { ud, vd};

    _error = _measurement - projection;
  }

  virtual void linearizeOplus() override {
    VertexPrincipalPoint* vPrincipalPoint = static_cast<VertexPrincipalPoint*>(_vertices[0]);
    double cx = vPrincipalPoint->estimate()(0); double cy = vPrincipalPoint->estimate()(1);

    VertexFocal2D* vFocal2D = static_cast<VertexFocal2D*>(_vertices[1]);
    double fx = vFocal2D->estimate()(0); double fy = vFocal2D->estimate()(1);

    VertexRadialDistortion* vRadialDistortion = static_cast<VertexRadialDistortion*>(_vertices[2]);
    double k1 = vRadialDistortion->estimate()(0); double k2 = vRadialDistortion->estimate()(1); double k3 = vRadialDistortion->estimate()(2);

    g2o::VertexSE3Expmap* vPose = static_cast<g2o::VertexSE3Expmap*>(_vertices[3]);
    Eigen::Isometry3d Tcw = vPose->estimate();

    // Variables
    Eigen::Vector3d Xc = Tcw * mXw;
    double X = Xc(0); double Y = Xc(1); double Z = Xc(2); double invz = 1.0 / Z; double invz_2 = invz * invz;
    double r2 = (X*X + Y*Y) / (Z*Z); double r4 = r2 * r2; double r6 = r4 * r2;
    double distTerm = 1 + k1 * r2 + k2 * r4 + k3 * r6;

    // 0 - Derivative w.r.t principal point ############################################################################
    Eigen::MatrixXd J_principal = Eigen::Matrix<double, 2, 2>::Zero();
    double dU_dcx = 1; double dU_dcy = 0;
    double dV_dcx = 0; double dV_dcy = 1;
    J_principal(0, 0) = -dU_dcx; J_principal(0, 1) = -dU_dcy;
    J_principal(1, 0) = -dV_dcx; J_principal(1, 1) = -dV_dcy;
    std::get<0>(_jacobianOplus) = J_principal;
    // #################################################################################################################

    // 1 - Derivative w.r.t Focal2D
    Eigen::MatrixXd J_focal = Eigen::Matrix<double, 2, 2>::Zero();
    double dU_dfx = (X/Z) * distTerm; double dU_dfy = 0;
    double dV_dfx = 0;                double dV_dfy = (Y/Z) * distTerm;
    J_focal(0, 0) = -dU_dfx; J_focal(0, 1) = -dU_dfy;
    J_focal(1, 0) = -dV_dfx; J_focal(1, 1) = -dV_dfy;
    std::get<1>(_jacobianOplus) = J_focal;
    // #################################################################################################################

    // 2 - Derivative w.r.t radial distortion ##########################################################################
    Eigen::MatrixXd J_radi = Eigen::Matrix<double, 2, 3>::Zero();
    double dU_dk1 = (X/Z) * fx * r2; double dU_dk2 = (X/Z) * fx * r4; double dU_dk3 = (X/Z) * fx * r6;
    double dV_dk1 = (Y/Z) * fy * r2; double dV_dk2 = (Y/Z) * fy * r4; double dV_dk3 = (Y/Z) * fy * r6;
    J_radi(0, 0) = -dU_dk1; J_radi(0, 1) = -dU_dk2; J_radi(0, 2) = -dU_dk3;
    J_radi(1, 0) = -dV_dk1; J_radi(1, 1) = -dV_dk2; J_radi(1, 2) = -dV_dk3;
    std::get<2>(_jacobianOplus) = J_radi;
    // #################################################################################################################

    // 3- Derivative w.r.t pose
    Eigen::Matrix<double, 2, 6> J_pose = Eigen::Matrix<double, 2, 6>::Zero();
    double x = X; double y = Y; double z = Z;
    J_pose(0, 0) = x * y * invz_2 * fx * distTerm;
    J_pose(0, 1) = -(1 + (x * x * invz_2)) * fx * distTerm;
    J_pose(0, 2) = y * invz * fx * distTerm;
    J_pose(0, 3) = -invz * fx * distTerm;
    J_pose(0, 4) = 0;
    J_pose(0, 5) = x * invz_2 * fx * distTerm;
    J_pose(1, 0) = (1 + y * y * invz_2) * fy * distTerm;
    J_pose(1, 1) = -x * y * invz_2 * fy * distTerm;
    J_pose(1, 2) = -x * invz * fy * distTerm;
    J_pose(1, 3) = 0;
    J_pose(1, 4) = -invz * fy * distTerm;
    J_pose(1, 5) = y * invz_2 * fy * distTerm;
    std::get<3>(_jacobianOplus) = J_pose;
    // #################################################################################################################

  }

  virtual bool read(std::istream &in) override { return true; }
  virtual bool write(std::ostream &out) const override { return true; }

private:
  Eigen::Vector3d mXw;
};