| 2696 | } |
| 2697 | |
| 2698 | void STDescManager::triangle_solver(std::pair<STDesc, STDesc> &std_pair, |
| 2699 | Eigen::Vector3d &t, Eigen::Matrix3d &rot) { |
| 2700 | Eigen::Matrix3d src = Eigen::Matrix3d::Zero(); |
| 2701 | Eigen::Matrix3d ref = Eigen::Matrix3d::Zero(); |
| 2702 | src.col(0) = std_pair.first.vertex_A_ - std_pair.first.center_; |
| 2703 | src.col(1) = std_pair.first.vertex_B_ - std_pair.first.center_; |
| 2704 | src.col(2) = std_pair.first.vertex_C_ - std_pair.first.center_; |
| 2705 | ref.col(0) = std_pair.second.vertex_A_ - std_pair.second.center_; |
| 2706 | ref.col(1) = std_pair.second.vertex_B_ - std_pair.second.center_; |
| 2707 | ref.col(2) = std_pair.second.vertex_C_ - std_pair.second.center_; |
| 2708 | Eigen::Matrix3d covariance = src * ref.transpose(); |
| 2709 | Eigen::JacobiSVD<Eigen::MatrixXd> svd(covariance, Eigen::ComputeThinU | |
| 2710 | Eigen::ComputeThinV); |
| 2711 | Eigen::Matrix3d V = svd.matrixV(); |
| 2712 | Eigen::Matrix3d U = svd.matrixU(); |
| 2713 | rot = V * U.transpose(); |
| 2714 | if (rot.determinant() < 0) { |
| 2715 | Eigen::Matrix3d K; |
| 2716 | K << 1, 0, 0, 0, 1, 0, 0, 0, -1; |
| 2717 | rot = V * K * U.transpose(); |
| 2718 | } |
| 2719 | t = -rot * std_pair.first.center_ + std_pair.second.center_; |
| 2720 | } |
| 2721 | |
| 2722 | double STDescManager::plane_geometric_verify( |
| 2723 | const pcl::PointCloud<pcl::PointXYZINormal>::Ptr &source_cloud, |
nothing calls this directly
no outgoing calls
no test coverage detected