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