Commit 94c1d9a3 authored by Matthew Hausknecht's avatar Matthew Hausknecht

Fixed bug in open angle detection.

parent 41359ca4
...@@ -209,7 +209,7 @@ float FeatureExtractor::calcLargestOpenAngle(const rcsc::WorldModel &wm, ...@@ -209,7 +209,7 @@ float FeatureExtractor::calcLargestOpenAngle(const rcsc::WorldModel &wm,
//float oppAngleTop = oppAngle; //float oppAngleTop = oppAngle;
float oppAngleBottom = oppAngle - halfWidthAngle; float oppAngleBottom = oppAngle - halfWidthAngle;
float oppAngleTop = oppAngle + halfWidthAngle; float oppAngleTop = oppAngle + halfWidthAngle;
//std::cout << " to split? " << oppDist << " " << maxDist << std::endl; // std::cout << " to split? " << oppDist << " " << maxDist << std::endl;
if (oppDist < maxDist) { if (oppDist < maxDist) {
splitAngles(openAngles,oppAngleBottom,oppAngleTop); splitAngles(openAngles,oppAngleBottom,oppAngleTop);
} }
...@@ -218,7 +218,7 @@ float FeatureExtractor::calcLargestOpenAngle(const rcsc::WorldModel &wm, ...@@ -218,7 +218,7 @@ float FeatureExtractor::calcLargestOpenAngle(const rcsc::WorldModel &wm,
float largestOpening = 0; float largestOpening = 0;
for (uint i = 0; i < openAngles.size(); ++i) { for (uint i = 0; i < openAngles.size(); ++i) {
OpenAngle &open = openAngles[i]; OpenAngle &open = openAngles[i];
//std::cout << " opening: " << RAD_T_DEG * open.first << " " << RAD_T_DEG * open.second << std::endl; // std::cout << " opening: " << RAD_T_DEG * open.first << " " << RAD_T_DEG * open.second << std::endl;
float opening = open.second - open.first; float opening = open.second - open.first;
if (opening > largestOpening) { if (opening > largestOpening) {
largestOpening = opening; largestOpening = opening;
...@@ -230,8 +230,8 @@ float FeatureExtractor::calcLargestOpenAngle(const rcsc::WorldModel &wm, ...@@ -230,8 +230,8 @@ float FeatureExtractor::calcLargestOpenAngle(const rcsc::WorldModel &wm,
void FeatureExtractor::splitAngles(std::vector<OpenAngle> &openAngles, void FeatureExtractor::splitAngles(std::vector<OpenAngle> &openAngles,
float oppAngleBottom, float oppAngleTop) { float oppAngleBottom, float oppAngleTop) {
std::vector<OpenAngle> resAngles; std::vector<OpenAngle> resAngles;
for (uint i = 0; i < resAngles.size(); ++i) { for (uint i = 0; i < openAngles.size(); ++i) {
OpenAngle& open = resAngles[i]; OpenAngle& open = openAngles[i];
if ((oppAngleTop < open.first) || (oppAngleBottom > open.second)) { if ((oppAngleTop < open.first) || (oppAngleBottom > open.second)) {
resAngles.push_back(open); resAngles.push_back(open);
} else { } else {
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment