Ceres

example official

Eigen::Matrix2d ProjectionCameraMapFactor::sqrt_info;

ProjectionCameraMapFactor::ProjectionCameraMapFactor(const Eigen::Vector3d &_map_point, 
                           const Eigen::Vector2d &_camera_point, 
                           double _fx, double _fy, double _cx, double _cy): 
                     fx(_fx), fy(_fy), cx(_cx), cy(_cy)
{
    map_point.x() = _map_point.x();
    map_point.y() = _map_point.y();
    map_point.z() = _map_point.z();

    camera_point.x() = _camera_point.x();
    camera_point.y() = _camera_point.y();

}

// calculate jacobian and residuals here
//    -> residual should be Vector2d
//    -> jacobian should be 1 * (2 * 7) -> ( one node * ( 2 residual elements * node's dimension 7 ))
bool ProjectionCameraMapFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
{
    Eigen::Vector3d Pc(parameters[0][0], parameters[0][1], parameters[0][2]);
    Eigen::Quaterniond Qc(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]);
    Eigen::Matrix3d rwc = Qc.toRotationMatrix();

    Eigen::Vector3d Xc = rwc.transpose() * map_point - rwc.transpose() * Pc;
    double x = Xc(0);
    double y = Xc(1);
    double invz = 1.0 / Xc(2);
    double invz_2 = invz * invz;

    double res1 = x * invz * fx + cx - camera_point(0);
    double res2 = y * invz * fy + cy - camera_point(1);
  
    residuals[0] = sqrt_info(0,0) * res1 + sqrt_info(0,1) * res2;
    residuals[1] = sqrt_info(1,0) * res1 + sqrt_info(1,1) * res2;

    // For this simple it is overkill to check if jacobians[0]
    // is NULL, but in general when writing more complex
    // CostFunctions, it is possible that Ceres may only demand the
    // derivatives w.r.t. a subset of the parameter blocks.
    if (jacobians != NULL && jacobians[0] != NULL) {
        Eigen::Matrix<double, 2, 3> reduce(2, 3);
        reduce << invz * fx , 0         , - x * fx * invz_2,
                  0         , invz * fy , - y * fx * invz_2;

        reduce = sqrt_info * reduce;
 
        Eigen::Map<Eigen::Matrix<double, 2, 7, Eigen::RowMajor>> jacobian_pose_j(jacobians[0]);

        Eigen::Matrix<double, 3, 6> jaco_j;
        jaco_j.leftCols<3>() = - rwc.transpose();
        jaco_j.rightCols<3>() = Utility::skewSymmetric(Xc);

        jacobian_pose_j.leftCols<6>() = reduce * jaco_j;
        jacobian_pose_j.rightCols<1>().setZero();
    }

    return true;
}

void ProjectionCameraMapFactor::check(double **parameters)
{
    double *res = new double[2];
    double **jaco = new double *[1];
    jaco[0] = new double[2 * 7];
    Evaluate(parameters, res, jaco);
    puts("\n check begins");
    puts("my:");
    Eigen::Matrix<double, 2, 7> mine_jacobian;
    for(int i = 0; i < 14; i ++){
            int x = i/7;
            int y = i%7;
        mine_jacobian(x,y) = jaco[0][i];
    }
    std::cout << mine_jacobian << std::endl;

    Eigen::Vector2d residual;
    residual(0) = res[0];
    residual(1) = res[1];

    const double eps = 1e-6;
    Eigen::Matrix<double, 2, 6> num_jacobian;
    for (int k = 0; k < 6; k++)
    {
        Eigen::Vector3d Pc(parameters[0][0], parameters[0][1], parameters[0][2]);
        Eigen::Quaterniond Qc(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]);

        int a = k / 3, b = k % 3;
        Eigen::Vector3d delta = Eigen::Vector3d(b == 0, b == 1, b == 2) * eps;

        if (a == 0)
            Pc += delta;
        else if (a == 1)
            Qc = Qc * Utility::deltaQ(delta);

        Eigen::Matrix3d rwc = Qc.toRotationMatrix();
        Eigen::Vector3d Xc = rwc.transpose() * map_point - rwc.transpose() * Pc;
        double x = Xc(0);
        double y = Xc(1);
        double invz = 1.0 / Xc(2);

        Eigen::Vector2d tmp_residual;
        double res1 = x * invz * fx + cx - camera_point(0);
        double res2 = y * invz * fy + cy - camera_point(1);

        tmp_residual(0) = sqrt_info(0,0) * res1 + sqrt_info(0,1) * res2;
        tmp_residual(1) = sqrt_info(1,0) * res1 + sqrt_info(1,1) * res2;

        num_jacobian.col(k) = (tmp_residual - residual) / eps;
    }
 
    std::cout << num_jacobian << std::endl;

}

