void sampler_quincunx::get(double *p, const double *a, const double *b, const double *c, const double *d) { double M[3]; double A[3]; double B[3]; double C[3]; double D[3]; mid4(M, a, b, c, d); mid2(A, a, M); mid2(B, b, M); mid2(C, c, M); mid2(D, d, M); double pm[4]; double pa[4]; double pb[4]; double pc[4]; double pd[4]; source->get(pm, M); source->get(pa, A); source->get(pb, B); source->get(pc, C); source->get(pd, D); switch (source->getc()) { case 4: p[3] = (pm[3] + pa[3] + pb[3] + pc[3] + pd[3]) / 5; case 3: p[2] = (pm[2] + pa[2] + pb[2] + pc[2] + pd[2]) / 5; case 2: p[1] = (pm[1] + pa[1] + pb[1] + pc[1] + pd[1]) / 5; case 1: p[0] = (pm[0] + pa[0] + pb[0] + pc[0] + pd[0]) / 5; } }
vector<Point2i> PlaneDetector::surroundingDots(Mat& rgbImage, PointCloud<PointXYZ>& pcl, Vec4i line, double distFromStart) { vector<Point2i> result; fixLineCoords(line); Point2i a; Point2i b; distFromStart = min(0.8, distFromStart); distFromStart = max(0.2, distFromStart); for (int j = 0; j < 2; j++) { Point2i diff = b - a; float len = cv::norm(diff); Point2i mid1(a.x + diff.x * (distFromStart - 0.2), a.y + diff.y * (distFromStart - 0.2)); Point2i mid2(a.x + diff.x * (distFromStart), a.y + diff.y * (distFromStart)); Point2i mid3(a.x + diff.x * (distFromStart + 0.2), a.y + diff.y * (distFromStart + 0.2)); Point2i rotated; if (j == 0) rotated = Point2i(-diff.y, diff.x); else rotated = Point2i(diff.y, -diff.x); result.push_back( Point2i(mid1.x + distFromStart * (rotated.x / 5), mid1.y + distFromStart * (rotated.y / 5))); result.push_back( Point2i(mid2.x + distFromStart * (rotated.x / 2), mid2.y + distFromStart * (rotated.y / 2))); result.push_back( Point2i(mid3.x + distFromStart * (rotated.x / 5), mid3.y + distFromStart * (rotated.y / 5))); } return result; }
double findMedianSortedArrays(int A[], int m, int B[], int n) { if(m<=0) return mid(B,n); if(n<=0) return mid(A,m); if(m>n) return findMedianSortedArrays(B,n,A,m); if(m==1) { return mid2(A[0],B,n); } if(A[0]<=B[0] && A[m-1]>=B[n-1]) return findMedianSortedArrays(&A[1],m-2,B,n); else if (B[0]<=A[0] && B[n-1]>=A[m-1]) return findMedianSortedArrays(A,m, &B[1],n-2); double aMid = mid(A,m); double bMid = mid(B,n); if(aMid==bMid) return aMid; else if(aMid<bMid) { if((m&1)==0 && m!=2) return findMedianSortedArrays(&A[m/2-1],m-(m/2-1), B, n-(m/2-1)); else return findMedianSortedArrays(&A[m/2],m-m/2, B, n-m/2); } else { if(m!=2) return findMedianSortedArrays(A,m/2+1, &B[m-(m/2+1)] ,n-(m-(m/2+1))); else return findMedianSortedArrays(A,1, &B[1] ,n-1); } }
void House::CreateRoof(std::vector<Vertex*> &vect, int style, double base, double height) // style = 0 -> empty // style = 1 -> 2 pan // style = 2 -> 4 pan { assert(base != height); assert(vect.size() == 4); std::vector<Vertex*> roofPan; if (style == 0) return; else if (style == 1) { Vertex mid1(GravityCenter(vect[0], vect[1])); Vertex mid2(GravityCenter(vect[2], vect[3])); roofPan.push_back(new Vertex(vect[0]->X(), vect[0]->Y(), base)); roofPan.push_back(new Vertex(mid1.X(), mid1.Y(), height)); roofPan.push_back(new Vertex(vect[3]->X(), vect[3]->Y(), base)); roofPan.push_back(new Vertex(mid2.X(), mid2.Y(), height)); faces->push_back(new Face(new std::vector<Vertex*>(roofPan))); roofPan.clear(); roofPan.push_back(new Vertex(vect[1]->X(), vect[1]->Y(), base)); roofPan.push_back(new Vertex(mid1.X(), mid1.Y(), height)); roofPan.push_back(new Vertex(vect[2]->X(), vect[2]->Y(), base)); roofPan.push_back(new Vertex(mid2.X(), mid2.Y(), height)); faces->push_back(new Face(new std::vector<Vertex*>(roofPan))); roofPan.clear(); roofPan.push_back(new Vertex(vect[0]->X(), vect[0]->Y(), base)); roofPan.push_back(new Vertex(mid1.X(), mid1.Y(), height)); roofPan.push_back(new Vertex(vect[1]->X(), vect[1]->Y(), base)); faces->push_back(new Face(new std::vector<Vertex*>(roofPan))); roofPan.clear(); roofPan.push_back(new Vertex(vect[2]->X(), vect[2]->Y(), base)); roofPan.push_back(new Vertex(mid2.X(), mid2.Y(), height)); roofPan.push_back(new Vertex(vect[3]->X(), vect[3]->Y(), base)); faces->push_back(new Face(new std::vector<Vertex*>(roofPan))); } else if (style == 2) { Vertex cen(GravityCenter(vect)); unsigned int n = vect.size(); for(unsigned int i = 0; i < n; i++) { roofPan.clear(); roofPan.push_back(new Vertex(vect[i]->X(), vect[i]->Y(), base)); roofPan.push_back(new Vertex(vect[(i+1)%n]->X(), vect[(i+1)%n]->Y(), base)); roofPan.push_back(new Vertex(cen.X(), cen.Y(), height)); faces->push_back(new Face(new std::vector<Vertex*>(roofPan))); } } }
void sampler_adaptive::adapt(double *p, const double *a, const double *b, const double *c, const double *d, int r) { double v[3]; double va[3], pa[4]; double vb[3], pb[4]; double vc[3], pc[4]; double vd[3], pd[4]; mid4(v, a, b, c, d); mid2(va, v, a); mid2(vb, v, b); mid2(vc, v, c); mid2(vd, v, d); source->get(pa, va); source->get(pb, vb); source->get(pc, vc); source->get(pd, vd); if (r < 4) { if (contrast(pa[0], pb[0], pc[0], pd[0]) > kr || contrast(pa[1], pb[1], pc[1], pd[1]) > kg || contrast(pa[2], pb[2], pc[2], pd[2]) > kb) { double n[3]; double s[3]; double e[3]; double w[3]; mid2(n, a, b); mid2(s, c, d); mid2(e, a, c); mid2(w, b, d); adapt(pa, a, n, e, v, r + 1); adapt(pb, n, b, v, w, r + 1); adapt(pc, e, v, c, s, r + 1); adapt(pd, v, w, s, d, r + 1); } } switch (source->getc()) { case 4: p[3] = (pa[3] + pb[3] + pc[3] + pd[3]) / 4; case 3: p[2] = (pa[2] + pb[2] + pc[2] + pd[2]) / 4; case 2: p[1] = (pa[1] + pb[1] + pc[1] + pd[1]) / 4; case 1: p[0] = (pa[0] + pb[0] + pc[0] + pd[0]) / 4; } }
vector<Point3d> PlaneDetector::getPlanes(Mat& rgbImage, PointCloud<PointXYZ>& pcl, vector<Vec4i>& lines) { vector<Point3d> foundPlanes; for (int i = 0; i < lines.size(); i++) { Vec4i line = lines[i]; Point2i a = Point2i(line[0], line[1]); Point2i b = Point2i(line[2], line[3]); if (norm(b - a) < 100) continue; for (int j = 0; j < 2; j++) { Point2i diff = b - a; float len = cv::norm(diff); Point2i mid1(a.x + 4 * diff.x / 10, a.y + 3 * diff.y / 10); Point2i mid2(a.x + 5 * diff.x / 10, a.y + 5 * diff.y / 10); Point2i mid3(a.x + 6 * diff.x / 10, a.y + 7 * diff.y / 10); Point2i rotated; if (j == 0) rotated = Point2i(-diff.y, diff.x); else rotated = Point2i(diff.y, -diff.x); Point2i check1(mid1.x + rotated.x / 5, mid1.y + rotated.y / 5); Point2i check2(mid2.x + rotated.x / 2, mid2.y + rotated.y / 2); Point2i check3(mid3.x + rotated.x / 5, mid3.y + rotated.y / 5); Point3d d1 = depthCloseToPoint(check1, pcl, 10); Point3d d2 = depthCloseToPoint(check2, pcl, 10); Point3d d3 = depthCloseToPoint(check3, pcl, 10); bool planeAlreadyFound = false; for (int j = 0; j < foundPlanes.size(); j++) { if (fabs(pointOnPlane(d1, foundPlanes[j])) < DIST_EPSILON || fabs(pointOnPlane(d2, foundPlanes[j])) < DIST_EPSILON || fabs(pointOnPlane(d3, foundPlanes[j])) < DIST_EPSILON) { planeAlreadyFound = true; break; } } if (planeAlreadyFound) { cv::line(rgbImage, check1, check2, Scalar(0, 0, 255), 3, 8); cv::line(rgbImage, check2, check3, Scalar(0, 0, 255), 3, 8); cv::line(rgbImage, check3, check1, Scalar(0, 0, 255), 3, 8); continue; } cv::line(rgbImage, check1, check2, Scalar(255, 0, 0), 3, 8); cv::line(rgbImage, check2, check3, Scalar(255, 0, 0), 3, 8); cv::line(rgbImage, check3, check1, Scalar(255, 0, 0), 3, 8); if (d1.z != -1 && d2.z != -1 && d3.z != -1) { cv::line(rgbImage, check1, check2, Scalar(255, 255, 255), 3, 8); cv::line(rgbImage, check2, check3, Scalar(255, 255, 255), 3, 8); cv::line(rgbImage, check3, check1, Scalar(255, 255, 255), 3, 8); vector<Point3d> v; v.push_back(d1); v.push_back(d2); v.push_back(d3); Point3d plane = getPlane(v); foundPlanes.push_back(plane); } } } return foundPlanes; }
vector<Point3d> PlaneDetector::getPlanes(Mat& rgbImage, PointCloud<PointXYZ>& pcl, vector<Vec4i>& lines) { vector<Point3d> foundPlanes; for (int i = 0; i < 2; i++) { Vec4i line = lines[i]; Point2i a = Point2i(line[0], line[1]); Point2i b = Point2i(line[2], line[3]); if (norm(b - a) < 100) continue; for (int j = 0; j < 2; j++) { Point2i diff = b - a; float len = cv::norm(diff); Point2i mid1(a.x + 4 * diff.x / 10, a.y + 3 * diff.y / 10); Point2i mid2(a.x + 5 * diff.x / 10, a.y + 5 * diff.y / 10); Point2i mid3(a.x + 6 * diff.x / 10, a.y + 7 * diff.y / 10); Point2i rotated; if (j == 0) rotated = Point2i(-diff.y, diff.x); else rotated = Point2i(diff.y, -diff.x); Point2i check1; Point2i check2; Point2i check3; Point3d d1; Point3d d2; Point3d d3; if (i==0){ check1.x=300; check1.y=260; check2.x=320; check2.y=220; check3.x=340; check3.y=240; d1 = depthCloseToPoint(check1, pcl, 10); d2 = depthCloseToPoint(check2, pcl, 10); d3 = depthCloseToPoint(check3, pcl, 10); // Point2i check1(mid1.x + rotated.x / 5, mid1.y + rotated.y / 5); // Point2i check2(mid2.x + rotated.x / 2, mid2.y + rotated.y / 2); // Point2i check3(mid3.x + rotated.x / 5, mid3.y + rotated.y / 5); // int pt_1_x = 300; // int pt_2_x = 320; // int pt_3_x = 340; // int pt_1_y = 260; // int pt_2_y = 220; // int pt_3_y = 240; } else { check1.x=330; check1.y=400; check2.x=450; check2.y=420; check3.x=390; check3.y=380; // check1(400, 260); // check2(420, 220); // check3(380, 240); d1 = depthCloseToPoint(check1, pcl, 10); d2 = depthCloseToPoint(check2, pcl, 10); d3 = depthCloseToPoint(check3, pcl, 10); } // Point3d d1 = depthCloseToPoint(check1, pcl, 10); // Point3d d2 = depthCloseToPoint(check2, pcl, 10); // Point3d d3 = depthCloseToPoint(check3, pcl, 10); bool planeAlreadyFound = false; for (int j = 0; j < foundPlanes.size(); j++) { if (fabs(pointOnPlane(d1, foundPlanes[j])) < DIST_EPSILON || fabs(pointOnPlane(d2, foundPlanes[j])) < DIST_EPSILON || fabs(pointOnPlane(d3, foundPlanes[j])) < DIST_EPSILON) { planeAlreadyFound = true; break; } } if (planeAlreadyFound) { cv::line(rgbImage, check1, check2, Scalar(0, 0, 255), 3, 8); cv::line(rgbImage, check2, check3, Scalar(0, 0, 255), 3, 8); cv::line(rgbImage, check3, check1, Scalar(0, 0, 255), 3, 8); continue; } cv::line(rgbImage, check1, check2, Scalar(255, 0, 0), 3, 8); cv::line(rgbImage, check2, check3, Scalar(255, 0, 0), 3, 8); cv::line(rgbImage, check3, check1, Scalar(255, 0, 0), 3, 8); if (d1.z != -1 && d2.z != -1 && d3.z != -1) { printf( "3dots: %.2f %.2f %.2f, %.2f %.2f %.2f, %.2f %.2f %.2f\n", d1.x, d1.y, d1.z, d2.x, d2.y, d2.z, d3.x, d3.y, d3.z); cv::line(rgbImage, check1, check2, Scalar(255, 255, 255), 3, 8); cv::line(rgbImage, check2, check3, Scalar(255, 255, 255), 3, 8); cv::line(rgbImage, check3, check1, Scalar(255, 255, 255), 3, 8); vector<Point3d> v; v.push_back(d1); v.push_back(d2); v.push_back(d3); Point3d plane = getPlane(v); printf("Plane: %.2f %.2f %.2f\n", plane.x, plane.y, plane.z); foundPlanes.push_back(plane); } } } return foundPlanes; }