////////////////////////////////////////////////////////////////////////////////////
///
/// \brief Calculates/generates values for objects based on a scan in
/// polar coordinates.
///
////////////////////////////////////////////////////////////////////////////////////
void ObjectScan::CalculatePolar(const Point3D::List& rangeScan)
{
std::vector<int> startIndex;
std::vector<int> endIndex;
startIndex.push_back(0);
for(int i =0; i < (int)rangeScan.size()-1;i++)
{
double derivative = rangeScan[i+1].mX - rangeScan[i].mX;
if(fabs(derivative) > mDerivativeThreshold)
{
endIndex.push_back(i);
startIndex.push_back(i+1);
}
}
endIndex.push_back(rangeScan.size()-1);
Point3D tmp;
Point3D::List allObjects;
for(int i=0;i < (int)startIndex.size();i++)
{
if(endIndex[i] - startIndex[i] > 2)
{
tmp = (rangeScan[startIndex[i]] + rangeScan[endIndex[i]]) / 2;
allObjects.push_back(tmp);
}
}
mObjects.clear();
for(int i=0;i < (int)allObjects.size();i++)
{
bool flag = false;
for(int j=0; j < (int)mObjects.size();j++)
{
if( ((allObjects[i].mX * allObjects[i].mX)
+ (mObjects[j].mX * mObjects[j].mX))
- ((2*allObjects[i].mX * mObjects[j].mX)
* cos(allObjects[i].mY - mObjects[j].mY)) < mDistanceThreshold * mDistanceThreshold)
{
mObjects[j]= allObjects[i] + mObjects[j] / 2;
flag = true;
break;
}
}
if(flag == false)
{
mObjects.push_back(allObjects[i]);
}
}
}
////////////////////////////////////////////////////////////////////////////////////
///
/// \brief Calculates/generates values for objects based on a scan in
/// cartesian coordinates.
///
/// \param[in] rangeScan LIDAR scan data.
/// \param[in] giveRadius If true, radius of object is saved (z component).
/// \param[in] angleRange Angle of laser (total scan range) (degrees).
/// \param[in] startAngle Angle to start calculations from (degrees).
/// \param[in] endAngle Angle to end calculations at (degrees).
///
////////////////////////////////////////////////////////////////////////////////////
void ObjectScan::CalculateCartesian(const CxUtils::Point3D::List& rangeScan,
bool giveRadius,
double angleRange,
double startAngle,
double endAngle)
{
std::vector<int> startIndex;
std::vector<int> endIndex;
int startI = (int)((rangeScan.size()/2) + ((startAngle/(angleRange/2.0)) * (rangeScan.size()/2)));
if(startI<0)
{
startI=0;
}
int endI = (int)((rangeScan.size()/2) + ((endAngle/(angleRange/2.0)) * (rangeScan.size()/2)));
if(endI >= (int)rangeScan.size())
{
endI=rangeScan.size();
}
startIndex.push_back(startI);
for(int i =startI;i<endI-1;i++)
{
if(AI::Utility::CalcDistance(rangeScan[i].mX,rangeScan[i].mY,rangeScan[i+1].mX,rangeScan[i+1].mY)>mDerivativeThreshold)
{
endIndex.push_back(i);
startIndex.push_back(i+1);
}
}
endIndex.push_back(endI-1);
Point3D tmp;
Point3D::List allObjects;
std::vector<double> allRadii;
for(int i=0;i< (int)startIndex.size();i++)
{
if(endIndex[i] - startIndex[i] > 2)
{
tmp = (rangeScan[startIndex[i]] + rangeScan[endIndex[i]]) / 2;
tmp.mZ=endIndex[i]-startIndex[i];
double radius=(rangeScan[startIndex[i]] - rangeScan[endIndex[i]]).Magnitude()/2.0;
allRadii.push_back(radius);
allObjects.push_back(tmp);
}
}
std::vector<double> tmpRadii;
Point3D::List tmpObjects;
for(int i=0;i< (int)allObjects.size();i++)
{
if(AI::Utility::CalcDistance(0.0,0.0,allObjects[i].mX,allObjects[i].mY) < mMinDistance)
{
continue;
}
bool flag = false;
for(int j=0; j < (int)tmpObjects.size();j++)
{
double tmpDist=AI::Utility::CalcDistance(tmpObjects[j].mX,tmpObjects[j].mY,allObjects[i].mX,allObjects[i].mX);
if( tmpDist < mDistanceThreshold )
{
tmpObjects[j].mX = (allObjects[i].mX*allObjects[i].mZ + tmpObjects[j].mX*tmpObjects[j].mZ)/(tmpObjects[j].mZ+allObjects[i].mZ);
tmpObjects[j].mY = (allObjects[i].mY*allObjects[i].mZ + tmpObjects[j].mY*tmpObjects[j].mZ)/(tmpObjects[j].mZ+allObjects[i].mZ);
//grow radius
if(tmpDist>tmpRadii[j])
{
tmpRadii[j]=tmpDist;
}
tmpObjects[j].mZ+=allObjects[i].mZ;
flag = true;
break;
}
}
if(flag == false)
{
allObjects[i].mZ=allObjects[i].mZ;
tmpRadii.push_back(allRadii[i]);
tmpObjects.push_back(allObjects[i]);
}
}
mObjects.clear();
for(int i= (int)(tmpObjects.size()-1);i>=0;i--)
{
if(AI::Utility::CalcDistance(0.0,0.0,tmpObjects[i].mX,tmpObjects[i].mY) < mMaxDistance)
{
if(giveRadius)
{
tmpObjects[i].mZ=tmpRadii[i];
}
mObjects.push_back(tmpObjects[i]);
}
}
}
