三维重建-单目相机标定

最新推荐文章于 2024-05-02 23:14:39 发布

点云兔子

最新推荐文章于 2024-05-02 23:14:39 发布

阅读量849

点赞数

分类专栏：三维重建文章标签：三维重建相机标定单目相机

本文链接：https://blog.csdn.net/qq_40301351/article/details/130604234

版权

三维重建专栏收录该内容

2 篇文章 2 订阅

订阅专栏

文章目录

前言

一、代码讲解

1、SingleCamera

2、SingleCamera::compose()

3、SingleCamera::svdP()

4、SingleCamera::workIntrinsicAndExtrinsic()

5、SingleCamera::selfcheck

二、完整代码

三、 Cmake配置

总结

前言

理论知识可以看北邮鲁鹏老师的课程，哔哩哔哩上有。

一、代码讲解

1、SingleCamera

class SingleCamera {
public:
    // 构造函数，初始化成员变量
    SingleCamera(Eigen::MatrixXf world_coor, Eigen::MatrixXf pixel_coor, int n)
            : world_coor(world_coor), pixel_coor(pixel_coor), point_num(n),
              P(Eigen::MatrixXf::Zero(2 * n, 12)), M(Eigen::MatrixXf::Zero(3, 4)),
              A(Eigen::MatrixXf::Zero(3, 3)), b(Eigen::MatrixXf::Zero(3, 1)),
              K(Eigen::MatrixXf::Zero(3, 3)), R(Eigen::MatrixXf::Zero(3, 3)),
              t(Eigen::MatrixXf::Zero(3, 1)) {}

    // 用像素坐标和世界坐标计算相机矩阵 P
    void composeP();

    // 对相机矩阵 P 进行奇异值分解，计算相机内部矩阵 A 和外部矩阵 M
    void svdP();

    // 计算相机的内部和外部参数
    void workIntrinsicAndExtrinsic();

    // 对标定结果进行自检
    void selfcheck(const Eigen::MatrixXf &w_check, const Eigen::MatrixXf &c_check);

private:
    // 成员变量，存储世界坐标和像素坐标以及相关矩阵
    Eigen::MatrixXf world_coor;
    Eigen::MatrixXf pixel_coor;
    int point_num;
    Eigen::MatrixXf P;
    Eigen::MatrixXf M;
    Eigen::MatrixXf A;
    Eigen::MatrixXf b;
    Eigen::MatrixXf K;
    Eigen::MatrixXf R;
    Eigen::MatrixXf t;
};

类SingleCamera实现了与相机标定和 3D 重建相关的功能。该类具有几个成员变量和函数：

成员变量：

world_coor：一个 Eigen::MatrixXf 矩阵，表示世界坐标系中点的 3D 坐标。

pixel_coor：一个 Eigen::MatrixXf 矩阵，表示相应点在相机图像平面上的 2D 坐标。

point_num：一个整数，表示点的数量。

P：一个大小为 2n x 12 的 Eigen::MatrixXf 矩阵，其中 n 是点的数量。该矩阵用于表示相机矩阵。

M：一个大小为 3 x 4 的 Eigen::MatrixXf 矩阵，表示相机矩阵形式为 [R|t]。

A：一个大小为 3 x 3 的 Eigen::MatrixXf 矩阵，表示相机的内部矩阵。

b：一个大小为 3 x 1 的 Eigen::MatrixXf 矩阵，表示相机的平移向量。

K：一个大小为 3 x 3 的 Eigen::MatrixXf 矩阵，表示相机的校准矩阵。

2、SingleCamera::compose()


