C++ CSimpleMap类代码示例

/*---------------------------------------------------------------
					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);
}

/*---------------------------------------------------------------
						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;
}

// 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);
}

// ------------------------------------------------------
//				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);
}

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;
	}
}

/*---------------------------------------------------------------
					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 );
}

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);
}

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;
}