#include <linlsq.h>
Used to hold the rms error of the fit to pos/dir
Definition at line 80 of file linlsq.h.
PDLSQ::PDLSQ | ( | ) | [inline] |
void PDLSQ::clear | ( | ) | [inline] |
INT32 PDLSQ::count | ( | ) | [inline] |
float PDLSQ::fit | ( | DIR128 & | ang, | |
float & | sin_ang, | |||
float & | cos_ang, | |||
float & | r | |||
) |
Return all the parameters of the fit to pos/dir. The return value is the rms error.
Definition at line 169 of file linlsq.cpp.
References dir, M_PI, MODULUS, LLSQ::n, pos, LLSQ::sigx, LLSQ::sigxx, LLSQ::sigxy, LLSQ::sigy, and LLSQ::sigyy.
00173 { 00174 double a, b; //itermediates 00175 double angle; //resulting angle 00176 double avg_angle; //simple average 00177 double error; //total error 00178 double sinx, cosx; //return values 00179 00180 if (pos.n > 0) { 00181 a = pos.sigxy - pos.sigx * pos.sigy / pos.n 00182 + pdlsq_posdir_ratio * dir.sigxy; 00183 b = 00184 pos.sigxx - pos.sigyy + (pos.sigy * pos.sigy - 00185 pos.sigx * pos.sigx) / pos.n + 00186 pdlsq_posdir_ratio * (dir.sigxx - dir.sigyy); 00187 if (dir.sigy != 0 || dir.sigx != 0) 00188 avg_angle = atan2 (dir.sigy, dir.sigx); 00189 else 00190 avg_angle = 0; 00191 if ((a != 0 || b != 0) && pos.n > 1) 00192 angle = atan2 (2 * a, b) / 2; 00193 else 00194 angle = avg_angle; 00195 error = avg_angle - angle; 00196 if (error > M_PI / 2) { 00197 error -= M_PI; 00198 angle += M_PI; 00199 } 00200 if (error < -M_PI / 2) { 00201 error += M_PI; 00202 angle -= M_PI; 00203 } 00204 if (error > M_PI * pdlsq_threshold_angleavg 00205 || error < -M_PI * pdlsq_threshold_angleavg) 00206 angle = avg_angle; //go simple 00207 //convert direction 00208 ang = (INT16) (angle * MODULUS / (2 * M_PI)); 00209 sinx = sin (angle); 00210 cosx = cos (angle); 00211 r = (sinx * pos.sigx - cosx * pos.sigy) / pos.n; 00212 // tprintf("x=%g, y=%g, xx=%g, xy=%g, yy=%g, a=%g, b=%g, ang=%g, r=%g\n", 00213 // pos.sigx,pos.sigy,pos.sigxx,pos.sigxy,pos.sigyy, 00214 // a,b,angle,r); 00215 error = dir.sigxx * sinx * sinx + dir.sigyy * cosx * cosx 00216 - 2 * dir.sigxy * sinx * cosx; 00217 error *= pdlsq_posdir_ratio; 00218 error += sinx * sinx * pos.sigxx + cosx * cosx * pos.sigyy 00219 - 2 * sinx * cosx * pos.sigxy 00220 - 2 * r * (sinx * pos.sigx - cosx * pos.sigy) + r * r * pos.n; 00221 if (error >= 0) 00222 //rms value 00223 error = sqrt (error / pos.n); 00224 else 00225 error = 0; //-0 00226 sin_ang = sinx; 00227 cos_ang = cosx; 00228 } 00229 else { 00230 sin_ang = 0.0f; 00231 cos_ang = 0.0f; 00232 ang = 0; 00233 error = 0; //too little 00234 } 00235 return error; 00236 }
LLSQ PDLSQ::dir [private] |
LLSQ PDLSQ::pos [private] |