void SingleCamera::compose() {
 根据输入的二维点和三维点，构造P矩阵
Eigen::RowVectorXf row(12), p(4);
float u, v;
int p_idx = 0;
for (int i = 0; i < point_num; i++) {
// 一次生成两个方程系数，用于构造P矩阵
p = world_coor.row(i);
u = pixel_coor(i, 0);
v = pixel_coor(i, 1);
row << p, // 第一行：三维点
Eigen::RowVectorXf::Zero(1, 4), // 第一行：第二个四维0向量
-u * p; // 第一行：负的像素坐标x值乘以三维点坐标
P.row(p_idx++) = row; // 将该行加入P矩阵中，并将行索引值加1
row << Eigen::RowVectorXf::Zero(1, 4), // 第二行：第一个四维0向量
p, // 第二行：三维点
-v * p; // 第二行：负的像素坐标y值乘以三维点坐标
P.row(p_idx++) = row; // 将该行加入P矩阵中，并将行索引值加1
}

std::cout << "P:\n" << P << std::endl;

}

        其中的 composeP 函数用于生成 P 矩阵。在计算机视觉中，P 矩阵是一个 3x4 的矩阵，它包含了相机的内参、外参以及相机的位置和姿态等信息，可以用于将三维世界坐标系中的点映射到二维像素坐标系中。

        具体地，该函数根据输入的二维点 pixel_coor 和三维点 world_coor，构造 P 矩阵。在生成 P 矩阵的过程中，对于每一个三维点，通过将其与对应的二维点组合构造两个方程式，从而构成一个 2x12 的子矩阵，最后将所有子矩阵组合成一个 2n x 12 的 P 矩阵。其中，n 表示输入的点的数量。

        具体实现过程是，在 for 循环中，首先从 world_coor 中取出当前点的三维坐标，然后将其与对应的二维点 pixel_coor(i, 0) 和 pixel_coor(i, 1) 组合，构造出一个 2x1 的向量。接着，在构造 2x12 的子矩阵时，将该向量与一些零元素组合成一个 1x12 的行向量，然后再将该行向量作为子矩阵的第一行，构造出一个 2x12 的子矩阵。具体地，子矩阵的第一行为 [X, Y, Z, 1, 0, 0, 0, 0, -uX, -uY, -uZ, -u]，其中 X、Y、Z 分别表示当前点的三维坐标，u 表示当前点的横坐标 pixel_coor(i, 0)。接下来，将该子矩阵加入到 P 矩阵中。然后，再以类似的方式构造出一个第二行为 [0, 0, 0, 0, X, Y, Z, 1, -vX, -vY, -vZ, -v] 的子矩阵，并将其也加入到 P 矩阵中。最终生成的 P 矩阵的大小为 2n x 12。

        需要注意的是，该函数并没有对 P 矩阵进行求解，它只是将输入的点转化为 P 矩阵的形式。P 矩阵的求解需要使用其他方法，例如使用 SVD 分解或者直接进行线性求解等。

3、SingleCamera::svdP()

void SingleCamera::svdP() {
    // homework2: 根据P矩阵求解M矩阵和A、b矩阵

    // 使用JacobiSVD求解P矩阵的奇异值分解
    Eigen::JacobiSVD<Eigen::MatrixXf> svd;
    svd.compute(P, Eigen::ComputeThinU | Eigen::ComputeThinV);

    // 取出V矩阵的最后一列并重构成4x3矩阵，得到M矩阵
    M = svd.matrixV().col(svd.matrixV().cols() - 1);
    M.resize(4, 3);
    M.transposeInPlace();

    // 将M矩阵分解为A和b矩阵
    A = M.leftCols(3);   // 3x3的A矩阵
    b = M.rightCols(1);  // 3x1的b矩阵
}

通过输入的二维像素点和对应的三维世界坐标点，求解相机的姿态矩阵。通过对之前构造的P矩阵进行奇异值分解（SVD），得到M矩阵，并将其拆分成A矩阵和b向量的形式。其中M矩阵的最后一列是一个齐次坐标，所以取矩阵V中的最后一列作为M矩阵，并将其resize为4x3的形式。最后通过对M矩阵的拆分得到A矩阵和b向量，A矩阵是一个3x3的矩阵，代表了相机内参，b向量是一个3x1的向量，代表了相机的平移向量。

4、SingleCamera::workIntrinsicAndExtrinsic()

