| 94 | } |
| 95 | |
| 96 | void Plane::computeModel() |
| 97 | { |
| 98 | // Compute centroid |
| 99 | PointT centroid = computeCentroid(features); |
| 100 | |
| 101 | // Subtract out centroid |
| 102 | MatrixX centeredGf = MatrixX::Zero(3, features.size()); |
| 103 | int counter = 0; |
| 104 | for (auto p : features) |
| 105 | { |
| 106 | centeredGf(0, counter) = p.x - centroid.x; |
| 107 | centeredGf(1, counter) = p.y - centroid.y; |
| 108 | centeredGf(2, counter) = p.z - centroid.z; |
| 109 | counter++; |
| 110 | } |
| 111 | |
| 112 | // Estimate plane using SVD |
| 113 | JacobiSVD<MatrixX> svd(centeredGf, Eigen::ComputeThinU | Eigen::ComputeThinV); |
| 114 | // Normal is left singular vector of least singular value |
| 115 | Vector3 normal = (svd.matrixU().block(0, 2, 3, 1)).eval(); |
| 116 | // normal = normal / normal.norm(); |
| 117 | Scalar d = -(normal(0) * centroid.x + normal(1) * centroid.y + |
| 118 | normal(2) * centroid.z); |
| 119 | |
| 120 | model.plane(0) = normal(0); |
| 121 | model.plane(1) = normal(1); |
| 122 | model.plane(2) = normal(2); |
| 123 | model.plane(3) = d; |
| 124 | |
| 125 | model.centroid(0) = centroid.x; |
| 126 | model.centroid(1) = centroid.y; |
| 127 | model.centroid(2) = centroid.z; |
| 128 | } |
| 129 | |
| 130 | // not implemented |
| 131 | Scalar Plane::distance(const PlaneParameters &tgt) const |
nothing calls this directly
no test coverage detected