| 135 | } |
| 136 | |
| 137 | Eigen::Vector3d computeCentroid(const PointView& view, |
| 138 | const PointIdList& ids) |
| 139 | { |
| 140 | using namespace Eigen; |
| 141 | |
| 142 | double mx, my, mz; |
| 143 | mx = my = mz = 0.0; |
| 144 | point_count_t n(0); |
| 145 | for (auto const& j : ids) |
| 146 | { |
| 147 | auto update = [&n](double value, double average) |
| 148 | { |
| 149 | double delta, delta_n; |
| 150 | delta = value - average; |
| 151 | delta_n = delta / n; |
| 152 | return average + delta_n; |
| 153 | }; |
| 154 | n++; |
| 155 | mx = update(view.getFieldAs<double>(Dimension::Id::X, j), mx); |
| 156 | my = update(view.getFieldAs<double>(Dimension::Id::Y, j), my); |
| 157 | mz = update(view.getFieldAs<double>(Dimension::Id::Z, j), mz); |
| 158 | } |
| 159 | |
| 160 | Vector3d centroid; |
| 161 | centroid << mx, my, mz; |
| 162 | |
| 163 | return centroid; |
| 164 | } |
| 165 | |
| 166 | Eigen::Matrix3d computeCovariance(const PointView& view, |
| 167 | const PointIdList& ids) |
no outgoing calls