diff options
Diffstat (limited to 'ARMFCaptureD3D/Calibration.cpp')
| -rw-r--r-- | ARMFCaptureD3D/Calibration.cpp | 254 |
1 files changed, 254 insertions, 0 deletions
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 <math.h>
+#include <assert.h>
+#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<double>("A1",a1);
+ config.add<double>("B1",b1);
+ config.add<double>("C1",c1);
+ config.add<double>("A2",a2);
+ config.add<double>("B2",b2);
+ config.add<double>("C2",c2);
+ config.add<double>("A3",a3);
+ config.add<double>("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<double>("A1");
+ b1 = config.read<double>("B1");
+ c1 = config.read<double>("C1");
+ a2 = config.read<double>("A2");
+ b2 = config.read<double>("B2");
+ c2 = config.read<double>("C2");
+ a3 = config.read<double>("A3");
+ b3 = config.read<double>("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 |