void SingleCamera::workIntrinsicAndExtrinsic() {
    // homework3: 求解相机的内参和外参
    Eigen::RowVector3f a1(A.row(0)), a2(A.row(1)), a3(A.row(2));  // 将A矩阵的前三行分别作为a1, a2, a3

    auto roh = -1 / a3.norm();  // 计算rho，为a3的模长的倒数的相反数

    auto cx = roh * roh * (a1.dot(a3));  // 计算cx
    auto cy = roh * roh * (a2.dot(a3));  // 计算cy

    auto a1xa3 = a1.cross(a3);  // 计算向量a1和a3的叉积
    auto a2xa3 = a2.cross(a3);  // 计算向量a2和a3的叉积
    auto cos_theta = -1 * a1xa3.dot(a2xa3) / (a1xa3.norm() * a2xa3.norm());  // 计算cos(theta)，即向量a1xa3和a2xa3的点积除以模长的积的相反数
    auto sin_theta = sqrt(1 - cos_theta * cos_theta);  // 计算sin(theta)

    auto alpha = roh * roh * a1xa3.norm() * sin_theta;  // 计算alpha
    auto beta = roh * roh * a2xa3.norm() * sin_theta;   // 计算beta

    auto r1 = 1 * a2xa3 / a2xa3.norm();  // 计算向量r1
    auto r2 = -r1 * cos_theta / sin_theta - (roh * roh) / alpha * a1xa3;  // 计算向量r2
    auto r3 = r1.cross(r2);  // 计算向量r3

    // calculate the sign of roh
    auto rohs = r3.array() / a3.array();  // 将r3的每个元素都除以a3对应的元素
    if (rohs[0] > 0) {  // 如果r3的第一个元素大于0
        roh *= -1;  // 取rho的相反数
    }

    K(0, 0) = alpha;  // 将alpha赋值给K矩阵的(0,0)位置
    K(0, 1) = -alpha * cos_theta / sin_theta;  // 将alpha, cos(theta), sin(theta)计算出的值赋给K矩阵的(0,1)位置
    K(0, 2) = cx;  // 将cx赋值给K矩阵的(0,2)位置
    K(1, 1) = beta / sin_theta;  // 将beta, sin(theta)计算出的值赋给K矩阵的(1,1)位置
    K(1, 2) = cy;  // 将cy赋值给K矩阵的(1,2)位置
    K(2, 2) = 1;
    R.row(0) = r1;
    R.row(1) = r2;
    R.row(2) = r3;
    t = roh * K.inverse() * b;

    std::cout << "K is " << std::endl << K << std::endl;
    std::cout << "R is " << std::endl << R << std::endl;
    std::cout << "t is " << std::endl << t.transpose() << std::endl;
}

在 workIntrinsicAndExtrinsic() 函数中，首先提取矩阵 A 的三行，并根据 A 矩阵的特征推导相机内参，包括焦距（alpha 和 beta）、图像中心的位置（cx 和 cy）和像素间距的尺度因子（roh）。

接下来，根据 A 矩阵推导出的旋转矩阵的第一列 r1，使用旋转矩阵的正交性质推导出 r2 和 r3。

最后，根据推导出的相机内参和外参，计算相机的平移向量 t。

代码的最后，输出相机内参矩阵 K、旋转矩阵 R 和平移向量 t。

5、SingleCamera::selfcheck

