Point2f GoalPostDetector::findTheShift ()
{
CannyThreshold();
createPaddedImg();
getPointsAlongTheGoalBar();
CMAES<float> evo;
float *arFunvals, *xfinal, *const*pop;
// Initialize everything
const int dim = 2;
float xstart[dim];
for(int i=0; i<dim; i++) xstart[i] = 0.0;
float stddev[dim];
for(int i=0; i<dim; i++) stddev[i] = 0.05;
Parameters<float> parameters;
parameters.init(dim, xstart, stddev);
arFunvals = evo.init(parameters);
// Iterate until stop criterion holds
while(!evo.testForTermination())
{
// Generate lambda new search points, sample population
pop = evo.samplePopulation();
// condition: solution dx and dy should not be bigger than squareSize
for (int i = 0; i < evo.get(CMAES<float>::PopSize); ++i)
while (abs(pop[i][0])*scale>=windowSearchSize||abs(pop[i][1])*scale>=windowSearchSize)
evo.reSampleSingle(i);
// evaluate the new search points using objectiveFunction from above
for (int i = 0; i < evo.get(CMAES<float>::Lambda); ++i)
{
arFunvals[i]= objectiveFunction(pop[i]);
}
evo.updateDistribution(arFunvals);
}
return Point2f(evo.getNew(CMAES<float>::XMean)[0]*scale,
evo.getNew(CMAES<float>::XMean)[1]*scale);
}
void tutorial(){
vector<Mat> imagenes_solucion;
// Leo imagen.
Mat prueba = leeimagen("yalefaces/subject01.png");
// La añado al vector de imágenes solución.
imagenes_solucion.push_back(prueba);
// Aplico el detector de fronteras de Canny.
prueba = CannyThreshold(prueba, 40);
// Añado la imagen al vector de imágenes solución.
imagenes_solucion.push_back(prueba);
// Pinto las imágenes del vector.
pintaMI(imagenes_solucion, "Canny Edge Detector");
}
void BookSegmenter::runSegmenter(Mat input, Point& p1, Point& p2)
{
//get image width and height
imagewidth = input.size().width;
imageheight = input.size().height;
cout << "image dim(hxw): " << imageheight << "x" << imagewidth << endl;
//convert to gray scale image
Mat gray_input;
cvtColor(input, gray_input, CV_BGR2GRAY);
//Gaussian blur
GaussianBlur(gray_input, gray_input, Size(5, 5), 0);
//canny edge detection
Mat edges = CannyThreshold(gray_input);
//extract lines
vector<Vec4i> lines = extractLines(edges);
//filter lines
lines = filterHorizentalLines(lines, 0.1);
//segment lines
Vec4i segment_boundry = segmentLinesSequence(lines, 0, 10);
//draw lines
Mat canvas = drawLines(gray_input, lines);
//draw boundry
p1.x = segment_boundry[0];
p1.y = segment_boundry[1];
p2.x = segment_boundry[2];
p2.y = segment_boundry[3];
rectangle(canvas, p1, p2, Scalar(255, 0, 0), 3, CV_AA);
imshow("canvas", canvas);
}
void StairDetection::Run(cv::InputArray colorImg, cv::InputArray depthImg, std::vector<cv::Point> &stairsConvexHull)
{
cv::Mat detected_edges, detected_edges_inv;
std::vector<cv::Point> stairsConvexHull_normal, stairsConvexHull_inverse;
cv::Mat scaledColor, scaledDepth;
std::vector<cv::Vec4i> allLines;
int stairsAngle = -999;
cv::Point stairMidPoint;
std::vector<std::vector<int>> angles = std::vector<std::vector<int>>(180, std::vector<int>());
std::vector<cv::Point> stairPoints, stairsHull;
std::vector<cv::Point> stairMidLine, stairsMidPoints;
std::string timestamp = std::to_string(cv::getTickCount());
cv::resize(colorImg, scaledColor, cv::Size(320, 240));
cv::resize(depthImg, scaledDepth, cv::Size(320, 240));
//cv::imshow("Color Stairs", scaledColor);
//cv::imshow("DEPTH Stairs", scaledDepth);
CannyThreshold(scaledColor, detected_edges);
ApplyFilter(detected_edges, scaledDepth, 254, 255, CV_THRESH_BINARY);
Probabilistic_Hough(detected_edges, allLines);
SortLinesByAngle(allLines, angles);
DetermineStairAngle(angles, stairsAngle);
// stairs angle not found.
if (stairsAngle == -999)
return;
GetStairMidLine(allLines, angles[stairsAngle], stairsAngle, stairMidLine);
GetStairPoints(allLines, stairMidLine, stairsAngle, stairPoints, stairsMidPoints);
if (!DetermineStairs(scaledDepth, stairMidLine, stairsMidPoints))
return;
ExtractStairsHull(stairPoints, stairsConvexHull);
return;
}
void Dimage::salient_detection() {
Mat hsl,h,s,l,hg,lg, blkWhte, grad, img;
vector<vector<Point> > contours;
Mat src = this->image;
Mat cropped;
string buffer;
GaussianBlur(this->image, img, Size(3, 3), 0, 0, BORDER_DEFAULT);
cout << "blurred" << endl;
vector<Mat> channels(3);
cvtColor(img, hsl, CV_RGB2HLS);
split(hsl, channels);
cout << "splitted" << endl;
h = channels[1];
s = channels[0];
l = channels[2];
hg = gradient(h);
lg = gradient(l);
grad = h + l;
grad = CannyThreshold(grad,grad);
h = CannyThreshold(h,h);
cout << "thresholded" << endl;
grad = (grad & h);
/// Find contours
findContours(grad, contours, RETR_EXTERNAL, CHAIN_APPROX_SIMPLE, Point(0, 0));
vector<vector<Point> > contours_poly(contours.size());
vector<Rect> boundRect(contours.size());
/// Approximate contours to polygons + get bounding rects and circles
cout << "countours found" << endl;
cout << contours.size() << endl;
for (int i = 0; i < contours.size(); i++)
{
cout << "i: " << i << endl;
if (contourArea(contours.at(i)) < 600)
{ cout << "in da if" << endl;
approxPolyDP(Mat(contours.at(i)), contours_poly.at(i), 3, true);
cout << "at roi" << endl;
//boundingRect(Mat(contours[i]));
Rect roi = boundingRect(Mat(contours.at(i)));
/*cropped = src(roi);
imshow("Cropped image", cropped);
cvWaitKey(0);*/
this->ROI.push_back(roi);
cout << "size of victor: " << this->ROI.size() << endl;
}
}
cout << "bounding rect found" << endl;
};
std::vector<Circle> CirclePacker::go()
{
std::vector<Circle> result;
// Create a matrix of the same size and type as src
dst.create( src.size(), src.type() );
// Convert to grayscale
//cvtColor(src, src_gray, CV_BGR2GRAY);
// Get the edges
ros::Time t_start_edge_detect = ros::Time::now();
CannyThreshold(0, 0);
ros::Duration d_edges_detect(ros::Time::now()-t_start_edge_detect);
// Get the contour points
ros::Time t_start_contour = ros::Time::now();
std::vector< std::vector<cv::Point> > detected_contours;
std::vector<cv::Vec4i> hierarchy;
cv::findContours(detected_edges, detected_contours, hierarchy, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);
ros::Duration d_contour(ros::Time::now() - t_start_contour);
//ROS_INFO("detected_contours size: %i", (int)detected_contours.size());
/*
* Get every edge in 1 vector, and make a circle for each edge
*/
// Make Edges from detected contour points (endpoints of edges)
std::vector< std::vector<Edge> > edge_sets;
std::vector<Edge> edges;
for(int i=0;i<detected_contours.size();i++)
{
std::vector<Edge> set;
//ROS_INFO("detected_contours[%i].size(): %i", i, (int)detected_contours[i].size());
for(int j=0;j<detected_contours[i].size()-1;j++)
{
//ROS_INFO("detected_contours[%i][%i]: (%i, %i)", i, j, detected_contours[i][j].x, detected_contours[i][j].y);
Edge temp;
temp.start.x = detected_contours[i][j].x;
temp.start.y = detected_contours[i][j].y;
temp.end.x = detected_contours[i][j+1].x;
temp.end.y = detected_contours[i][j+1].y;
edges.push_back(temp);
set.push_back(temp);
}
Edge temp;
temp.start.x = detected_contours[i][detected_contours[i].size()-1].x;
temp.start.y = detected_contours[i][detected_contours[i].size()-1].y;
temp.end.x = detected_contours[i][0].x;
temp.end.y = detected_contours[i][0].y;
edges.push_back(temp);
set.push_back(temp);
edge_sets.push_back(set);
}
//ROS_INFO("Edges.size(): %i", (int)edges.size());
/*ros::Time t_start_cirs_from_edges = ros::Time::now();
cv::Point robo_cen;
robo_cen.x = 0;
robo_cen.y = 0;
std::vector<Circle> cirs_from_edges = getCirclesFromEdges(edges, robo_cen);
ros::Duration d_cirs_from_edges(ros::Time::now() - t_start_cirs_from_edges);
//ROS_INFO("cirs_from_edges size: %i", (int)cirs_from_edges.size());
for(int i=0;i<cirs_from_edges.size();i++)
{
ROS_INFO("Circle %i - Center: (%i, %i) Radius: %f", i, cirs_from_edges[i].center.x, cirs_from_edges[i].center.y, cirs_from_edges[i].radius);
}*/
ros::Time t_start_cirs_from_sets = ros::Time::now();
std::vector<Circle> cirs_from_sets = getCirclesFromEdgeSets(edge_sets);
ros::Duration d_cirs_from_sets(ros::Time::now() - t_start_cirs_from_sets);
for(int i=0;i<cirs_from_sets.size();i++)
{
//ROS_INFO("Circle %i - Center: (%f, %f) Radius: %f", i, cirs_from_sets[i].center.x, cirs_from_sets[i].center.y, cirs_from_sets[i].radius);
result.push_back(cirs_from_sets[i]);
}
/*
* Get every set of edges, and make a circle for each set
*/
// Find convex hull for each set of contour points
/*std::vector< std::vector<cv::Point> > hull(detected_contours.size());
for(int i=0;i<detected_contours.size();i++)
{
cv::convexHull( detected_contours[i], hull[i], false );
}
ROS_INFO("hull.size(): %i", (int)hull.size());
// Build a polygon for each convex hull
for(int h=0;h<hull.size();h++)
{
Polygon p;
for(int i=0;i<hull[h].size()-1;i++)
{
std::cout<<"\nPoint "<<i<<" ("<<hull[h][i].x<<", "<<hull[h][i].y<<")";
Edge temp;
temp.start.x = hull[h][i].x;
temp.start.y = hull[h][i].y;
temp.end.x = hull[h][i+1].x;
temp.end.y = hull[h][i+1].y;
p.edges.push_back(temp);
}
// Last edge
Edge temp;
temp.start.x = hull[h][hull[h].size()-1].x;
temp.start.y = hull[h][hull[h].size()-1].y;
temp.end.x = hull[h][0].x;
temp.end.y = hull[h][0].y;
p.edges.push_back(temp);
// Build the set of normals for the polygon
for(int i=0;i<p.edges.size();i++)
{
p.normals.push_back(computeNormal(p.edges[i]));
}
// Pack the polygon and return the set of circles
std::vector<Circle> cirs = getCirclesFromPoly(p);
ROS_INFO("cirs.size(): %i", (int)cirs.size());
result.push_back(cirs);
}
ROS_INFO("result size: %i", (int)result.size());
for(int i=0;i<result.size();i++)
{
ROS_INFO("result[%i].size(): %i", i, (int)result[i].size());
for(int j=0;j<result[i].size();j++)
{
ROS_INFO("Circle %i, Center: (%i, %i) Radius: %f", j, result[i][j].center.x, result[i][j].center.y, result[i][j].radius);
}
}*/
ROS_INFO("d_edges_detect: %f", d_edges_detect.toSec());
ROS_INFO("d_contour: %f", d_contour.toSec());
//ROS_INFO("d_cirs_from_edges: %f", d_cirs_from_edges.toSec());
ROS_INFO("d_cirs_from_sets: %f", d_cirs_from_sets.toSec());
//ROS_INFO("Leaving go()");
return result;
}
