//--------------------------- findBox_r ---------------------------------
//(do not touch, ever)
void findBox_r(struct Pattern* p_list, CvPoint* ret){
int i, tempx, tempy, min=32000, value;
CvPoint tl, bl, tr, br;
struct Rectangle s_list[4];
for(i=0; i<4; i++){
s_list[i] = p_list[i].r;
}
//find top left
for(i=0; i<4; i++){
value = sqrt( pow(p_list[i].center.x, 2) + pow(p_list[i].center.y, 2)); //Uses x,y coords of the PATTERNS
if(value < min){
min = value;
bl= s_list[i].pt[ findCorner(&s_list[i].pt[0], 0) ]; //Checks x,y coords of the corners of a single pattern
}
}
min = 32000;
//find top right
for(i=0; i<4; i++){
value = sqrt( pow(640-p_list[i].center.x, 2) + pow(p_list[i].center.y, 2));
if(value < min){
min = value;
br = s_list[i].pt[ findCorner(&s_list[i].pt[0], 1) ];
}
}
min = 32000;
//find bottom right
for(i=0; i<4; i++){
value = sqrt( pow(640-p_list[i].center.x, 2) + pow(480-p_list[i].center.y, 2));
if(value < min){
min = value;
tr=s_list[i].pt[ findCorner(&s_list[i].pt[0], 2) ];
}
}
min = 32000;
//find bottom left
for(i=0; i<4; i++){
value = sqrt( pow(p_list[i].center.x, 2) + pow(480 - p_list[i].center.y, 2));
if(value < min){
min = value;
tl=s_list[i].pt[ findCorner(&s_list[i].pt[0], 3) ];
}
}
ret[0] = bl;
ret[1] = br;
ret[2] = tl;
ret[3] = tr;
//--------------------------- end findBox_r ---------------------------------
}
int main(){
vector<Point2f>corners;
vector<vector<Point2f>>corners_Seq;
vector<Mat>image_Seq;
findCorner(corners,corners_Seq,image_Seq);
return 1;
}
std::vector<CornerPoint> CornerDetector::run(const std::vector<FieldLine> &lines) const
{
std::vector<FieldLine>::const_iterator it1, it2;
std::vector<CornerPoint> results;
if(lines.size() < 2) {
return results;
}
if(m_tolerance < 0 || m_tolerance > 1) {
errorlog << "CornerDetector::run called with invalid tolerance: " << m_tolerance << " (must be in [0, 1]." << std::endl;
return results;
}
for(it1 = lines.begin(); it1 != lines.end()-1; it1++) {
Line l1 = it1->getGroundLineEquation();
Vector2<NUPoint> l1_pts = it1->getEndPoints();
for(it2 = it1+1; it2 < lines.end(); it2++) {
Line l2 = it2->getGroundLineEquation();
Vector2<NUPoint> l2_pts = it2->getEndPoints();
if(l1.getAngleBetween(l2) > (1-m_tolerance)*mathGeneral::PI*0.5) {
//nearly perpendicular
//now build corner from end points
NUPoint intersection;
if(l1.getIntersection(l2, intersection.groundCartesian)) {
CornerPoint::TYPE type = findCorner(l1_pts, l2_pts, intersection, m_tolerance);
if(type != CornerPoint::INVALID) {
//need screen loc
if(it1->getScreenLineEquation().getIntersection(it2->getScreenLineEquation(), intersection.screenCartesian)) {
results.push_back(CornerPoint(type, intersection));
}
else {
errorlog << "CornerDetector::run - no intersection found for screen lines - transforms are probably not valid, not publishing corner" <<
"\t" << it1->getScreenLineEquation() << "\t" << it2->getScreenLineEquation() << std::endl;
}
}
}
}
}
}
return results;
}
bool cv::find4QuadCornerSubpix(InputArray _img, InputOutputArray _corners, Size region_size)
{
CV_INSTRUMENT_REGION();
Mat img = _img.getMat(), cornersM = _corners.getMat();
int ncorners = cornersM.checkVector(2, CV_32F);
CV_Assert( ncorners >= 0 );
Point2f* corners = cornersM.ptr<Point2f>();
const int nbins = 256;
float ranges[] = {0, 256};
const float* _ranges = ranges;
Mat hist;
Mat black_comp, white_comp;
for(int i = 0; i < ncorners; i++)
{
int channels = 0;
Rect roi(cvRound(corners[i].x - region_size.width), cvRound(corners[i].y - region_size.height),
region_size.width*2 + 1, region_size.height*2 + 1);
Mat img_roi = img(roi);
calcHist(&img_roi, 1, &channels, Mat(), hist, 1, &nbins, &_ranges);
int black_thresh = 0, white_thresh = 0;
segment_hist_max(hist, black_thresh, white_thresh);
threshold(img, black_comp, black_thresh, 255.0, THRESH_BINARY_INV);
threshold(img, white_comp, white_thresh, 255.0, THRESH_BINARY);
const int erode_count = 1;
erode(black_comp, black_comp, Mat(), Point(-1, -1), erode_count);
erode(white_comp, white_comp, Mat(), Point(-1, -1), erode_count);
std::vector<std::vector<Point> > white_contours, black_contours;
findContours(black_comp, black_contours, RETR_LIST, CHAIN_APPROX_SIMPLE);
findContours(white_comp, white_contours, RETR_LIST, CHAIN_APPROX_SIMPLE);
if(black_contours.size() < 5 || white_contours.size() < 5) continue;
// find two white and black blobs that are close to the input point
std::vector<std::pair<int, float> > white_order, black_order;
orderContours(black_contours, corners[i], black_order);
orderContours(white_contours, corners[i], white_order);
const float max_dist = 10.0f;
if(black_order[0].second > max_dist || black_order[1].second > max_dist ||
white_order[0].second > max_dist || white_order[1].second > max_dist)
{
continue; // there will be no improvement in this corner position
}
const std::vector<Point>* quads[4] = {&black_contours[black_order[0].first], &black_contours[black_order[1].first],
&white_contours[white_order[0].first], &white_contours[white_order[1].first]};
std::vector<Point2f> quads_approx[4];
Point2f quad_corners[4];
for(int k = 0; k < 4; k++)
{
std::vector<Point2f> temp;
for(size_t j = 0; j < quads[k]->size(); j++) temp.push_back((*quads[k])[j]);
approxPolyDP(Mat(temp), quads_approx[k], 0.5, true);
findCorner(quads_approx[k], corners[i], quad_corners[k]);
quad_corners[k] += Point2f(0.5f, 0.5f);
}
// cross two lines
Point2f origin1 = quad_corners[0];
Point2f dir1 = quad_corners[1] - quad_corners[0];
Point2f origin2 = quad_corners[2];
Point2f dir2 = quad_corners[3] - quad_corners[2];
double angle = acos(dir1.dot(dir2)/(norm(dir1)*norm(dir2)));
if(cvIsNaN(angle) || cvIsInf(angle) || angle < 0.5 || angle > CV_PI - 0.5) continue;
findLinesCrossPoint(origin1, dir1, origin2, dir2, corners[i]);
}
return true;
}
void DrawPolygon2(int index, RTREE& rtree2, std::vector<polygon>& polygons, int pmap[][HEIGHT])
{
int xmin, ymin, xmax, ymax;
box b;
findCorner(xmin, ymin, xmax, ymax, b, index);
std::vector<value> result;
rtree2.query(bgi::intersects(b), std::back_inserter(result));
RTREE rtree(result.begin(), result.end());
// std::this_thread::sleep_for(std::chrono::seconds(index));
// std::cout << bg::wkt<box>(b) << std::endl;
// std::cout<<"index = "<<index<<"min_corner = "<<xmin<<","<<ymin<<" max_corner = "<<xmax<<","<<ymax<<std::endl;
// std::cout<<"polygon size: "<<polygons.size()<<std::endl;
for (int i = xmin; i <= xmax; i++)
for (int j = ymin; j <= ymax; j++)
{
if (pmap[i][j] == -1)
{
point sought = point(i, j);
std::vector<value> result_n;
for ( RTREE::const_query_iterator it = qbegin(rtree, bgi::nearest(sought, 100)) ;
it != qend(rtree) ; ++it )
{
int id = it->second;
auto& poly = polygons[id];
auto box = it->first;
if (!bg::within(sought, box))
break;
if ( bg::within(sought, poly) ) {
// std::cout<<"**********************\n";
// pmap[i][j] = id;
point pmin = box.min_corner();
point pmax = box.max_corner();
int xmin2 = pmin.get<0>();
int ymin2 = pmin.get<1>();
int xmax2 = pmax.get<0>();
int ymax2 = pmax.get<1>();
for (int w = xmin2; w <= xmax2; w++) {
for (int h = ymin2; h <= ymax2; h++)
{
if (w < xmin || h < ymin || w >= xmax || h >= ymax || pmap[w][h] >= 0)
continue;
if (bg::within(point(w, h), poly))
{
pmap[w][h] = id;
}
}
}
break;
}
}
}
}
// //random color
// std::vector<int> colors;
// int size = polygons.size();
// for ( int i = 0 ; i < size ; ++i )
// {
// colors.push_back(i % 3 + 1);
// }
// //Draw pixel
// // t1 = std::chrono::high_resolution_clock::now();
// cv::Mat mat(HEIGHT, WIDTH, CV_8UC4);
// mat = cv::Scalar(0, 0, 0, 0);
// for (int i = 0; i < WIDTH; i++)
// for (int j = 0; j < HEIGHT; j++)
// {
// if (pmap[i][j] != -1) {
// int c = colors[pmap[i][j]];
// auto color = c == 1 ? RED : c == 2 ? GREEN : BLUE;
// line( mat, cv::Point(i, j), cv::Point(i, j), color, 2, 8);
// }
// }
// line(mat, cv::Point(10, 10), cv::Point(251, 240), cv::Scalar(0, 0, 0, 255), 1, 8);
// std::vector<int> compression_params;
// compression_params.push_back(CV_IMWRITE_PNG_COMPRESSION);
// compression_params.push_back(9);
// std::string filename = "DrawPolygon7_" + std::to_string(index) + ".png";
// imwrite(filename, mat, compression_params);
}
int main()
{
int pmap[NUMTHREAD][WIDTH][HEIGHT];
//fill all member = -1
memset(pmap, -1, sizeof(pmap) );
// polygons
std::vector<polygon> polygons;
std::vector<value> values;
for (int i = 0; i < NUMPOLYGON; i++)
{
polygon p = makePolygon(WIDTH, HEIGHT);
bg::correct(p);
polygons.push_back(p);
}
//std::cout<<polygons.size()<<std::endl;
// display polygons
// std::cout << "generated polygons:" << std::endl;
// BOOST_FOREACH(polygon const & p, polygons)
// std::cout << bg::wkt<polygon>(p) << std::endl;
std::chrono::high_resolution_clock::time_point t1 = std::chrono::high_resolution_clock::now();
// fill the spatial index
for ( int i = 0 ; i < polygons.size() ; ++i )
{
// calculate polygon bounding box
box b = bg::return_envelope<box>(polygons[i]);
// insert new value
values.push_back(std::make_pair(b, i));
}
RTREE rtree(values.begin(), values.end());
std::thread threads[NUMTHREAD];
for (int i = 0; i < NUMTHREAD; i++)
{
threads[i] = std::thread([i, &rtree, &polygons, &pmap]() {
DrawPolygon2(i, rtree, polygons, pmap[i]);
});
}
for (int i = 0; i < NUMTHREAD; i++)
{
threads[i].join();
}
//concatenate
int mapresult[WIDTH][HEIGHT];
memset(mapresult,-1,sizeof(mapresult));
for (int index = NUMTHREAD - 1; index >= 0; index--)
{
int xmin, ymin, xmax, ymax;
box b;
findCorner(xmin, ymin, xmax, ymax, b, index);
for (int i = xmin; i < xmax; i++)
{
for (int j = ymin; j < ymax; j++)
{
int id = pmap[index][i][j];
if (id != -1)
{
mapresult[i][j] = id;
}
}
}
}
std::chrono::high_resolution_clock::time_point t2 = std::chrono::high_resolution_clock::now();
float duration = (float)(std::chrono::duration_cast<std::chrono::milliseconds>( t2 - t1 ).count()) / 1000;
std::cout << "duration : " << duration << std::endl;
//random color
std::vector<int> colors;
int size = polygons.size();
for ( int i = 0 ; i < size ; ++i )
{
colors.push_back(i % 3 + 1);
}
//Draw pixel
// t1 = std::chrono::high_resolution_clock::now();
cv::Mat mat(HEIGHT, WIDTH, CV_8UC4);
mat = cv::Scalar(0, 0, 0, 0);
for (int i = 0; i < WIDTH; i++)
for (int j = 0; j < HEIGHT; j++)
{
if (mapresult[i][j] != -1) {
int c = colors[mapresult[i][j]];
auto color = c == 1 ? RED : c == 2 ? GREEN : BLUE;
line( mat, cv::Point(i, j), cv::Point(i, j), color, 2, 8);
}
}
line(mat, cv::Point(10, 10), cv::Point(251, 240), cv::Scalar(0, 0, 0, 255), 1, 8);
std::vector<int> compression_params;
compression_params.push_back(CV_IMWRITE_PNG_COMPRESSION);
compression_params.push_back(9);
imwrite("DrawPolygon7.png", mat, compression_params);
return 0;
}
