void C4TableGraph::Reset(TimeType iToTime) {
// flush buffer
if (!!szDumpFile) DumpToFile(szDumpFile, fWrapped);
// reset stuff
iInitialStartTime = iTime = iToTime;
fWrapped = false;
iBackLogPos = 0;
*pValues = 0;
}
//--------------------------------------------------------------------------------------
BOOL WriteVal(
DWORD Ptr,
DWORD Size,
DWORD Val_1, DWORD Val_2, DWORD Val_3)
{
ZeroMemory(m_pAlignedData, m_dwAlignedDataSize);
memcpy(m_pAlignedData, m_pData, m_dwDataSize);
if (m_bNoisy)
{
DbgMsg(
__FILE__, __LINE__,
__FUNCTION__"(): Probing value 0x%.2x 0x%.4x 0x%.8x\n",
(UCHAR)Val_1, (USHORT)Val_2, Val_3
);
}
// zero-bytes stuff
switch (Size)
{
case 1:
*(PUCHAR)((PUCHAR)m_pAlignedData + Ptr) = (UCHAR)Val_1;
break;
case 2:
*(PUSHORT)((PUCHAR)m_pAlignedData + Ptr) = (USHORT)Val_2;
break;
case 4:
*(PULONG)((PUCHAR)m_pAlignedData + Ptr) = Val_3;
break;
}
POTF_TABLE_HEADER Table = NULL;
if (m_dwFontType == FONT_TYPE_OTF || m_dwFontType == FONT_TYPE_TTF)
{
Table = OTF_TableByOffset(m_pAlignedData, Ptr);
}
if (Table)
{
// fix OTF/TTF table checksum
ULONG Offset = htonl(Table->offset), Length = htonl(Table->length);
ULONG Sum = OTF_CalcTableChecksum((ULONG *)((PUCHAR)m_pAlignedData + Offset), Length);
Table->checkSum = htonl(Sum);
}
// dump output file
if (DumpToFile(m_TmpFontPath, m_pAlignedData, m_dwDataSize))
{
FuzzIteration();
m_dwCasesProcessed++;
return TRUE;
}
return FALSE;
}
void C4TableGraph::RecordValue(ValueType iValue) {
// rec value
pValues[iBackLogPos] = iValue;
// calc time
++iTime;
if (++iBackLogPos >= iBackLogLength) {
// create dump before overwriting last buffer
if (!!szDumpFile) DumpToFile(szDumpFile, fWrapped);
// restart buffer
fWrapped = true;
iBackLogPos = 0;
}
}
void StopRun(char reason[255])
{
isRunning=false;
DumpToFile();
//CaptureScene(); don't know why this doesn't work
printf("Run Stopped. (Reason: %s)\n",reason);
char logFile[255];
sprintf(logFile,"%s\\%s\\%s.log",_getcwd(NULL,0),gRunBaseName,gRunBaseName);
LogRunStats(logFile);
LogExperimentStats(logFile);
WritePrivateProfileString("experiment","stop_reason",reason,logFile);
SetThreadExecutionState(ES_CONTINUOUS); // save electricity for the uni
if (bQuitWhenDone) exit(0);
UpdateHUD();
}
BOOL CED6AsDecompiler::DecompilerFile(LPWSTR pszAsFileName, LPWSTR pszOutput /* = NULL */)
{
LONG Status;
WCHAR szOutput[MAX_PATH];
CFileDisk file;
Reset();
if (!file.Open(pszAsFileName))
return FALSE;
m_AsInfo.BufferSize = file.GetSize();
m_AsInfo.pbAsBuffer = (PBYTE)m_mem.Alloc(m_AsInfo.BufferSize);
if (m_AsInfo.pbAsBuffer == NULL)
return FALSE;
if (!file.Read(m_AsInfo.pbAsBuffer))
return FALSE;
Status = DecompilerFile(&m_AsInfo);
if (Status != ASDECL_ERROR_UNKNOWN_INSTRUCTION)
AS_IF_FAIL_RETURN(Status);
if (pszOutput == NULL)
{
LPWSTR pszExtension;
pszExtension = findextw(pszAsFileName);
if (!StrICompareW(pszExtension, WSTRING(NAME_DEFAULT_EXTENSION)))
pszExtension += countof(WSTRING(NAME_DEFAULT_EXTENSION)) - 1;
lstrcpyW(szOutput, pszAsFileName);
pszExtension = szOutput + (pszExtension - pszAsFileName);
lstrcpyW(pszExtension, WSTRING(NAME_DEFAULT_EXTENSION));
pszOutput = szOutput;
}
Status = DumpToFile(&m_AsInfo, pszAsFileName, pszOutput);
return Status;
}
// Finds the closest KeyFrame in a given region
KeyFrame* MapMakerBase::ClosestKeyFrame(KeyFrame& kf, KeyFrameRegion region, bool bSameCamName)
{
if (region == KF_ONLY_SELF) // If we're only looking in our MKF neighborhood
ROS_ASSERT(!bSameCamName); // Make sure we are not just looking for same cam name otherwise we'll find nothing
double dClosestDist = std::numeric_limits<double>::max();
KeyFrame* pClosestKF = NULL;
MultiKeyFrame& parent = *kf.mpParent;
if (region == KF_ONLY_SELF) // Only search through parent's keyframes
{
for (KeyFramePtrMap::iterator it = parent.mmpKeyFrames.begin(); it != parent.mmpKeyFrames.end(); it++)
{
KeyFrame& currentKF = *(it->second);
if (¤tKF == &kf)
continue;
double dDist = kf.Distance(currentKF);
if (dDist < dClosestDist)
{
dClosestDist = dDist;
pClosestKF = ¤tKF;
}
}
}
else // Otherwise search all keyframes in the map
{
for (MultiKeyFramePtrList::iterator it = mMap.mlpMultiKeyFrames.begin(); it != mMap.mlpMultiKeyFrames.end(); ++it)
{
MultiKeyFrame& mkf = *(*it);
if (&mkf == &parent && region == KF_ONLY_OTHER)
continue;
for (KeyFramePtrMap::iterator jit = mkf.mmpKeyFrames.begin(); jit != mkf.mmpKeyFrames.end(); ++jit)
{
KeyFrame& currentKF = *(jit->second);
if (¤tKF == &kf)
continue;
if (bSameCamName && currentKF.mCamName != kf.mCamName)
continue;
double dDist = kf.Distance(currentKF);
if (dDist < dClosestDist)
{
dClosestDist = dDist;
pClosestKF = ¤tKF;
}
}
}
}
if (!bSameCamName && pClosestKF == NULL)
{
// Dump the cameras and map data
DumpToFile("fail_map.dat");
ROS_ASSERT(pClosestKF != NULL);
}
return pClosestKF;
}
//--------------------------------------------------------------------------------------
int _tmain(int argc, _TCHAR* argv[])
{
m_hInstance = (HINSTANCE)GetModuleHandle(NULL);
if (argc >= 3)
{
m_lpFontPath = argv[2];
m_lpFontName = argv[1];
printf(__FUNCTION__"(): Using external font %ws \"%ws\"\n", m_lpFontName, m_lpFontPath);
}
else
{
printf("USAGE: MsFontsFuzz.exe <font_name> <font_file> [options]\n");
goto end;
}
_stprintf_s(m_TmpFontPath, _T("__TMP__%s"), _tGetNameFromFullPath(m_lpFontPath));
DbgMsg(__FILE__, __LINE__, "[+] Temporary font file is \"%ws\"\n", m_TmpFontPath);
if (_tcslen(m_TmpFontPath) >= 4)
{
_tcslwr(m_TmpFontPath + _tcslen(m_TmpFontPath) - 4);
if (!_tcscmp(m_TmpFontPath + _tcslen(m_TmpFontPath) - 4, _T(".otf")))
{
m_dwFontType = FONT_TYPE_OTF;
DbgMsg(__FILE__, __LINE__, "[+] Font type is .OTF\n");
}
else if (!_tcscmp(m_TmpFontPath + _tcslen(m_TmpFontPath) - 4, _T(".ttf")))
{
m_dwFontType = FONT_TYPE_TTF;
DbgMsg(__FILE__, __LINE__, "[+] Font type is .TTF\n");
}
}
RemoveFontResource(m_TmpFontPath);
#ifdef USE_BOADCAST_MESSAGES
SendMessage(HWND_BROADCAST, WM_FONTCHANGE, 0, 0);
#endif
char ch = 0;
memset(m_szTable, '.', sizeof(m_szTable) - 1);
for (int i = 0; i < sizeof(m_szTable); i++)
{
if (i != 0 && i % 16 == 0)
{
m_szTable[i] = '\n';
continue;
}
if (ch >= 0x20)
{
m_szTable[i] = ch;
}
if (ch == 0x7f)
{
m_szTable[i] = 0;
break;
}
ch += 1;
}
if (argc > 3)
{
// enumerate additional parameters
for (int i = 3; i < argc; i++)
{
if (!_tcscmp(argv[i], _T("--test")))
{
// single launch mode
m_bTest = TRUE;
}
else if (!_tcscmp(argv[i], _T("--resume")))
{
// resume fuzzing in the new process
m_bResume = TRUE;
}
else if (!_tcscmp(argv[i], _T("--noisy")))
{
// show lot of output information
m_bNoisy = TRUE;
}
else if (!_tcscmp(argv[i], _T("--text")) && argc - i > 1)
{
#ifdef UNICODE
// use caller-specified text for display
WideCharToMultiByte(
CP_ACP, 0,
argv[i + 1],
-1,
m_szTable,
sizeof(m_szTable) - 1,
NULL, NULL
);
#else
strcpy_s(m_szTable, argv[i + 1]);
#endif
i++;
}
else if (!_tcscmp(argv[i], _T("--fix-crcs")))
{
// fix incorrect checksums for the original font file
m_bFixCrcs = TRUE;
}
else if (argc - i > 1 && argv[i][0] == '-')
{
/**
* Process data generation options.
*/
LPCTSTR lpParam = argv[i] + 1;
DWORD dwValue = 0;
BOOL bFound = FALSE;
if (!StrToIntEx(argv[i + 1], STIF_SUPPORT_HEX, (int *)&dwValue))
{
DbgMsg(__FILE__, __LINE__, "[!] ERROR: Invalid value for parameter \"%ws\"\n", argv[i]);
continue;
}
for (int i_n = 0; i_n < sizeof(m_Params) / sizeof(ENGINE_PARAM); i_n++)
{
// search parameter by name
if (!_tcscmp(m_Params[i_n].lpName, lpParam))
{
*(m_Params[i_n].pdwValue) = dwValue;
bFound = TRUE;
break;
}
}
if (!bFound)
{
DbgMsg(__FILE__, __LINE__, "[!] ERROR: Unknown parameter \"%ws\"\n", argv[i]);
}
i++;
}
}
}
DbgInit(LOG_FILE_NAME);
// check block size and range
if (BLOCK_SIZE == 1)
{
if (BLOCK_RANGE_START >= 0xFF)
{
DbgMsg(__FILE__, __LINE__, __FUNCTION__"(): Invalid BLOCK_RANGE_START value (it must be <0xFF)\n");
goto end;
}
if (BLOCK_RANGE_END > 0xFF)
{
DbgMsg(__FILE__, __LINE__, __FUNCTION__"(): Invalid BLOCK_RANGE_END value (it must be <=0xFF)\n");
goto end;
}
}
else if (BLOCK_SIZE == 2)
{
if (BLOCK_RANGE_START >= 0xFFFF)
{
DbgMsg(__FILE__, __LINE__, __FUNCTION__"(): Invalid BLOCK_RANGE_START value (it must be <0xFFFF)\n");
goto end;
}
if (BLOCK_RANGE_END > 0xFFFF)
{
DbgMsg(__FILE__, __LINE__, __FUNCTION__"(): Invalid BLOCK_RANGE_END value (it must be <=0xFFFF)\n");
goto end;
}
}
else if (BLOCK_SIZE == 4)
{
if (BLOCK_RANGE_START >= 0xFFFFFFFF)
{
DbgMsg(__FILE__, __LINE__, __FUNCTION__"(): Invalid BLOCK_RANGE_START value (it must be <0xFFFFFFFF)\n");
goto end;
}
if (BLOCK_RANGE_END > 0xFFFFFFFF)
{
DbgMsg(__FILE__, __LINE__, __FUNCTION__"(): Invalid BLOCK_RANGE_END value (it must be <=0xFFFFFFFF)\n");
goto end;
}
}
else
{
DbgMsg(__FILE__, __LINE__, __FUNCTION__"(): Invalid BLOCK_SIZE value (it must be 1, 2 or 4)\n");
goto end;
}
// check step size
if (BLOCK_RANGE_N > BLOCK_RANGE_END)
{
DbgMsg(__FILE__, __LINE__, __FUNCTION__"(): Invalid BLOCK_RANGE_N value (it must be <=BLOCK_RANGE_END)\n");
goto end;
}
WNDCLASSEX wcex;
ZeroMemory(&wcex, sizeof(wcex));
wcex.cbSize = sizeof(WNDCLASSEX);
wcex.style = CS_HREDRAW | CS_VREDRAW;
wcex.lpfnWndProc = WndProc;
wcex.hInstance = m_hInstance;
wcex.lpszClassName = _T(WND_CLASS);
wcex.hbrBackground = (HBRUSH)(COLOR_WINDOW + 1);
m_hWndEvent = CreateEvent(NULL, TRUE, FALSE, NULL);
if (m_hWndEvent == NULL)
{
DbgMsg(__FILE__, __LINE__, "CreateEvent() ERROR %d\n", GetLastError());
goto end;
}
// register window class
if (RegisterClassEx(&wcex) == NULL)
{
DbgMsg(__FILE__, __LINE__, "RegisterClassEx() ERROR %d\n", GetLastError());
goto end;
}
// init random number generator
init_genrand(GetTickCount());
SetUnhandledExceptionFilter(UnhandledExceptionError);
// read input file
if (ReadFromFile(m_lpFontPath, &m_pData, &m_dwDataSize))
{
if (FILE_RANGE_START >= m_dwDataSize)
{
DbgMsg(__FILE__, __LINE__, __FUNCTION__"(): Invalid FILE_RANGE_START value (it must be <=FILE_SIZE)\n");
M_FREE(m_pData);
return -1;
}
if (FILE_RANGE_END > m_dwDataSize)
{
DbgMsg(__FILE__, __LINE__, __FUNCTION__"(): Invalid FILE_RANGE_END value (it must be <FILE_SIZE)\n");
M_FREE(m_pData);
return -1;
}
if (FILE_RANGE_END == 0)
{
FILE_RANGE_END = m_dwDataSize;
}
if (FILE_RANGE_START >= FILE_RANGE_END)
{
DbgMsg(__FILE__, __LINE__, __FUNCTION__"(): Invalid FILE_RANGE_START/FILE_RANGE_END values\n");
M_FREE(m_pData);
return -1;
}
DbgMsg(__FILE__, __LINE__, "[+] %d bytes readed from \"%ws\"\n", m_dwDataSize, m_lpFontPath);
if (!m_bResume && (m_dwFontType == FONT_TYPE_OTF || m_dwFontType == FONT_TYPE_TTF))
{
OTF_TableByOffset(m_pData, (ULONG)-1);
}
if (m_bFixCrcs)
{
// write fixed checksums into the original file
if (DumpToFile(m_lpFontPath, m_pData, m_dwDataSize))
{
DbgMsg(__FILE__, __LINE__, "[+] Checksums has been fixed for font file \"%ws\"\n", m_lpFontPath);
}
}
else if (m_bTest)
{
// single run with the unchanged font file
if (DumpToFile(m_TmpFontPath, m_pData, m_dwDataSize))
{
FuzzIteration();
}
}
else
{
DbgMsg(__FILE__, __LINE__, "[+] Fuzzing params:\n\n");
// print parameters values
for (int i_n = 0; i_n < sizeof(m_Params) / sizeof(ENGINE_PARAM); i_n++)
{
DbgMsg(__FILE__, __LINE__, " %20ws = 0x%.8x\n", m_Params[i_n].lpName, *(m_Params[i_n].pdwValue));
}
DbgMsg(__FILE__, __LINE__, "\n");
DbgMsg(__FILE__, __LINE__, "[+] Processing cases...\n\n");
// align buffer size by block size
m_dwAlignedDataSize = XALIGN_UP(m_dwDataSize, BLOCK_SIZE);
// allocate output buffer
if (m_pAlignedData = M_ALLOC(m_dwAlignedDataSize))
{
char *lpszBigBuff = (char *)M_ALLOC(BIG_BUFFER_LENGTH);
if (lpszBigBuff)
{
FillMemory(lpszBigBuff, BIG_BUFFER_LENGTH, 'A');
}
PVOID pBigData = M_ALLOC(m_dwDataSize + BIG_BUFFER_LENGTH);
// for each byte/word/dword of input file...
for (DWORD i = FILE_RANGE_START; i < FILE_RANGE_END; i += BLOCK_SIZE)
{
DbgMsg(__FILE__, __LINE__, "Offset=0x%.8x TotalSize=0x%.8x File=%.8x\n", i, m_dwDataSize, m_dwCasesProcessed);
POTF_TABLE_HEADER Table = NULL;
if (m_dwFontType == FONT_TYPE_OTF || m_dwFontType == FONT_TYPE_TTF)
{
Table = OTF_TableByOffset(m_pData, i);
if (Table == NULL)
{
// skip OTF/TTF data outside the tables
continue;
}
}
if (BLOCK_RANGE_N > 0)
{
// fuze each value with the step size == BLOCK_RANGE_N
for (DWORD n = XALIGN_DOWN(BLOCK_RANGE_START, BLOCK_RANGE_N);
n < XALIGN_DOWN(BLOCK_RANGE_END, BLOCK_RANGE_N);
n += BLOCK_RANGE_N)
{
// write plain value
WriteVal(i, BLOCK_SIZE, n, n, n);
if (BLOCK_SIZE > 1)
{
// write randomized value
WriteVal(i, BLOCK_SIZE,
n,
n + getrand(0, BLOCK_RANGE_N - 1),
n + getrand(0, BLOCK_RANGE_N - 1)
);
}
}
}
// zero-bytes stuff
WriteVal(i, BLOCK_SIZE, 0x00, 0x0000, 0x00000000);
// integer overflow stuff
WriteVal(i, BLOCK_SIZE, 0xFF, 0xFFFF, 0xFFFFFFFF);
// invalid user-mode pointers
WriteVal(i, BLOCK_SIZE, 0x0D, 0x0D0D, 0x0D0D0D0D);
if (lpszBigBuff && pBigData)
{
/**
* Write big ASCI data after the each byte.
*/
memcpy(pBigData, m_pData, i);
memcpy((PUCHAR)pBigData + i, lpszBigBuff, BIG_BUFFER_LENGTH);
memcpy((PUCHAR)pBigData + i + BIG_BUFFER_LENGTH, (PUCHAR)m_pData + i, m_dwDataSize - i);
if (m_dwFontType == FONT_TYPE_OTF || m_dwFontType == FONT_TYPE_TTF)
{
POTF_FILE_HEADER Hdr = (POTF_FILE_HEADER)pBigData;
POTF_TABLE_HEADER Table = (POTF_TABLE_HEADER)((PUCHAR)pBigData + sizeof(OTF_FILE_HEADER));
POTF_TABLE_HEADER CurrentTable = NULL;
for (USHORT t = 0; t < htons(Hdr->numTables); t++)
{
ULONG Offset = htonl(Table->offset), Length = htonl(Table->length);
if (i >= Offset &&
i < Offset + Length)
{
// fix OTF/TTF table checksum and length
ULONG Sum = OTF_CalcTableChecksum((ULONG *)((PUCHAR)pBigData + Offset), Length);
Table->checkSum = htonl(Sum);
Table->length = htonl(Length);
CurrentTable = Table;
break;
}
Table += 1;
}
if (CurrentTable)
{
Table = (POTF_TABLE_HEADER)((PUCHAR)pBigData + sizeof(OTF_FILE_HEADER));
for (USHORT t = 0; t < htons(Hdr->numTables); t++)
{
ULONG Offset = htonl(Table->offset), Length = htonl(Table->length);
if (Offset > htonl(CurrentTable->offset))
{
// fix offsets of the other tables
Table->offset = htonl(Offset + BIG_BUFFER_LENGTH);
}
Table += 1;
}
}
}
if (DumpToFile(m_TmpFontPath, pBigData, m_dwDataSize + BIG_BUFFER_LENGTH))
{
FuzzIteration();
m_dwCasesProcessed++;
}
}
if (m_dwCasesProcessed > MAX_CASES_PER_PROCESS)
{
TCHAR szSelf[MAX_PATH], szCmdLine[MAX_PATH];
GetModuleFileName(GetModuleHandle(NULL), szSelf, MAX_PATH);
_stprintf_s(
szCmdLine, MAX_PATH,
_T("\"%s\" \"%s\" \"%s\" -BLOCK_SIZE 0x%x -BLOCK_RANGE_START 0x%x -BLOCK_RANGE_END 0x%x -BLOCK_RANGE_N 0x%x -FILE_RANGE_START 0x%x --resume Y"),
szSelf, m_lpFontName, m_lpFontPath, BLOCK_SIZE, BLOCK_RANGE_START, BLOCK_RANGE_END, BLOCK_RANGE_N, i
);
if (m_bNoisy)
{
_tcscat(szCmdLine, _T(" --noisy Y"));
}
STARTUPINFO si;
PROCESS_INFORMATION pi;
ZeroMemory(&pi, sizeof(pi));
ZeroMemory(&si, sizeof(si));
si.cb = sizeof(si);
// create a new fuzzer instance
if (!CreateProcess(NULL, szCmdLine, NULL, NULL, FALSE, 0, NULL, NULL, &si, &pi))
{
MessageBox(0, _T("CreateProcess() fails"), _T("ERROR"), MB_ICONERROR);
}
ExitProcess(0);
}
}
DbgMsg(__FILE__, __LINE__, "Done; %d cases processed\n", m_dwCasesProcessed);
if (pBigData)
{
M_FREE(pBigData);
}
if (lpszBigBuff)
{
M_FREE(lpszBigBuff);
}
M_FREE(m_pAlignedData);
}
}
M_FREE(m_pData);
}
else
{
DbgMsg(__FILE__, __LINE__, __FUNCTION__"(): Error while reading input file\n");
}
end:
if (m_hWndEvent)
{
CloseHandle(m_hWndEvent);
}
printf("Press any key to quit...\n");
_getch();
return 0;
}
void DumpWithIncrement() { DumpToFile(); fSegment++; }
// Initialize the map
bool MapMakerCalib::InitFromCalibImage(CalibImageTaylor& calibImage, double dSquareSize, std::string cameraName,
TooN::SE3<>& se3TrackerPose)
{
// Create a new MKF
MultiKeyFrame* pMKF = new MultiKeyFrame;
pMKF->mse3BaseFromWorld = calibImage.mse3CamFromWorld; // set mkf pose to be pose from tracker
pMKF->mse3BaseFromWorld.get_translation() *= dSquareSize; // scale it
pMKF->mbFixed = false;
// Create a new KF (there will only be one for now)
KeyFrame* pKF = new KeyFrame(pMKF, cameraName);
pMKF->mmpKeyFrames[cameraName] = pKF;
pKF->mse3CamFromWorld = pMKF->mse3BaseFromWorld; // same as parent MKF
pKF->mse3CamFromBase = TooN::SE3<>(); // set relative pose to identity;
pKF->mbActive = true;
pKF->MakeKeyFrame_Lite(calibImage.mImage, true);
pKF->MakeKeyFrame_Rest();
// Create MapPoints where the calib image corners are
// Testing to see if we should create points at all levels or just level 0
for (int l = 0; l < LEVELS; ++l)
{
if (l >= 1)
break;
int nLevelScale = LevelScale(l);
for (unsigned i = 0; i < calibImage.mvGridCorners.size(); ++i)
{
MapPoint* pNewPoint = new MapPoint;
pNewPoint->mv3WorldPos.slice<0, 2>() = dSquareSize * CVD::vec(calibImage.mvGridCorners[i].mirGridPos);
pNewPoint->mv3WorldPos[2] = 0.0; // on z=0 plane
pNewPoint->mbFixed = true; // the calibration pattern is fixed
pNewPoint->mbOptimized = true; // since it won't move it's already in its optimal location
// Patch source stuff:
pNewPoint->mpPatchSourceKF = pKF;
pNewPoint->mnSourceLevel = l;
pNewPoint->mv3Normal_NC = TooN::makeVector(0, 0, -1);
TooN::Vector<2> v2RootPos = calibImage.mvGridCorners[i].mParams.v2Pos;
// Same code as in MapMakerServerBase::AddPointEpipolar
pNewPoint->mirCenter = CVD::ir_rounded(LevelNPos(v2RootPos, l));
pNewPoint->mv3Center_NC = mmCameraModels[cameraName].UnProject(v2RootPos);
pNewPoint->mv3OneRightFromCenter_NC =
mmCameraModels[cameraName].UnProject(v2RootPos + CVD::vec(CVD::ImageRef(nLevelScale, 0)));
pNewPoint->mv3OneDownFromCenter_NC =
mmCameraModels[cameraName].UnProject(v2RootPos + CVD::vec(CVD::ImageRef(0, nLevelScale)));
normalize(pNewPoint->mv3Center_NC);
normalize(pNewPoint->mv3OneDownFromCenter_NC);
normalize(pNewPoint->mv3OneRightFromCenter_NC);
pNewPoint->RefreshPixelVectors();
mMap.mlpPoints.push_back(pNewPoint);
// Create a measurement of the point
Measurement* pMeas = new Measurement;
pMeas->eSource = Measurement::SRC_ROOT;
pMeas->v2RootPos = v2RootPos;
pMeas->nLevel = l;
pMeas->bSubPix = true;
pKF->mmpMeasurements[pNewPoint] = pMeas;
pNewPoint->mMMData.spMeasurementKFs.insert(pKF);
}
}
mMap.mlpMultiKeyFrames.push_back(pMKF);
/*
{
ROS_DEBUG("After creating points, before optimization: ");
int i=0;
double dErrorSum = 0;
for(MapPointPtrList::iterator it = mMap.mlpPoints.begin(); it != mMap.mlpPoints.end(); ++it, ++i)
{
std::cout<<"Point pos: "<<(*it)->mv3WorldPos;
TooN::Vector<3> v3Cam = pKF->mse3CamFromWorld * (*it)->mv3WorldPos;
TooN::Vector<2> v2Image = mmCameraModels[cameraName].Project(v3Cam);
std::cout<<" Reprojected: "<<v2Image<<" Original: "<<calibImage.mvGridCorners[i].mParams.v2Pos;
std::cout<<std::endl;
TooN::Vector<2> v2Error = v2Image - calibImage.mvGridCorners[i].mParams.v2Pos;
dErrorSum += v2Error * v2Error;
}
std::cout<<"Error sum: "<<dErrorSum<<" mean: "<<dErrorSum/i<<std::endl;
}
*/
mBundleAdjuster.SetNotConverged();
int nSanityCounter = 0;
std::vector<std::pair<KeyFrame*, MapPoint*>> vOutliers;
ROS_DEBUG("MapMakerCalib: Initialized from calib image, running bundle adjuster");
while (!mBundleAdjuster.ConvergedFull())
{
mBundleAdjuster.BundleAdjustAll(vOutliers);
if (ResetRequested() || nSanityCounter > 5)
{
ROS_WARN("Dumping map to map_after.dat");
DumpToFile("map_after.dat");
ROS_ERROR("MapMakerCalib: Exceeded sanity counter or reset requested, bailing");
return false;
}
nSanityCounter++;
}
// Don't allow any outliers when we're initializing, all of the calibration points turned MapPoints
// should have been found
if (vOutliers.size() > 0)
{
ROS_ERROR("MapMakerCalib: Found outliers in initialization, bailing");
return false;
}
/*
{
ROS_DEBUG("After optimization: ");
int i=0;
double dErrorSum = 0;
for(MapPointPtrList::iterator it = mMap.mlpPoints.begin(); it != mMap.mlpPoints.end(); ++it, ++i)
{
std::cout<<"Point pos: "<<(*it)->mv3WorldPos;
TooN::Vector<3> v3Cam = pKF->mse3CamFromWorld * (*it)->mv3WorldPos;
TooN::Vector<2> v2Image = mmCameraModels[cameraName].Project(v3Cam);
std::cout<<" Reprojected: "<<v2Image<<" Original: "<<calibImage.mvGridCorners[i].mParams.v2Pos;
std::cout<<std::endl;
TooN::Vector<2> v2Error = v2Image - calibImage.mvGridCorners[i].mParams.v2Pos;
dErrorSum += v2Error * v2Error;
}
std::cout<<"Error sum: "<<dErrorSum<<" mean: "<<dErrorSum/i<<std::endl;
}
*/
// Get the updated pose, tracker will initialize with this
se3TrackerPose = pKF->mse3CamFromWorld;
ROS_INFO_STREAM("MapMakerCalib: Made initial map with " << mMap.mlpPoints.size() << " points.");
ROS_INFO_STREAM("MapMakerCalib: tracker pose: " << se3TrackerPose);
mState = MM_RUNNING; // not doing anything special for initialization (unlike MapMaker)
mMap.mbGood = true;
return true;
}
MVOID getHDRExpSetting(const HDRExpSettingInputParam_T& rInput, HDRExpSettingOutputParam_T& rOutput)
{
// Tuning parameters
MUINT32 HDRFlag = CUST_HDR_CAPTURE_ALGORITHM;
MUINT32 HDR_NEOverExp_Percent = CUST_HDR_NEOverExp_Percent;
MUINT32 u4MaxHDRExpTimeInUS = 200000; // Manually set, no longer than 0.5s (unit: us)
MUINT32 u4MaxSafeHDRExpTimeInUS = 31250; // Manually set, no longer than 0.5s (unit: us)
MUINT32 u4MaxHDRSensorGain = 4848; //Manually set, no larger than max gain in normal capture
MUINT32 u4TimeMode = 1; // 0:Depend on default AE parameters; 1: Manually set
MUINT32 u4GainMode = 1; // 0:Depend on default AE parameters; 1: Manually set
double dfTargetTopEV = 1.5; // Target EV of long exposure image
double dfSafeTargetTopEV = 0.5; // Target EV of long exposure image
double dfTargetBottomEV = -2; // Target EV of short exposure image
double dfTopEVBound = 0.2; // Long exposure image should be at least "dfTopEVBound" EV or would be discarded (2 frame case)
// MBOOL bGainEnable = MTRUE; // True: Enable long exposure image to increase sensor gain; False: Disable
MBOOL bGain0EVLimit = MFALSE; // True: Limit the gain of 0EV and short exposure image; False: Keep it
double dfFlag2TopEV = +0.5;
double dfFlag2BottomEV = -1.5;
double dfFlag2TopGain = pow(2, dfFlag2TopEV);
double dfFlag2BottomGain = pow(2, dfFlag2BottomEV);
//Add by ChingYi
// Tuning parameters
// MUINT32 TargetToneMaxExposureTime = 100000; //Decide the exposure time start to decrease target tone
// MUINT32 TargetToneCurve = 10000; //Decide the curve to decide target tone
// MUINT32 TargetToneIn = 150; //Decide the curve to decide target tone
// Temporary parameters
MUINT32 u4MaxExpTimeInUS;
MUINT32 u4MaxSensorGain;
MUINT32 i;
double dfRemainGain[3];
double dfGainDiff[2];
double dfTopGain = pow(2, dfTargetTopEV);
double dfSafeTopGain = pow(2, dfSafeTargetTopEV);
double dfBottomGain = pow(2, dfTargetBottomEV);
if (u4TimeMode == 0) {
u4MaxExpTimeInUS = rInput.u4MaxAEExpTimeInUS; // Depend on default AE parameters
}
else {
u4MaxExpTimeInUS = u4MaxHDRExpTimeInUS; // Manually set
}
if (u4GainMode == 0) {
u4MaxSensorGain = rInput.u4MaxAESensorGain; // Depend on default AE parameters
}
else {
u4MaxSensorGain = u4MaxHDRSensorGain; // Manually set
if (u4MaxSensorGain > rInput.u4MaxSensorAnalogGain) {
u4MaxSensorGain = rInput.u4MaxSensorAnalogGain;
}
}
// Final output without any limitation
if(rInput.u4SensorGain0EV > u4MaxHDRSensorGain)
rOutput.u4ExpTimeInUS[2] = static_cast<MUINT32>(rInput.u4ExpTimeInUS0EV * dfSafeTopGain + 0.5); //High ISO case: Long exposurerInput.u4ExpTimeInUS0EV;
else
rOutput.u4ExpTimeInUS[2] = static_cast<MUINT32>(rInput.u4ExpTimeInUS0EV * dfTopGain + 0.5); //Low ISO case: Long exposurerInput.u4ExpTimeInUS0EV;
rOutput.u4ExpTimeInUS[1] = rInput.u4ExpTimeInUS0EV; // 0EV
rOutput.u4ExpTimeInUS[0] = static_cast<MUINT32>(rInput.u4ExpTimeInUS0EV * dfBottomGain + 0.5); // Short exposure
rOutput.u4SensorGain[2] = rInput.u4SensorGain0EV;
rOutput.u4SensorGain[1] = rInput.u4SensorGain0EV;
rOutput.u4SensorGain[0] = rInput.u4SensorGain0EV;
if (bGain0EVLimit == MTRUE) {// Limit 0EV or even short exposure image's gain but keep the brightness
for (MUINT32 i = 0; i < 2; i++) {
dfRemainGain[i] = 1;
if (rOutput.u4SensorGain[i] > u4MaxHDRSensorGain) {
dfRemainGain[i] = static_cast<double>(rOutput.u4SensorGain[i]) / u4MaxHDRSensorGain;
rOutput.u4SensorGain[i] = u4MaxHDRSensorGain;
}
if (dfRemainGain[i] > 1) {
rOutput.u4ExpTimeInUS[i] = static_cast<MUINT32>(rOutput.u4ExpTimeInUS[i] * dfRemainGain[i] + 0.5);
if (rOutput.u4ExpTimeInUS[i] > u4MaxHDRExpTimeInUS) { //It means that we cannot keep original brightness, so we decide to keep original setting
rOutput.u4ExpTimeInUS[1] = rInput.u4ExpTimeInUS0EV; // 0EV
rOutput.u4ExpTimeInUS[0] = static_cast<MUINT32>(rInput.u4ExpTimeInUS0EV * dfBottomGain + 0.5); // Short exposure
rOutput.u4SensorGain[1] = rInput.u4SensorGain0EV;
rOutput.u4SensorGain[0] = rInput.u4SensorGain0EV;
}
}
}
}
// if (bGainEnable == MFALSE) {
// u4MaxSensorGain = rOutput.u4SensorGain[1]; // MaxSensor gain should be equal to 0EV
// }
dfRemainGain[2] = 1;
if(rInput.u4ExpTimeInUS0EV>u4MaxSafeHDRExpTimeInUS)
u4MaxSafeHDRExpTimeInUS = rInput.u4ExpTimeInUS0EV;
if (rOutput.u4ExpTimeInUS[2] > u4MaxSafeHDRExpTimeInUS) {
dfRemainGain[2] = static_cast<double>(rOutput.u4ExpTimeInUS[2]) / u4MaxSafeHDRExpTimeInUS;
rOutput.u4ExpTimeInUS[2] = u4MaxSafeHDRExpTimeInUS;
}
if (dfRemainGain[2] > 1) {
rOutput.u4SensorGain[2] = static_cast<MUINT32>(rOutput.u4SensorGain[2] * dfRemainGain[2] + 0.5);
if (rOutput.u4SensorGain[2] > u4MaxSensorGain) {
dfRemainGain[2] = static_cast<double>(rOutput.u4SensorGain[2]) / u4MaxSensorGain;
rOutput.u4SensorGain[2] = u4MaxSensorGain;
}
else
dfRemainGain[2] = 1;
}
if (dfRemainGain[2] > 1) {
rOutput.u4ExpTimeInUS[2] = static_cast<MUINT32>(rOutput.u4ExpTimeInUS[2] * dfRemainGain[2] + 0.5);
if (rOutput.u4ExpTimeInUS[2] >u4MaxExpTimeInUS)
{
dfRemainGain[2] = static_cast<double>(rOutput.u4ExpTimeInUS[2]) / u4MaxExpTimeInUS;
rOutput.u4ExpTimeInUS[2] = u4MaxExpTimeInUS;
}
}
dfGainDiff[0] = static_cast<double>(rOutput.u4SensorGain[0]*rOutput.u4ExpTimeInUS[0]) / rOutput.u4SensorGain[1] / rOutput.u4ExpTimeInUS[1];
dfGainDiff[1] = static_cast<double>(rOutput.u4SensorGain[2]*rOutput.u4ExpTimeInUS[2]) / rOutput.u4SensorGain[1] / rOutput.u4ExpTimeInUS[1];
rOutput.u1FlareOffset[1] = rInput.u1FlareOffset0EV;
if(rOutput.u1FlareOffset[1]>4)
rOutput.u1FlareOffset[1] = 4;
rOutput.u1FlareOffset[0] = static_cast<MUINT8>(rOutput.u1FlareOffset[1] * dfGainDiff[0] + 0.5);
rOutput.u1FlareOffset[2] = static_cast<MUINT8>(rOutput.u1FlareOffset[1] * dfGainDiff[1] + 0.5);
if(rOutput.u1FlareOffset[2]>63)
rOutput.u1FlareOffset[2] = 63; //hardware limitation --> may lead to alignment issue
double dfTopGainBound = pow(2, dfTopEVBound);
if (dfTopGainBound < dfGainDiff[1]) {
rOutput.u4OutputFrameNum = 3;
}
else {
rOutput.u4OutputFrameNum = 2;
}
//Add by ChingYi
// if(rOutput.u4ExpTimeInUS[2] > TargetToneMaxExposureTime && rOutput.u4OutputFrameNum == 3)
// rOutput.u4TargetTone = TargetToneIn - (rOutput.u4ExpTimeInUS[2]-TargetToneMaxExposureTime)/TargetToneCurve;
// else
// rOutput.u4TargetTone = TargetToneIn;
rOutput.u4TargetTone = 150;
rOutput.u4FinalGainDiff[0] = static_cast<MUINT32>(1024 / dfGainDiff[0] + 0.5);
rOutput.u4FinalGainDiff[1] = static_cast<MUINT32>(1024 / dfGainDiff[1] + 0.5);
MUINT32 HISsum = 0;
if( HDRFlag == 1) // adaptive version, if original version use -2EV less, we only capture 2 frames (0EV and +2EV). If original version use -2EV a lot, we still capture 3 frames.
{
//calculate total bin count of AE histogram
HISsum = 0;
for(i=0 ; i<128 ; i++)
HISsum = HISsum + rInput.u4Histogram[i];
if( (int)((rInput.u4Histogram[126] + rInput.u4Histogram[127]) * 1000 / HISsum + 0.5) < HDR_NEOverExp_Percent) //calculate the percentage of bin[126]+bin[127]
{
rOutput.u4OutputFrameNum = 2; //capture 2 frames NE(0EV) and LE(+2EV)
rOutput.u4ExpTimeInUS[0] = rOutput.u4ExpTimeInUS[2];
rOutput.u4SensorGain[0] = rOutput.u4SensorGain[2];
rOutput.u1FlareOffset[0] = rOutput.u1FlareOffset[2];
rOutput.u4FinalGainDiff[0] = rOutput.u4FinalGainDiff[1];
}
else
{
rOutput.u4OutputFrameNum = 3;
}
}
if( HDRFlag == 2) // performance priority version, always capture 2 frames. The EV settings are decided adapively.
{
rOutput.u4OutputFrameNum = 2;
//calculate total bin count of AE histogram
HISsum = 0;
for(i=0 ; i<128 ; i++)
HISsum = HISsum + rInput.u4Histogram[i];
if( (int)((rInput.u4Histogram[126] + rInput.u4Histogram[127]) * 1000 / HISsum + 0.5) < HDR_NEOverExp_Percent) //calculate the percentage of bin[126]+bin[127]
{
rOutput.u4ExpTimeInUS[0] = rOutput.u4ExpTimeInUS[2]; //capture 2 frames NE(0EV) and LE(+2EV)
rOutput.u4SensorGain[0] = rOutput.u4SensorGain[2];
rOutput.u1FlareOffset[0] = rOutput.u1FlareOffset[2];
rOutput.u4FinalGainDiff[0] = rOutput.u4FinalGainDiff[1];
}
else
{ //capture 2 frames NE(-0.5 EV) and LE(+1.5 EV)
rOutput.u4ExpTimeInUS[0] = static_cast<MUINT32>(rInput.u4ExpTimeInUS0EV * dfFlag2BottomGain + 0.5); // long exposure
rOutput.u4ExpTimeInUS[1] = static_cast<MUINT32>(rInput.u4ExpTimeInUS0EV * dfFlag2TopGain + 0.5); // normal exposure
rOutput.u4SensorGain[0] = rInput.u4SensorGain0EV;
rOutput.u4SensorGain[1] = rInput.u4SensorGain0EV;
dfGainDiff[0] = static_cast<double>(rOutput.u4SensorGain[0]*rOutput.u4ExpTimeInUS[0]) / rOutput.u4SensorGain[1] / rOutput.u4ExpTimeInUS[1];
rOutput.u4FinalGainDiff[0] = static_cast<MUINT32>(1024 / dfGainDiff[0] + 0.5);
rOutput.u1FlareOffset[1] = rInput.u1FlareOffset0EV;
if(rOutput.u1FlareOffset[1]>4)
rOutput.u1FlareOffset[1] = 4;
rOutput.u1FlareOffset[0] = static_cast<MUINT8>(rOutput.u1FlareOffset[1] * dfGainDiff[0] + 0.5);
}
}
#if 0 // for debug only: adb logcat HdrCustome:V *:S
#ifdef LOG_TAG
#undef LOG_TAG
#endif
#define LOG_TAG "HdrCustome"
XLOGD("System Paramters\n");
XLOGD("0EV Exposure Time = %d\n0EV Sensor Gain = %d\n", rInput.u4ExpTimeInUS0EV, rInput.u4SensorGain0EV);
XLOGD("Max Exposure Time 0EV = %d\nMaxSensor Gain 0EV = %d\n", rInput.u4MaxAEExpTimeInUS, rInput.u4MaxAESensorGain);
XLOGD("Max Exposure Time Sensor = %d\nMaxSensor Gain Sensor = %d\n", u4MaxHDRExpTimeInUS, u4MaxHDRSensorGain);
XLOGD("Final Max Exposure Time = %d\nFinal MaxSensor Gain = %d\n", u4MaxExpTimeInUS, u4MaxSensorGain);
XLOGD("\nTuning Paramters\n");
XLOGD("Target Top EV = %f\nTarget Bottom EV = %f\n", dfTargetTopEV, dfTargetBottomEV);
XLOGD("dfTopGainBound = %f\n",dfTopGainBound);
XLOGD("bGain0EVLimit = %s\n", (bGain0EVLimit ? "true" : "false"));
XLOGD("\nOutput Paramters\n");
for (i = 0; i < 3; i++) {
XLOGD("Final Frame %d ExposureTime = %d\nSensorGain = %d\n", i, rOutput.u4ExpTimeInUS[i], rOutput.u4SensorGain[i]);
}
XLOGD("Final EVdiff[0] = %d\nFinal EVdiff[1] = %d\n", rOutput.u4FinalGainDiff[0], rOutput.u4FinalGainDiff[1]);
XLOGD("OutputFrameNum = %d\n", rOutput.u4OutputFrameNum);
XLOGD("Final FlareOffsetOut[0]= %d\nFinal FlareOffsetOut[1]= %d\nFinal FlareOffsetOut[2]= %d\n", rOutput.u1FlareOffset[0], rOutput.u1FlareOffset[1], rOutput.u1FlareOffset[2]);
XLOGD("Final TargetTone1 = %d\n", rOutput.u4TargetTone);
#endif // for debug only: adb logcat HdrCustome:V *:S
//#if (ENABLE_HDR_AE_DEBUG_INFO)
// Save HDR AE debug info to a file.
char value[32] = {'\0'};
property_get("mediatek.hdr.debug", value, "0");
int hdr_debug_mode = atoi(value) || CUST_HDR_DEBUG;
if(hdr_debug_mode) {
// Increase 4-digit running number (range: 1 ~ 9999).
if (S_u4RunningNumber >= 9999)
S_u4RunningNumber = 1;
else
S_u4RunningNumber++;
pucLogBufPosition = (char*)GS_ucLogBuf;
::sprintf(pucLogBufPosition, "< No.%04d > ----------------------------------------------------------------------\n", S_u4RunningNumber);
pucLogBufPosition += strlen(pucLogBufPosition);
::sprintf(pucLogBufPosition, "[System Paramters]\n");
pucLogBufPosition += strlen(pucLogBufPosition);
::sprintf(pucLogBufPosition, "0EV Exposure Time = %d\n0EV Sensor Gain = %d\n", rInput.u4ExpTimeInUS0EV, rInput.u4SensorGain0EV);
pucLogBufPosition += strlen(pucLogBufPosition);
::sprintf(pucLogBufPosition, "Max Exposure Time Sensor= %d\nMaxSensor Gain Sensor= %d\n", rInput.u4MaxAEExpTimeInUS, rInput.u4MaxAESensorGain);
pucLogBufPosition += strlen(pucLogBufPosition);
::sprintf(pucLogBufPosition, "Max Exposure Time Manual= %d\nMaxSensor Gain Manual= %d\n", u4MaxHDRExpTimeInUS, u4MaxHDRSensorGain);
pucLogBufPosition += strlen(pucLogBufPosition);
::sprintf(pucLogBufPosition, "Max Exposure Time = %d\nMaxSensor Gain = %d\n", u4MaxExpTimeInUS, u4MaxSensorGain);
pucLogBufPosition += strlen(pucLogBufPosition);
::sprintf(pucLogBufPosition, "\n[Tuning Paramters]\n");
pucLogBufPosition += strlen(pucLogBufPosition);
::sprintf(pucLogBufPosition, "Target Top EV = %f\nTarget Bottom EV = %f\n", dfTargetTopEV, dfTargetBottomEV);
pucLogBufPosition += strlen(pucLogBufPosition);
::sprintf(pucLogBufPosition, "dfTopGainBound = %f\n",dfTopGainBound);
pucLogBufPosition += strlen(pucLogBufPosition);
::sprintf(pucLogBufPosition, "bGain0EVLimit = %s\n", (bGain0EVLimit ? "true" : "false"));
pucLogBufPosition += strlen(pucLogBufPosition);
::sprintf(pucLogBufPosition, "\n[Output Paramters]\n");
pucLogBufPosition += strlen(pucLogBufPosition);
for (i = 0; i < 3; i++) {
::sprintf(pucLogBufPosition, "Final Frame %d ExposureTime = %d\nSensorGain = %d\n", i, rOutput.u4ExpTimeInUS[i], rOutput.u4SensorGain[i]);
pucLogBufPosition += strlen(pucLogBufPosition);
}
::sprintf(pucLogBufPosition, "Final EVdiff[0] = %d\nFinal EVdiff[1] = %d\n", rOutput.u4FinalGainDiff[0], rOutput.u4FinalGainDiff[1]);
pucLogBufPosition += strlen(pucLogBufPosition);
::sprintf(pucLogBufPosition, "OutputFrameNum = %d\n", rOutput.u4OutputFrameNum);
pucLogBufPosition += strlen(pucLogBufPosition);
::sprintf(pucLogBufPosition, "Final FlareOffsetOut[0]= %d\nFinal FlareOffsetOut[1]= %d\nFinal FlareOffsetOut[2]= %d\n", rOutput.u1FlareOffset[0], rOutput.u1FlareOffset[1], rOutput.u1FlareOffset[2]);
pucLogBufPosition += strlen(pucLogBufPosition);
::sprintf(pucLogBufPosition, "Final TargetTone= %d\n", rOutput.u4TargetTone);
pucLogBufPosition += strlen(pucLogBufPosition);
char szFileName[100];
//::sprintf(szFileName, "sdcard/Photo/%04d_HDR_ExposureSetting.txt", S_u4RunningNumber); // For ALPS.GB2.
::sprintf(szFileName, HDR_DEBUG_OUTPUT_FOLDER"%04d_HDR_ExposureSetting.txt", S_u4RunningNumber); // For ALPS.ICS.
DumpToFile(szFileName, (unsigned char *)GS_ucLogBuf, MAX_LOG_BUF_SIZE);
}
//#endif // ENABLE_HDR_AE_DEBUG_INFO
}
