//////////////////////////////////////////////////////////////////////////////////// /// /// \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]); } } }
/** 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 { } }
//////////////////////////////////////////////////////////////////////////////////// /// /// \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]); } } }
//////////////////////////////////////////////////////////////////////////////////// /// /// \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; } }