| 90 | } |
| 91 | |
| 92 | void CenterAndNormalizeImagePoints(const std::vector<Eigen::Vector2d>& points, |
| 93 | std::vector<Eigen::Vector2d>* normed_points, |
| 94 | Eigen::Matrix3d* normed_from_orig) { |
| 95 | const size_t num_points = points.size(); |
| 96 | THROW_CHECK_GT(num_points, 0); |
| 97 | |
| 98 | // Calculate centroid. |
| 99 | Eigen::Vector2d centroid(0, 0); |
| 100 | for (const Eigen::Vector2d& point : points) { |
| 101 | centroid += point; |
| 102 | } |
| 103 | centroid /= num_points; |
| 104 | |
| 105 | // Root mean square distance to centroid of all points. |
| 106 | double rms_mean_dist = 0; |
| 107 | for (const Eigen::Vector2d& point : points) { |
| 108 | rms_mean_dist += (point - centroid).squaredNorm(); |
| 109 | } |
| 110 | rms_mean_dist = std::sqrt(rms_mean_dist / num_points); |
| 111 | |
| 112 | // Compose normalization matrix. |
| 113 | const double norm_factor = std::sqrt(2.0) / rms_mean_dist; |
| 114 | *normed_from_orig << norm_factor, 0, -norm_factor * centroid(0), 0, |
| 115 | norm_factor, -norm_factor * centroid(1), 0, 0, 1; |
| 116 | |
| 117 | // Apply normalization matrix. |
| 118 | normed_points->resize(num_points); |
| 119 | for (size_t i = 0; i < num_points; ++i) { |
| 120 | (*normed_points)[i] = |
| 121 | (*normed_from_orig * points[i].homogeneous()).hnormalized(); |
| 122 | } |
| 123 | } |
| 124 | |
| 125 | } // namespace colmap |