/********************************************************************** * File: linlsq.cpp (Formerly llsq.c) * Description: Linear Least squares fitting code. * Author: Ray Smith * Created: Thu Sep 12 08:44:51 BST 1991 * * (C) Copyright 1991, Hewlett-Packard Ltd. ** Licensed under the Apache License, Version 2.0 (the "License"); ** you may not use this file except in compliance with the License. ** You may obtain a copy of the License at ** http://www.apache.org/licenses/LICENSE-2.0 ** Unless required by applicable law or agreed to in writing, software ** distributed under the License is distributed on an "AS IS" BASIS, ** WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. ** See the License for the specific language governing permissions and ** limitations under the License. * **********************************************************************/ #include "mfcpch.h" #include #include #include "errcode.h" #include "linlsq.h" #ifndef __UNIX__ #define M_PI 3.14159265359 #endif const ERRCODE EMPTY_LLSQ = "Can't delete from an empty LLSQ"; #define EXTERN EXTERN double_VAR (pdlsq_posdir_ratio, 4e-6, "Mult of dir to cf pos"); EXTERN double_VAR (pdlsq_threshold_angleavg, 0.1666666, "Frac of pi for simple fit"); /********************************************************************** * LLSQ::clear * * Function to initialize a LLSQ. **********************************************************************/ void LLSQ::clear() { //initialize n = 0; //no elements sigx = 0; //update accumulators sigy = 0; sigxx = 0; sigxy = 0; sigyy = 0; } /********************************************************************** * LLSQ::add * * Add an element to the accumulator. **********************************************************************/ void LLSQ::add( //add an element double x, //xcoord double y //ycoord ) { n++; //count elements sigx += x; //update accumulators sigy += y; sigxx += x * x; sigxy += x * y; sigyy += y * y; } /********************************************************************** * LLSQ::remove * * Delete an element from the acculuator. **********************************************************************/ void LLSQ::remove( //delete an element double x, //xcoord double y //ycoord ) { if (n <= 0) //illegal EMPTY_LLSQ.error ("LLSQ::remove", ABORT, NULL); n--; //count elements sigx -= x; //update accumulators sigy -= y; sigxx -= x * x; sigxy -= x * y; sigyy -= y * y; } /********************************************************************** * LLSQ::m * * Return the gradient of the line fit. **********************************************************************/ double LLSQ::m() { //get gradient if (n > 1) return (sigxy - sigx * sigy / n) / (sigxx - sigx * sigx / n); else return 0; //too little } /********************************************************************** * LLSQ::c * * Return the constant of the line fit. **********************************************************************/ double LLSQ::c( //get constant double m //gradient to fit with ) { if (n > 0) return (sigy - m * sigx) / n; else return 0; //too little } /********************************************************************** * LLSQ::rms * * Return the rms error of the fit. **********************************************************************/ double LLSQ::rms( //get error double m, //gradient to fit with double c //constant to fit with ) { double error; //total error if (n > 0) { error = sigyy + m * (m * sigxx + 2 * (c * sigx - sigxy)) + c * (n * c - 2 * sigy); if (error >= 0) error = sqrt (error / n); //sqrt of mean else error = 0; } else error = 0; //too little return error; } /********************************************************************** * LLSQ::spearman * * Return the spearman correlation coefficient. **********************************************************************/ double LLSQ::spearman() { //get error double error; //total error if (n > 1) { error = (sigxx - sigx * sigx / n) * (sigyy - sigy * sigy / n); if (error > 0) { error = (sigxy - sigx * sigy / n) / sqrt (error); } else error = 1; } else error = 1; //too little return error; } /********************************************************************** * PDLSQ::fit * * Return all the parameters of the fit to pos/dir. * The return value is the rms error. **********************************************************************/ float PDLSQ::fit( //get fit DIR128 &ang, //output angle float &sin_ang, //r,theta parameterisation float &cos_ang, float &r) { double a, b; //itermediates double angle; //resulting angle double avg_angle; //simple average double error; //total error double sinx, cosx; //return values if (pos.n > 0) { a = pos.sigxy - pos.sigx * pos.sigy / pos.n + pdlsq_posdir_ratio * dir.sigxy; b = pos.sigxx - pos.sigyy + (pos.sigy * pos.sigy - pos.sigx * pos.sigx) / pos.n + pdlsq_posdir_ratio * (dir.sigxx - dir.sigyy); if (dir.sigy != 0 || dir.sigx != 0) avg_angle = atan2 (dir.sigy, dir.sigx); else avg_angle = 0; if ((a != 0 || b != 0) && pos.n > 1) angle = atan2 (2 * a, b) / 2; else angle = avg_angle; error = avg_angle - angle; if (error > M_PI / 2) { error -= M_PI; angle += M_PI; } if (error < -M_PI / 2) { error += M_PI; angle -= M_PI; } if (error > M_PI * pdlsq_threshold_angleavg || error < -M_PI * pdlsq_threshold_angleavg) angle = avg_angle; //go simple //convert direction ang = (inT16) (angle * MODULUS / (2 * M_PI)); sinx = sin (angle); cosx = cos (angle); r = (sinx * pos.sigx - cosx * pos.sigy) / pos.n; // tprintf("x=%g, y=%g, xx=%g, xy=%g, yy=%g, a=%g, b=%g, ang=%g, r=%g\n", // pos.sigx,pos.sigy,pos.sigxx,pos.sigxy,pos.sigyy, // a,b,angle,r); error = dir.sigxx * sinx * sinx + dir.sigyy * cosx * cosx - 2 * dir.sigxy * sinx * cosx; error *= pdlsq_posdir_ratio; error += sinx * sinx * pos.sigxx + cosx * cosx * pos.sigyy - 2 * sinx * cosx * pos.sigxy - 2 * r * (sinx * pos.sigx - cosx * pos.sigy) + r * r * pos.n; if (error >= 0) //rms value error = sqrt (error / pos.n); else error = 0; //-0 sin_ang = sinx; cos_ang = cosx; } else { sin_ang = 0.0f; cos_ang = 0.0f; ang = 0; error = 0; //too little } return error; }