Bundle Adjustment in the Large and Rodrigues' Rotation Formula

Question

We use a pinhole camera model; the parameters we estimate for each camera area rotation R, a translation t, a focal length f and two radial distortion parameters k1 and k2. The formula for projecting a 3D point X into a camera R,t,f,k1,k2 is:

P  =  R * X + t       (conversion from world to camera coordinates)
p  = -P / P.z         (perspective division)
p' =  f * r(p) * p    (conversion to pixel coordinates)

where P.z is the third (z) coordinate of P. In the last equation, r(p) is a function that computes a scaling factor to undo the radial distortion: r(p) = 1.0 + k1 * ||p||^2 + k2 * ||p||^4. This gives a projection in pixels, where the origin of the image is the center of the image, the positive x-axis points right, and the positive y-axis points up (in addition, in the camera coordinate system, the positive z-axis points backwards, so the camera is looking down the negative z-axis, as in OpenGL).

Where, there camera and point indices start from 0. Each camera is a set of 9 parameters - R,t,f,k1 and k2. The rotation R is specified as a Rodrigues' vector.

Is the R in the pinhole model the same R as they talk about in the 9 parameters in the buttom? I cant seem to make sense of R*X+t should give a new 3x1 vector P if R is just a vector?

What part am I mising?

I would like to understand their way of using the pinhole model.

Answer 1

No its not same.

From your equations;

P  =  R * X + t       (conversion from world to camera coordinates)
p  = -P / P.z         (perspective division)
p' =  f * r(p) * p    (conversion to pixel coordinates)

R * X is not possible as R and X are both 3x1 vectors. So R has to be 3x3 matrix.

Lets assume that R is a 3x3 rotation matrix in pinhole model whereas R_ is 3x1 rotation vector obtained by taking Rodrigues of R. K is camera Intrinsic matrix containing f,k1,k2. Following will be the proper way to solve this problem.

H = R_ x K;
P  =  H * X + t       (conversion from world to camera coordinates)
p  = -P / P.z         (perspective division)
p' =  f * r(p) * p    (conversion to pixel coordinates)

The code below is taken from OpenCV's bundle adjuster. It shows how to get a new vector P'.

    double R1[9];
    Mat R1_(3, 3, CV_64F, R1);
    Mat rvec(3, 1, CV_64F);
    rvec.at<double>(0, 0) = cam_params_.at<double>(i * 4 + 1, 0);
    rvec.at<double>(1, 0) = cam_params_.at<double>(i * 4 + 2, 0);
    rvec.at<double>(2, 0) = cam_params_.at<double>(i * 4 + 3, 0);
    Rodrigues(rvec, R1_);

    double R2[9];
    Mat R2_(3, 3, CV_64F, R2);
    rvec.at<double>(0, 0) = cam_params_.at<double>(j * 4 + 1, 0);
    rvec.at<double>(1, 0) = cam_params_.at<double>(j * 4 + 2, 0);
    rvec.at<double>(2, 0) = cam_params_.at<double>(j * 4 + 3, 0);
    Rodrigues(rvec, R2_);

    const ImageFeatures& features1 = features_[i];
    const ImageFeatures& features2 = features_[j];
    const MatchesInfo& matches_info = pairwise_matches_[i * num_images_ + j];

    Mat_<double> K1 = Mat::eye(3, 3, CV_64F);
    K1(0, 0) = f1; K1(0, 2) = features1.img_size.width * 0.5;
    K1(1, 1) = f1; K1(1, 2) = features1.img_size.height * 0.5;

    //LOGLN("\nf1: " << f1 << "\tf2: "<<f2);

    Mat_<double> K2 = Mat::eye(3, 3, CV_64F);
    K2(0, 0) = f2; K2(0, 2) = features2.img_size.width * 0.5;
    K2(1, 1) = f2; K2(1, 2) = features2.img_size.height * 0.5;

    Mat_<double> H1 = R1_ * K1.inv();
    Mat_<double> H2 = R2_ * K2.inv();

    for (size_t k = 0; k < matches_info.matches.size(); ++k)
    {
        if (!matches_info.inliers_mask[k])
            continue;

        const DMatch& m = matches_info.matches[k];

        Point2f p1 = features1.keypoints[m.queryIdx].pt;
        double x1 = H1(0, 0)*p1.x + H1(0, 1)*p1.y + H1(0, 2);
        double y1 = H1(1, 0)*p1.x + H1(1, 1)*p1.y + H1(1, 2);
        double z1 = H1(2, 0)*p1.x + H1(2, 1)*p1.y + H1(2, 2);
        double len = std::sqrt(x1*x1 + y1*y1 + z1*z1);
        //x1 /= z1; y1 /= z1; 
        x1 /= len; y1 /= len; z1 /= len;

        Point2f p2 = features2.keypoints[m.trainIdx].pt;
        double x2 = H2(0, 0)*p2.x + H2(0, 1)*p2.y + H2(0, 2);
        double y2 = H2(1, 0)*p2.x + H2(1, 1)*p2.y + H2(1, 2);
        double z2 = H2(2, 0)*p2.x + H2(2, 1)*p2.y + H2(2, 2);
        len = std::sqrt(x2*x2 + y2*y2 + z2*z2);
        //x2 /= z2; y2 /= z2; 
        x2 /= len; y2 /= len; z2 /= len;

        double mult = (f1 + f2)/2; //std::sqrt(f1 * f2);
        err.at<double>(3 * match_idx, 0) = mult * (x1 - x2);
        err.at<double>(3 * match_idx + 1, 0) = mult * (y1 - y2);
        err.at<double>(3 * match_idx + 2, 0) = mult * (z1 - z2);

        match_idx++;
    }