#include "StdAfx.h"
|
#include "MotorCalibrator.h"
|
#include "OpenCV/cxcore.h"
|
#include "CHCommonClasses/MacroFile.h"
|
|
CMotorCalibrator::CMotorCalibrator(int nIndex)
|
{
|
m_nIndex = nIndex;
|
Reset();
|
}
|
|
CMotorCalibrator::~CMotorCalibrator(void)
|
{
|
Reset();
|
}
|
|
void CMotorCalibrator::Reset(void)
|
{
|
m_nRows = 0;
|
m_nCols = 0;
|
m_vectorDesignPos.clear();
|
m_vectorMotorPos.clear();
|
m_vectorCaliResultF.clear();
|
m_vectorCaliResultB.clear();
|
}
|
|
void CMotorCalibrator::SetDesignPosition(const VectorDouble& vectorRows, const VectorDouble& vectorCols)
|
{
|
Reset();
|
|
if (vectorRows.size()<2 || vectorCols.size()<2) return;
|
|
m_nRows = (int)vectorRows.size();
|
m_nCols = (int)vectorCols.size();
|
|
int nCount = 0;
|
for (constVectorDoubleIt itR=vectorRows.begin(); itR!=vectorRows.end(); itR++)
|
{
|
for (constVectorDoubleIt itC=vectorCols.begin(); itC!=vectorCols.end(); itC++)
|
{
|
m_vectorDesignPos.push_back(SPosition2D(*itC, *itR));
|
m_vectorMotorPos.push_back(SPosition2D(0, 0));
|
nCount++;
|
}
|
}
|
}
|
|
void CMotorCalibrator::SetMotorPosition(int nIndex, const SPosition2D& sMotorPos)
|
{
|
if (nIndex<0 || nIndex>=(int)m_vectorMotorPos.size()) return;
|
|
m_vectorMotorPos[nIndex] = sMotorPos;
|
}
|
|
|
void CMotorCalibrator::SetMotorPosition(int nRow, int nCol, const SPosition2D& sMotorPos)
|
{
|
if (nRow<0 || nCol<0) return;
|
|
int nIndex = (nRow*m_nCols) + nCol;
|
|
if (nIndex >=(int) m_vectorMotorPos.size()) return;
|
|
m_vectorMotorPos[nIndex] = sMotorPos;
|
}
|
|
BOOL CMotorCalibrator::FCalibratePosition(double& dPosX, double& dPosY)
|
{
|
for (VectorCalibrateResultIt it=m_vectorCaliResultF.begin(); it!=m_vectorCaliResultF.end(); it++)
|
{
|
if (it->InRect(dPosX, dPosY))
|
{
|
double x = dPosX;
|
double y = dPosY;
|
|
double div = (it->dCoefficient[6]*x)+(it->dCoefficient[7]*y)+it->dCoefficient[8];
|
|
dPosX = ((it->dCoefficient[0]*x)+(it->dCoefficient[1]*y)+it->dCoefficient[2]) / div;
|
dPosY = ((it->dCoefficient[3]*x)+(it->dCoefficient[4]*y)+it->dCoefficient[5]) / div;
|
|
return TRUE;
|
}
|
}
|
|
return FALSE;
|
}
|
|
BOOL CMotorCalibrator::BCalibratePosition(double& dPosX, double& dPosY)
|
{
|
for (VectorCalibrateResultIt it=m_vectorCaliResultB.begin(); it!=m_vectorCaliResultB.end(); it++)
|
{
|
if (it->InRect(dPosX, dPosY))
|
{
|
double x = dPosX;
|
double y = dPosY;
|
|
double div = (it->dCoefficient[6]*x)+(it->dCoefficient[7]*y)+it->dCoefficient[8];
|
|
dPosX = ((it->dCoefficient[0]*x)+(it->dCoefficient[1]*y)+it->dCoefficient[2]) / div;
|
dPosY = ((it->dCoefficient[3]*x)+(it->dCoefficient[4]*y)+it->dCoefficient[5]) / div;
|
|
return TRUE;
|
}
|
}
|
|
return FALSE;
|
}
|
|
|
SPosition2D GetStartPosition(const VectorPosition2D& vectorPos)
|
{
|
SPosition2D sPos(DBL_MAX, DBL_MAX);
|
for (constVectorPosition2DIt it=vectorPos.begin(); it!=vectorPos.end(); it++)
|
{
|
if (sPos.dPositionX > it->dPositionX)
|
{
|
sPos.dPositionX = it->dPositionX;
|
}
|
|
if (sPos.dPositionY > it->dPositionY)
|
{
|
sPos.dPositionY = it->dPositionY;
|
}
|
}
|
|
return sPos;
|
}
|
|
|
SPosition2D GetEndPosition(const VectorPosition2D& vectorPos)
|
{
|
SPosition2D sPos(DBL_MIN, DBL_MIN);
|
for (constVectorPosition2DIt it=vectorPos.begin(); it!=vectorPos.end(); it++)
|
{
|
if (sPos.dPositionX < it->dPositionX)
|
{
|
sPos.dPositionX = it->dPositionX;
|
}
|
|
if (sPos.dPositionY < it->dPositionY)
|
{
|
sPos.dPositionY = it->dPositionY;
|
}
|
}
|
|
return sPos;
|
}
|
|
|
|
int CMotorCalibrator::CalculateCoefficient()
|
{
|
if (m_nRows<2 || m_nCols<2) return 1;
|
|
int nResultRows = m_nRows-1;
|
int nResultCols = m_nCols-1;
|
|
m_vectorCaliResultF.clear();
|
m_vectorCaliResultF.resize(nResultRows*nResultCols);
|
|
m_vectorCaliResultB.clear();
|
m_vectorCaliResultB.resize(nResultRows*nResultCols);
|
|
int temp_idx = 0, nFSuccessCount = 0, nFFailCount = 0, nBSuccessCount = 0, nBFailCount = 0;
|
VectorPosition2D vectorSrc, vectorDst;
|
|
for (int nIndex = 0; nIndex<(int)m_vectorCaliResultF.size(); nIndex++)
|
{
|
int nRow = nIndex / nResultCols;
|
int nCol = nIndex % nResultCols;
|
|
vectorSrc.clear();
|
vectorDst.clear();
|
|
// get position
|
temp_idx = (nRow*m_nCols)+nCol;
|
vectorSrc.push_back(m_vectorMotorPos[temp_idx]);
|
vectorDst.push_back(m_vectorDesignPos[temp_idx]);
|
|
temp_idx++;
|
vectorSrc.push_back(m_vectorMotorPos[temp_idx]);
|
vectorDst.push_back(m_vectorDesignPos[temp_idx]);
|
|
temp_idx = ((nRow+1)*m_nCols)+nCol;
|
vectorSrc.push_back(m_vectorMotorPos[temp_idx]);
|
vectorDst.push_back(m_vectorDesignPos[temp_idx]);
|
|
temp_idx++;
|
vectorSrc.push_back(m_vectorMotorPos[temp_idx]);
|
vectorDst.push_back(m_vectorDesignPos[temp_idx]);
|
|
// foreward calculate coeff
|
if (CalculatePerspectiveTransform(vectorSrc, vectorDst, m_vectorCaliResultF[nIndex])!=0)
|
{
|
m_vectorCaliResultF[nIndex].vectorRegion = vectorSrc;
|
m_vectorCaliResultF[nIndex].sStartPosition = GetStartPosition(vectorSrc);
|
m_vectorCaliResultF[nIndex].sEndPosition = GetEndPosition(vectorSrc);
|
nFSuccessCount++;
|
}
|
else
|
{
|
nFFailCount++;
|
}
|
|
// backward calculate coeff
|
if (CalculatePerspectiveTransform(vectorDst, vectorSrc, m_vectorCaliResultB[nIndex])!=0)
|
{
|
m_vectorCaliResultB[nIndex].vectorRegion = vectorDst;
|
m_vectorCaliResultB[nIndex].sStartPosition = GetStartPosition(vectorDst);
|
m_vectorCaliResultB[nIndex].sEndPosition = GetEndPosition(vectorDst);
|
nBSuccessCount++;
|
}
|
else
|
{
|
nBFailCount++;
|
}
|
}
|
|
return nFFailCount;
|
}
|
|
int CMotorCalibrator::CalculatePerspectiveTransform(const VectorPosition2D& vectorSrc, const VectorPosition2D& vectorDst, SCalibrateResult& result)
|
{
|
if (vectorSrc.size()!=4 || vectorDst.size()!=4) return 0;
|
|
double a[8][8], b[8];
|
for( int i = 0; i < 4; ++i )
|
{
|
a[i][0] = a[i+4][3] = vectorSrc[i].dPositionX;
|
a[i][1] = a[i+4][4] = vectorSrc[i].dPositionY;
|
a[i][2] = a[i+4][5] = 1;
|
a[i][3] = a[i][4] = a[i][5] = a[i+4][0] = a[i+4][1] = a[i+4][2] = 0.0;
|
a[i][6] = -vectorSrc[i].dPositionX * vectorDst[i].dPositionX;
|
a[i][7] = -vectorSrc[i].dPositionY * vectorDst[i].dPositionX;
|
a[i+4][6] = -vectorSrc[i].dPositionX * vectorDst[i].dPositionY;
|
a[i+4][7] = -vectorSrc[i].dPositionY * vectorDst[i].dPositionY;
|
b[i] = vectorDst[i].dPositionX;
|
b[i+4] = vectorDst[i].dPositionY;
|
}
|
|
CvMat A = cvMat(8, 8, CV_64FC1, a);
|
CvMat B = cvMat(8, 1, CV_64FC1, b);
|
CvMat X = cvMat(8, 1, CV_64FC1, result.dCoefficient);
|
|
int nValue = cvSolve(&A, &B, &X, CV_SVD);
|
|
result.dCoefficient[8] = 1.;
|
|
return nValue;
|
}
|
|
BOOL CMotorCalibrator::LoadDesignPosition(const CString& strFilename)
|
{
|
int nRows=0, nCols=0;
|
if (!LoadPosition(strFilename, m_vectorDesignPos, nRows, nCols))
|
{
|
return FALSE;
|
}
|
|
m_nRows = nRows;
|
m_nCols = nCols;
|
|
return TRUE;
|
}
|
|
BOOL CMotorCalibrator::SaveDesignPosition(const CString& strFilename)
|
{
|
return LoadPosition(strFilename, m_vectorDesignPos, m_nRows, m_nCols);
|
}
|
|
BOOL CMotorCalibrator::LoadMotorPosition(const CString& strFilename)
|
{
|
int nRows=0, nCols=0;
|
if (!LoadPosition(strFilename, m_vectorMotorPos, nRows, nCols))
|
{
|
return FALSE;
|
}
|
|
m_nRows = nRows;
|
m_nCols = nCols;
|
|
return TRUE;
|
}
|
|
BOOL CMotorCalibrator::SaveMotorPosition(const CString& strFilename)
|
{
|
return SavePosition(strFilename, m_vectorMotorPos, m_nRows, m_nCols);
|
}
|
|
BOOL CMotorCalibrator::LoadCalibrateResultF(const CString& strFilename)
|
{
|
return LoadCalibrateResult(strFilename, m_vectorCaliResultF);
|
}
|
|
BOOL CMotorCalibrator::SaveCalibrateResultF(const CString& strFilename)
|
{
|
return SaveCalibrateResult(strFilename, m_vectorCaliResultF);
|
}
|
|
BOOL CMotorCalibrator::LoadCalibrateResultB(const CString& strFilename)
|
{
|
return LoadCalibrateResult(strFilename, m_vectorCaliResultB);
|
}
|
|
BOOL CMotorCalibrator::SaveCalibrateResultB(const CString& strFilename)
|
{
|
return SaveCalibrateResult(strFilename, m_vectorCaliResultB);
|
}
|
|
BOOL CMotorCalibrator::LoadPosition(const CString& strFilename, VectorPosition2D& vectorPos, int& nRows, int& nCols)
|
{
|
FILE *fp = NULL;
|
|
_tfopen_s(&fp, strFilename, _T("r"));
|
|
if (fp==NULL) return FALSE;
|
|
vectorPos.clear();
|
|
TCHAR strValue[200];
|
|
_ftscanf_s(fp, _T("%s,"), strValue, 5);
|
_ftscanf_s(fp, _T("%d\n"), &nRows);
|
|
_ftscanf_s(fp, _T("%s,"), strValue, 5);
|
_ftscanf_s(fp, _T("%d\n"), &nCols);
|
|
SPosition2D pos;
|
for (int i=0; i<nRows*nCols; i++)
|
{
|
pos.Reset();
|
_ftscanf_s(fp, _T("%lf,%lf\n"), &pos.dPositionX, &pos.dPositionY);
|
vectorPos.push_back(pos);
|
}
|
|
fclose(fp);
|
|
return TRUE;
|
|
/*
|
CMacroFile macroFile;
|
if (!macroFile.Read(strFilename))
|
{
|
return FALSE;
|
}
|
|
vectorPos.clear();
|
|
macroFile.GetItem(_T("POSITION_COUNT_ROW"), nRows, 0);
|
macroFile.GetItem(_T("POSITION_COUNT_COL"), nCols, 0);
|
|
CString strItem = _T("");
|
|
SPosition2D pos;
|
for (int nIndex=0; nIndex<nRows*nCols; nIndex++)
|
{
|
pos.Reset();
|
|
strItem.Format(_T("POSITION_X_%02d"), nIndex);
|
macroFile.GetItem(strItem, pos.dPositionX, 0.0);
|
|
strItem.Format(_T("POSITION_Y_%02d"), nIndex);
|
macroFile.GetItem(strItem, pos.dPositionY, 0.0);
|
|
vectorPos.push_back(pos);
|
}
|
|
return TRUE;
|
*/
|
}
|
|
BOOL CMotorCalibrator::SavePosition(const CString& strFilename, const VectorPosition2D& vectorPos, int nRows, int nCols)
|
{
|
if (int(vectorPos.size())!=(nRows*nCols)) return FALSE;
|
|
FILE *fp = NULL;
|
|
_tfopen_s(&fp, strFilename, _T("w"));
|
|
if (fp==NULL) return FALSE;
|
|
_ftprintf_s(fp, _T("ROW_COUNT,%d\n"), nRows);
|
_ftprintf_s(fp, _T("COL_COUNT,%d\n"), nCols);
|
|
for (constVectorPosition2DIt it=vectorPos.begin(); it!=vectorPos.end(); it++)
|
{
|
_ftprintf_s(fp, _T("%lf,%lf\n"), it->dPositionX, it->dPositionY);
|
}
|
|
fclose(fp);
|
|
return TRUE;
|
|
/*
|
CMacroFile macroFile;
|
|
int nValue = 0;
|
CString strItem = _T("");
|
|
macroFile.SetItem(_T("POSITION_COUNT_ROW"), nRows);
|
macroFile.SetItem(_T("POSITION_COUNT_COL"), nCols);
|
|
int nIndex = 0;
|
for (constVectorPosition2DIt it=vectorPos.begin(); it!=vectorPos.end(); it++)
|
{
|
strItem.Format(_T("POSITION_X_%02d"), nIndex);
|
macroFile.SetItem(strItem, it->dPositionX);
|
|
strItem.Format(_T("POSITION_Y_%02d"), nIndex);
|
macroFile.SetItem(strItem, it->dPositionY);
|
|
nIndex++;
|
}
|
|
return macroFile.Write(strFilename);
|
*/
|
}
|
|
BOOL CMotorCalibrator::LoadCalibrateResult(const CString& strFilename, VectorCalibrateResult& vectorResult)
|
{
|
CMacroFile macroFile;
|
if (!macroFile.Read(strFilename))
|
{
|
return FALSE;
|
}
|
|
vectorResult.clear();
|
|
int nValue = 0;
|
CString strItem = _T("");
|
SCalibrateResult calResult;
|
macroFile.GetItem(_T("CALIBRATE_COUNT"), nValue, 0);
|
|
for (int nIndex=0; nIndex<nValue; nIndex++)
|
{
|
calResult.Reset();
|
|
// start pos
|
strItem.Format(_T("CALI_START_POS_X_%02d"), nIndex);
|
macroFile.GetItem(strItem, calResult.sStartPosition.dPositionX, 0.0);
|
strItem.Format(_T("CALI_START_POS_Y_%02d"), nIndex);
|
macroFile.GetItem(strItem, calResult.sStartPosition.dPositionY, 0.0);
|
|
// end pos
|
strItem.Format(_T("CALI_END_POS_X_%02d"), nIndex);
|
macroFile.GetItem(strItem, calResult.sEndPosition.dPositionX, 0.0);
|
strItem.Format(_T("CALI_END_POS_Y_%02d"), nIndex);
|
macroFile.GetItem(strItem, calResult.sEndPosition.dPositionY, 0.0);
|
|
// cali coefficient
|
for (int j=0; j<9; j++)
|
{
|
strItem.Format(_T("CALI_COEFFICIENT_%02d_%02d"), nIndex, j);
|
macroFile.GetItem(strItem, calResult.dCoefficient[j], 0.0);
|
}
|
|
// vector region
|
for (int j=0; j<4; j++)
|
{
|
SPosition2D pos;
|
strItem.Format(_T("CALI_REGION_X_%02d_%02d"), nIndex, j);
|
macroFile.GetItem(strItem, pos.dPositionX, 0.0);
|
|
strItem.Format(_T("CALI_REGION_Y_%02d_%02d"), nIndex, j);
|
macroFile.GetItem(strItem, pos.dPositionY, 0.0);
|
calResult.vectorRegion.push_back(pos);
|
}
|
|
vectorResult.push_back(calResult);
|
}
|
|
return TRUE;
|
}
|
|
BOOL CMotorCalibrator::SaveCalibrateResult(const CString& strFilename, const VectorCalibrateResult& vectorResult)
|
{
|
CMacroFile macroFile;
|
|
int nValue = 0;
|
CString strItem = _T("");
|
|
nValue = (int)vectorResult.size();
|
macroFile.SetItem(_T("CALIBRATE_COUNT"), nValue);
|
|
int nIndex = 0;
|
for (constVectorCalibrateResultIt it=vectorResult.begin(); it!=vectorResult.end(); it++)
|
{
|
// start pos
|
strItem.Format(_T("CALI_FOR_START_POS_X_%02d"), nIndex);
|
macroFile.SetItem(strItem, it->sStartPosition.dPositionX);
|
strItem.Format(_T("CALI_FOR_START_POS_Y_%02d"), nIndex);
|
macroFile.SetItem(strItem, it->sStartPosition.dPositionY);
|
|
// end pos
|
strItem.Format(_T("CALI_FOR_END_POS_X_%02d"), nIndex);
|
macroFile.SetItem(strItem, it->sEndPosition.dPositionX);
|
strItem.Format(_T("CALI_FOR_END_POS_Y_%02d"), nIndex);
|
macroFile.SetItem(strItem, it->sEndPosition.dPositionY);
|
|
// cali coefficient
|
for (int j=0; j<9; j++)
|
{
|
strItem.Format(_T("CALI_FOR_COEFFICIENT_%02d_%02d"), nIndex, j);
|
macroFile.SetItem(strItem, it->dCoefficient[j]);
|
}
|
|
// vector region
|
for (int j=0; j<4; j++)
|
{
|
strItem.Format(_T("CALI_FOR_REGION_X_%02d_%02d"), nIndex, j);
|
macroFile.SetItem(strItem, it->vectorRegion[j].dPositionX);
|
|
strItem.Format(_T("CALI_FOR_REGION_Y_%02d_%02d"), nIndex, j);
|
macroFile.SetItem(strItem, it->vectorRegion[j].dPositionY);
|
}
|
|
nIndex++;
|
}
|
|
return macroFile.Write(strFilename);
|
}
|
