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

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
}