3dpcp/.svn/pristine/35/35308cc3cdb801e83a4d492f5c93141c5f7f2fa5.svn-base
2012-09-16 14:33:11 +02:00

929 lines
24 KiB
Text

/*
* trackermanager implementation
*
* Copyright (C) ZhangLiang, YuanJun, Li Wei, Li Ming, Andreas Nuechter
*
* Released under the GPL version 3.
*
*/
/**
* @file
* @brief Main programm for dynamic Velodyne SLAM
*
* @author Andreas Nuechter. Jacobs University Bremen, Germany
* @author YuanJun, Wuhan University, China
* @author ZhangLiang, Wuhan University, China
* @author Li Wei, Wuhan University, China
* @author Li Ming, Wuhan University, China
*/
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include<cmath>
#ifdef _MSC_VER
#include <windows.h>
#else
#include <dlfcn.h>
#endif
#include "slam6d/scan.h"
#include "slam6d/globals.icc"
#include "veloslam/color_util.h"
#include "veloslam/veloscan.h"
#include "veloslam/trackermanager.h"
#include "veloslam/debugview.h"
#include "veloslam/kalmanfilter.h"
#include "veloslam/lap.h"
#define KG 35
#define BIGNUM 100000
int sliding_window_size = 6;
int current_sliding_window_pos =0;
Trajectory VelodyneTrajectory;
VeloScan * g_pfirstScan;
float constant_static_or_moving = 20.0;
int GetScanID_in_SlidingWindow(int absNO, int current_pos, int window_size)
{
int firstNO=-1;
// relative scanID absolute scanID
// absNO (6,7,8,9) current_pos(6,7,8,9) window_size =6
#ifdef NO_SLIDING_WINDOW
firstNO = absNO;
#else
if( (absNO >=(current_pos - window_size) )
&& (absNO <= current_pos) )
{
firstNO =absNO - (current_pos - window_size );
}
else
{
//in first scans
firstNO = -1;
}
if(current_pos < window_size)
{
//in first scans
firstNO = absNO;
}
#endif
// cout << " absNO " << absNO << " firstNO "<<firstNO
// << " current_pos " << current_pos << " window_size "<< window_size <<endl ;
return firstNO;
}
TrackerManager::TrackerManager(void)
{
colorIdx=0;
rollAngle=0;
trackerStartID=0;
delta_Pos[0]=delta_Pos[1]=delta_Pos[2]=0;
}
TrackerManager::~TrackerManager(void)
{
}
int TrackerManager::Init(void)
{
trackerStartID=0;
return 0;
}
/** @brief Initialize function */
int TrackerManager::getNumberofTracker(void)
{
return tracks.size();
}
int TrackerManager::HandleScan(VeloScan& scanRef,int trackingAlgo)
{
//each scan only process once.
if(scanRef.isTrackerHandled==true)
return 0;
else
scanRef.isTrackerHandled=true;
//cout<<"HandleScan is running!"<<endl;
int i;
int size=scanRef.scanClusterArray.size();
//how many cluster need process.
if(size==0)
return 0;
// maybe the problem for the scans allway changed.
clusterStatus.resize(size);
// all the objects in currect scan should be no matched and filtered.
for(i=0; i<size;++i)
{
clusterStatus[i].Matched=false;
clusterStatus[i].FilterRet=false;
}
//filter not good target object
FilterObject(scanRef);
//update same tracker this is need matching
switch (trackingAlgo)
{
case 0:
UpdateTrackers(scanRef);
break;
case 1:
MatchTracksWithClusters(scanRef);
break;
}
//add no tracked objects in to new tracker.
AddTrackers(scanRef);
//Remove No used trackers
RemoveNoUsedTracker(scanRef);
//preScan=&scanRef;
return 0;
}
int TrackerManager::FilterObject(VeloScan& scanRef)
{
int i;
int size=scanRef.scanClusterArray.size();
scanRef.clusterNum=0;
for(i=0; i<size;++i)
{
clusterFeature &glu=scanRef.scanClusterFeatureArray[i];
cluster &gluData=scanRef.scanClusterArray[i];
clusterStatus[i].FilterRet = TrackerManager::TrackerFilter(glu, gluData);
if (clusterStatus[i].FilterRet)
{
scanRef.clusterNum++;
}
}
cout<<"scanID: "<<scanRef.scanid<<" clusters to be tracked: "<<scanRef.clusterNum<<endl;
return 0;
}
bool TrackerManager::TrackerFilter(clusterFeature &glu, cluster &gluData)
{
if(FilterNOMovingObjcets(glu, gluData))
return true;
else
return false;
}
//MatchTrackers with Kalman filters.
int TrackerManager::MatchTrackers(VeloScan& scanRef,Tracker& tracker,float kg)
{
int i;
int minID=-1;
float value,minValue=360.0;
float radiusDiff;
float thetaDiff;
float sizeDiff;
float positionDiff;
float shapeDiff;
CMatrix standardDeviation(2,2),measurementErro(2,1);
Measurement predictMeasurement;
bool IsSmaller1, IsSmaller2, flag=false;
clusterFeature temp;
ObjectState movestate;
predictMeasurement=tracker.kalmanFilter.GetPredictMeasurement(rollAngle,delta_Pos);
standardDeviation=tracker.kalmanFilter.CalMeasureDeviation();
for(i=0; i<scanRef.scanClusterArray.size();++i)
{
clusterFeature &glu=scanRef.scanClusterFeatureArray[i];
cluster &gluData=scanRef.scanClusterArray[i];
// filter out other objects.
if( clusterStatus.size()!=0 && clusterStatus[i].FilterRet==false)
// if( clusterStatus[i].FilterRet==false)
continue;
// allways get the moving for current scans
measurementErro.m_pTMatrix[0][0]= glu.avg_x - predictMeasurement.x_measurement;
measurementErro.m_pTMatrix[1][0]= glu.avg_z - predictMeasurement.z_measurement;
IsSmaller1=(fabs(measurementErro.m_pTMatrix[0][0]) < kg*standardDeviation.m_pTMatrix[0][0]);
IsSmaller2=(fabs(measurementErro.m_pTMatrix[1][0]) < kg*standardDeviation.m_pTMatrix[1][1]);
if (IsSmaller1 && IsSmaller2)
{
// cout<<"The "<<i<<"th object is matching with tracker "<<tracker.trackerID<<endl
// <<"with predictErro_x:"<<measurementErro.m_pTMatrix[0][0]<<"with predictErro_z:"<<measurementErro.m_pTMatrix[1][0]<<endl;
if(tracker.statusList.size() >0)
{
radiusDiff=fabs(tracker.statusList.back().radius-glu.radius);
thetaDiff=fabs(tracker.statusList.back().theta-glu.theta);
sizeDiff =abs(tracker.statusList.back().size-glu.size);
shapeDiff =fabs( fabs(tracker.statusList.back().size_x-glu.size_x) +
fabs(tracker.statusList.back().size_y-glu.size_y) +
fabs(tracker.statusList.back().size_z-glu.size_z) );
positionDiff = sqrt( (tracker.statusList.back().avg_x -glu.avg_x)*(tracker.statusList.back().avg_x -glu.avg_x) +
(tracker.statusList.back().avg_z -glu.avg_z)*(tracker.statusList.back().avg_z -glu.avg_z)
) ;
}
value= radiusDiff*1.0 + thetaDiff*1.0 + sizeDiff*0.8 + shapeDiff*0.8 + positionDiff * 0.3 ;
if(value < minValue)
{
minValue=value;
minID=i;
temp=glu;
flag=true;
}
}
}
// find matched tracker.
if (flag)
{
tracker.kalmanFilter.stateUpdate(temp,rollAngle,delta_Pos);
tracker.kalmanFilter.timeUpdate();
// movestate=tracker.kalmanFilter.GetCurrentState();
// cout<<"The tracker with trackerID:"<<tracker.trackerID<<"is matched with the "<<minID<<"th object!"<<endl;
// cout<<"with x_positon:"<<movestate.x_position<<", z_position:"<<movestate.z_positon<<endl;
// cout<<"with x_speed:"<<movestate.x_speed<<", z_speed:"<<movestate.z_speed<<endl;
}
return minID;
}
int TrackerManager::UpdateTrackers(VeloScan& scanRef)
{
int i,j;
int matchID;
list<Tracker>::iterator it;
int trackNO =0;
for(it=tracks.begin() ; it!=tracks.end();it++ )
{
Tracker &tracker=*it;
trackNO ++;
if (!(tracker.missMatch))
{
matchID=MatchTrackers(scanRef,tracker,KG);
}
else
{
matchID=MatchTrackers(scanRef,tracker,1.5*KG);
}
if(matchID==-1)
{
// log miss times of tracker
tracker.missMatch=true;
tracker.missedTime++;
// cout << "missing tracker" << tracker.trackerID
// <<" times " <<tracker.missedTime<<endl;
}
// find match tracker
if(matchID >=0 )
{
scanRef.scanClusterFeatureArray[matchID].trackNO = trackNO;
clusterFeature &glu=scanRef.scanClusterFeatureArray[matchID];
cluster &gluData=scanRef.scanClusterArray[matchID];
glu.frameNO= scanRef.scanid;
glu.selfID = matchID;
// save feature
tracker.statusList.push_back(glu);
tracker.dataList.push_back(gluData);
tracker.matchClusterID=matchID;
tracker.missMatch=false;
clusterStatus[matchID].Matched=true;
// cout << "Match tracker" << tracker.trackerID
// <<" size of " <<tracker.statusList.size()<<endl;
tracker.missedTime=0;
MoveState targetMove;
targetMove.targetState=tracker.kalmanFilter.GetCurrentState();
targetMove.frameNo=scanRef.scanid;
tracker.moveStateList.push_back(targetMove);
//FILE *fp=fopen("F:\\VeloSlamTemp\\bin\\Release\\Update.txt","a");
//fprintf(fp,"TrackerID:%d,ScanID:%d,matchID:%d,clusterID:%d\n",tracker.trackerID,scanRef.scanid,matchID,glu.selfID);
//fclose(fp);
}
}
return 0;
}
int TrackerManager::AddTrackers(VeloScan& scanRef)
{
// cout<<"AddTrackers is running!"<<endl;
int i;
int size=scanRef.scanClusterArray.size();
// i is glu id in scan
for(i=0; i<size; ++i)
{
clusterFeature &glu=scanRef.scanClusterFeatureArray[i];
cluster &gluData=scanRef.scanClusterArray[i];
// what cluster should added new tracker
if(clusterStatus[i].FilterRet == true &&
clusterStatus[i].Matched == false)
{
Tracker newTracker(glu,rollAngle);
MoveState targetMove;
targetMove.targetState =newTracker.kalmanFilter.GetCurrentState();
targetMove.frameNo=scanRef.scanid;
newTracker.moveStateList.push_back(targetMove);
newTracker.kalmanFilter.timeUpdate();
// for find scan id for each cluster.
glu.frameNO= scanRef.scanid;
glu.selfID = i;
newTracker.statusList.push_back(glu);
newTracker.dataList.push_back(gluData);
/*cout << "Add track" << TrackerManager::trackerStartID <<" glu.selfID "<< glu.selfID
<<" scanid" <<scanRef.scanid << endl;*/
newTracker.colorIdx=colorIdx;
newTracker.matchClusterID=i;
newTracker.trackerID = TrackerManager::trackerStartID;
tracks.push_back(newTracker);
TrackerManager::trackerStartID++;
colorIdx++;
}
}
return 0;
}
int TrackerManager::ListTrackers()
{
list<Tracker>::iterator it;
cout<<" tracker list : "<<endl;
for(it=tracks.begin() ; it!=tracks.end(); it++)
{
Tracker &tracker=*it;
cout<<" ID "<<tracker.trackerID
<<" matchClusterID "<<tracker.matchClusterID
<<" missMatch " <<tracker.missMatch
<<" glusize " <<tracker.statusList.size()
<<" moving_distance " <<tracker.moving_distance
<<endl;
}
return 0;
}
// removednoUsedTrackers and out of sliding window's scans
int TrackerManager::RemoveNoUsedTracker(VeloScan& scanRef)
{
// cout<<"RemoveNoUsedTracker is running!"<<endl;
// cout << " current_sliding_window_pos: " << current_sliding_window_pos << " current id " << scanRef.scanid<< endl;
list<Tracker>::iterator it;
int trackNO =0;
for(it=tracks.begin() ; it!=tracks.end();)
{
Tracker &tracker=*it;
#ifndef NO_SLIDING_WINDOW
if (!tracker.statusList.empty())
{
deque<clusterFeature>::iterator Iter1;
deque<cluster>::iterator Iter2;
vector<MoveState>::iterator Iter3;
Iter1=tracker.statusList.begin();
Iter2=tracker.dataList.begin();
Iter3=tracker.moveStateList.begin();
do
{
clusterFeature &tempClu=*Iter1;
if(tempClu.frameNO <current_sliding_window_pos - sliding_window_size
|| tempClu.frameNO >current_sliding_window_pos)
{
//cout<<"trackerID: "<<tracker.trackerID<<" cluster "<<tempClu.selfID<<" in "<< tempClu.frameNO<<" scan is deleted!"<<endl;
Iter1=tracker.statusList.erase(Iter1);
Iter2=tracker.dataList.erase(Iter2);
Iter3=tracker.moveStateList.erase(Iter3);
continue;
}
Iter1++;
Iter2++;
Iter3++;
}
while(Iter1!=tracker.statusList.end());
}
#endif
if (tracker.missedTime==4)
{
//cout<<" erase tracker "<<tracker.trackerID<<" is terminated"<<endl;
it= tracks.erase(it);
continue;
}
it++;
}
return 0;
}
void TrackerManager::TrackerManagerReset()
{
clusterStatus.clear();
list<Tracker>::iterator Iter;
for (Iter=tracks.begin();Iter!=tracks.end();Iter++)
{
Tracker temp=*Iter;
temp.TrackerReset();
}
tracks.clear();
return;
}
int TrackerManager::CalculateTrackersFeature(vector <Scan *> allScans, int currentNO ,int windowsize)
{
int i,j,k,colorIdx;
float movement;
list<Tracker>::iterator it;
int n;
for(it=tracks.begin(); it!=tracks.end(); it++)
{
Tracker &tracker=*it;
tracker.moving_distance = 0.0;
int size=tracker.statusList.size();
if ( size <2 )
{
continue;
}
movement =0;
n=0;
for ( i=0;i<size-2;i++)
{
clusterFeature &glu1=tracker.statusList[i];
clusterFeature &glu2=tracker.statusList[i+1];
Point p1(glu1.avg_x, glu1.avg_y, glu1.avg_z);
Point p2(glu2.avg_x, glu2.avg_y, glu2.avg_z);
int firstNO = GetScanID_in_SlidingWindow(glu1.frameNO,
current_sliding_window_pos,
sliding_window_size);
int secondNO = GetScanID_in_SlidingWindow(glu2.frameNO,
current_sliding_window_pos,
sliding_window_size);
if(firstNO <0 || secondNO< 0 )
continue;
Scan *CurrentScan = allScans[firstNO];
Scan *CurrentScanNext = allScans[secondNO];
double deltaMat[16];
double deltaMatNext[16];
GetCurrecntdelteMat(*CurrentScan , deltaMat);
GetCurrecntdelteMat(*CurrentScanNext , deltaMatNext);
// cout << " pose no" << tracker.matchClusterID <<" "
// << p1.x <<" " << p2.x <<" "
//<< p1.y <<" " << p2.y <<" "
//<< p1.z <<" " << p2.z <<" " << movement <<" " << endl;
p1.transform(deltaMat);
p2.transform(deltaMatNext);
#ifdef NO_SLIDING_WINDOW
if(glu1.frameNO>=current_sliding_window_pos - sliding_window_size&&glu2.frameNO <=current_sliding_window_pos)
{
movement += sqrt( (p1.x -p2.x)*(p1.x -p2.x)+(p1.z -p2.z)*(p1.z -p2.z) );
n++;
}
#else
movement += sqrt( (p1.x -p2.x)*(p1.x -p2.x)+(p1.z -p2.z)*(p1.z -p2.z) );
#endif
// cout << " tracker no" << tracker.matchClusterID <<" "
// << p1.x <<" " << p2.x <<" "
//<< p1.y <<" " << p2.y <<" "
//<< p1.z <<" " << p2.z <<" " << movement <<" " << endl;
}
#ifdef NO_SLIDING_WINDOW
if (n!=0)
{
tracker.moving_distance=movement/n;
}
else
{
tracker.moving_distance=0;
}
#else
tracker.moving_distance = movement/(size-1);
#endif
}
return 0;
}
int TrackerManager::MarkClassifiyTrackersResult(vector <Scan *> allScans, int currentNO ,int windowsize)
{
int i,j,k,colorIdx;
float movement;
list<Tracker>::iterator it;
// mark all objets type such as moving or static
for(it=tracks.begin(); it!=tracks.end(); it++)
{
int currentScanNo =0;
Tracker &tracker=*it;
int size=tracker.statusList.size();
// no tracking all for slam
// for tracking classifiation //not transfrom to scan
if (size < 3)
{
for(i=0;i<size;i++)
{
clusterFeature &glu1=tracker.statusList[i];
currentScanNo= GetScanID_in_SlidingWindow(glu1.frameNO,
current_sliding_window_pos,
sliding_window_size);
if(currentScanNo <0 )
continue;
VeloScan *CurrentScan = ( VeloScan *)( allScans[currentScanNo]);
clusterFeature &realglu1=CurrentScan->scanClusterFeatureArray[glu1.selfID];
cluster &realgclu = CurrentScan->scanClusterArray[glu1.selfID];
realglu1.clusterType = CLUSTER_TYPE_STATIC_OBJECT;
for(j =0; j< realgclu.size() ; ++j)
{
cellFeature &gcF = *(realgclu[j]);
gcF.cellType = CELL_TYPE_STATIC;
}
}
continue;
}
for(i=0;i<size;i++)
{
clusterFeature &glu1=tracker.statusList[i];
currentScanNo= GetScanID_in_SlidingWindow(glu1.frameNO,
current_sliding_window_pos,
sliding_window_size);
if(currentScanNo <0 )
continue;
VeloScan *CurrentScan = ( VeloScan *)( allScans[currentScanNo]);
clusterFeature &realglu1=CurrentScan->scanClusterFeatureArray[glu1.selfID];
cluster &realgclu = CurrentScan->scanClusterArray[glu1.selfID];
if(tracker.moving_distance < constant_static_or_moving)
{
realglu1.clusterType = CLUSTER_TYPE_STATIC_OBJECT;
for(j =0; j< realgclu.size() ; ++j)
{
cellFeature &gcF = *(realgclu[j]);
gcF.cellType = CELL_TYPE_STATIC;
}
}
else
{
realglu1.clusterType = CLUSTER_TYPE_MOVING_OBJECT;
for(j =0; j< realgclu.size() ; ++j)
{
cellFeature &gcF = *(realgclu[j]);
gcF.cellType = CELL_TYPE_MOVING;
}
}
}
}
return 0;
}
int TrackerManager::ClassifiyTrackersObjects(vector <Scan *> allScans, int currentNO ,int windowsize)
{
CalculateTrackersFeature(allScans, currentNO, windowsize);
MarkClassifiyTrackersResult(allScans, currentNO , windowsize);
return 0;
}
int TrackerManager::UpdateClustersPoistioninTrackers()
{
int i,j;
list<Tracker>::iterator it;
for(it=tracks.begin() ; it!=tracks.end();it++ )
{
Tracker &tracker=*it;
/*
firstNO = GetScanID_in_SlidingWindow(glu1.frameNO,
current_sliding_window_pos,
sliding_window_size);
secondNO = GetScanID_in_SlidingWindow(glu2.frameNO,
current_sliding_window_pos,
sliding_window_size);
if(firstNO <0 || secondNO< 0 )
continue;
Scan *firstScan = (Scan *)g_pfirstScan;
Scan *CurrentScan = allScans[firstNO];
Scan *CurrentScanNext = allScans[secondNO];
double deltaMat[16];
double deltaMatNext[16];
GetCurrecntdelteMat(*CurrentScan , *firstScan, deltaMat);
GetCurrecntdelteMat(*CurrentScanNext , *firstScan, deltaMatNext);
p1.x = glu1.avg_x; p1.y= glu1.avg_y;p1.z=glu1.avg_z;
p1text.x= glu1.avg_x+150; p1text.y= glu1.avg_y+80; p1text.z=glu1.avg_z+50;
p2.x = glu2.avg_x; p2.y= glu2.avg_y; p2.z=glu2.avg_z;
p2text.x= glu2.avg_x+150; p2text.y= glu2.avg_y+80; p2text.z=glu2.avg_z+50;
*/
}
return 0;
}
void TrackerManager::GetTwoScanRoll(Scan *CurrentScan,Scan *preScan)//???
{
double tempMat[16],deltaMat[16];
M4inv(preScan->get_transMat(), tempMat);
MMult(CurrentScan->get_transMat(), tempMat,deltaMat);
Matrix4ToEuler(deltaMat,delta_Theta,delta_Pos);
//cout<<"delta_Theta: "<<delta_Theta[0]<<" "<<delta_Theta[1]<<" "<<delta_Theta[2]<<endl;
//cout<<"delta_pos: "<<delta_Pos[0]<<" "<<delta_Pos[1]<<" "<<delta_Pos[2]<<endl;
}
CMatrix TrackerManager::ConstructCostMatrix(VeloScan &scanRef,int *clusterIndex)
{
int clusterSize=scanRef.clusterNum;
int trackSize=getNumberofTracker();
int maxSize;
if (clusterSize!=trackSize)
{
maxSize=clusterSize>trackSize?clusterSize:trackSize;
}
else
{
maxSize=clusterSize;
}
CMatrix costMatrix(maxSize,maxSize);
int i=0,j,k;
double costValue;
float radiusDiff;
float thetaDiff;
float sizeDiff;
float positionDiff;
CMatrix standardDeviation(2,2),measurementErro(2,1);
Measurement predictMeasurement;
bool IsSmaller1,IsSmaller2;
float kg;
list<Tracker>::iterator it;
for(it=tracks.begin(); it!=tracks.end(); it++)
{
Tracker &tracker=*it;
predictMeasurement=tracker.kalmanFilter.GetPredictMeasurement(rollAngle,delta_Pos);
standardDeviation=tracker.kalmanFilter.CalMeasureDeviation();
if (tracker.missMatch)
{
kg=1.5*KG;
}
else
{
kg=KG;
}
k=0;
for(j=0;j<scanRef.scanClusterArray.size();j++)
{
clusterFeature &glu=scanRef.scanClusterFeatureArray[j];
cluster &gluData=scanRef.scanClusterArray[j];
if(clusterStatus.size()!=0&&clusterStatus[j].FilterRet==false)
continue;
measurementErro.m_pTMatrix[0][0]=glu.avg_x-predictMeasurement.x_measurement;
measurementErro.m_pTMatrix[1][0]=glu.avg_z-predictMeasurement.z_measurement;
IsSmaller1=(fabs(measurementErro.m_pTMatrix[0][0])<kg*standardDeviation.m_pTMatrix[0][0]);
IsSmaller2=(fabs(measurementErro.m_pTMatrix[1][0])<kg*standardDeviation.m_pTMatrix[1][1]);
if (IsSmaller1&&IsSmaller2)
{
radiusDiff=fabs(tracker.statusList.back().radius-glu.radius);
thetaDiff=fabs(tracker.statusList.back().theta-glu.theta);
sizeDiff =abs(tracker.statusList.back().size-glu.size);
positionDiff = sqrt(sqr(tracker.statusList.back().avg_x -glu.avg_x) + sqr(tracker.statusList.back().avg_z -glu.avg_z) ) ;
costValue= radiusDiff*1.0 + thetaDiff*1.0 + sizeDiff*0.8 + positionDiff * 0.03 ;
costMatrix.m_pTMatrix[i][k]=costValue;
}
else
{
costMatrix.m_pTMatrix[i][k]=BIGNUM;
}
if (i==0)
{
clusterIndex[k]=j;
}
k++;
}
i++;
}
if (clusterSize!=trackSize)
{
if (maxSize==clusterSize)
{
for (i=trackSize;i<maxSize;i++)
{
for (j=0;j<maxSize;j++)
{
costMatrix.m_pTMatrix[i][j]=BIGNUM;
}
}
}
else
{
for (i=0;i<maxSize;i++)
{
for (j=clusterSize;j<maxSize;j++)
{
costMatrix.m_pTMatrix[i][j]=BIGNUM;
}
}
}
}
return costMatrix;
}
int TrackerManager::MatchTracksWithClusters(VeloScan &scanRef)
{
// cout<<"MatchTracksWithClusters is running!"<<endl;
if (tracks.empty())
{
return 0;
}
CMatrix costMatrix;
int clusterSize=scanRef.clusterNum;
int trackSize=getNumberofTracker();
int *clusterIndex;
clusterIndex=new int[clusterSize];
costMatrix=ConstructCostMatrix(scanRef,clusterIndex);
int dim=costMatrix.m_nCol;
//FILE *fp=fopen("F:\\VeloSlamTemp\\bin\\Release\\CostMatix.txt","a");
//fprintf(fp,"scanID:%d\n",scanRef.scanid);
//for (int m=0;m<dim;m++)
//{
// for (int l=0;l<dim;l++)
// {
// fprintf(fp,"%.1f\t",costMatrix.m_pTMatrix[m][l]);
// }
// fprintf(fp,"\n");
//}
//fclose(fp);
int *colsol,*rowsol;
colsol=new int[dim];
rowsol=new int[dim];
double *u,*v;
u=new double[dim];
v=new double[dim];
int i,j;
bool *missFlag;
missFlag=new bool[trackSize];
for (i=0;i<trackSize;i++)
{
missFlag[i]=false;
}
double **assignCost;
assignCost = new double*[dim];
for (i = 0; i < dim; i++)
assignCost[i] = new double[dim];
for (i=0;i<dim;i++)
{
for (j=0;j<dim;j++)
{
assignCost[i][j]=costMatrix.m_pTMatrix[i][j];
}
}
lap(dim,assignCost,rowsol,colsol,u,v);
for (i=0;i<trackSize;i++)
{
int col=rowsol[i];
if (assignCost[i][col]==BIGNUM)
{
missFlag[i]=true;
}
}
int trackerIndex=-1;
int trackNO =0;
list<Tracker>::iterator it;
for(it=tracks.begin() ; it!=tracks.end();it++)
{
Tracker &tracker=*it;
trackNO ++;
trackerIndex++;
if (missFlag[trackerIndex])
{
tracker.missMatch=true;
tracker.missedTime++;
}
else
{
int clusterID=rowsol[trackerIndex];
int matchID=clusterIndex[clusterID];
//cout<<"TrackerID: "<<tracker.trackerID<<"scanID: "<<scanRef.scanid<<"MatchID: "<<matchID<<endl;
scanRef.scanClusterFeatureArray[matchID].trackNO = trackNO;
clusterFeature &glu=scanRef.scanClusterFeatureArray[matchID];
cluster &gluData=scanRef.scanClusterArray[matchID];
glu.frameNO= scanRef.scanid;
glu.selfID = matchID;
tracker.kalmanFilter.stateUpdate(glu,rollAngle,delta_Pos);
tracker.kalmanFilter.timeUpdate();
tracker.statusList.push_back(glu);
tracker.dataList.push_back(gluData);
tracker.matchClusterID=matchID;
tracker.missMatch=false;
tracker.missedTime=0;
clusterStatus[matchID].Matched=true;
MoveState targetMove;
targetMove.targetState=tracker.kalmanFilter.GetCurrentState();
targetMove.frameNo=scanRef.scanid;
tracker.moveStateList.push_back(targetMove);
//FILE *fp=fopen("F:\\VeloSlamTemp\\bin\\Release\\Lap.txt","a");
//fprintf(fp,"TrackerID:%d,ScanID:%d,matchID:%d,clusterID:%d\n",tracker.trackerID,scanRef.scanid,matchID,glu.selfID);
//fclose(fp);
}
}
delete[] assignCost;
delete[] rowsol;
delete[] colsol;
delete[] u;
delete[] v;
delete[] clusterIndex;
delete[] missFlag;
return 0;
}