q.transpose() = R * p.transpose() + tTemp ;
void registerSVD(Eigen::Matrix3d &p, Eigen::Matrix3d &q, Eigen::Matrix3d &R, Eigen::Matrix3d &tTemp) { Eigen::Vector3d pc = p.colwise().mean(); Eigen::Vector3d qc = q.colwise().mean(); Eigen::Matrix3d C = Eigen::Matrix3d::Zero(); for (int i = 0; i < p.col(0).size(); i++) { Eigen::Vector3d pTemp = (p.block(i, 0, 1, 3).transpose() - pc); Eigen::Vector3d qTemp = (q.block(i, 0, 1, 3).transpose() - qc); Eigen::Matrix3d CTemp = pTemp * qTemp.transpose(); C += CTemp; } C /= p.col(0).size(); Eigen::JacobiSVD<Eigen::Matrix3d> svd(C, Eigen::ComputeFullU | Eigen::ComputeFullV); R = svd.matrixV() * svd.matrixU().transpose(); // Eigen::Matrix3d tTemp = q.transpose() - R * p.transpose(); // T = tTemp.rowwise().mean(); /* T(0,0) = tTemp(0,0); T(1, 0) = tTemp(1, 0); T(2, 0) = tTemp(2, 0);*/ std::cout << "svd:T: " << tTemp << std::endl; }
