LaserReading* CarmenLogReader::parseFLaser(std::istream& _stream) const{
std::string sensorName, robotName;
std::vector<double> phi,rho;
unsigned int number;
OrientedPoint2D laserPose, robotPose;
double timestamp;
double start, fov, resolution, maxRange;
_stream >> sensorName >> number;
phi.resize(number);
rho.resize(number);
start = -M_PI_2;
fov = M_PI;
resolution = fov/number;
maxRange = 81.9;
for(uint i=0; i<number; i++){
phi[i] = start + i*resolution;
_stream >> rho[i];
}
_stream >> laserPose.x >> laserPose.y >> laserPose.theta;
_stream >> robotPose.x >> robotPose.y >> robotPose.theta;
_stream >> timestamp >> robotName;
LaserReading *result = new LaserReading(phi, rho, timestamp, sensorName, robotName);
result->setMaxRange(maxRange);
result->setLaserPose(laserPose);
return result;
}
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 NormalDetector::computeSignal(const LaserReading& reading, std::vector<double>& signal, std::vector<unsigned int>& maxRangeMapping) const{
// const std::vector<Point2D>& points = reading.getCartesian();
std::vector<double> ranges;
ranges.reserve(reading.getRho().size());
maxRangeMapping.reserve(reading.getRho().size());
for(unsigned int i = 0; i < reading.getRho().size(); i++){
if(reading.getRho()[i] < reading.getMaxRange()){
ranges.push_back(reading.getRho()[i]);
maxRangeMapping.push_back(i);
} else if (m_useMaxRange){
ranges.push_back(reading.getMaxRange());
maxRangeMapping.push_back(i);
}
}
int offsetRange = floor((int)m_filterBank[0].size()/2.0);
const std::vector<double>& rangeData = convolve1D(ranges, m_filterBank[0], -offsetRange);
const std::vector<double>& phiData = reading.getPhi();
std::vector<Point2D> points(rangeData.size());
for(unsigned int i = 0; i < rangeData.size(); i++){
if(rangeData[i]<reading.getMaxRange()){
points[i].x = cos(phiData[maxRangeMapping[i]])*rangeData[i];
points[i].y = sin(phiData[maxRangeMapping[i]])*rangeData[i];
} else {
points[i].x = cos(phiData[maxRangeMapping[i]])*reading.getMaxRange();
points[i].y = sin(phiData[maxRangeMapping[i]])*reading.getMaxRange();
}
}
signal.resize(points.size());
unsigned int offset = floor((double)m_windowSize * 0.5);
std::vector<Point2D>::const_iterator first = points.begin();
std::vector<Point2D>::const_iterator last = first + m_windowSize;
double oldangle = 0;
for(unsigned int i = offset; i < signal.size() - offset; i++){
LineParameters param = computeNormals(std::vector<Point2D>(first,last));
signal[i] = normAngle(param.alpha, oldangle - M_PI);
oldangle = signal[i];
first++; last++;
}
for(unsigned int i = 0; i < offset; i++){
signal[i] = signal[offset];
}
for(unsigned int i = signal.size() - offset; i < signal.size(); i++){
signal[i] = signal[signal.size() - offset - 1];
}
}
LaserReading* CarmenLogReader::parseRobotLaser(std::istream& _stream) const{
std::string sensorName, robotName;
std::vector<double> phi,rho,remission;
unsigned int number=0, remissionNumber=0;
OrientedPoint2D laserPose, robotPose;
double timestamp;
double start, fov, resolution, maxRange, accuracy;
int laserType, remissionMode;
double tv, rv, forward_safety_dist, side_safety_dist, turn_axis;
_stream >> sensorName >> laserType >> start >> fov >> resolution >> maxRange >> accuracy >> remissionMode; // Laser sensor parameters
_stream >> number;
phi.resize(number);
rho.resize(number);
for(uint i=0; i < number; i++){
phi[i] = start + i*resolution;
_stream >> rho[i];
}
_stream >> remissionNumber;
remission.resize(remissionNumber);
for(uint i=0; i<remissionNumber; i++){
_stream >> remission[i];
}
_stream >> laserPose.x >> laserPose.y >> laserPose.theta;
_stream >> robotPose.x >> robotPose.y >> robotPose.theta;
_stream >> tv >> rv >> forward_safety_dist >> side_safety_dist >> turn_axis;
_stream >> timestamp >> robotName;
LaserReading *result = new LaserReading(phi, rho, timestamp, sensorName, robotName);
result->setMaxRange(maxRange);
result->setRemission(remission);
result->setLaserPose(laserPose);
result->setRobotPose(robotPose);
return result;
}
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);
}
}
}
void RangeDetector::computeSignal(const LaserReading& reading, std::vector<double>& signal, std::vector<unsigned int>& maxRangeMapping) const{
signal.reserve(reading.getRho().size());
maxRangeMapping.reserve(reading.getRho().size());
for(unsigned int i = 0; i < reading.getRho().size(); i++){
if(reading.getRho()[i] < reading.getMaxRange()){
signal.push_back(reading.getRho()[i]);
maxRangeMapping.push_back(i);
} else if (m_useMaxRange){
signal.push_back(reading.getMaxRange());
maxRangeMapping.push_back(i);
}
}
}
unsigned int CurvatureDetector::detect( const LaserReading& reading, std::vector<InterestPoint*>& point,
std::vector< double >& signal,
std::vector< std::vector<double> >& signalSmooth,
std::vector< std::vector<double> >& signalDiff,
std::vector< std::vector<unsigned int> >& indexes) const
{
std::vector<unsigned int> maxRangeMapping;
std::vector<Point2D> graphPoints;
Graph graph;
std::vector< std::vector<Point2D> > operatorA;
computeGraph(reading, graphPoints, graph, maxRangeMapping);
detect(graph, graphPoints, operatorA, signalDiff, indexes);
signal.resize(graphPoints.size());
signalSmooth.resize(m_scales.size(), std::vector<double> (graphPoints.size()));
for(unsigned int i = 0; i < graphPoints.size(); i++){
Point2D difference = graphPoints[i] - reading.getLaserPose();
signal[i] = hypot(difference.x, difference.y);
for(unsigned int scale = 0; scale < m_scales.size(); scale++){
difference = operatorA[scale][i] - reading.getLaserPose();
signalSmooth[scale][i] = hypot(difference.x, difference.y);
}
}
return computeInterestPoints(reading, operatorA, point, indexes, maxRangeMapping);
}
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();
}
