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;
}
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)));
}
}
}
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;
}
/* private */
void
OffsetCurveBuilder::addInsideTurn(int orientation, bool addStartPoint)
{
::geos::ignore_unused_variable_warning(orientation);
::geos::ignore_unused_variable_warning(addStartPoint);
// add intersection point of offset segments (if any)
li.computeIntersection(offset0.p0, offset0.p1, offset1.p0, offset1.p1);
if (li.hasIntersection())
{
vertexList->addPt(li.getIntersection(0));
return;
}
// If no intersection is detected, it means the angle is so small
// and/or the offset so large that the offsets segments don't
// intersect. In this case we must add a "closing segment" to make
// sure the buffer curve is continuous,
// fairly smooth (e.g. no sharp reversals in direction)
// and tracks the buffer correctly around the corner.
// The curve connects the endpoints of the segment offsets to points
// which lie toward the centre point of the corner.
// The joining curve will not appear in the final buffer outline,
// since it is completely internal to the buffer polygon.
//
// In complex buffer cases the closing segment may cut across many
// other segments in the generated offset curve.
// In order to improve the performance of the noding, the closing
// segment should be kept as short as possible.
// (But not too short, since that would defeat it's purpose).
// This is the purpose of the closingSegFactor heuristic value.
/**
* The intersection test above is vulnerable to robustness errors;
* i.e. it may be that the offsets should intersect very close to
* their endpoints, but aren't reported as such due to rounding.
* To handle this situation appropriately, we use the following test:
* If the offset points are very close, don't add closing segments
* but simply use one of the offset points
*/
if (offset0.p1.distance(offset1.p0) <
distance * INSIDE_TURN_VERTEX_SNAP_DISTANCE_FACTOR)
{
vertexList->addPt(offset0.p1);
}
else
{
// add endpoint of this segment offset
vertexList->addPt(offset0.p1);
// Add "closing segment" of required length.
if ( closingSegFactor > 0 )
{
Coordinate mid0(
(closingSegFactor*offset0.p1.x + s1.x)/(closingSegFactor + 1),
(closingSegFactor*offset0.p1.y + s1.y)/(closingSegFactor + 1)
);
vertexList->addPt(mid0);
Coordinate mid1(
(closingSegFactor*offset1.p0.x + s1.x)/(closingSegFactor + 1),
(closingSegFactor*offset1.p0.y + s1.y)/(closingSegFactor + 1)
);
vertexList->addPt(mid1);
}
else
{
// This branch is not expected to be used
// except for testing purposes.
// It is equivalent to the JTS 1.9 logic for
// closing segments (which results in very poor
// performance for large buffer distances)
vertexList->addPt(s1);
}
// add start point of next segment offset
vertexList->addPt(offset1.p0);
}
}
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;
}
