| 379 | } |
| 380 | |
| 381 | bool AlignReconstructionsViaPoints(const Reconstruction& src_reconstruction, |
| 382 | const Reconstruction& tgt_reconstruction, |
| 383 | const size_t min_common_observations, |
| 384 | const double max_error, |
| 385 | const double min_inlier_ratio, |
| 386 | Sim3d* tgt_from_src) { |
| 387 | THROW_CHECK_GT(min_common_observations, 0); |
| 388 | THROW_CHECK_GT(max_error, 0.0); |
| 389 | THROW_CHECK_GE(min_inlier_ratio, 0.0); |
| 390 | THROW_CHECK_LE(min_inlier_ratio, 1.0); |
| 391 | |
| 392 | std::vector<Eigen::Vector3d> src_xyz; |
| 393 | std::vector<Eigen::Vector3d> tgt_xyz; |
| 394 | std::unordered_map<point3D_t, size_t> counts; |
| 395 | // Associate 3D points using point2D_idx |
| 396 | for (const auto& src_point3D : src_reconstruction.Points3D()) { |
| 397 | counts.clear(); |
| 398 | // Count how often a 3D point in tgt is associated to this 3D point. |
| 399 | for (const auto& track_el : src_point3D.second.track.Elements()) { |
| 400 | const Image& tgt_image = tgt_reconstruction.Image(track_el.image_id); |
| 401 | if (!tgt_image.HasPose()) { |
| 402 | continue; |
| 403 | } |
| 404 | const Point2D& tgt_point2D = tgt_image.Point2D(track_el.point2D_idx); |
| 405 | if (tgt_point2D.HasPoint3D()) { |
| 406 | if (counts.find(tgt_point2D.point3D_id) != counts.end()) { |
| 407 | counts[tgt_point2D.point3D_id]++; |
| 408 | } else { |
| 409 | counts[tgt_point2D.point3D_id] = 0; |
| 410 | } |
| 411 | } |
| 412 | } |
| 413 | if (counts.empty()) { |
| 414 | continue; |
| 415 | } |
| 416 | // The 3D point in tgt who is associated the most is selected |
| 417 | auto best_point3D = |
| 418 | std::max_element(counts.begin(), |
| 419 | counts.end(), |
| 420 | [](const std::pair<point3D_t, size_t>& p1, |
| 421 | const std::pair<point3D_t, size_t>& p2) { |
| 422 | return p1.second < p2.second; |
| 423 | }); |
| 424 | if (best_point3D->second >= min_common_observations) { |
| 425 | src_xyz.push_back(src_point3D.second.xyz); |
| 426 | tgt_xyz.push_back(tgt_reconstruction.Point3D(best_point3D->first).xyz); |
| 427 | } |
| 428 | } |
| 429 | THROW_CHECK_EQ(src_xyz.size(), tgt_xyz.size()); |
| 430 | LOG(INFO) << "Found " << src_xyz.size() << " / " |
| 431 | << src_reconstruction.NumPoints3D() << " valid correspondences."; |
| 432 | |
| 433 | RANSACOptions ransac_options; |
| 434 | ransac_options.max_error = max_error; |
| 435 | ransac_options.min_inlier_ratio = min_inlier_ratio; |
| 436 | const auto report = |
| 437 | EstimateSim3dRobust(src_xyz, tgt_xyz, ransac_options, *tgt_from_src); |
| 438 | return report.success; |