inline Iter_t mid9 (Iter_t iter_1, Iter_t iter_2, Iter_t iter_3, Iter_t iter_4,
Iter_t iter_5, Iter_t iter_6, Iter_t iter_7, Iter_t iter_8,
Iter_t iter_9, Compare comp)
{
return mid3 (mid3 (iter_1, iter_2, iter_3, comp),
mid3 (iter_4, iter_5, iter_6, comp),
mid3 (iter_7, iter_8, iter_9, comp), comp);
};
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;
}
int main(void)
{
int x[] = {123, 12345, 1234567, 987654321, 10001, -10001,
-123, -100, 100, -12345, 1, 2, -1, -10, 2002, -2002, 0,
1234567890};
int i;
char *m;
for (i = 0; i < sizeof(x)/sizeof(x[0]); i++) {
if (!(m = mid3(x[i])))
m = "error";
printf("%d: %s\n", x[i], m);
}
return 0;
}
int main(void)
{
int num;
char buf[4] = "";
while (scanf("%d", &num) != EOF) {
printf("%d: ", num);
if ((num = mid3(num, buf)) == 0)
printf("%s\n", buf);
else
puts(errors[--num]);
}
return 0;
}
inline void pivot3 (Iter_t first, Iter_t last, Compare comp)
{
auto N2 = (last - first) >> 1;
Iter_t it_val = mid3 (first + 1, first + N2, last - 1, comp);
std::swap (*first, *it_val);
};
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;
}
