/*---------------------------------------------------------------
initialize
---------------------------------------------------------------*/
void CMetricMapBuilderRBPF::initialize(
const CSimpleMap& initialMap, const CPosePDF* x0)
{
MRPT_LOG_INFO_STREAM(
"[initialize] Called with " << initialMap.size()
<< " nodes in fixed map");
this->clear();
std::lock_guard<std::mutex> csl(
critZoneChangingMap); // Enter critical section (updating map)
mrpt::poses::CPose3D curPose;
if (x0)
{
curPose = mrpt::poses::CPose3D(x0->getMeanVal());
}
else if (!initialMap.empty())
{
// get pose of last keyframe:
curPose = initialMap.rbegin()->first->getMeanVal();
}
MRPT_LOG_INFO_STREAM("[initialize] Initial pose: " << curPose);
// Clear maps for each particle & set pose:
mapPDF.clear(initialMap, curPose);
}
/**
* Get named GUID for debug log.
* Attempt to translate it through standard Registry lookup, or our
* custom Registry repository of interface, GUID, Co-class names.
*/
LPCWSTR DbgGetIID(REFIID riid)
{
static CComAutoCriticalSection s_lock;
CComCritSecLock<CComCriticalSection> lock(s_lock);
static CSimpleMap<GUID, CComBSTR> s_Cache;
LPCWSTR pwstrResult = s_Cache.Lookup(riid);
if( pwstrResult != NULL ) return pwstrResult;
TCHAR szName[80] = { 0 };
OLECHAR wszGUID[50] = { 0 };
::StringFromGUID2(riid, wszGUID, lengthof(wszGUID));
// Attempt to find it in the interfaces section
CRegKey key;
DWORD dwType = 0;
DWORD dwCount = sizeof(szName) - sizeof(TCHAR);
key.Open(HKEY_CLASSES_ROOT, _T("Interface"), KEY_READ);
if( szName[0] == '\0' && key.Open(key, wszGUID, KEY_READ) == NOERROR ) {
::RegQueryValueEx(key.m_hKey, NULL, NULL, &dwType, (LPBYTE) szName, &dwCount);
}
// Attempt to find it in the CLSID section
key.Open(HKEY_CLASSES_ROOT, _T("CLSID"), KEY_READ);
if( szName[0] == '\0' && key.Open(key, wszGUID, KEY_READ) == NOERROR ) {
::RegQueryValueEx(key.m_hKey, NULL, NULL, &dwType, (LPBYTE) szName, &dwCount);
}
// Attempt to find it in our Named GUIDs section
key.Open(HKEY_CLASSES_ROOT, _T("Named GUIDs"), KEY_READ);
if( szName[0] == '\0' && key.Open(key, wszGUID, KEY_READ) == NOERROR ) {
::RegQueryValueEx(key.m_hKey, NULL, NULL, &dwType, (LPBYTE) szName, &dwCount);
}
if( szName[0] == '\0' ) wcscpy_s(szName, lengthof(szName), CW2T(wszGUID));
s_Cache.Add(riid, CComBSTR(szName));
return s_Cache.Lookup(riid);
}
void CDSMMuxerFilter::MuxStreamInfo(IBitStream* pBS, CBaseMuxerInputPin* pPin)
{
int len = 1;
CSimpleMap<CStringA, CStringA> si;
for(int i = 0; i < pPin->GetSize(); i++)
{
CStringA key = CStringA(CString(pPin->GetKeyAt(i))), value = UTF16To8(pPin->GetValueAt(i));
if(key.GetLength() != 4) continue;
si.Add(key, value);
len += 4 + value.GetLength() + 1;
}
if(len > 1)
{
MuxPacketHeader(pBS, DSMP_STREAMINFO, len);
pBS->BitWrite(pPin->GetID(), 8);
for(int i = 0; i < si.GetSize(); i++)
{
CStringA key = si.GetKeyAt(i), value = si.GetValueAt(i);
pBS->ByteWrite((LPCSTR)key, 4);
pBS->ByteWrite((LPCSTR)value, value.GetLength()+1);
}
}
}
/**
* Get named PROPERTYKEY for debug log.
* Attempt to translate it through standard property system lookup, or our
* custom Registry repository of GUIDs.
*/
LPCWSTR DbgGetPKEY(REFPROPERTYKEY pkey)
{
static CComAutoCriticalSection s_lock;
CComCritSecLock<CComCriticalSection> lock(s_lock);
static CSimpleMap< PROPERTYKEY, CComBSTR, CSimpleMapPkeyEqualHelper<PROPERTYKEY, CComBSTR> > s_Cache;
LPCWSTR pwstrResult = s_Cache.Lookup(pkey);
if( pwstrResult != NULL ) return pwstrResult;
WCHAR wszName[80] = { 0 };
// Let Windows tell us the property name
CCoTaskString str;
if( SUCCEEDED( ::PSGetNameFromPropertyKey(pkey, &str) ) ) wcscpy_s(wszName, lengthof(wszName), str);
// Attempt to find it in our Named GUIDs section
if( wszName[0] == '\0' ) {
WCHAR wszGUID[70] = { 0 };
CRegKey key;
DWORD dwType = 0, dwCount = sizeof(wszGUID) - sizeof(TCHAR);
key.Open(HKEY_CLASSES_ROOT, _T("Named GUIDs"), KEY_READ);
if( key.Open(key, wszGUID, KEY_READ) == NOERROR ) ::RegQueryValueExW(key.m_hKey, NULL, NULL, &dwType, (LPBYTE) wszName, &dwCount);
}
// Just format the GUID
if( wszName[0] == '\0' ) ::PSStringFromPropertyKey(pkey, wszName, lengthof(wszName));
// Add it to cache and return
s_Cache.Add(pkey, CComBSTR(wszName));
return s_Cache.Lookup(pkey);
}
void SetEncConverter(const CStringW& strConverterName, CSilEncConverter* pEC)
{
int nIndex;
if ((nIndex = m_mapECs.FindKey(strConverterName)) != -1)
{
CSilEncConverter* p = m_mapECs.GetValueAt(nIndex);
m_mapECs.RemoveAt(nIndex);
delete p;
}
m_mapECs.Add(strConverterName, pEC);
}
/*---------------------------------------------------------------
changeCoordinateOrigin
---------------------------------------------------------------*/
void CLocalMetricHypothesis::changeCoordinateOrigin( const TPoseID &newOrigin )
{
CPose3DPDFParticles originPDF( m_particles.size() );
CParticleList::const_iterator it;
CPose3DPDFParticles::CParticleList::iterator itOrgPDF;
for ( it = m_particles.begin(), itOrgPDF=originPDF.m_particles.begin(); it!=m_particles.end(); it++, itOrgPDF++ )
{
TMapPoseID2Pose3D::iterator refPoseIt = it->d->robotPoses.find( newOrigin );
ASSERT_( refPoseIt != it->d->robotPoses.end() )
const CPose3D &refPose = refPoseIt->second;
// Save in pdf to compute mean:
*itOrgPDF->d = refPose;
itOrgPDF->log_w = it->log_w;
TMapPoseID2Pose3D::iterator End = it->d->robotPoses.end();
// Change all other poses first:
for (TMapPoseID2Pose3D::iterator itP=it->d->robotPoses.begin();itP!=End;++itP)
if (itP!=refPoseIt)
itP->second = itP->second - refPose;
// Now set new origin to 0:
refPoseIt->second.setFromValues(0,0,0);
}
// Rebuild metric maps for consistency:
rebuildMetricMaps();
// Change coords in incr. partitioning as well:
{
synch::CCriticalSectionLocker locker ( &m_robotPosesGraph.lock );
CSimpleMap *SFseq = m_robotPosesGraph.partitioner.getSequenceOfFrames();
for (std::map<uint32_t,TPoseID>::const_iterator it=m_robotPosesGraph.idx2pose.begin();it!=m_robotPosesGraph.idx2pose.end();++it)
{
CPose3DPDFPtr pdf;
CSensoryFramePtr sf;
SFseq->get( it->first, pdf, sf);
// Copy from particles:
ASSERT_( pdf->GetRuntimeClass() == CLASS_ID(CPose3DPDFParticles) );
CPose3DPDFParticlesPtr pdfParts = CPose3DPDFParticlesPtr(pdf);
getPoseParticles( it->second, *pdfParts);
}
}
}
//------------------------------------------------------------------------
//! Create a group for each unique values within a column
//!
//! @param nCol The index of the column
//! @return Succeeded in creating the group
//------------------------------------------------------------------------
BOOL CGridListCtrlGroups::GroupByColumn(int nCol)
{
CWaitCursor waitCursor;
SetSortArrow(-1, false);
SetRedraw(FALSE);
RemoveAllGroups();
EnableGroupView( GetItemCount() > 0 );
if (IsGroupViewEnabled())
{
CSimpleMap<CString,CSimpleArray<int> > groups;
// Loop through all rows and find possible groups
for(int nRow=0; nRow<GetItemCount(); ++nRow)
{
CString cellText = GetItemText(nRow, nCol);
int nGroupId = groups.FindKey(cellText);
if (nGroupId==-1)
{
CSimpleArray<int> rows;
groups.Add(cellText, rows);
nGroupId = groups.FindKey(cellText);
}
groups.GetValueAt(nGroupId).Add(nRow);
}
// Look through all groups and assign rows to group
for(int nGroupId = 0; nGroupId < groups.GetSize(); ++nGroupId)
{
const CSimpleArray<int>& groupRows = groups.GetValueAt(nGroupId);
DWORD dwState = LVGS_NORMAL;
#ifdef LVGS_COLLAPSIBLE
if (IsGroupStateEnabled())
dwState = LVGS_COLLAPSIBLE;
#endif
VERIFY( InsertGroupHeader(nGroupId, nGroupId, groups.GetKeyAt(nGroupId), dwState) != -1);
for(int groupRow = 0; groupRow < groupRows.GetSize(); ++groupRow)
{
VERIFY( SetRowGroupId(groupRows[groupRow], nGroupId) );
}
}
SetRedraw(TRUE);
Invalidate(FALSE);
return TRUE;
}
SetRedraw(TRUE);
Invalidate(FALSE);
return FALSE;
}
CSilEncConverter* GetEncConverter(const CStringW& strConverterName)
{
CSilEncConverter* pEC = 0;
int nIndex;
if ((nIndex = m_mapECs.FindKey(strConverterName)) != -1)
{
pEC = m_mapECs.GetValueAt(nIndex);
}
else
{
pEC = new CSilEncConverter();
SetEncConverter(strConverterName, pEC);
}
ATLASSERT(pEC != 0);
return pEC;
}
// CSimpleMap
void CSimpleMap_insert(CSimpleMap &self, CPose3DPDF &in_posePDF, CSensoryFrame &in_SF)
{
// create smart pointers
CPose3DPDFPtr in_posePDFPtr = (CPose3DPDFPtr) in_posePDF.duplicateGetSmartPtr();
CSensoryFramePtr in_SFPtr = (CSensoryFramePtr) in_SF.duplicateGetSmartPtr();
// insert smart pointers
self.insert(in_posePDFPtr, in_SFPtr);
}
//------------------------------------------------------------------------
//! Fills the combobox with the items of the fixed item-list
//!
//! @param comboList List of CComboBox items
//! @param nCurSel Index in the list to choose as currently selected (-1 = No selection)
//------------------------------------------------------------------------
void CGridColumnTraitCombo::LoadList(const CSimpleMap<int,CString>& comboList, int nCurSel)
{
VERIFY(m_pComboBox!=NULL);
m_pComboBox->SetRedraw(FALSE);
m_pComboBox->InitStorage(comboList.GetSize(), 32);
for(int i = 0; i < comboList.GetSize(); ++i)
{
int nIndex = m_pComboBox->AddString(comboList.GetValueAt(i));
m_pComboBox->SetItemData(nIndex, comboList.GetKeyAt(i));
}
m_pComboBox->SetRedraw(TRUE);
m_pComboBox->Invalidate();
m_pComboBox->UpdateWindow();
if (nCurSel!=-1)
m_pComboBox->SetCurSel(nCurSel);
}
CCrashHandler::CCrashHandler(LPGETLOGFILE lpfn /*=NULL*/,
const char * lpcszTo /*=NULL*/,
const char * lpcszSubject /*=NULL*/,
bool bInstallHandler /*= true*/)
{
// wtl initialization stuff...
HRESULT hRes = ::CoInitialize(NULL);
//Если COM уже проинициализировали, но в другом режиме - мы не бужем вызывать разинициализацию
if (hRes == RPC_E_CHANGED_MODE)
m_COMInited = false;
else
{
ATLASSERT(SUCCEEDED(hRes));
m_COMInited = SUCCEEDED(hRes);
}
hRes = _Module.Init(NULL, GetModuleHandle("CrashRpt.dll"));
ATLASSERT(SUCCEEDED(hRes));
::DefWindowProc(NULL, 0, 0, 0L);
// initialize member data
m_lpfnCallback = NULL;
m_oldFilter = NULL;
// save user supplied callback
if (lpfn)
m_lpfnCallback = lpfn;
if (bInstallHandler)
{
// add this filter in the exception callback chain
m_oldFilter = SetUnhandledExceptionFilter(CustomUnhandledExceptionFilter);
// attach this handler with this process
m_pid = _getpid();
if (!_crashStateMap.SetAt(m_pid, this))
_crashStateMap.Add(m_pid, this);
}
// save optional email info
m_sTo = lpcszTo;
m_sSubject = lpcszSubject;
}
// ------------------------------------------------------
// TestVoronoi
// ------------------------------------------------------
void TestVoronoi()
{
if (!mrpt::system::fileExists(sample_simplemap_file))
{
cerr << "Error: file doesn't exist: " << sample_simplemap_file << endl;
return;
}
// Load simplemap:
cout << "Loading simplemap: " << sample_simplemap_file << endl;
CSimpleMap simplemap;
simplemap.loadFromFile(sample_simplemap_file);
// Load a grid map:
cout << "Building gridmap...\n";
COccupancyGridMap2D gridmap(-5,5, -5,5, 0.10);
gridmap.loadFromSimpleMap(simplemap);
// Build voronoi:
cout << "Building Voronoi diagram...\n";
gridmap.buildVoronoiDiagram(0.5,0.3);
// Show results:
CImage img_grid;
gridmap.getAsImage(img_grid);
CImage img_voronoi;
CMatrixDouble mat_voronoi;
gridmap.getVoronoiDiagram().getAsMatrix(mat_voronoi);
img_voronoi.setFromMatrix(mat_voronoi, false /* do normalization */ );
// Show results:
CDisplayWindow win1("Grid map");
win1.showImage(img_grid);
CDisplayWindow win2("Voronoi map");
win2.showImage(img_voronoi);
mrpt::system::pause();
}
// CSimpleMap
void CSimpleMap_insert(
CSimpleMap& self, CPose3DPDF& in_posePDF, CSensoryFrame& in_SF)
{
// create smart pointers
CPose3DPDF::Ptr in_posePDFPtr = std::dynamic_pointer_cast<CPose3DPDF>(
in_posePDF.duplicateGetSmartPtr());
CSensoryFrame::Ptr in_SFPtr =
std::dynamic_pointer_cast<CSensoryFrame>(in_SF.duplicateGetSmartPtr());
// insert smart pointers
self.insert(in_posePDFPtr, in_SFPtr);
}
CCrashHandler::~CCrashHandler()
{
// reset exception callback
if (m_oldFilter)
SetUnhandledExceptionFilter(m_oldFilter);
_crashStateMap.Remove(m_pid);
// uninitialize
_Module.Term();
if (m_COMInited)
::CoUninitialize();
}
void CDSMMuxerFilter::MuxFileInfo(IBitStream* pBS)
{
int len = 1;
CSimpleMap<CStringA, CStringA> si;
for (int i = 0; i < GetSize(); i++) {
CStringA key = CStringA(CString(GetKeyAt(i))), value = UTF16To8(GetValueAt(i));
if (key.GetLength() != 4) {
continue;
}
si.Add(key, value);
len += 4 + value.GetLength() + 1;
}
MuxPacketHeader(pBS, DSMP_FILEINFO, len);
pBS->BitWrite(DSMF_VERSION, 8);
for (int i = 0; i < si.GetSize(); i++) {
CStringA key = si.GetKeyAt(i), value = si.GetValueAt(i);
pBS->ByteWrite((LPCSTR)key, 4);
pBS->ByteWrite((LPCSTR)value, value.GetLength() + 1);
}
}
void CPluginPropertyPage::_GetData()
{
CSimpleMap<int , CSimpleArray<CString>*> map;
for (int nType = PLT_TOOLBAR/*1*/; nType <= PLUGIN_TYPECNT; nType++) {
CSimpleArray<CString>*pAry = new CSimpleArray<CString>;
map.Add( nType, pAry );
}
int nIndex;
for (nIndex = 0; nIndex < m_listview.GetItemCount(); nIndex++) {
CString strFile;
m_listview.GetItemText( nIndex, 0, strFile );
if ( FALSE == m_listview.GetCheckState( nIndex ) )
continue;
int nType = int( m_listview.GetItemData( nIndex ) );
CSimpleArray<CString>* pAry = NULL;
pAry = map.Lookup( nType );
if (NULL == pAry)
continue;
pAry->Add( strFile );
}
for (nIndex = 0; nIndex < map.GetSize(); nIndex++) {
CSimpleArray<CString>*pAry = NULL;
pAry = map.GetValueAt( nIndex );
int nType = map.GetKeyAt( nIndex );
CString strKey;
strKey.Format( _T("Plugin%02d"), nType );
CIniFileO pr( g_szIniFileName, strKey );
pr.SetValue( pAry->GetSize(), _T("Count") );
for (int nNo = 0; nNo < pAry->GetSize(); nNo++) {
strKey.Format(_T("%02d"), nNo);
pr.SetString( (*pAry)[nNo], strKey );
}
delete pAry;
}
}
/*---------------------------------------------------------------
initialize
---------------------------------------------------------------*/
void CMetricMapBuilderRBPF::initialize(
const CSimpleMap &initialMap,
CPosePDF *x0 )
{
// Enter critical section (updating map)
enterCriticalSection();
MRPT_LOG_INFO_STREAM << "[initialize] Called with " << initialMap.size() << " nodes in fixed map\n";
if (x0)
MRPT_LOG_INFO_STREAM << "[initialize] x0: " << x0->getMeanVal() << "\n";
else
MRPT_LOG_INFO_STREAM << "[initialize] x0: (Not supplied)\n";
this->clear();
if (x0) {
const CPose2D meanPose = x0->getMeanVal();
// Clear maps for each particle & set pose:
mapPDF.clear( meanPose );
}
// Leaving critical section (updating map)
leaveCriticalSection();
}
/*---------------------------------------------------------------
convertIntoSimplemap
---------------------------------------------------------------*/
void CRobotPosesGraph::convertIntoSimplemap( CSimpleMap &out_simplemap) const
{
out_simplemap.clear();
for (std::map<TPoseID,TPoseInfo>::const_iterator it=begin();it!=end();++it)
out_simplemap.insert( &it->second.pdf, it->second.sf );
}
// unhandled exception callback set with SetUnhandledExceptionFilter()
int32_t WINAPI CustomUnhandledExceptionFilter(PEXCEPTION_POINTERS pExInfo)
{
_crashStateMap.Lookup(_getpid())->GenerateErrorReport(pExInfo);
return EXCEPTION_EXECUTE_HANDLER;
}