void PerformanceEvaluator::EvaluateIntermediate(pair<StateVector,StateCovarianceMatrix> estimate,
double MOD2PR,
MeasurementVector z,
StateVector x) {
SVref xEst = estimate.first;
SCMref P = estimate.second;
SVref xReal = x;
StateVector zTemp;
zTemp << z(0),0,z(1),0,0;
for(auto v:_performanceValueTuples) {
string key = v.first;
VecPtr vec = get<0>(v.second);//get the vector
PerformanceFunction f = get<1>(v.second);//get the performance function
if (_runCount == 0) {
if(key == "MOD2PR"){vec->push_back(MOD2PR);}
else if(key=="RAWRMSPOS"){vec->push_back(f(zTemp,P,xReal));}
else{vec->push_back(f(xEst, P, xReal));}
}
else {
if(key == "MOD2PR"){(*vec)[_sampleCount]+=MOD2PR;}
else if(key=="RAWRMSPOS"){(*vec)[_sampleCount]+=f(zTemp,P,xReal);}
else{ (*vec)[_sampleCount] += f(xEst, P, xReal);} //then we can perform addition assignment
}
}
_sampleCount++;
}
void * AsioHandlerCache::allocate(std::size_t size)
{
//return malloc(size);
VecPtr vecPtr;
Lock lock(mHandlerMutex);
if (mHandlerFreeList.empty())
{
vecPtr.reset( new std::vector<char>(size) );
}
else
{
vecPtr = mHandlerFreeList.back();
mHandlerFreeList.pop_back();
}
if (vecPtr->size() != size)
{
vecPtr->resize(size);
}
mHandlerUsedList.push_back(vecPtr);
return & (*vecPtr)[0];
}
void ObjectPool::put(VecPtr & vecPtr)
{
RCF_ASSERT(vecPtr);
RCF_ASSERT(vecPtr.unique());
vecPtr->resize(0);
Lock lock(mVecPtrPoolMutex);
mVecPtrPool.push_back(vecPtr);
vecPtr.reset();
}
void IMFT<Ptr>::updatePtrsInLastFrame(VecPtr ptrs)
{
for(ListGraph::NodeIt n(m_g); n != INVALID; ++n){
if((*m_gNodeMap)[n].nFrame == cnt){
for(int j=0;j<ptrs.size();j++){
if((*m_gNodeMap)[n].ptr.id = ptrs.at(j).id)
(*m_gNodeMap)[n].ptr = ptrs.at(j);
}
}
}
}
std::vector<Ptr> IMFT<Ptr>::getUnmatchedPtrs()
{
VecPtr unmatchedPtrs;
if(cnt < m_nWindow) return unmatchedPtrs;
for(ListGraph::NodeIt n(m_g); n != INVALID; ++n){
if((*m_gNodeMap)[n].nFrame == cnt && (*m_gNodeMap)[n].isIn){
int eId = (*m_gNodeMap)[n].edgeID;
if(eId < 0){
unmatchedPtrs.push_back((*m_gNodeMap)[n].ptr);
}
}
}
return unmatchedPtrs;
}
void IMFT<Ptr>::addToDGraph(VecPtr ptrs)
{
// save old edges
m_vecOldEdge.clear();
for(ListGraph::EdgeIt e(m_g); e != INVALID; ++e){
m_vecOldEdge.push_back(m_g.id(e));
}
// making nodes
for(int i=0;i<ptrs.size();i++){
// inNode
ListGraph::Node xi = m_g.addNode();
V vi;
vi.id = m_g.id(xi);
vi.isIn = 1;
vi.ptr = ptrs[i];
vi.nFrame = cnt;
vi.edgeID = -1;
vi.isTrack = 0;
vi.nTrack = 0;
(*m_gNodeMap)[xi] = vi;
// outNode
ListGraph::Node xo = m_g.addNode();
V vo;
vo.id = m_g.id(xo);
vo.isIn = 0;
vo.ptr = ptrs[i];
vo.nFrame = cnt;
vo.edgeID = -1;
vo.isTrack = 0;
vo.nTrack = 0;
(*m_gNodeMap)[xo] = vo;
// connection to previous nodes
for(ListGraph::NodeIt n(m_g); n != INVALID; ++n){
if((*m_gNodeMap)[n].nFrame != vi.nFrame && !(*m_gNodeMap)[n].isIn){
// double weight = gain((*m_gNodeMap)[n], vi);
double weight = gain((*m_gNodeMap)[m_g.nodeFromId(m_g.id(n)-1)], vi);
// m_g.nodeFromId(m_g.id((*m_gNodeMap)[n])-1);
(*m_gEdgeMap)[m_g.addEdge(n,xi)] = weight;
// ListGraph::Edge e = m_g.addEdge(n,xi);
// (*m_gEdgeMap)[e] = weight;
// (*m_gNodeMap)[m_g.u(e)].edgeID = m_g.id(e);
// (*m_gNodeMap)[m_g.v(e)].edgeID = m_g.id(e);
}
}
}
}
bool Overlay::process_batch_command (const std::string& cmd, const std::string& args)
{
// BATCH_COMMAND overlay.load path # Loads the specified image on the overlay tool.
if (cmd == "overlay.load") {
VecPtr<MR::Image::Header> list;
try { list.push_back (new MR::Image::Header (args)); }
catch (Exception& e) { e.display(); }
add_images (list);
return true;
}
// BATCH_COMMAND overlay.opacity value # Sets the overlay opacity to floating value [0-1].
else if (cmd == "overlay.opacity") {
try {
float n = to<float> (args);
opacity_slider->setSliderPosition(int(1.e3f*n));
}
catch (Exception& e) { e.display(); }
return true;
}
return false;
}
void IMFT<Ptr>::setFrame(VecPtr ptrs)
{
cnt++;
if(m_isDebug)
std::cout << "||CNT : " << cnt << " || # of objects : " << ptrs.size() << " || # of tracks : " << m_tracks.size() << " ||" <<std::endl;
if(cnt > m_nWindow) movingWindow();
addToDGraph(ptrs);
tracking();
if(cnt == m_nWindow) {
backtracking();
makeTracks();
}
else if(cnt > m_nWindow){
trackUpdate();
// setCurrentFrame(cnt);
}
setCurrentFrame(cnt);
confirmDGraph();
}
void PerformanceEvaluator::CalculateRM(VecPtr vec) {
CalculateAverage(vec);
transform(vec->begin(),vec->end(),vec->begin(),[](const double& x) {return sqrt(x);});
}
void PerformanceEvaluator::CalculateAverage(VecPtr vec) {
transform(vec->begin(),vec->end(),vec->begin(),[this](const double& x) {return x/(1.0*_runCount);});//multiply by 1.0 to make it a double
}
std::size_t getVectorCapacity(VecPtr vecPtr)
{
return vecPtr->capacity();
}