Added code which calculates the error for each calibration type; also made sure to always save the calibration

1 files changed, 39 insertions, 0 deletions

diff --git a/src/survive_cal.c b/src/survive_cal.c
index 2ed0df0..bcbe25c 100755
--- a/src/survive_cal.c
+++ b/src/survive_cal.c
@@ -10,6 +10,8 @@
 
 #include "survive_cal.h"
 #include "survive_internal.h"
+#include "survive_reproject.h"
+
 #include <math.h>
 #include <string.h>
 #include <sys/stat.h>
@@ -634,6 +636,8 @@ static void handle_calibration( struct SurviveCalData *cd )
 			return;
 		}
 
+		int compute_reprojection_error = config_read_uint32(
+			ctx->global_config_values, "ComputeReprojectError", 0);
 
 		int lh;
 		for( lh = 0; lh < NUM_LIGHTHOUSES; lh++ )
@@ -645,7 +649,42 @@ static void handle_calibration( struct SurviveCalData *cd )
 
 			fprintf( fobjp, "%f %f %f\n", objfromlh->Pos[0], objfromlh->Pos[1], objfromlh->Pos[2] );
 			fprintf( fobjp, "%f %f %f %f\n", objfromlh->Rot[0], objfromlh->Rot[1], objfromlh->Rot[2], objfromlh->Rot[3] );
+
+			if (ctx->bsd[lh].PositionSet) {
+				config_set_lighthouse(ctx->lh_config, &ctx->bsd[0], 0);
+				config_set_lighthouse(ctx->lh_config, &ctx->bsd[1], 1);
+
+				if (compute_reprojection_error) {
+					FLT reproj_err = 0;
+					size_t cnt = 0;
+					SurviveObject *so = cd->poseobjects[obj];
+					for (size_t idx = 0; idx < so->nr_locations; idx++) {
+						FLT *lengths = fsd.lengths[idx][lh];
+						FLT *pt = fsd.angles[idx][lh];
+						if (lengths[0] < 0 || lengths[1] < 0)
+							continue;
+
+						cnt++;
+						FLT reproj_pt[2];
+						survive_reproject(ctx, lh, so->sensor_locations,
+										  reproj_pt);
+
+						FLT err = 0;
+						for (int dim = 0; dim < 2; dim++) {
+							err += (reproj_pt[dim] - pt[dim]) *
+								   (reproj_pt[dim] - pt[dim]);
+						}
+						reproj_err += sqrt(err);
+					}
+
+					reproj_err = reproj_err / cnt;
+					SV_INFO("Reproject error was %.13g for lighthouse %d",
+							reproj_err, lh);
+				}
+			}
 		}
+
+		config_save(ctx, "config.json");
 	}
 	fclose( fobjp );