double fx = 493.7;
double fy = 493.7;
double cx = 320;
double cy = 240;

void initPoints(vector<Eigen::Vector3d> &mapPoints, vector<Eigen::Vector2d> &cameraPoints, double* groundTruthPose)
{
    ProjectionCameraMapFactor::sqrt_info = 400 / 15 * Eigen::Matrix2d::Identity();

    // random generators
    double simulateRadius = 10;
    double w_sigma= 0.001;
    double camera_pixel_n = 2;
    std::default_random_engine generator;
    std::default_random_engine generatorP;
    std::normal_distribution<double> noise(0.,w_sigma);
    std::normal_distribution<double> pointP(0.,simulateRadius);
    std::normal_distribution<double> noisePixel(0.,camera_pixel_n);

    // ground truth pose
    Eigen::Vector3d Pc(groundTruthPose[0], groundTruthPose[1], groundTruthPose[2]);
    Eigen::Quaterniond Qc(groundTruthPose[6], groundTruthPose[3], groundTruthPose[4], groundTruthPose[5]);
    Eigen::Matrix3d rwc = Qc.toRotationMatrix();

    // make simulation points
    int N = 100;
    mapPoints.clear();
    mapPoints.reserve(N);

    cameraPoints.clear();
    cameraPoints.reserve(N);

    for(int i = 0; i < N ;  i++){
        Eigen::Vector3d map_point(pointP(generator), pointP(generator), pointP(generator));
        //Eigen::Vector3d noiseVec(noise(generator), noise(generator), noise(generator));

        Eigen::Vector3d Xc = rwc.transpose() * (map_point)  - rwc.transpose() * Pc;

        double x = Xc(0);
        double y = Xc(1);
        double invz = 1.0 / Xc(2);

        double u = x * invz * fx + cx + noisePixel(generator);
        double v = y * invz * fy + cy + noisePixel(generatorP);

        Eigen::Vector2d camera_point(u, v);

        mapPoints.push_back(map_point);
        cameraPoints.push_back(camera_point);
    }

    cout << " Initialized " << N << " Points for simulation. \n";

}


int test_ceres()
{
    cout << " Test ceres optimziation jacobians etc. \n";

    // init parameters
    double camerapose[7] = {0.1,0.1,0.1,0,0,0,1};
    vector<Eigen::Vector3d> mapPoints;
    vector<Eigen::Vector2d> cameraPoints;

    initPoints(mapPoints, cameraPoints, camerapose);


    ceres::Problem *problem = new ceres::Problem;

    ceres::LocalParameterization *local_parameterization = new GPoseLocalParameterization();
    problem->AddParameterBlock(camerapose, 7, local_parameterization);

    for(size_t i=0, iend = cameraPoints.size() ; i < iend ; i++){
        ProjectionCameraMapFactor *f_cam = new ProjectionCameraMapFactor(mapPoints[i],cameraPoints[i], fx, fy, cx, cy);
        problem->AddResidualBlock(f_cam, NULL, camerapose);
    }
 
    ceres::Solver::Options options;
    options.linear_solver_type=ceres::DENSE_SCHUR;
    options.trust_region_strategy_type = ceres::DOGLEG;
    options.minimizer_progress_to_stdout = true;

    ceres::Solver::Summary summary;
    ceres::Solve(options, problem,&summary);
    std::cout << summary.BriefReport() << std::endl;

    cout << " Result Camera pose : ";
    for(int i = 0; i< 7 ; i++){
        std::cout << camerapose[i] << " ";
    }
    std::cout << std::endl;

    return 0;
}