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];
    }
Esempio n. 3
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();
    }
Esempio n. 4
0
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);
           }
       }
    }
}
Esempio n. 5
0
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;
}
Esempio n. 6
0
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);

            }
        }
    }
}
Esempio n. 7
0
        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;
        }
Esempio n. 8
0
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
}
Esempio n. 11
0
 std::size_t getVectorCapacity(VecPtr vecPtr)
 {
     return vecPtr->capacity();
 }