QR分解之GivensRotation法

在SLAM过程中，通常最终需要求解一个最小二乘问题，
$$Ax=b$$ 其中 $A$ 非常庞大并且稀疏，可以利用舒尔布或者 $Cholesky$ 分解，从而提高求解的效率。求解这个问题有很多中方法，如 $QR$ 分解， $LLT$ 分解等等。本文主要介绍使用Givens Rotation解这个问题。很多SLAM算法中都有使用Givens Rotation，比如MSCKF，iSAM等

// Note: i is col, j is row.
void GivensCosSin(const double aii, const double aji, double* c, double* s) {
    if (std::abs(aji) < 1e-12) {
        *c = 1.;
        *s = 0.;
    } else if (std::abs(aji) > std::abs(aii)) {
        const double aii_over_aji = aii / aji;
        const double one_over_sqrt = 1. / std::sqrt(1. + aii_over_aji * aii_over_aji);
        *c = - aii_over_aji * one_over_sqrt;
        *s = one_over_sqrt;
    } else {
        const double aji_over_aii = aji / aii;
        const double one_over_sqrt = 1. / std::sqrt(1 + aji_over_aii * aji_over_aii);
        *c = one_over_sqrt;
        *s = - aji_over_aii * one_over_sqrt;
    }
}

// A * x = b to R * x = d.
void GivensRotationQRInPlace(Eigen::MatrixXd* A, Eigen::VectorXd* b) {
    Eigen::MatrixXd& AA = *A;
    Eigen::VectorXd& bb = *b;
    const int rows = AA.rows();
    const int cols = AA.cols();

    assert(rows > cols);
    assert(rows == bb.rows());

    // Traverse all columns
    for (int col = 0; col < cols; ++col) {
        // Bottom to up.
        for (int row = rows - 1; row > col; --row) {
            // Compute cos_theta, sin_theta
            double c, s;
            GivensCosSin(AA(col, col), AA(row, col), &c, &s);

            // Porcess A.
            // For all elements at the ith, and jth rows.
            for (int n = col; n < cols; ++n) {
                const double a = AA(col, n);
                const double b = AA(row, n);
                AA(col, n) = c * a - s * b;
                AA(row, n) = s * a + c * b;
            } 

            // Process b.
            {
                const double a = bb(col);
                const double b = bb(row);
                bb(col) = c * a - s * b;
                bb(row) = s * a + c * b; 
            }
        }   
    }
}

bool BackSubstitution(const Eigen::MatrixXd& R, const Eigen::VectorXd& d, Eigen::VectorXd* x) {
    x->resize(R.rows());
    // From bottom to up.
    for (int row = R.rows() - 1; row >= 0; --row) {
        if (std::abs(R(row, row)) < 1e-12) {
            return false;
        }

        double tmp = 0.;
        for (size_t col = row + 1; col < R.cols(); ++col) {
            tmp += R(row, col) * (*x)(col);
        }

        (*x)(row) = (d(row) - tmp) /  R(row, row);
    }

    return true;
}

bool SolveLinearSystemInPlace(Eigen::MatrixXd* A, Eigen::VectorXd* b, Eigen::VectorXd* x) {
    // Ax=b to Rx=d.
    GivensRotationQRInPlace(A, b);
    
    // Solve Rx = d.
    return BackSubstitution(A->topRows(A->cols()), b->topRows(A->cols()), x);
}

具体推导公式及代码：Givens Rotation解Ax=b - 小葡萄的文章 - 知乎

相比于其他两种实现 QR 分解的方法，基于 Givens 选择的方法的优点在于其计算易于并行化，且在面对稀疏矩阵 (Sparse Matrix) 是可以减少计算量。