unsigned int NormalDetector::computeInterestPoints(const LaserReading& reading, const std::vector<double>& signal, std::vector<InterestPoint*>& point,
std::vector< std::vector<unsigned int> >& indexes, std::vector<unsigned int>& maxRangeMapping) const
{
point.clear();
point.reserve(reading.getRho().size());
const std::vector<Point2D>& worldPoints = reading.getWorldCartesian();
for(unsigned int i = 0; i < indexes.size(); i++){
for(unsigned int j = 0; j < indexes[i].size(); j++){
OrientedPoint2D pose;
unsigned int pointIndex = maxRangeMapping[indexes[i][j]];
// Reomoving the detection in the background and pushing it to the foreground
double rangeBefore = (pointIndex > 1)? reading.getRho()[pointIndex - 1] : reading.getMaxRange();
double rangeAfter = (pointIndex < worldPoints.size() - 1)? reading.getRho()[pointIndex + 1] : reading.getMaxRange();
double rangeCurrent = reading.getRho()[pointIndex];
if(rangeBefore < rangeAfter){
if(rangeBefore < rangeCurrent){
pointIndex = pointIndex - 1;
}
} else if(rangeAfter < rangeCurrent){
pointIndex = pointIndex + 1;
}
// Removing max range reading
if(reading.getRho()[pointIndex] >= reading.getMaxRange()){
continue;
}
pose.x = (reading.getWorldCartesian()[pointIndex]).x;
pose.y = (reading.getWorldCartesian()[pointIndex]).y;
pose.theta = normAngle(signal[indexes[i][j]], -M_PI);
bool exists = false;
for(unsigned int k = 0; !exists && k < point.size(); k++){
exists = exists || (fabs(pose.x - point[k]->getPosition().x) <= 0.2 && fabs(pose.y - point[k]->getPosition().y) <= 0.2);
}
if(exists) continue;
unsigned int first = indexes[i][j] - floor((int)m_filterBank[i].size()/2.0);
unsigned int last = indexes[i][j] + floor((int)m_filterBank[i].size()/2.0);
std::vector<Point2D> support(last - first + 1);
for(unsigned int p = 0; p < support.size(); p++) {
support[p] = Point2D(worldPoints[maxRangeMapping[p + first]]);
}
double maxDistance = -1e20;
for(unsigned int k = 0; k < support.size(); k++){
double distance = sqrt((pose.x - support[k].x)*(pose.x - support[k].x) + (pose.y - support[k].y)*(pose.y - support[k].y));
maxDistance = maxDistance < distance ? distance : maxDistance;
}
InterestPoint *interest = new InterestPoint(pose, maxDistance);
// InterestPoint *interest = new InterestPoint(pose, m_scales[i]);
interest->setSupport(support);
interest->setScaleLevel(i);
point.push_back(interest);
}
}
return point.size();
}
InterestPoint* fromRos (const InterestPointRos& m)
{
OrientedPoint2D pose(m.pose.x, m.pose.y, m.pose.theta);
Descriptor* descriptor = fromRos(m.descriptor);
InterestPoint* pt = new InterestPoint(pose, m.scale, descriptor);
pt->setScaleLevel(m.scale_level);
vector<Point2D> support_points(m.support_points.size());
transform(m.support_points.begin(), m.support_points.end(),
support_points.begin(), toPoint2D);
pt->setSupport(support_points);
return pt;
}
unsigned int CurvatureDetector::computeInterestPoints(const LaserReading& reading, const std::vector< std::vector<Point2D> >& operatorA, std::vector<InterestPoint*>& point,
const std::vector< std::vector<unsigned int> >& indexes, std::vector<unsigned int>& maxRangeMapping) const
{
point.clear();
point.reserve(reading.getRho().size());
const std::vector<Point2D>& worldPoints = reading.getWorldCartesian();
for(unsigned int i = 0; i < indexes.size(); i++){
for(unsigned int j = 0; j < indexes[i].size(); j++){
OrientedPoint2D pose;
unsigned int pointIndex = maxRangeMapping[indexes[i][j]];
// Reomoving the detection in the background and pushing it to the foreground
double rangeBefore = (pointIndex > 1)? reading.getRho()[pointIndex - 1] : reading.getMaxRange();
double rangeAfter = (pointIndex < worldPoints.size() - 1)? reading.getRho()[pointIndex + 1] : reading.getMaxRange();
double rangeCurrent = reading.getRho()[pointIndex];
if(rangeBefore < rangeAfter){
if(rangeBefore < rangeCurrent){
pointIndex = pointIndex - 1;
}
} else if(rangeAfter < rangeCurrent){
pointIndex = pointIndex + 1;
}
// Removing max range reading
if(reading.getRho()[pointIndex] >= reading.getMaxRange()){
continue;
}
pose.x = (worldPoints[pointIndex]).x;
pose.y = (worldPoints[pointIndex]).y;
Point2D difference = operatorA[i][indexes[i][j]] - worldPoints[pointIndex];
pose.theta = atan2(difference.y, difference.x);
bool exists = false;
for(unsigned int k = 0; !exists && k < point.size(); k++){
exists = exists || (fabs(pose.x - point[k]->getPosition().x) <= 0.2 && fabs(pose.y - point[k]->getPosition().y) <= 0.2);
}
if(exists) continue;
double distance = 2. * m_scales[i];
Point2D diffStart = pose - worldPoints.front();
Point2D diffEnd = pose - worldPoints.back();
if(hypot(diffStart.x, diffStart.y) < distance || hypot(diffEnd.x, diffEnd.y) < distance){
continue;
}
std::vector<Point2D> support;
for(unsigned int k = 0; k < worldPoints.size(); k++){
Point2D diff2 = pose - worldPoints[k];
if(hypot(diff2.x, diff2.y) < distance) support.push_back(worldPoints[k]);
}
LineParameters param = computeNormals(support);
pose.theta = normAngle(param.alpha, - M_PI);
InterestPoint *interest = new InterestPoint(pose, distance);
// InterestPoint *interest = new InterestPoint(pose, m_scales[i]);
interest->setSupport(support);
interest->setScaleLevel(i);
point.push_back(interest);
}
}
return point.size();
}