From 6bcf419d2e8f739b432d4790d1ba9d48ab65365b Mon Sep 17 00:00:00 2001 From: fcolin Date: Fri, 18 Nov 2011 12:14:12 +0000 Subject: --- ARMFCaptureD3D/Calibration.cpp | 254 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 254 insertions(+) create mode 100644 ARMFCaptureD3D/Calibration.cpp (limited to 'ARMFCaptureD3D/Calibration.cpp') diff --git a/ARMFCaptureD3D/Calibration.cpp b/ARMFCaptureD3D/Calibration.cpp new file mode 100644 index 0000000..a843e50 --- /dev/null +++ b/ARMFCaptureD3D/Calibration.cpp @@ -0,0 +1,254 @@ +#include "ARMFCaptureD3D.h" +#include "Calibration.h" +#include +#include +#include "debug.h" +#include "ConfigFile\ConfigFile.h" + +#define DUMP_CALIBRATION + +Calibration::Calibration(double width, double height, double calibInset) +{ + //dot is the original calibration image + dots[TopLeft][0] = calibInset; //top left X + dots[TopLeft][1] = calibInset; //top left Y + dots[TopRight][0] = width - calibInset; //top right + dots[TopRight][1] = calibInset; //top right + dots[BottomLeft][0] = calibInset; //bot left + dots[BottomLeft][1] = height - calibInset; //bot left + dots[BottomRight][0] = width - calibInset; //bot right + dots[BottomRight][1] = height - calibInset; //bot right + RazCalibration(); +} +void Calibration::SetCalibrationPoint(Corners i, double x, double y) +{ + cal[i][0] = x; + cal[i][1] = y; + cal_set[i] = true; + calib_points_ok = true; + for ( int j = 0; j <4; j++ ) + if ( !cal_set[j] ) + { + calib_points_ok = false; + break; + } +} +void Calibration::RazCalibration() +{ + a1 = b1 = c1 = a3 = b3 = a2 = b2 = c2 = 0.0; + for ( int i = 0; i <4; i++ ) + cal_set[i] = false; + calib_points_ok = false; +} +void Calibration::translateCoords(double X, double Y, double *cx, double *cy) +{ + assert( calibrated == true ); + + *cx = (a1 * X + b1 * Y + c1) / (a3 * X + b3 * Y + 1.0); + *cy = (a2 * X + b2 * Y + c2) / (a3 * X + b3 * Y + 1.0); +} +bool Calibration::Calibrate() +{ +#ifdef DUMP_CALIBRATION + DBGMSG(TEXT("running calibration\n")); +#endif + if ( !calib_points_ok ) return false; + + RazCalibration(); + + double matrix[8][8] = + { + { -1, -1, -1, -1, 0, 0, 0, 0 }, + + { -cal[0][0], -cal[1][0], -cal[2][0], -cal[3][0], 0, 0, 0, 0 }, + { -cal[0][1], -cal[1][1], -cal[2][1], -cal[3][1], 0,0,0,0 }, + { 0,0,0,0,-1,-1,-1,-1 }, + { 0,0,0,0, -cal[0][0], -cal[1][0], -cal[2][0], -cal[3][0] }, + { 0,0,0,0, -cal[0][1], -cal[1][1], -cal[2][1], -cal[3][1] }, + { cal[0][0] * dots[0][0], cal[1][0] * dots[1][0], cal[2][0] * dots[2][0], cal[3][0] * dots[3][0], cal[0][0] * dots[0][1], cal[1][0] * dots[1][1], cal[2][0] * dots[2][1], cal[3][0] * dots[3][1] }, + { cal[0][1] * dots[0][0], cal[1][1] * dots[1][0], cal[2][1] * dots[2][0], cal[3][1] * dots[3][0], cal[0][1] * dots[0][1], cal[1][1] * dots[1][1], cal[2][1] * dots[2][1], cal[3][1] * dots[3][1] }, + }; + + + double bb[] = { -dots[0][0], -dots[1][0], -dots[2][0], -dots[3][0], -dots[0][1], -dots[1][1], -dots[2][1], -dots[3][1] }; + + // gauß-elimination + + for (int j = 1; j < 4; j++) + { + + for (int i = 1; i < 8; i++) + { + matrix[i][j] = -matrix[i][j] + matrix[i][0]; + } + bb[j] = -bb[j] + bb[0]; + matrix[0][j] = 0; + + } + + + for (int i = 2; i < 8; i++) + { + matrix[i][2] = -matrix[i][2] / matrix[1][2] * matrix[1][1] + matrix[i][1]; + } + bb[2] = -bb[2] / matrix[1][2] * matrix[1][1] + bb[1]; + matrix[1][2] = 0; + + + for (int i = 2; i < 8; i++) + { + matrix[i][3] = -matrix[i][3] / matrix[1][3] * matrix[1][1] + matrix[i][1]; + } + bb[3] = -bb[3] / matrix[1][3] * matrix[1][1] + bb[1]; + matrix[1][3] = 0; + + + + for (int i = 3; i < 8; i++) + { + matrix[i][3] = -matrix[i][3] / matrix[2][3] * matrix[2][2] + matrix[i][2]; + } + bb[3] = -bb[3] / matrix[2][3] * matrix[2][2] + bb[2]; + matrix[2][3] = 0; +#ifdef DUMP_CALIBRATION + DBGMSG(TEXT("var57, var56, var55\n")); + DBGMSG(TEXT("%0.3f %0.3f %03.f\n"),matrix[4][6], matrix[4][5] , + matrix[4][4]); +#endif + for (int j = 5; j < 8; j++) + { + for (int i = 4; i < 8; i++) + { + matrix[i][j] = -matrix[i][j] + matrix[i][4]; + } + bb[j] = -bb[j] + bb[4]; + matrix[3][j] = 0; + } + + + for (int i = 5; i < 8; i++) + { + matrix[i][6] = -matrix[i][6] / matrix[4][6] * matrix[4][5] + matrix[i][5]; + } + + bb[6] = -bb[6] / matrix[4][6] * matrix[4][5] + bb[5]; + matrix[4][6] = 0; + + + for (int i = 5; i < 8; i++) + { + matrix[i][7] = -matrix[i][7] / matrix[4][7] * matrix[4][5] + matrix[i][5]; + } + bb[7] = -bb[7] / matrix[4][7] * matrix[4][5] + bb[5]; + matrix[4][7] = 0; + + + for (int i = 6; i < 8; i++) + { + matrix[i][7] = -matrix[i][7] / matrix[5][7] * matrix[5][6] + matrix[i][6]; + } + bb[7] = -bb[7] / matrix[5][7] * matrix[5][6] + bb[6]; + matrix[5][7] = 0; + + + + matrix[7][7] = -matrix[7][7] / matrix[6][7] * matrix[6][3] + matrix[7][3]; + bb[7] = -bb[7] / matrix[6][7] * matrix[6][3] + bb[3]; + matrix[6][7] = 0; +#ifdef DUMP_CALIBRATION + + DBGMSG(TEXT("data dump\n")); + for (int i = 0; i < 8; i++) + { + for (int j = 0; j < 8; j++) + { + DBGMSG(TEXT("%0.3f,"),matrix[i][j]); + } + DBGMSG(TEXT("\n")); + } + + DBGMSG(TEXT("bb\n")); + for (int j = 0; j < 8; j++) + { + DBGMSG(TEXT("%0.3f,"),bb[j]); + } + + DBGMSG(TEXT("\n")); +#endif + b3 = bb[7] / matrix[7][7]; + b2 = (bb[6] - (matrix[7][6] * b3 + matrix[6][6] * a3)) / matrix[5][6]; + a2 = (bb[5] - (matrix[7][5] * b3 + matrix[6][5] * a3 + matrix[5][5] * b2)) / matrix[4][5]; + c2 = (bb[4] - (matrix[7][4] * b3 + matrix[6][5] * a3 + matrix[5][4] * b2 + matrix[4][4] * a2)) / matrix[3][4]; + a3 = (bb[3] - (matrix[7][3] * b3)) / matrix[6][3]; + b1 = (bb[2] - (matrix[7][2] * b3 + matrix[6][2] * a3 + matrix[5][2] * b2 + matrix[4][2] * a2 + matrix[3][2] * c2)) / matrix[2][2]; + a1 = (bb[1] - (matrix[7][1] * b3 + matrix[6][1] * a3 + matrix[5][1] * b2 + matrix[4][1] * a2 + matrix[3][1] * c2 + matrix[2][1] * b1)) / matrix[1][1]; + c1 = (bb[0] - (matrix[7][0] * b3 + matrix[6][0] * a3 + matrix[5][0] * b2 + matrix[4][0] * a2 + matrix[3][0] * c2 + matrix[2][0] * b1 + matrix[1][0] * a1)) / matrix[0][0]; + + if ( _isnan(b3)|| _isnan(b2)|| _isnan(a2)|| _isnan(c2)|| _isnan(a3)|| _isnan(b1)|| _isnan(a1)|| _isnan(c1) ) + { + +#ifdef DUMP_CALIBRATION + DBGMSG(TEXT("calibrated BAd Result NAN!!\n")); +#endif + calibrated = false; + } + else + { +#ifdef DUMP_CALIBRATION + DBGMSG(TEXT("calibrated OK\n")); +#endif + calibrated = true; + } + return calibrated ; +} + + + +void Calibration::SaveCalibration( string filename ) +{ + ConfigFile config; + config.add("A1",a1); + config.add("B1",b1); + config.add("C1",c1); + config.add("A2",a2); + config.add("B2",b2); + config.add("C2",c2); + config.add("A3",a3); + config.add("B3",b3); + + // Construct a ConfigFile, write keys and values from given file + std::ofstream out( filename.c_str() ); + + //if( !out ) throw file_not_found( filename ); + + out << config; + out.close(); +} +bool Calibration::RestoreCalibration(string filename) +{ + try + { + ConfigFile config(filename); + a1 = config.read("A1"); + b1 = config.read("B1"); + c1 = config.read("C1"); + a2 = config.read("A2"); + b2 = config.read("B2"); + c2 = config.read("C2"); + a3 = config.read("A3"); + b3 = config.read("B3"); + calibrated = true; + return true; + } + catch(ConfigFile::file_not_found ex) + { + WCHAR msg[MAX_PATH]; + HRESULT hr = S_OK; + hr = StringCbPrintfW(msg, sizeof(msg), L"Fichier de Calibration not trouve: %S", ex.filename.c_str()); + + if (SUCCEEDED(hr)) + { + MessageBox(NULL, msg, L"Error", MB_ICONERROR); + } + return false; + } +} \ No newline at end of file -- cgit v1.1