예제 #1
0
////////////////////////////////////////////////////////////////////////////////////
///
///  \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]);
        }
    }


}
예제 #2
0
////////////////////////////////////////////////////////////////////////////////////
///
///  \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]);
        }
    }
}