930 lines
24 KiB
Text
930 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;
|
||
|
|
}
|