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;
}
