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(); }
void CurvatureDetector::computeGraph(const LaserReading& reading, std::vector<Point2D>& graphPoints, Graph& graph, std::vector<unsigned int>& maxRangeMapping) const { const std::vector<Point2D>& worldPoints = reading.getWorldCartesian(); graphPoints.reserve(worldPoints.size()); std::vector<GraphEdge> edges; std::vector< boost::property < boost::edge_weight_t, double > > weights; edges.reserve(worldPoints.size()*worldPoints.size()); weights.reserve(worldPoints.size()*worldPoints.size()); unsigned int currentVertexNumber = 0; for(unsigned int i = 0; i < worldPoints.size(); i++){ if(m_useMaxRange || reading.getRho()[i] < reading.getMaxRange()){ graphPoints.push_back(worldPoints[i]); maxRangeMapping.push_back(i); unsigned int targetVertexNumber = currentVertexNumber + 1; for(unsigned int j = i + 1; j < worldPoints.size(); j++){ if(m_useMaxRange || reading.getRho()[j] < reading.getMaxRange()){ Point2D difference = worldPoints[i] - worldPoints[j]; double weight = hypot(difference.x, difference.y); edges.push_back(GraphEdge(currentVertexNumber,targetVertexNumber)); weights.push_back(weight); targetVertexNumber++; } } currentVertexNumber++; } } MatrixGraph costGraph(currentVertexNumber); for(unsigned int i = 0; i < edges.size(); i++){ boost::add_edge(edges[i].first, edges[i].second, weights[i], costGraph); } boost::remove_edge(0, currentVertexNumber - 1, costGraph); for(unsigned int iter = 0; iter <= m_dmst; iter++){ std::vector < boost::graph_traits < Graph >::vertex_descriptor > predecessor(boost::num_vertices(costGraph)); boost::prim_minimum_spanning_tree(costGraph, &predecessor[0]); for(unsigned int i = 1; i < predecessor.size(); i++){ boost::add_edge(predecessor[i], i, boost::get(boost::edge_weight, costGraph, boost::edge(predecessor[i], i, costGraph).first), graph); boost::remove_edge(predecessor[i], i, costGraph); } } }
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(); }