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