// 传入测试点w_check和对应的像素坐标c_check，计算误差
void SingleCamera::selfcheck(const Eigen::MatrixXf &w_check, const Eigen::MatrixXf &c_check) {
float average_err = DBL_MAX;// 将R、t拼接成M矩阵，再用相机内参矩阵K相乘得到投影矩阵P，然后将测试点w_check通过投影矩阵P投影到像素坐标系下
Eigen::MatrixXf m(3, 4);
m << R, t;
auto trans_xyz = w_check * (K * m).transpose();

// 逐个计算每个像素坐标的误差
for (int i = 0; i < c_check.rows(); ++i) {
    if (i == 0) average_err = 0;

    // 计算投影后的点在像素坐标系下的坐标值
    auto trans_z = trans_xyz(i, 2);
    auto trans_x = trans_xyz(i, 0) / trans_z;
    auto trans_y = trans_xyz(i, 1) / trans_z;

    // 计算投影后的点和对应测试点的误差，并将误差累加
    average_err += abs(trans_x - c_check(i, 0));
    average_err += abs(trans_y - c_check(i, 1));
}

// 计算平均误差
average_err /= c_check.size();

// 输出结果
std::cout << "The average error is " << average_err << "," << std::endl;
if (average_err > 0.1) {
    std::cout << "which is more than 0.1" << std::endl;
} else {
    std::cout << "which is smaller than 0.1, the M is acceptable" << std::endl;
}

用于对相机参数进行自检。输入是两个矩阵w_check和c_check，分别表示世界坐标系下的点和相机图像平面上对应的点。函数的作用是利用之前通过作业3求解得到的相机参数，计算这些参数对给定的测试点的投影误差，判断相机参数的有效性。具体实现过程如下：

首先，通过将旋转和平移向量组合成一个3x4的矩阵，构造一个包含所有相机参数的矩阵m。然后，将这个矩阵与内部矩阵K相乘，得到一个3x3的投影矩阵P，再将其与世界坐标系下的点w_check相乘，得到一个3xN的齐次坐标矩阵trans_xyz，其中N表示测试点的数量。

接下来，对于每一个测试点，通过齐次坐标矩阵中对应的第i行，计算出点的三维坐标。具体来说，先将trans_xyz中第i行的第三个元素作为点的Z坐标，然后将其它两个元素分别除以这个Z坐标，得到点在相机坐标系下的X和Y坐标。这些坐标可以通过在内部矩阵K中乘以它们来得到对应的像素坐标。

最后，通过计算每个测试点的投影误差，得到所有测试点的平均误差。如果这个平均误差大于0.1，则认为相机参数不够精确，否则认为参数有效。函数将输出平均误差以及相机参数是否有效的信息。

二、完整代码

#include <iostream>
#include <vector>
#include <cmath>
#include <cfloat>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/SVD>

class SingleCamera {
public:
    SingleCamera(Eigen::MatrixXf world_coor, Eigen::MatrixXf pixel_coor, int n)
            : world_coor(world_coor), pixel_coor(pixel_coor), point_num(n),
              P(Eigen::MatrixXf::Zero(2 * n, 12)), M(Eigen::MatrixXf::Zero(3, 4)),
              A(Eigen::MatrixXf::Zero(3, 3)), b(Eigen::MatrixXf::Zero(3, 1)),
              K(Eigen::MatrixXf::Zero(3, 3)), R(Eigen::MatrixXf::Zero(3, 3)),
              t(Eigen::MatrixXf::Zero(3, 1)) {}

    void composeP();

    void svdP();

    void workIntrinsicAndExtrinsic();

    void selfcheck(const Eigen::MatrixXf &w_check, const Eigen::MatrixXf &c_check);

private:
    Eigen::MatrixXf world_coor;
    Eigen::MatrixXf pixel_coor;
    int point_num;

    // 变量都是与课程PPT相对应的
    Eigen::MatrixXf P;
    Eigen::MatrixXf M;
    Eigen::MatrixXf A;
    Eigen::MatrixXf b;
    Eigen::MatrixXf K;
    Eigen::MatrixXf R;
    Eigen::MatrixXf t;
};

void SingleCamera::composeP() {
    // homework1: 根据输入的二维点和三维点，构造P矩阵
    Eigen::RowVectorXf row(12), p(4);
    float u, v;
    int p_idx = 0;
    for (int i = 0; i < point_num; i++) {
        // 一次生成两个方程系数
        p = world_coor.row(i);
        u = pixel_coor(i, 0);
        v = pixel_coor(i, 1);
        row << p,
                Eigen::RowVectorXf::Zero(1, 4),
                -u * p;
        P.row(p_idx++) = row;
        row << Eigen::RowVectorXf::Zero(1, 4),
                p,
                -v * p;
        P.row(p_idx++) = row;
    }

//    std::cout << "P:\n" << P << std::endl;

}

void SingleCamera::svdP() {
    // homework2: 根据P矩阵求解M矩阵和A、b矩阵
    Eigen::JacobiSVD<Eigen::MatrixXf> svd;
    svd.compute(P, Eigen::ComputeThinU | Eigen::ComputeThinV);
    M = svd.matrixV().col(svd.matrixV().cols() - 1);
    M.resize(4, 3);
    M.transposeInPlace();
    A = M.leftCols(3);   //[3, 3]
    b = M.rightCols(1);  //[3, 1]
}

void SingleCamera::workIntrinsicAndExtrinsic() {
    // homework3: 求解相机的内参和外参
    Eigen::RowVector3f a1(A.row(0)), a2(A.row(1)), a3(A.row(2));

    auto roh = -1 / a3.norm();

    auto cx = roh * roh * (a1.dot(a3));
    auto cy = roh * roh * (a2.dot(a3));

    auto a1xa3 = a1.cross(a3);
    auto a2xa3 = a2.cross(a3);
    auto cos_theta = -1 * a1xa3.dot(a2xa3) / (a1xa3.norm() * a2xa3.norm());
    auto sin_theta = sqrt(1 - cos_theta * cos_theta);

    auto alpha = roh * roh * a1xa3.norm() * sin_theta;
    auto beta = roh * roh * a2xa3.norm() * sin_theta;

//    auto r1 = a2xa3 / a2xa3.norm();
//    auto r3 = a3 / a3.norm();
//    auto r2 = r3.cross(r1);
    auto r1 = 1 * a2xa3 / a2xa3.norm();
    auto r2 = -r1 * cos_theta / sin_theta - (roh * roh) / alpha * a1xa3;
    auto r3 = r1.cross(r2);
    // calculate the sign of roh
    auto rohs = r3.array() / a3.array();
    if (rohs[0] > 0) {
        roh *= -1;
    }

    K(0, 0) = alpha;
    K(0, 1) = -alpha * cos_theta / sin_theta;
    K(0, 2) = cx;
    K(1, 1) = beta / sin_theta;
    K(1, 2) = cy;
    K(2, 2) = 1;
    R.row(0) = r1;
    R.row(1) = r2;
    R.row(2) = r3;
    t = roh * K.inverse() * b;

    std::cout << "K is " << std::endl << K << std::endl;
    std::cout << "R is " << std::endl << R << std::endl;
    std::cout << "t is " << std::endl << t.transpose() << std::endl;
}

void SingleCamera::selfcheck(const Eigen::MatrixXf &w_check, const Eigen::MatrixXf &c_check) {
    float average_err = DBL_MAX;
    // homework4: 根据homework3求解得到的相机的参数，使用测试点进行验证，计算误差
    Eigen::MatrixXf m(3, 4);
    m << R, t;

    auto trans_xyz = w_check * (K * m).transpose();

    for (int i = 0; i < c_check.rows(); ++i) {
        if (i == 0) average_err = 0;
        auto trans_z = trans_xyz(i, 2);
        auto trans_x = trans_xyz(i, 0) / trans_z;
        auto trans_y = trans_xyz(i, 1) / trans_z;
        average_err += abs(trans_x - c_check(i, 0));
        average_err += abs(trans_y - c_check(i, 1));
    }
    average_err /= c_check.size();

    std::cout << "The average error is " << average_err << "," << std::endl;
    if (average_err > 0.1) {
        std::cout << "which is more than 0.1" << std::endl;
    } else {
        std::cout << "which is smaller than 0.1, the M is acceptable" << std::endl;
    }
}

int main(int argc, char **argv) {
// 三维世界坐标系中的点
Eigen::MatrixXf w_xz(4, 4);
w_xz << 8, 0, 9, 1,
        8, 0, 1, 1,
        6, 0, 1, 1,
        6, 0, 9, 1;

Eigen::MatrixXf w_xy(4, 4);
w_xy << 5, 1, 0, 1,
        5, 9, 0, 1,
        4, 9, 0, 1,
        4, 1, 0, 1;

Eigen::MatrixXf w_yz(4, 4);
w_yz << 0, 4, 7, 1,
        0, 4, 3, 1,
        0, 8, 3, 1,
        0, 8, 7, 1;

// 三维世界坐标系中的所有点
Eigen::MatrixXf w_coor(12, 4);
w_coor << w_xz,
        w_xy,
        w_yz;

// 对应的图像坐标系中的点
Eigen::MatrixXf c_xz(4, 2);
c_xz << 275, 142,
        312, 454,
        382, 436,
        357, 134;

Eigen::MatrixXf c_xy(4, 2);
c_xy << 432, 473,
        612, 623,
        647, 606,
        464, 465;

Eigen::MatrixXf c_yz(4, 2);
c_yz << 654, 216,
        644, 368,
        761, 420,
        781, 246;

// 对应的图像坐标系中的所有点
Eigen::MatrixXf c_coor(12, 2);
c_coor << c_xz,
        c_xy,
        c_yz;

// 定义SingleCamera对象，传入三维世界坐标系中所有点和对应的图像坐标系中的点，共使用12个点
SingleCamera aCamera = SingleCamera(w_coor, c_coor, 12);

aCamera.composeP();  // 用所有点求解P矩阵
aCamera.svdP();      // 对P矩阵进行奇异值分解
aCamera.workIntrinsicAndExtrinsic();  // 求解相机的内外参数

// 使用测试点进行验证，计算误差
Eigen::MatrixXf w_check(5, 4);
w_check << 6, 0, 5, 1,
        3, 3, 0, 1,
        0, 4, 0, 1,
        0, 4, 4, 1,
        0, 0, 7, 1;

Eigen::MatrixXf c_check(5, 2);
c_check << 369, 297,
            531, 484,
            640, 468,
            646, 333,
            556, 194;

    SingleCamera aCamera = SingleCamera(w_coor, c_coor, 12);  // 12 points in total are used
    aCamera.composeP();
    aCamera.svdP();
    aCamera.workIntrinsicAndExtrinsic();
    aCamera.selfcheck(w_check, c_check);  // test 5 points and verify M


    return 0;
}

三、 Cmake配置

cmake_minimum_required(VERSION 3.10)


set(CMAKE_BUILD_TYPE release)
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -fPIC")
set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake")

find_package(Eigen 3.0 REQUIRED)

add_executable(camera_calib main.cpp )