Exemplo n.º 1
0
/** Trys to match a given buoy classified in the image to
    data in the ground plane. **/
void BuoyNav::MatchBuoy(const Vision::Return& buoy,
                        const Buoy::Color color,
                        Buoy::List& buoysFound)
{
    Point3D::List polygon;
    Point3D vertex;

    vertex = this->mHomography.FromImageToGround(Point3D(buoy.mMinI, buoy.mMinJ, 0), mInitFrame);
    polygon.push_back(vertex);

    vertex = this->mHomography.FromImageToGround(Point3D(buoy.mMaxI, buoy.mMinJ, 0), mInitFrame);
    polygon.push_back(vertex);

    vertex = this->mHomography.FromImageToGround(Point3D(buoy.mMaxI, buoy.mMaxJ, 0), mInitFrame);
    polygon.push_back(vertex);

    vertex = this->mHomography.FromImageToGround(Point3D(buoy.mMinI, buoy.mMaxJ, 0), mInitFrame);
    polygon.push_back(vertex);

    vertex = this->mHomography.FromImageToGround(Point3D((buoy.mMaxI + buoy.mMinI)/2.0, buoy.mMaxJ, 0), mInitFrame);

    int matched = -1;
    double minDistance = std::numeric_limits<double>::max();
    for(unsigned int i = 0; i < (unsigned int)buoysFound.size(); i++)
    {
        if(buoysFound[i].GetBuoyColor() == Buoy::Unknown)
        {
            // Try determine the best match within reason.

            double distance = vertex.Distance(buoysFound[i].GetOrigin());
            if( (distance < mBuoyMatchDistance && distance < minDistance) ||
                    buoysFound[i].GetOrigin().IsInside2D(polygon))
            {
                matched = (int)i;
                minDistance = distance;
            }
        }
    }
    if(matched >= 0)
    {
        buoysFound[matched].SetBuoyColor(color);
    }
    else
    {
    }
}
Exemplo n.º 2
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]);
        }
    }


}
Exemplo n.º 3
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]);
        }
    }
}
Exemplo n.º 4
0
////////////////////////////////////////////////////////////////////////////////////
///
///   \brief Processes message received by the Service.  If not supported, then
///          message is passed to inheriting services.
///
///   This Service supports the following message: Report Image
///
///   \param[in] message Message data to process.
///
////////////////////////////////////////////////////////////////////////////////////
void RangeSubscriberExtras::Receive(const Message* message)
{
    switch(message->GetMessageCode())
    {
    case REPORT_LOCAL_RANGE_SCAN:
        {
            const ReportLocalRangeScan* report = 
                    dynamic_cast<const ReportLocalRangeScan*>(message);
            if(report)
            {
                Point3D::List scan;
                Point3D::List scanPolar;
                mRangeSensorMutex.Lock();
                
                std::map<Address, RangeSensorConfig::Map>::iterator comp;
                comp = mRangeSensors.find(report->GetSourceID());
                if(comp != mRangeSensors.end())
                {
                    RangeSensorConfig::Map::iterator config = comp->second.find(report->GetSensorID());
                    if(config != comp->second.end())
                    {
                        // Convert the data using the configuration info.
                        ReportLocalRangeScan::Scan::const_iterator v;
                        const ReportLocalRangeScan::Scan* ptr = report->GetScan();
                        double angle = config->second.mScanAngle/-2.0;
                        double incr = config->second.mAngleIncrement;
                        double divider = 1000.0;
                        if(config->second.mUnitType == RangeSensorConfig::CM)
                        {
                            divider = 100.0;
                        }

                        for(v = ptr->begin();
                            v != ptr->end();
                            v++)
                        {
                            Point3D point((*v)/divider);  // Convert to meters.
                            Point3D polar(point.mX);
                            
                            // Convert to cartesian coordinates, relative to
                            // the sensor.
                            point = point.Rotate(-angle, Point3D::Z, false);
                            
                            // Now translate and rotate relative to the
                            // platform frame.                                                     
                            point = point.Rotate(report->GetSensorOrientation().mX, Point3D::X, false);
                            point = point.Rotate(report->GetSensorOrientation().mY, Point3D::Y, false);
                            point = point.Rotate(-report->GetSensorOrientation().mZ, Point3D::Z, false);
                            point += report->GetSensorLocation();
                            
                            // Now add to final scan.
                            scan.push_back(point);

                            polar.mY = atan2(point.mZ, point.mX);
                            polar.mZ = atan2(point.mY, point.mX);
                            scanPolar.push_back(polar);

                            angle += incr;
                        }                        
                    }
                }

                mRangeSensorMutex.Unlock();

                ProcessLocalRangeScan(scan,
                                      report->GetSourceID(),
                                      report->GetSensorID(),
                                      report->GetTimeStamp());

                mRangeCallbacksMutex.Lock();

                Callback::Set::iterator cb;
                for(cb = mRangeCallbacks.begin();
                    cb != mRangeCallbacks.end();
                    cb++)
                {
                    (*cb)->ProcessLocalRangeScan(scan,
                                                 report->GetSourceID(),
                                                 report->GetSensorID(),
                                                 report->GetTimeStamp());
                    (*cb)->ProcessLocalRangeScanPolar(scanPolar,
                                                      report->GetSourceID(),
                                                      report->GetSensorID(),
                                                      report->GetTimeStamp());
                }

                mRangeCallbacksMutex.Unlock();
                
            }
        }
        break;
    case REPORT_RANGE_SENSOR_CONFIGURATION_EXTRAS:
        {
            const ReportRangeSensorConfiguration* report = 
                dynamic_cast<const ReportRangeSensorConfiguration*>(message);
            if(report)
            {
                Mutex::ScopedLock lock(&mRangeSensorMutex);
                RangeSensorConfig::List::const_iterator c;
                for(c = report->GetConfiguration()->begin();
                    c != report->GetConfiguration()->end();
                    c++)
                {
                    mRangeSensors[report->GetSourceID()][c->mID] = *c;
                }
            }
        }
        break;
    default:
        break;
    }
}