jlpoole/MotionCal
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Add quality metrics

Browse source
This commit is contained in:
PaulStoffregen 2016-03-19 17:55:03 -07:00
commit 0aff5bef36
4 changed files with 137 additions and 81 deletions
Download patch file Download diff file Expand all files Collapse all files

3
Makefile
View file

@ -35,7 +35,7 @@ VERSION = 0.01
endif
OBJS = visualize.o serialdata.o rawdata.o magcal.o matrix.o fusion.o
OBJS = visualize.o serialdata.o rawdata.o magcal.o matrix.o fusion.o quality.o
all: $(ALL)
@ -71,5 +71,6 @@ rawdata.o: rawdata.c imuread.h
magcal.o: magcal.c imuread.h
matrix.o: matrix.c imuread.h
fusion.o: fusion.c imuread.h
quality.o: quality.c imuread.h

7
imuread.h
View file

@ -74,7 +74,12 @@ void apply_calibration(int16_t rawx, int16_t rawy, int16_t rawz, Point_t *out);
void display_callback(void);
void resize_callback(int width, int height);
void MagCal_Run(void);
void quality_reset(void);
void quality_update(const Point_t *point);
float quality_surface_gap_error(void);
float quality_magnitude_variance_error(void);
float quality_centroid_placement_error(void);
float quality_spherical_fit_error(void);
// magnetic calibration & buffer structure
typedef struct {

122
quality.c Normal file
View file

@ -0,0 +1,122 @@
#include "imuread.h"
// longitude = 0 to 2pi (meaning 0 to 360 degrees)
// latitude = -pi/2 to +pi/2 (meaning -90 to +90 degrees)
// return 0 to 99 - which region on the sphere (100 of equal surface area)
static int sphere_region(float longitude, float latitude)
{
int region;
// https://etna.mcs.kent.edu/vol.25.2006/pp309-327.dir/pp309-327.html
// sphere equations....
// area of unit sphere = 4*pi
// area of unit sphere cap = 2*pi*h h = cap height
// lattitude of unit sphere cap = arcsin(1 - h)
if (latitude > 1.37046f /* 78.52 deg */) {
// arctic cap, 1 region
return 0;
} else if (latitude < -1.37046f /* -78.52 deg */) {
// antarctic cap, 1 region
return 99;
} else if (latitude > 0.74776f /* 42.84 deg */ || latitude < -0.74776f ) {
// temperate zones, 15 regions each
region = floorf(longitude * (float)(15.0 / (M_PI * 2.0)));
if (region < 0) region = 0;
else if (region > 14) region = 14;
if (latitude > 0.0) {
return region + 1; // 1 to 15
} else {
return region + 84; // 84 to 98
}
} else {
// tropic zones, 34 regions each
region = floorf(longitude * (float)(34.0 / (M_PI * 2.0)));
if (region < 0) region = 0;
else if (region > 33) region = 33;
if (latitude >= 0.0) {
return region + 16; // 16 to 49
} else {
return region + 50; // 50 to 83
}
}
}
static int count=0;
static int spheredist[100];
static float magnitude[MAGBUFFSIZE];
void quality_reset(void)
{
count=0;
memset(spheredist, 0, sizeof(spheredist));
}
void quality_update(const Point_t *point)
{
float x, y, z;
float latitude, longitude;
x = point->x;
y = point->y;
z = point->z;
magnitude[count] = sqrtf(x * x + y * y + z * z);
longitude = atan2f(y, x) + (float)M_PI;
latitude = atan2f(sqrtf(x * x + y * y), z) - (float)(M_PI / 2.0);
spheredist[sphere_region(longitude, latitude)]++;
count++;
}
// How many surface gaps
float quality_surface_gap_error(void)
{
float error=0.0f;
int i, num;
for (i=0; i < 100; i++) {
num = spheredist[i];
if (num == 0) {
error += 1.0f;
} else if (num == 1) {
error += 0.2f;
} else if (num == 2) {
error += 0.01f;
}
}
return error;
}
// Variance in magnitude
float quality_magnitude_variance_error(void)
{
float sum, mean, diff, variance;
int i;
sum = 0.0f;
for (i=0; i < count; i++) {
sum += magnitude[i];
}
mean = sum / (float)count;
variance = 0.0f;
for (i=0; i < count; i++) {
diff = magnitude[i] - mean;
variance += diff * diff;
}
variance /= (float)count;
return sqrtf(variance) / mean * 100.0f;
}
// Offset of centroid
float quality_centroid_placement_error(void)
{
return 0.0f;
}
// Freescale's algorithm fit error
float quality_spherical_fit_error(void)
{
return magcal.FitError;
}

86
visualize.c
View file

@ -4,47 +4,6 @@ MagCalibration_t magcal;
Quaternion_t current_orientation;
// longitude = 0 to 2pi (meaning 0 to 360 degrees)
// latitude = -pi/2 to +pi/2 (meaning -90 to +90 degrees)
// return 0 to 99 - which region on the sphere (100 of equal surface area)
static int sphere_region(float longitude, float latitude)
{
int region;
// https://etna.mcs.kent.edu/vol.25.2006/pp309-327.dir/pp309-327.html
// sphere equations....
// area of unit sphere = 4*pi
// area of unit sphere cap = 2*pi*h h = cap height
// lattitude of unit sphere cap = arcsin(1 - h)
if (latitude > 1.37046f /* 78.52 deg */) {
// arctic cap, 1 region
return 0;
} else if (latitude < -1.37046f /* -78.52 deg */) {
// antarctic cap, 1 region
return 99;
} else if (latitude > 0.74776f /* 42.84 deg */ || latitude < -0.74776f ) {
// temperate zones, 15 regions each
region = floorf(longitude * (float)(15.0 / (M_PI * 2.0)));
if (region < 0) region = 0;
else if (region > 14) region = 14;
if (latitude > 0.0) {
return region + 1; // 1 to 15
} else {
return region + 84; // 84 to 98
}
} else {
// tropic zones, 34 regions each
region = floorf(longitude * (float)(34.0 / (M_PI * 2.0)));
if (region < 0) region = 0;
else if (region > 33) region = 33;
if (latitude >= 0.0) {
return region + 16; // 16 to 49
} else {
return region + 50; // 50 to 83
}
}
}
void apply_calibration(int16_t rawx, int16_t rawy, int16_t rawz, Point_t *out)
{
@ -89,22 +48,16 @@ static GLuint spherelowreslist;
void display_callback(void)
{
//static int updatenum=0;
int i, count=0;
int i;
float xscale, yscale, zscale;
float xoff, yoff, zoff;
float rotation[9];
Point_t point, draw;
float magnitude[MAGBUFFSIZE];
float latitude[MAGBUFFSIZE]; // 0 to PI
float longitude[MAGBUFFSIZE]; // -PI to +PI
float sumx=0.0, sumy=0.0, sumz=0.0, summag=0.0;
int spheredist[100];
Quaternion_t orientation;
quality_reset();
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glColor3f(1, 0, 0); // set current color to red
memset(spheredist, 0, sizeof(spheredist));
glLoadIdentity();
#if 0
gluLookAt(0.0, 0.0, 0.8, // eye location
@ -129,25 +82,13 @@ void display_callback(void)
orientation.q3 *= -1.0f;
quad_to_rotation(&orientation, rotation);
for (i=0; i < MAGBUFFSIZE; i++) {
//if (caldata[i].valid) {
if (magcal.valid[i]) {
//apply_calibration(&caldata[i], &point);
apply_calibration(magcal.BpFast[0][i], magcal.BpFast[1][i],
magcal.BpFast[2][i], &point);
//point.x *= -1.0f;
//point.y *= -1.0f;
//point.z *= -1.0f;
sumx += point.x;
sumy += point.y;
sumz += point.z;
magnitude[i] = sqrtf(point.x * point.x +
point.y * point.y + point.z * point.z);
summag += magnitude[i];
longitude[i] = atan2f(point.y, point.x) + (float)M_PI;
latitude[i] = atan2f(sqrtf(point.x * point.x +
point.y * point.y), point.z) - (float)(M_PI / 2.0);
spheredist[sphere_region(longitude[i], latitude[i])]++;
count++;
quality_update(&point);
rotate(&point, &draw, rotation);
glPushMatrix();
glTranslatef(
@ -165,23 +106,10 @@ void display_callback(void)
}
}
#if 0
if (updatenum++ == 500) {
for (i=0; i < MAGBUFFSIZE; i++) {
if (caldata[i].valid) {
printf("long: %6.2f lat: %6.2f (%.2f)\n",
longitude[i] * 180.0 / M_PI,
latitude[i] * 180.0 / M_PI, latitude[i]);
}
}
for (i=0; i < 100; i++) {
printf("sphere %d region: %d points\n", 1, spheredist[i]);
}
printf("count = %d\n", count);
printf("sum = %.2f %.2f %.2f\n", sumx, sumy, sumz);
printf("summag = %.2f avg: %.2f\n", summag, summag / (float)count);
printf("exit here\n");
exit(1);
}
printf(" quality: %5.1f %5.1f %5.1f\n",
quality_surface_gap_error(),
quality_magnitude_variance_error(),
quality_spherical_fit_error());
#endif
}