bool WallFollowingStrategy::lineCondition(std::pair<cv::Vec4i, float> line,
cv::Vec4i initialLineSegment) {
float slope = calcSlope(initialLineSegment);
return fabs(line.second - slope) < 0.3 &&
abs(line.first[0] - initialLineSegment[0]) <
getDifference(line.first[0], line.first[2]) &&
compareY(line.first[1], initialLineSegment[1]) <
compareY(line.first[1], line.first[3]);
}
void calculateMatrix(std::vector<cv::Point2f>& pointList,
float radius,
std::vector<std::vector<cv::Point2f>>& pointMatrix)
{
if (!pointList.size())
return;
std::sort(pointList.begin(), pointList.end(), compareY());
/// try to find automaticly pixel distance *************
if (radius < 0.)
{
// calculate derivation of y axis;
std::vector<float> derivationY;
derivationY.reserve(pointList.size());
for (size_t i = 1; i < pointList.size(); ++i)
derivationY.push_back(pointList[i].y - pointList[i - 1].y);
radius = (*std::max_element(derivationY.begin(), derivationY.end())) / 2;
}
/// find Lines *************
std::vector<size_t> jumpLines;
jumpLines.push_back(0);
for (size_t i = 1; i < pointList.size(); ++i)
{
float dy = abs(pointList[i - 1].y - pointList[i].y);
if (dy > radius)
jumpLines.push_back(i);
}
jumpLines.push_back(pointList.size());
// find columns *************
std::vector<std::vector<size_t>> jumps;
for (size_t i = 1; i < jumpLines.size(); ++i)
{
size_t lo = jumpLines[i - 1];
size_t hi = jumpLines[i];
auto b = pointList.begin() + lo;
auto e = pointList.begin() + hi;
std::sort(b, e, compareX());
std::vector<size_t> jumpColumn;
jumpColumn.push_back(lo);
for (size_t j = lo + 1; j < hi; ++j)
{
float dy = abs(pointList[j - 1].x - pointList[j].x);
if (dy > radius)
jumpColumn.push_back(j);
}
jumpColumn.push_back(hi);
jumps.push_back(jumpColumn);
}
// create matrix
pointMatrix.clear();
pointMatrix.reserve(jumps.size());
for (size_t y = 0; y < jumps.size(); ++y)
{
// calculate y mean
size_t lineB = jumps[y].front();
size_t lineE = jumps[y].back();
float meanY = 0.0;
for (size_t i = lineB; i < lineE; ++i)
meanY += pointList[i].y;
meanY /= (lineE - lineB);
// calculate x
std::vector<cv::Point2f> linePoints;
linePoints.reserve(jumps[y].size());
for (size_t x = 1; x < jumps[y].size(); ++x)
{
size_t columnB = jumps[y][x - 1];
size_t columnE = jumps[y][x];
float meanX = 0.0;
for (size_t i = columnB; i < columnE; ++i)
meanX += pointList[i].x;
meanX /= (columnE - columnB);
// store point
linePoints.push_back(cv::Point2f(meanX, meanY));
}
pointMatrix.push_back(linePoints);
}
}
void WallFollowingStrategy::removeLines(std::vector< cv::Vec4i > lines) {
// "bucket" for each of the group of "similar" lines
std::vector< std::pair< cv::Vec4i, float > > temp;
// sort the lines by the x coordinate of the starting point
std::sort(lines.begin(), lines.end(), compareStart);
bool pushed = true;
int count = 0;
// loop through all the lines that have been detected by HoughLinesP
for (size_t i = 0; i < lines.size(); i++) {
float slope = calcSlope(lines[i]);
std::pair<cv::Vec4i, float> lastLine;
if (!temp.empty()) {
lastLine = temp.back();
}
// if the bucket is empty push the current line to be processed
if (temp.empty()) {
std::pair< cv::Vec4i, float > p = std::make_pair(lines[i], slope);
temp.push_back(p);
/* case 1 of similar line segments: the slopes are almost the same,
starting position of the x-coordinate of the second line segment
that is being processed lies between starting and ending
points of first line segment. Taking into account that the line
segments are not too far apart in y-direction
(e.g. two parallel walls) */
} else if (lineCondition(lastLine, lines[i])) {
std::pair< cv::Vec4i, float > p = std::make_pair(lines[i], slope);
temp.push_back(p);
/* case 2 and 3 of similar lines: the slopes are almost the same, but
for the second line segment x coordinate of the starting point lies a
bit farther from the endpoint of the first line segment */
} else if (slope < 0 && fabs(lastLine.second - slope) < 0.3 &&
abs(lastLine.first[2] - lines[i][0]) < 2) {
std::pair< cv::Vec4i, float > p = std::make_pair(lines[i], slope);
temp.push_back(p);
} else if (slope > 0 && compareY(lastLine.first[3], lines[i][1]) < 2 &&
fabs(lastLine.second - slope) < 0.3) {
std::pair< cv::Vec4i, float > p = std::make_pair(lines[i], slope);
temp.push_back(p);
} else {
/* all the similar line segments were pushed - find the average of them
and push in a resulting vector of line segments */
res.push_back(getAverLine(temp));
temp.clear();
/* as line segments sorted by x-coordinate of the starting point can be
shuffle with respect to the wall they belong to we need to check before
processing a new line segment whether the lines similar to it were
processed before */
for (int j = 0; j < res.size(); j++) {
if (slope > 0 && fabs(calcSlope(res[j]) - slope) < 2 ||
slope < 0 && fabs(calcSlope(res[j]) + slope) < 2) {
pushed = false;
break;
}
}
if (pushed) {
std::pair< cv::Vec4i, float > p = std::make_pair(lines[i], slope);
temp.push_back(p);
pushed = true;
}
}
// always process the current bucket before the end of vector of line
// segments
if (!temp.empty() && i == lines.size() - 1) {
res.push_back(getAverLine(temp));
temp.clear();
}
}
}
