相机畸变模型

 1. 径向畸变系数

// radial distortion coefficients
const float k1 = distort_params_[0];
const float k2 = distort_params_[1];
const float k3 = distort_params_[4];
// tangential distortion coefficients
const float p1 = distort_params_[2];
const float p2 = distort_params_[3];

2. 归一化

 const Eigen::Vector2f pt_normalized(point3d[0] / point3d[2],
                                     point3d[1] / point3d[2]);

3. 相关参数

const float x_n = pt_normalized[0];
const float y_n = pt_normalized[1];
const float x_mul_x = x_n * x_n;
const float y_mul_y = y_n * y_n;
const float x_mul_y = x_n * y_n;
const float r_squared = x_mul_x + y_mul_y;
const float r_to_the_4th = r_squared * r_squared;
const float r_to_the_6th = r_squared * r_to_the_4th;

3. 畸变求解

// radial distortion
pt2d_img = pt_normalized *( 1 + k1 * r_squared + k2 * r_to_the_4th + k3 *  
                            r_to_the_6th );

// tangential distortion
pt2d_img[0] += 2 * p1 * x_mul_y + p2 * (r_squared + 2 * x_mul_x);
pt2d_img[1] += p1 * (r_squared + 2 * y_mul_y) + 2 * p2 * x_mul_y;

4. 转换至图像坐标系

// radial distortion
pt2d_img = pt_normalized *( 1 + k1 * r_squared + k2 * r_to_the_4th + k3 * 
                            r_to_the_6th );

// tangential distortion
pt2d_img[0] += 2 * p1 * x_mul_y + p2 * (r_squared + 2 * x_mul_x);
pt2d_img[1] += p1 * (r_squared + 2 * y_mul_y) + 2 * p2 * x_mul_y;

 5. 求解畸变整体代码

Eigen::Vector2f BrownCameraDistortionModel::Project( const Eigen::Vector3f& point3d ) 
{
    if (std::isless(point3d[2], 0.f)) {
        AERROR << "The input point (" << point3d
               << ") should be in front of the camera";
    }
    // radial distortion coefficients
    const float k1 = distort_params_[0];
    const float k2 = distort_params_[1];
    const float k3 = distort_params_[4];
    // tangential distortion coefficients
    const float p1 = distort_params_[2];
    const float p2 = distort_params_[3];

    Eigen::Vector2f pt2d_img;
    // normalized
    const Eigen::Vector2f pt_normalized(point3d[0] / point3d[2],
                                        point3d[1] / point3d[2]);
    const float x_n = pt_normalized[0];
    const float y_n = pt_normalized[1];
    const float x_mul_x = x_n * x_n;
    const float y_mul_y = y_n * y_n;
    const float x_mul_y = x_n * y_n;
    const float r_squared = x_mul_x + y_mul_y;
    const float r_to_the_4th = r_squared * r_squared;
    const float r_to_the_6th = r_squared * r_to_the_4th;

    // radial distortion
    pt2d_img = pt_normalized *( 1 + k1 * r_squared + k2 * r_to_the_4th + k3 * r_to_the_6th );

    // tangential distortion
    pt2d_img[0] += 2 * p1 * x_mul_y + p2 * (r_squared + 2 * x_mul_x);
    pt2d_img[1] += p1 * (r_squared + 2 * y_mul_y) + 2 * p2 * x_mul_y;

    // transform to image coordinates
    const float fx = intrinsic_params_(0, 0);
    const float fy = intrinsic_params_(1, 1);
    const float cx = intrinsic_params_(0, 2);
    const float cy = intrinsic_params_(1, 2);
    pt2d_img[0] = fx * pt2d_img[0] + cx;
    pt2d_img[1] = fy * pt2d_img[1] + cy;

    return pt2d_img;
}

6. 相机内参、畸变参数

bool BrownCameraDistortionModel::set_params( size_t width, size_t height,
                                             const Eigen::VectorXf& params) 
{
    if (params.size() != 14) {
        return false;
    }

    width_ = width;
    height_ = height;
    intrinsic_params_(0, 0) = params(0);
    intrinsic_params_(0, 1) = params(1);
    intrinsic_params_(0, 2) = params(2);
    intrinsic_params_(1, 0) = params(3);
    intrinsic_params_(1, 1) = params(4);
    intrinsic_params_(1, 2) = params(5);
    intrinsic_params_(2, 0) = params(6);
    intrinsic_params_(2, 1) = params(7);
    intrinsic_params_(2, 2) = params(8);

    distort_params_[0] = params[9];
    distort_params_[1] = params[10];
    distort_params_[2] = params[11];
    distort_params_[3] = params[12];
    distort_params_[4] = params[13];

    return true;
}