Method compute_Sim3

src/PLPSLAM/solve/sim3_solver.cc:193–288

        }

        void sim3_solver::compute_Sim3(const Mat33_t &pts_1, const Mat33_t &pts_2,
                                       Mat33_t &rot_12, Vec3_t &trans_12, float &scale_12,
                                       Mat33_t &rot_21, Vec3_t &trans_21, float &scale_21)
        {
            // Based on "Closed-form solution of absolute orientation using unit quaternions"
            // http://people.csail.mit.edu/bkph/papers/Absolute_Orientation.pdf

            // Find the centroid of each point set
            const Vec3_t centroid_1 = pts_1.rowwise().mean();
            const Vec3_t centroid_2 = pts_2.rowwise().mean();

            // Move the center of the distribution to centroid
            Mat33_t ave_pts_1 = pts_1;
            ave_pts_1.colwise() -= centroid_1;
            Mat33_t ave_pts_2 = pts_2;
            ave_pts_2.colwise() -= centroid_2;

            // 4.A Matrix of Sums of Products

            // Find the matrix M
            const Mat33_t M = ave_pts_1 * ave_pts_2.transpose();

            // Find the matrix N
            const double &Sxx = M(0, 0);
            const double &Syx = M(1, 0);
            const double &Szx = M(2, 0);
            const double &Sxy = M(0, 1);
            const double &Syy = M(1, 1);
            const double &Szy = M(2, 1);
            const double &Sxz = M(0, 2);
            const double &Syz = M(1, 2);
            const double &Szz = M(2, 2);
            Eigen::Matrix4d N;
            N << (Sxx + Syy + Szz), (Syz - Szy), (Szx - Sxz), (Sxy - Syx),
                (Syz - Szy), (Sxx - Syy - Szz), (Sxy + Syx), (Szx + Sxz),
                (Szx - Sxz), (Sxy + Syx), (-Sxx + Syy - Szz), (Syz + Szy),
                (Sxy - Syx), (Szx + Sxz), (Syz + Szy), (-Sxx - Syy + Szz);

            // 4.B Eigenvector Maximizes Matrix Product

            // Eigenvalue decomposition of N
            Eigen::EigenSolver<Mat44_t> eigensolver(N);

            // Find the maximum eigenvalue
            const auto &eigenvalues = eigensolver.eigenvalues();
            int max_idx = -1;
            double max_eigenvalue = -INFINITY;
            for (int idx = 0; idx < 4; ++idx)
            {
                if (max_eigenvalue <= eigenvalues(idx, 0).real())
                {
                    max_eigenvalue = eigenvalues(idx, 0).real();
                    max_idx = idx;
                }
            }
            const auto max_eigenvector = eigensolver.eigenvectors().col(max_idx);

            // Since it is a complex number, only the real number is extracted.