/********************************************************************
** Image Component Library (ICL) **
** **
** Copyright (C) 2006-2013 CITEC, University of Bielefeld **
** Neuroinformatics Group **
** Website: www.iclcv.org and **
** http://opensource.cit-ec.de/projects/icl **
** **
** File : ICLCV/src/ICLCV/PositionTracker.cpp **
** Module : ICLCV **
** Authors: Christof Elbrechter, Robert Haschke **
** **
** **
** GNU LESSER GENERAL PUBLIC LICENSE **
** This file may be used under the terms of the GNU Lesser General **
** Public License version 3.0 as published by the **
** **
** Free Software Foundation and appearing in the file LICENSE.LGPL **
** included in the packaging of this file. Please review the **
** following information to ensure the license requirements will **
** be met: http://www.gnu.org/licenses/lgpl-3.0.txt **
** **
** The development of this software was supported by the **
** Excellence Cluster EXC 277 Cognitive Interaction Technology. **
** The Excellence Cluster EXC 277 is a grant of the Deutsche **
** Forschungsgemeinschaft (DFG) in the context of the German **
** Excellence Initiative. **
** **
********************************************************************/
#include <ICLCV/PositionTracker.h>
#include <ICLCV/Extrapolator.h>
#include <ICLCV/HungarianAlgorithm.h>
#include <cmath>
#include <set>
#include <limits>
#include <cstdio>
/** new data
| o o x(t) o o
-------------------------+-----------------------
o o o o | d d d d d
|
o o o o | d ...
|
x(t-3) x(t-2) x(t-1) Å·(t)|
|
o o o o |
|
o o o o |
|
Data[0] Data[1] ... | Dist[0], Dist[1], ...
Data Dist
/\
vecPredict
*/
using namespace std;
using namespace icl::utils;
using namespace icl::core;
namespace icl{
namespace cv{
namespace positiontracker{
const int X = 0;
const int Y = 1;
const int BLIND_VALUE = 9999;
}
using namespace positiontracker;
#define SAY(X)
void show_vec(const std::vector<int> &v, const std::string &s){
// {{{ open
printf("%s[%d][ ",s.c_str(),(unsigned int)v.size());
for(unsigned int i=0;i<v.size();i++){
printf("%d ",v[i]);
}
printf("]\n");
}
// }}}
void showDataMatrix(std::deque<std::vector<int> > m[2]){
// {{{ open
int DIM = m[0][0].size();
for(int d=0;d<DIM;d++){
for(int i=0;i<3;i++){
printf("(%3d,%3d) ",m[0][i][d],m[1][i][d]);
}
printf("\n");
}
printf("\n");
}
// }}}
template<class valueType>
void removeElemsFromVector(std::vector<valueType> &v,const std::vector<int> &rows){
// {{{ open
vector<valueType> newV;//(v.size()-rows.size());
int r=0,vidx=0;
for(; vidx<(int)(v.size()) && r<(int)rows.size() ; vidx++){
if(rows[r]!=vidx){
newV.push_back(v[vidx]);// [newvidx++] = v[vidx];
}else{
r++;
}
}
for(;vidx<(int)v.size();vidx++){
newV.push_back(v[vidx]);
}
v=newV;
}
// }}}
template<class valueType>
void removeRowsFromDataMatrix(std::deque<std::vector<valueType> > *m,const std::vector<int> &rows){
// {{{ open
/// rows must be sorted !
for(int d=0; d <= 1; d++){
std::deque<std::vector<valueType> > &md = m[d];
for(int x=0;x<3;x++){
vector<valueType> &v = md[x];
removeElemsFromVector(v,rows);
/*
vector<valueType> newV(v.size()-rows.size());
for(int r=0,vidx=0,newvidx=0;vidx<(int)v.size();vidx++){
if(rows[r]!=vidx){
newV[newvidx++] = v[vidx];
}else{
r++;
}
}
v=newV;
*/
}
}
}
// }}}
template<class valueType>
void PositionTracker<valueType>::pushData(valueType *xys, int n){
// {{{ open
Vec x(n),y(n);
for(int i=0;i<n;i++){
x[i] = xys[2*i];
y[i] = xys[2*i+1];
}
pushData(x,y);
}
// }}}
template<class valueType>
void PositionTracker<valueType>::pushData(std::vector<Point32f> points){
// {{{ open
int n = points.size();
Vec x(n),y(n);
for(int i=0;i<n;i++){
x[i] = points[i].x;
y[i] = points[i].y;
}
pushData(x,y);
}
// }}}
template<class valueType>
Array2D<valueType> createDistMat(const vector<valueType> a[2], const vector<valueType> b[2]){
// {{{ open
ICLASSERT( a[X].size() == b[X].size() );
// DEBUG if( a[X].size() != b[X].size() ) printf("error: %d != %d \n",a[X].size(),b[X].size());
int dim = (int)a[X].size();
Array2D<valueType> m(dim,dim);
for(int i=0;i<dim;++i){
for(int j=0;j<dim;++j){
double dx = a[X][j] - b[X][i];
double dy = a[Y][j] - b[Y][i];
// !! use sqrt of the euclidian distance to stabilize the association procedure (..)
m(i,j) = (int)round (sqrt(sqrt( dx*dx + dy*dy )) );
// m[i][j] = (valueType)sqrt(sqrt (pow((double) (a[X][j] - b[X][i]), 2) +
// pow((double) (a[Y][j] - b[Y][i]), 2) ));
}
}
return m;
}
// }}}
template<class valueType>
inline vector<valueType> predict(int dim, std::deque<std::vector<valueType> > &data, const vector<int> &good){
// {{{ open
vector<valueType> pred;
for(int y=0 ; y < dim; ++y){
//if(y>=(int)good.size()) printf("!!!!!!!!!!!!!! waring dim=%d good.size = %d \n",dim,good.size());
switch(good[y]){
case 1: pred.push_back( data[2][y] ); break;
case 2: pred.push_back( Extrapolator<valueType,int>::predict( data[1][y], data[2][y]) ); break;
default: pred.push_back( Extrapolator<valueType,int>::predict( data[0][y], data[1][y], data[2][y]) ); break;
}
}
// ok old pred.push_back( Extrapolator<valueType,int>::predict( data[0][y], data[1][y], data[2][y] ) );
return pred;
}
// }}}
inline vector<int> get_n_new_ids(const vector<int> ¤tIDS, int n, icl::cv::IDAllocationMode iaMode, int& lowestUnusedID){
// {{{ open
vector<int> ids;
set<int> lut;
switch(iaMode){
case allocateFirstFreeIDs:
for(unsigned int i=0;i<currentIDS.size();i++){
lut.insert( currentIDS[i] );
}
for(int i=0,id=0;i<n;i++){
while(lut.find(id) != lut.end()){
id++;
}
ids.push_back(id);
lut.insert(id);
}
return ids;
case allocateBrandNewIDs:{
for(int i=0;i<n;i++){
ids.push_back(lowestUnusedID);
++lowestUnusedID;
}
return ids;
}
default:
ERROR_LOG("unsupported IDAllocationmode");
return ids;
}
}
// }}}
template<class valueType>
void push_and_rearrange_data(int dim,
std::deque<std::vector<valueType> > data[2],
const vector<int> &assignment,
const vector<valueType> newData[2]){
// {{{ open
vector<valueType> arrangedData[2] = {vector<valueType>(dim),vector<valueType>(dim)};
for(int i=0;i<dim;i++){
arrangedData[X][ assignment[i] ] = newData[X][i];
arrangedData[Y][ assignment[i] ] = newData[Y][i];
}
data[X].push_back(arrangedData[X]);
data[Y].push_back(arrangedData[Y]);
data[X].pop_front();
data[Y].pop_front();
}
// }}}
template<class valueType>
void push_data_intern_diff_zero(int dim,
std::deque<vector<valueType> > data[2],
vector<int> &assignment,
vector<valueType> newData[2],
vector<int> &good){
// {{{ open
vector<valueType> pred[2] = { predict(dim,data[X],good), predict(dim,data[Y],good) };
Array2D<valueType> distMat = createDistMat( pred , newData );
assignment = HungarianAlgorithm<valueType>::apply(distMat);
push_and_rearrange_data(dim, data, assignment, newData);
for(unsigned int i=0;i<good.size();++i){
good[i]++;
}
}
// }}}
template<class valueType>
inline valueType distance(valueType x1, valueType y1, valueType x2, valueType y2){
return (valueType) sqrt( pow((double)(x1-x2),2) + pow((double)(y1-y2),2) );
}
template<class valueType>
bool push_data_first_optimized_try(int dim,
std::deque<vector<valueType> > data[2],
vector<int> &assignment,
vector<valueType> newData[2],
vector<int> &good,
valueType threshold){
// {{{ open
vector<valueType> xsOld(dim);
vector<valueType> ysOld(dim);
for(int y=0 ; y < dim; ++y){
xsOld[y] = data[0][2][y];
ysOld[y] = data[1][2][y];
}
vector<valueType> &xsNew = newData[0];
vector<valueType> &ysNew = newData[1];
vector<bool> assigned(dim,false);
vector<int> newAssignment(dim,-1);
// printf("==================== step ============\n");
//printf("optimzation ... for %d centers\n",dim);
for(int i=0;i<dim;i++){
//printf("processing step old data index %d \n",i);
// find best hit for nr i
valueType minDist = numeric_limits<valueType>::max();
int minIdx = -1;
for(int j=0;j<dim;j++){
valueType dist = distance(xsOld[i],ysOld[i],xsNew[j],ysNew[j]);
if(dist < minDist){
// printf("found new data %d was only %d away \n",j,dist);
minDist = dist;
minIdx = j;
}
}
// printf("ok, index %d was best for old index
// %d distance is %d thresh is %d \n",
// minIdx,i,minDist, threshold);
if(minDist < threshold && !assigned[minIdx]){
assigned[minIdx] = true;
newAssignment[minIdx] = i;
}else{
return false;
}
}
assignment = newAssignment;
push_and_rearrange_data(dim, data, assignment, newData);
for(unsigned int i=0;i<good.size();++i){
good[i]++;
}
return true;
}
// }}}
template<class valueType>
void push_data_intern_diff_gtz(int DIFF,
deque<vector<valueType> > data[2],
vector<int> &ids,
vector<int> &assignment,
vector<valueType> newData[2],
vector<int> &good){
// {{{ open
// new data contains less points -> enlarge new new data
for(int i=0;i<DIFF;i++){
newData[X].push_back(BLIND_VALUE);
newData[Y].push_back(BLIND_VALUE);
}
int dim = data[X][0].size();
vector<valueType> pred[2] = { predict(dim,data[X],good), predict(dim,data[Y],good) };
Array2D<valueType> distMat = createDistMat( pred , newData );
assignment = HungarianAlgorithm<valueType>::apply(distMat);
push_and_rearrange_data(dim, data, assignment, newData);
vector<int> delRows;
for(int i=0;i<DIFF;i++){
delRows.push_back( assignment [dim-1-i]);
}
std::sort(delRows.begin(),delRows.end());
removeRowsFromDataMatrix(data,delRows);
removeElemsFromVector(ids, delRows);
removeElemsFromVector(good, delRows);
for(unsigned int i=0;i<good.size();++i){
good[i]++;
}
}
// }}}
template<class valueType>
void push_data_intern_diff_ltz(int DIFF,
deque<vector<valueType> > data[2],
vector<int> &ids,
vector<int> &assignment,
vector<valueType> newData[2],
vector<int> &good,
icl::cv::IDAllocationMode iaMode,
int &lowestUnusedID){
// {{{ open
DIFF *= -1; // now positive
for(int j=0;j<DIFF;j++){
for(int i=0;i<3;i++){
data[X][i].push_back(BLIND_VALUE);
data[Y][i].push_back(BLIND_VALUE);
}
}
int dim = data[X][0].size();
/// good vector is temporarily filled with ones for prediction
for(int i=0;i<DIFF;i++){
good.push_back(1);
}
vector<valueType> pred[2] = { predict(dim,data[X],good), predict(dim,data[Y],good) };
/// restore good
good.resize(good.size()-DIFF);
Array2D<valueType> distMat = createDistMat( pred , newData );
assignment = HungarianAlgorithm<valueType>::apply(distMat);
/// <old>
//vector<int> newDataCols;
vector<valueType> newDataColsValues[2];
for(int x=0;x<dim;++x){ // x is the col index of newData
if(assignment[x] >= dim-DIFF){
newDataColsValues[X].push_back(newData[X][ assignment[x] ]);
newDataColsValues[Y].push_back(newData[Y][ assignment[x] ]);
}
}
if((int)newDataColsValues[X].size() != DIFF){
printf("WARNING: newDataColsValues[X].size()[%d] is != DIFF[%d]",(int)newDataColsValues[X].size(),DIFF);
}
vector<int> newIDS = get_n_new_ids(ids,DIFF,iaMode, lowestUnusedID);
for(int i=0;i<DIFF;i++){
for(int j=0;j<3;j++){
data[X][j][dim-DIFF+i] = newDataColsValues[X][i]; //newData[X][newDataCols[i]];
data[Y][j][dim-DIFF+i] = newDataColsValues[Y][i]; //newData[Y][newDataCols[i]];
}
ids.push_back( newIDS[i] );
good.push_back( 0 );
}
for(unsigned int i=0;i<good.size();++i){
good[i]++;
}
push_and_rearrange_data(dim, data, assignment, newData);
}
// }}}
template<class valueType>
void push_data_intern_first_step(deque<vector<valueType> > data[2],
vector<int> &ids,
vector<int> &assignment,
vector<valueType> newData[2],
vector<int> &good){
// {{{ open
for(int i=0;i<3;i++){
data[X].push_back(newData[X]);
data[Y].push_back(newData[Y]);
}
ids.resize(newData[X].size());
for(unsigned int i=0;i<newData[X].size();i++){
ids[i]=i;
good.push_back(1);
}
}
// }}}
template<class valueType>
void PositionTracker<valueType>::pushData(const Vec &dataXs, const Vec &dataYs){
// {{{ open
ICLASSERT_RETURN( dataXs.size() > 0 );
ICLASSERT_RETURN( dataXs.size() == dataYs.size() );
Vec newData[2] = {dataXs, dataYs};
if(!m_matData[X].size()){
push_data_intern_first_step(m_matData, m_vecIDs, m_vecCurrentAssignment, newData, m_vecGoodDataCount);
return;
}
const int DATA_MATRIX_HEIGHT = (int)(m_matData[X][0].size());
const int NEW_DATA_DIMENSION = (int)(dataXs.size());
const int DIFF = DATA_MATRIX_HEIGHT - NEW_DATA_DIMENSION;
if(DIFF < 0){
push_data_intern_diff_ltz(DIFF,m_matData, m_vecIDs, m_vecCurrentAssignment, newData,m_vecGoodDataCount, m_IDAllocationMode, m_currentID);
}else if(DIFF > 0){
push_data_intern_diff_gtz(DIFF,m_matData, m_vecIDs, m_vecCurrentAssignment, newData,m_vecGoodDataCount);
}else{
if(m_bTryOptimize && m_tThreshold > 0){
bool succ = push_data_first_optimized_try(DATA_MATRIX_HEIGHT,m_matData,m_vecCurrentAssignment,newData,m_vecGoodDataCount,m_tThreshold);
if(!succ){
push_data_intern_diff_zero(DATA_MATRIX_HEIGHT,m_matData, m_vecCurrentAssignment, newData,m_vecGoodDataCount);
}
}else{
push_data_intern_diff_zero(DATA_MATRIX_HEIGHT,m_matData, m_vecCurrentAssignment, newData,m_vecGoodDataCount);
}
}
}
// }}}
// {{{ old code
/*
if(!m_matData[X].size()){ // first step
for(int i=0;i<3;i++){
m_matData[X].push_back(dataXs);
m_matData[Y].push_back(dataYs);
}
}
m_matData[X][0].size(),m_matData[X][1].size(),m_matData[X][2].size(),
m_matData[Y][0].size(),m_matData[Y][1].size(),m_matData[Y][2].size());
//static const valueType BLIND_VALUE = valueType(999);
//const int DATA_MATRIX_HEIGHT = (int)(m_matData[X][0].size());
//const int NEW_DATA_DIMENSION = (int)(dataXs.size());
//const int DIFF = DATA_MATRIX_HEIGHT - NEW_DATA_DIMENSION;
// create a working copy of the new data
Vec newData[2] = {dataXs, dataYs};
if(DIFF > 0){ // redundant
// new data contains less points -> enlarge new new data
for(int i=0;i<DIFF;i++){
newData[X].push_back(BLIND_VALUE);
newData[Y].push_back(BLIND_VALUE);
}
}
if(DIFF < 0){ // todo optimize later
printf("part 5.a \n");
// push lines to the data matrix
for(int j=0;j<-DIFF;j++){
for(int i=0;i<3;i++){
m_matData[X][i].push_back(BLIND_VALUE);
m_matData[Y][i].push_back(BLIND_VALUE);
}
}
}
const int DATA_AND_MATRIX_DIM = (int)(newData[X].size());
Vec vecPrediction[2];
m_matData[X][0].size(),m_matData[X][1].size(),m_matData[X][2].size(),
m_matData[Y][0].size(),m_matData[Y][1].size(),m_matData[Y][2].size());
for(int y=0 ; y < DATA_AND_MATRIX_DIM; ++y){
vecPrediction[X].push_back( Extrapolator<valueType,int>::predict( m_matData[X][0][y], m_matData[X][1][y], m_matData[X][2][y] ) );
vecPrediction[Y].push_back( Extrapolator<valueType,int>::predict( m_matData[Y][0][y], m_matData[Y][1][y], m_matData[Y][2][y] ) );
}
Array2D<valueType> distMat(DATA_AND_MATRIX_DIM,DATA_AND_MATRIX_DIM);
for(int x=0;x<DATA_AND_MATRIX_DIM;++x){
for(int y=0;y<DATA_AND_MATRIX_DIM;++y){
distMat[x][y] = (valueType)sqrt (pow( vecPrediction[X][y] - newData[X][x], 2) + pow( vecPrediction[Y][y] - newData[Y][x], 2) );
}
}
m_vecCurrentAssignement = HungarianAlgorithm<valueType>::apply(distMat);
// add the new data column -> by assingned data elements
if(DIFF > 0){
vector<int> delRows;
for(int i=0;i<DIFF;i++){
delRows.push_back(m_vecCurrentAssignement[DATA_AND_MATRIX_DIM-1-i]);
}
removeRowsFromDataMatrix(m_matData,delRows);
newData[X].resize(newData[X].size()-DIFF);
newData[X].resize(newData[Y].size()-DIFF);
}else if(DIFF < 0){
vector<int> newDataCols;
for(int i=0;i<DATA_AND_MATRIX_DIM;i++){
if(m_vecCurrentAssignement[i] >= DATA_AND_MATRIX_DIM+DIFF){
newDataCols.push_back(i); //ERROR not i but assignment[i] !!
}
}
ICLASSERT( (int)newDataCols.size() == -DIFF );
for(int i=0;i<(-DIFF);i++){
for(int j=0;j<3;j++){
m_matData[X][j][DATA_AND_MATRIX_DIM+DIFF-i] = newData[X][newDataCols[i]];
m_matData[Y][j][DATA_AND_MATRIX_DIM+DIFF-i] = newData[Y][newDataCols[i]];
}
}
}
/// rearrange the new data arrays
vector<valueType> rearrangedData[2] = { vector<valueType>(DATA_AND_MATRIX_DIM), vector<valueType>(DATA_AND_MATRIX_DIM)} ;
for(int i=0;i < DATA_AND_MATRIX_DIM ;i++){
rearrangedData[X][i] = newData[X][ m_vecCurrentAssignement[i] ];
rearrangedData[Y][i] = newData[Y][ m_vecCurrentAssignement[i] ];
}
if(DIFF > 0){
rearrangedData[X].resize(DATA_AND_MATRIX_DIM-DIFF);
rearrangedData[Y].resize(DATA_AND_MATRIX_DIM-DIFF);
}
m_matData[X].push_back( rearrangedData[X] );
m_matData[Y].push_back( rearrangedData[Y] );
m_matData[X].pop_front();
m_matData[Y].pop_front();
*/
// }}}
template<class valueType>
int PositionTracker<valueType>::getID(valueType x, valueType y){
// {{{ open
vector<valueType> &rX = m_matData[X][2];
vector<valueType> &rY = m_matData[Y][2];
for(unsigned int i=0;i<rX.size();++i){
if(rX[i] == x && rY[i] == y){
return m_vecIDs[i];
}
}
return -1;
}
// }}}
template<class valueType>
int PositionTracker<valueType>::getID(int index){
// {{{ open
if(index >= 0 && index < (int)m_vecCurrentAssignment.size()){
return m_vecIDs[ m_vecCurrentAssignment[index] ];
}else{
return -1;
}
}
// }}}
template ICLCV_API class PositionTracker<icl32s>;
template ICLCV_API class PositionTracker<icl32f>;
//template class PositionTracker<icl64f>;
} // namespace cv
}