| 155 | } |
| 156 | |
| 157 | double SupervoxelFilter::dist(const PointRef& pi, const PointRef& pj) |
| 158 | { |
| 159 | Vector3d p1(pi.getFieldAs<double>(Id::X), |
| 160 | pi.getFieldAs<double>(Id::Y), |
| 161 | pi.getFieldAs<double>(Id::Z)); |
| 162 | Vector3d p2(pj.getFieldAs<double>(Id::X), |
| 163 | pj.getFieldAs<double>(Id::Y), |
| 164 | pj.getFieldAs<double>(Id::Z)); |
| 165 | Vector3d n1(pi.getFieldAs<double>(Id::NormalX), |
| 166 | pi.getFieldAs<double>(Id::NormalY), |
| 167 | pi.getFieldAs<double>(Id::NormalZ)); |
| 168 | Vector3d n2(pj.getFieldAs<double>(Id::NormalX), |
| 169 | pj.getFieldAs<double>(Id::NormalY), |
| 170 | pj.getFieldAs<double>(Id::NormalZ)); |
| 171 | double d = 1 - abs(n1.dot(n2)); |
| 172 | d += 0.4*(p1 - p2).norm()/m_R; |
| 173 | return d; |
| 174 | } |
| 175 | |
| 176 | void SupervoxelFilter::filter(PointView& view) { |
| 177 | const point_count_t n_points = view.size(); |