40 #include <visp3/core/vpConfig.h>
48 #include <visp3/core/vpMeterPixelConversion.h>
49 #include <visp3/core/vpPlane.h>
50 #include <visp3/mbt/vpMbtDistanceLine.h>
51 #include <visp3/visual_features/vpFeatureBuilder.h>
60 : name(), index(0), cam(), me(NULL), isTrackedLine(true), isTrackedLineWithVisibility(true), wmean(1), featureline(),
61 poly(), useScanLine(false), meline(), line(NULL), p1(NULL), p2(NULL), L(), error(), nbFeature(), nbFeatureTotal(0),
62 Reinit(false), hiddenface(NULL), Lindex_polygon(), Lindex_polygon_tracked(), isvisible(false)
75 for (
unsigned int i = 0; i <
meline.size(); i++)
128 double norm = sqrt(A * A + B * B + C * C);
129 plane.
setA(A / norm);
130 plane.
setB(B / norm);
131 plane.
setC(C / norm);
132 plane.
setD(D / norm);
152 buildPlane(P1, P2, P3, plane1);
153 buildPlane(P1, P2, P4, plane2);
190 if (std::fabs((V1 - V2).sumSquare()) > std::numeric_limits<double>::epsilon()) {
192 V3[0] = double(rand() % 1000) / 100;
193 V3[1] = double(rand() % 1000) / 100;
194 V3[2] = double(rand() % 1000) / 100;
201 vpPoint P3(V3[0], V3[1], V3[2]);
202 vpPoint P4(V4[0], V4[1], V4[2]);
205 vpPoint P3(V1[0], V1[1], V1[2]);
206 vpPoint P4(V2[0], V2[1], V2[2]);
231 unsigned int ind = 0;
233 if ((*
hiddenface)[(unsigned)(*itpoly)]->getName() == polyname) {
239 isTrackedLine =
false;
242 isTrackedLine =
true;
246 if (!isTrackedLine) {
247 isTrackedLineWithVisibility =
false;
261 if (!isTrackedLine) {
262 isTrackedLineWithVisibility =
false;
266 unsigned int ind = 0;
267 isTrackedLineWithVisibility =
false;
270 isTrackedLineWithVisibility =
true;
286 for (
unsigned int i = 0; i <
meline.size(); i++)
310 for (
unsigned int i = 0; i <
meline.size(); i++) {
330 std::vector<std::pair<vpPoint, vpPoint> > linesLst;
338 if (linesLst.size() == 0) {
354 while (theta > M_PI) {
357 while (theta < -M_PI) {
361 if (theta < -M_PI / 2.0)
362 theta = -theta - 3 * M_PI / 2.0;
364 theta = M_PI / 2.0 - theta;
366 for (
unsigned int i = 0; i < linesLst.size(); i++) {
369 linesLst[i].first.project();
370 linesLst[i].second.project();
375 vpMbtMeLine *melinePt =
new vpMbtMeLine;
376 melinePt->setMask(*mask);
379 melinePt->setInitRange(0);
383 melinePt->jmin = (int)ip1.
get_j() - marge;
384 melinePt->jmax = (int)ip2.
get_j() + marge;
386 melinePt->jmin = (int)ip2.
get_j() - marge;
387 melinePt->jmax = (int)ip1.
get_j() + marge;
390 melinePt->imin = (int)ip1.
get_i() - marge;
391 melinePt->imax = (int)ip2.
get_i() + marge;
393 melinePt->imin = (int)ip2.
get_i() - marge;
394 melinePt->imax = (int)ip1.
get_i() + marge;
398 melinePt->initTracking(I, ip1, ip2, rho, theta, doNotTrack);
399 meline.push_back(melinePt);
400 nbFeature.push_back((
unsigned int) melinePt->getMeList().size());
427 for (
size_t i = 0; i <
meline.size(); i++) {
433 for (
size_t i = 0; i <
meline.size(); i++) {
466 std::vector<std::pair<vpPoint, vpPoint> > linesLst;
474 if (linesLst.size() !=
meline.size() || linesLst.size() == 0) {
475 for (
size_t i = 0; i <
meline.size(); i++) {
491 for (
size_t j = 0; j <
meline.size(); j++) {
507 while (theta > M_PI) {
510 while (theta < -M_PI) {
514 if (theta < -M_PI / 2.0)
515 theta = -theta - 3 * M_PI / 2.0;
517 theta = M_PI / 2.0 - theta;
520 for (
unsigned int i = 0; i < linesLst.size(); i++) {
523 linesLst[i].first.project();
524 linesLst[i].second.project();
545 meline[i]->updateParameters(I, ip1, ip2, rho, theta);
550 for (
size_t j = 0; j <
meline.size(); j++) {
563 for (
size_t i = 0; i <
meline.size(); i++) {
587 for (
size_t i = 0; i <
meline.size(); i++) {
615 bool displayFullModel)
617 std::vector<std::vector<double> > models =
620 for (
size_t i = 0; i < models.size(); i++) {
640 bool displayFullModel)
642 std::vector<std::vector<double> > models =
645 for (
size_t i = 0; i < models.size(); i++) {
668 for (
size_t i = 0; i <
meline.size(); i++) {
677 for (
size_t i = 0; i <
meline.size(); i++) {
690 std::vector<std::vector<double> > features;
692 for (
size_t i = 0; i <
meline.size(); i++) {
693 vpMbtMeLine *me_l =
meline[i];
695 for (std::list<vpMeSite>::const_iterator it = me_l->getMeList().begin(); it != me_l->getMeList().end(); ++it) {
697 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
698 std::vector<double> params = {0,
701 static_cast<double>(p_me_l.
getState())};
703 std::vector<double> params;
707 params.push_back(static_cast<double>(p_me_l.
getState()));
709 features.push_back(params);
731 bool displayFullModel)
733 std::vector<std::vector<double> > models;
735 if ((
isvisible && isTrackedLine) || displayFullModel) {
754 std::vector<std::pair<vpPoint, vpPoint> > linesLst;
761 for (
unsigned int i = 0; i < linesLst.size(); i++) {
762 linesLst[i].first.project();
763 linesLst[i].second.project();
768 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
769 std::vector<double> params = {0,
775 std::vector<double> params;
777 params.push_back(ip1.
get_i());
778 params.push_back(ip1.
get_j());
779 params.push_back(ip2.
get_i());
780 params.push_back(ip2.
get_j());
782 models.push_back(params);
799 for (
size_t i = 0; i <
meline.size(); i++) {
802 std::list<vpMeSite> &me_site_list =
meline[i]->getMeList();
803 me_site_list.clear();
823 double rho = featureline.
getRho();
824 double theta = featureline.
getTheta();
826 double co = cos(theta);
827 double si = sin(theta);
829 double mx = 1.0 / cam.
get_px();
830 double my = 1.0 / cam.
get_py();
840 for (
size_t i = 0; i <
meline.size(); i++) {
841 for (std::list<vpMeSite>::const_iterator it =
meline[i]->getMeList().begin();
842 it !=
meline[i]->getMeList().end(); ++it) {
849 alpha_ = x * si - y * co;
852 double *Ltheta = H[1];
854 for (
unsigned int k = 0; k < 6; k++) {
855 L[j][k] = (Lrho[k] + alpha_ * Ltheta[k]);
857 error[j] = rho - (x * co + y * si);
865 for (
size_t i = 0; i <
meline.size(); i++) {
866 for (std::list<vpMeSite>::const_iterator it =
meline[i]->getMeList().begin();
867 it !=
meline[i]->getMeList().end(); ++it) {
868 for (
unsigned int k = 0; k < 6; k++) {
895 for (
size_t i = 0; i <
meline.size(); i++) {
896 for (std::list<vpMeSite>::const_iterator it =
meline[i]->getMeList().begin(); it !=
meline[i]->getMeList().end();
901 if (i_ < 0 || j_ < 0) {
905 if (((
unsigned int)i_ > (I.
getHeight() - threshold)) || (
unsigned int)i_ < threshold ||
906 ((
unsigned int)j_ > (I.
getWidth() - threshold)) || (
unsigned int)j_ < threshold) {