Function solve_scale_and_shift

src/solver.cpp:35–125

}

std::vector<Eigen::Vector4d> solve_scale_and_shift(const Eigen::Matrix3d &x_homo, const Eigen::Matrix3d &y_homo,
                                                   const Eigen::Vector3d &depth_x, const Eigen::Vector3d &depth_y) {
    // X: 3 x 3, column vectors are homogeneous of 2D points
    // Y: 3 x 3, column vectors are homogeneous of 2D points
    Eigen::Matrix3d x1 = x_homo.transpose();
    Eigen::Matrix3d x2 = y_homo.transpose();
    const Eigen::Vector3d &d1 = depth_x;
    const Eigen::Vector3d &d2 = depth_y;
    Eigen::VectorXd coeffs = Eigen::VectorXd::Zero(18);

    coeffs[0] = 2 * x2.row(0).dot(x2.row(1)) - x2.row(0).dot(x2.row(0)) - x2.row(1).dot(x2.row(1));
    coeffs[1] = x1.row(0).dot(x1.row(0)) + x1.row(1).dot(x1.row(1)) - 2 * x1.row(0).dot(x1.row(1));
    coeffs[2] = 2 * (d2[0] + d2[1]) * x2.row(0).dot(x2.row(1)) - 2 * d2[0] * x2.row(0).dot(x2.row(0)) -
                2 * d2[1] * x2.row(1).dot(x2.row(1));
    coeffs[3] = 2 * d2[0] * d2[1] * x2.row(0).dot(x2.row(1)) - d2[0] * d2[0] * x2.row(0).dot(x2.row(0)) -
                d2[1] * d2[1] * x2.row(1).dot(x2.row(1));
    coeffs[4] = 2 * d1[0] * x1.row(0).dot(x1.row(0)) + 2 * d1[1] * x1.row(1).dot(x1.row(1)) -
                2 * (d1[0] + d1[1]) * x1.row(0).dot(x1.row(1));
    coeffs[5] = d1[0] * d1[0] * x1.row(0).dot(x1.row(0)) + d1[1] * d1[1] * x1.row(1).dot(x1.row(1)) -
                2 * d1[0] * d1[1] * x1.row(0).dot(x1.row(1));
    coeffs[6] = 2 * x2.row(0).dot(x2.row(2)) - x2.row(0).dot(x2.row(0)) - x2.row(2).dot(x2.row(2));
    coeffs[7] = x1.row(0).dot(x1.row(0)) + x1.row(2).dot(x1.row(2)) - 2 * x1.row(0).dot(x1.row(2));
    coeffs[8] = 2 * (d2[0] + d2[2]) * x2.row(0).dot(x2.row(2)) - 2 * d2[0] * x2.row(0).dot(x2.row(0)) -
                2 * d2[2] * x2.row(2).dot(x2.row(2));
    coeffs[9] = 2 * d2[0] * d2[2] * x2.row(0).dot(x2.row(2)) - d2[0] * d2[0] * x2.row(0).dot(x2.row(0)) -
                d2[2] * d2[2] * x2.row(2).dot(x2.row(2));
    coeffs[10] = 2 * d1[0] * x1.row(0).dot(x1.row(0)) + 2 * d1[2] * x1.row(2).dot(x1.row(2)) -
                 2 * (d1[0] + d1[2]) * x1.row(0).dot(x1.row(2));
    coeffs[11] = d1[0] * d1[0] * x1.row(0).dot(x1.row(0)) + d1[2] * d1[2] * x1.row(2).dot(x1.row(2)) -
                 2 * d1[0] * d1[2] * x1.row(0).dot(x1.row(2));
    coeffs[12] = 2 * x2.row(1).dot(x2.row(2)) - x2.row(1).dot(x2.row(1)) - x2.row(2).dot(x2.row(2));
    coeffs[13] = x1.row(1).dot(x1.row(1)) + x1.row(2).dot(x1.row(2)) - 2 * x1.row(1).dot(x1.row(2));
    coeffs[14] = 2 * (d2[1] + d2[2]) * x2.row(1).dot(x2.row(2)) - 2 * d2[1] * x2.row(1).dot(x2.row(1)) -
                 2 * d2[2] * x2.row(2).dot(x2.row(2));
    coeffs[15] = 2 * d2[1] * d2[2] * x2.row(1).dot(x2.row(2)) - d2[1] * d2[1] * x2.row(1).dot(x2.row(1)) -
                 d2[2] * d2[2] * x2.row(2).dot(x2.row(2));
    coeffs[16] = 2 * d1[1] * x1.row(1).dot(x1.row(1)) + 2 * d1[2] * x1.row(2).dot(x1.row(2)) -
                 2 * (d1[1] + d1[2]) * x1.row(1).dot(x1.row(2));
    coeffs[17] = d1[1] * d1[1] * x1.row(1).dot(x1.row(1)) + d1[2] * d1[2] * x1.row(2).dot(x1.row(2)) -
                 2 * d1[1] * d1[2] * x1.row(1).dot(x1.row(2));

    const std::vector<int> coeff_ind0 = {0, 6,  12, 1,  7,  13, 2,  8, 0,  6,  12, 14, 6, 0, 12, 1,  7,  13, 3,
                                         9, 2,  8,  14, 15, 4,  10, 7, 1,  16, 13, 8,  2, 6, 12, 0,  14, 9,  3,
                                         8, 14, 2,  15, 3,  9,  15, 4, 10, 16, 7,  13, 1, 5, 11, 10, 4,  17, 16};
    const std::vector<int> coeff_ind1 = {11, 17, 5, 9, 15, 3, 5, 11, 17, 10, 16, 4, 11, 5, 17};
    const std::vector<int> ind0 = {0,   1,   9,   12,  13,  21,  24,  25,  26,  28,  29,  33,  39,  42,  47,
                                   50,  52,  53,  60,  61,  62,  64,  65,  69,  72,  73,  75,  78,  81,  83,
                                   87,  90,  91,  92,  94,  95,  99,  102, 103, 104, 106, 107, 110, 112, 113,
                                   122, 124, 125, 127, 128, 130, 132, 133, 135, 138, 141, 143};
    const std::vector<int> ind1 = {7, 8, 10, 19, 20, 22, 26, 28, 29, 31, 32, 34, 39, 42, 47};
    Eigen::MatrixXd C0 = Eigen::MatrixXd::Zero(12, 12);
    Eigen::MatrixXd C1 = Eigen::MatrixXd::Zero(12, 4);

    for (int k = 0; k < ind0.size(); k++) {
        int i = ind0[k] % 12;
        int j = ind0[k] / 12;
        C0(i, j) = coeffs[coeff_ind0[k]];
    }