| 239 | } |
| 240 | |
| 241 | static double |
| 242 | icvCalcDist3D( CvPoint3D32f * points, int count, float *_line, float *dist ) |
| 243 | { |
| 244 | int j; |
| 245 | float px = _line[3], py = _line[4], pz = _line[5]; |
| 246 | float vx = _line[0], vy = _line[1], vz = _line[2]; |
| 247 | double sum_dist = 0.; |
| 248 | |
| 249 | for( j = 0; j < count; j++ ) |
| 250 | { |
| 251 | float x, y, z; |
| 252 | double p1, p2, p3; |
| 253 | |
| 254 | x = points[j].x - px; |
| 255 | y = points[j].y - py; |
| 256 | z = points[j].z - pz; |
| 257 | |
| 258 | p1 = vy * z - vz * y; |
| 259 | p2 = vz * x - vx * z; |
| 260 | p3 = vx * y - vy * x; |
| 261 | |
| 262 | dist[j] = (float) sqrt( p1*p1 + p2*p2 + p3*p3 ); |
| 263 | sum_dist += dist[j]; |
| 264 | } |
| 265 | |
| 266 | return sum_dist; |
| 267 | } |
| 268 | |
| 269 | static void |
| 270 | icvWeightL1( float *d, int count, float *w ) |