42 const size_t N_PLANES = 3;
44 const size_t N_plane = 300;
45 const size_t N_noise = 300;
47 const double PLANE_EQ[N_PLANES][4] = {
48 {1, -1, 1, -2}, {1, +1.5, 1, -1}, {0, -1, 1, +2}};
51 for (
size_t p = 0; p < N_PLANES; p++)
53 for (
size_t i = 0; i < N_plane; i++)
60 -(PLANE_EQ[p][3] + PLANE_EQ[p][0] * xx + PLANE_EQ[p][1] * yy) /
68 for (
size_t i = 0; i < N_noise; i++)
77 vector<pair<size_t, TPlane>> detectedPlanes;
78 const double DIST_THRESHOLD = 0.05;
85 cout <<
"RANSAC method: ransac_detect_3D_planes" << endl;
86 cout <<
" Computation time: " << tictac.
Tac() * 1000.0 <<
" ms" << endl;
87 cout <<
" " << detectedPlanes.size() <<
" planes detected." << endl;
98 for (vector<pair<size_t, TPlane>>::iterator p = detectedPlanes.begin();
99 p != detectedPlanes.end(); ++p)
104 p->second.getAsPose3D(glPlanePose);
105 glPlane->setPose(glPlanePose);
112 scene->insert(glPlane);
117 points->setColor(0, 0, 1);
118 points->setPointSize(3);
119 points->enableColorFromZ();
122 vector<float> xsf, ysf, zsf;
127 points->setAllPoints(xsf, ysf, zsf);
129 scene->insert(points);
132 win->get3DSceneAndLock() = scene;
133 win->unlockAccess3DScene();
148 const size_t N_LINES = 4;
150 const size_t N_line = 30;
151 const size_t N_noise = 50;
153 const double LINE_EQ[N_LINES][3] = {
154 {1, -1, -2}, {1, +1.5, -1}, {0, -1, +2}, {0.5, -0.3, +1}};
157 for (
size_t p = 0; p < N_LINES; p++)
159 for (
size_t i = 0; i < N_line; i++)
164 (LINE_EQ[p][2] + LINE_EQ[p][0] * xx) / LINE_EQ[p][1];
170 for (
size_t i = 0; i < N_noise; i++)
178 vector<pair<size_t, TLine2D>> detectedLines;
179 const double DIST_THRESHOLD = 0.2;
186 cout <<
"RANSAC method: ransac_detect_2D_lines" << endl;
187 cout <<
" Computation time: " << tictac.
Tac() * 1000.0 <<
" ms" << endl;
188 cout <<
" " << detectedLines.size() <<
" lines detected." << endl;
194 win2.plot(xs, ys,
".b4",
"points");
197 for (vector<pair<size_t, TLine2D>>::iterator p = detectedLines.begin();
198 p != detectedLines.end(); ++p)
204 ly[q] = -(p->second.coefs[2] + p->second.coefs[0] * lx[q]) /
206 win2.plot(lx, ly,
"r-1",
format(
"line_%u", n++));
223 cout << endl <<
"Now running detection of lines..." << endl << endl;
230 catch (
const std::exception& e)
237 printf(
"Untyped exception!!");