38#include <visp3/core/vpConfig.h>
46#include <visp3/core/vpMeterPixelConversion.h>
47#include <visp3/core/vpPlane.h>
48#include <visp3/mbt/vpMbtDistanceLine.h>
49#include <visp3/visual_features/vpFeatureBuilder.h>
58 : name(), index(0), cam(), me(NULL), isTrackedLine(true), isTrackedLineWithVisibility(true), wmean(1), featureline(),
59 poly(), useScanLine(false), meline(), line(NULL), p1(NULL), p2(NULL), L(), error(), nbFeature(), nbFeatureTotal(0),
60 Reinit(false), hiddenface(NULL), Lindex_polygon(), Lindex_polygon_tracked(), isvisible(false)
72 for (
unsigned int i = 0; i <
meline.size(); i++)
125 double norm = sqrt(A * A + B * B + C * C);
126 plane.
setA(A / norm);
127 plane.
setB(B / norm);
128 plane.
setC(C / norm);
129 plane.
setD(D / norm);
149 buildPlane(P1, P2, P3, plane1);
150 buildPlane(P1, P2, P4, plane2);
188 if (std::fabs((V1 - V2).sumSquare()) > std::numeric_limits<double>::epsilon()) {
190 V3[0] = double(rand_gen.
next() % 1000) / 100;
191 V3[1] = double(rand_gen.
next() % 1000) / 100;
192 V3[2] = double(rand_gen.
next() % 1000) / 100;
199 vpPoint P3(V3[0], V3[1], V3[2]);
200 vpPoint P4(V4[0], V4[1], V4[2]);
203 vpPoint P3(V1[0], V1[1], V1[2]);
204 vpPoint P4(V2[0], V2[1], V2[2]);
229 unsigned int ind = 0;
237 isTrackedLine =
false;
240 isTrackedLine =
true;
244 if (!isTrackedLine) {
245 isTrackedLineWithVisibility =
false;
258 if (!isTrackedLine) {
259 isTrackedLineWithVisibility =
false;
263 unsigned int ind = 0;
264 isTrackedLineWithVisibility =
false;
267 isTrackedLineWithVisibility =
true;
283 for (
unsigned int i = 0; i <
meline.size(); i++)
307 for (
unsigned int i = 0; i <
meline.size(); i++) {
327 std::vector<std::pair<vpPoint, vpPoint> > linesLst;
335 if (linesLst.size() == 0) {
350 while (theta > M_PI) {
353 while (theta < -M_PI) {
357 if (theta < -M_PI / 2.0)
358 theta = -theta - 3 * M_PI / 2.0;
360 theta = M_PI / 2.0 - theta;
362 for (
unsigned int i = 0; i < linesLst.size(); i++) {
365 linesLst[i].first.project();
366 linesLst[i].second.project();
371 vpMbtMeLine *melinePt =
new vpMbtMeLine;
372 melinePt->setMask(*mask);
375 melinePt->setInitRange(0);
379 melinePt->jmin = (int)ip1.
get_j() - marge;
380 melinePt->jmax = (int)ip2.
get_j() + marge;
382 melinePt->jmin = (int)ip2.
get_j() - marge;
383 melinePt->jmax = (int)ip1.
get_j() + marge;
386 melinePt->imin = (int)ip1.
get_i() - marge;
387 melinePt->imax = (int)ip2.
get_i() + marge;
389 melinePt->imin = (int)ip2.
get_i() - marge;
390 melinePt->imax = (int)ip1.
get_i() + marge;
394 melinePt->initTracking(I, ip1, ip2, rho, theta, doNotTrack);
395 meline.push_back(melinePt);
396 nbFeature.push_back((
unsigned int)melinePt->getMeList().size());
423 for (
size_t i = 0; i <
meline.size(); i++) {
429 for (
size_t i = 0; i <
meline.size(); i++) {
462 std::vector<std::pair<vpPoint, vpPoint> > linesLst;
470 if (linesLst.size() !=
meline.size() || linesLst.size() == 0) {
471 for (
size_t i = 0; i <
meline.size(); i++) {
486 for (
size_t j = 0; j <
meline.size(); j++) {
502 while (theta > M_PI) {
505 while (theta < -M_PI) {
509 if (theta < -M_PI / 2.0)
510 theta = -theta - 3 * M_PI / 2.0;
512 theta = M_PI / 2.0 - theta;
515 for (
unsigned int i = 0; i < linesLst.size(); i++) {
518 linesLst[i].first.project();
519 linesLst[i].second.project();
540 meline[i]->updateParameters(I, ip1, ip2, rho, theta);
545 for (
size_t j = 0; j <
meline.size(); j++) {
558 for (
size_t i = 0; i <
meline.size(); i++) {
584 for (
size_t i = 0; i <
meline.size(); i++) {
612 bool displayFullModel)
614 std::vector<std::vector<double> > models =
617 for (
size_t i = 0; i < models.size(); i++) {
637 bool displayFullModel)
639 std::vector<std::vector<double> > models =
642 for (
size_t i = 0; i < models.size(); i++) {
665 for (
size_t i = 0; i <
meline.size(); i++) {
674 for (
size_t i = 0; i <
meline.size(); i++) {
687 std::vector<std::vector<double> > features;
689 for (
size_t i = 0; i <
meline.size(); i++) {
690 vpMbtMeLine *me_l =
meline[i];
692 for (std::list<vpMeSite>::const_iterator it = me_l->getMeList().begin(); it != me_l->getMeList().end(); ++it) {
694#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
695 std::vector<double> params = {0,
698 std::vector<double> params;
702 params.push_back(
static_cast<double>(p_me_l.
getState()));
704 features.push_back(params);
726 bool displayFullModel)
728 std::vector<std::vector<double> > models;
730 if ((
isvisible && isTrackedLine) || displayFullModel) {
749 std::vector<std::pair<vpPoint, vpPoint> > linesLst;
756 for (
unsigned int i = 0; i < linesLst.size(); i++) {
757 linesLst[i].first.project();
758 linesLst[i].second.project();
763#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
764 std::vector<double> params = {0,
767 std::vector<double> params;
769 params.push_back(ip1.
get_i());
770 params.push_back(ip1.
get_j());
771 params.push_back(ip2.
get_i());
772 params.push_back(ip2.
get_j());
774 models.push_back(params);
791 for (
size_t i = 0; i <
meline.size(); i++) {
794 std::list<vpMeSite> &me_site_list =
meline[i]->getMeList();
795 me_site_list.clear();
815 double rho = featureline.
getRho();
816 double theta = featureline.
getTheta();
818 double co = cos(theta);
819 double si = sin(theta);
821 double mx = 1.0 / cam.
get_px();
822 double my = 1.0 / cam.
get_py();
832 for (
size_t i = 0; i <
meline.size(); i++) {
833 for (std::list<vpMeSite>::const_iterator it =
meline[i]->getMeList().begin();
834 it !=
meline[i]->getMeList().end(); ++it) {
841 alpha_ = x * si - y * co;
844 double *Ltheta = H[1];
846 for (
unsigned int k = 0; k < 6; k++) {
847 L[j][k] = (Lrho[k] + alpha_ * Ltheta[k]);
849 error[j] = rho - (x * co + y * si);
857 for (
size_t i = 0; i <
meline.size(); i++) {
858 for (std::list<vpMeSite>::const_iterator it =
meline[i]->getMeList().begin();
859 it !=
meline[i]->getMeList().end(); ++it) {
860 for (
unsigned int k = 0; k < 6; k++) {
887 for (
size_t i = 0; i <
meline.size(); i++) {
888 for (std::list<vpMeSite>::const_iterator it =
meline[i]->getMeList().begin(); it !=
meline[i]->getMeList().end();
893 if (i_ < 0 || j_ < 0) {
897 if (((
unsigned int)i_ > (I.
getHeight() - threshold)) || (
unsigned int)i_ < threshold ||
898 ((
unsigned int)j_ > (I.
getWidth() - threshold)) || (
unsigned int)j_ < threshold) {
void resize(unsigned int nrows, unsigned int ncols, bool flagNullify=true, bool recopy_=true)
Generic class defining intrinsic camera parameters.
void computeFov(const unsigned int &w, const unsigned int &h)
Implementation of column vector and the associated operations.
static vpColVector cross(const vpColVector &a, const vpColVector &b)
void resize(unsigned int i, bool flagNullify=true)
Class to define RGB colors available for display functionalities.
static void displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpDot &d)
vpMatrix interaction(unsigned int select=FEATURE_ALL)
Implementation of an homogeneous matrix and operations on such kind of matrices.
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition of the vpImage class member functions.
unsigned int getWidth() const
unsigned int getHeight() const
Class that defines a 3D line in the object frame and allows forward projection of the line in the cam...
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const
Implementation of a matrix and operations on matrices.
void computeScanLineQuery(const vpPoint &a, const vpPoint &b, std::vector< std::pair< vpPoint, vpPoint > > &lines, const bool &displayResults=false)
void setMovingEdge(vpMe *Me)
std::vector< unsigned int > nbFeature
The number of moving edges.
void displayMovingEdges(const vpImage< unsigned char > &I)
std::vector< bool > Lindex_polygon_tracked
void updateMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo)
bool isvisible
Indicates if the line is visible or not.
void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo)
vpPoint * p2
The second extremity.
vpLine * line
The 3D line.
void initInteractionMatrixError()
void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false)
std::list< int > Lindex_polygon
Index of the faces which contain the line.
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, bool doNotTrack, const vpImage< bool > *mask=NULL)
void buildFrom(vpPoint &_p1, vpPoint &_p2, vpUniRand &rand_gen)
unsigned int nbFeatureTotal
The number of moving edges.
bool Reinit
Indicates if the line has to be reinitialized.
std::string getName() const
vpColVector error
The error vector.
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
bool closeToImageBorder(const vpImage< unsigned char > &I, const unsigned int threshold)
std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false)
virtual ~vpMbtDistanceLine()
void reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpImage< bool > *mask=NULL)
std::vector< std::vector< double > > getFeaturesForDisplay()
bool useScanLine
Use scanline rendering.
vpPoint * p1
The first extremity.
std::vector< vpMbtMeLine * > meline
The moving edge container.
vpMatrix L
The interaction matrix.
void setTracked(const std::string &name, const bool &track)
void addPolygon(const int &index)
void trackMovingEdge(const vpImage< unsigned char > &I)
Performs search in a given direction(normal) for a given distance(pixels) for a given 'site'....
vpMeSiteState getState() const
double get_ifloat() const
double get_jfloat() const
static void convertLine(const vpCameraParameters &cam, const double &rho_m, const double &theta_m, double &rho_p, double &theta_p)
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
This class defines the container for a plane geometrical structure.
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
double get_oX() const
Get the point oX coordinate in the object frame.
double get_oZ() const
Get the point oZ coordinate in the object frame.
double get_oY() const
Get the point oY coordinate in the object frame.
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const
vpPoint * p
corners in the object frame
void computePolygonClipped(const vpCameraParameters &cam=vpCameraParameters())
virtual void setNbPoint(unsigned int nb)
unsigned int getClipping() const
std::vector< std::pair< vpPoint, unsigned int > > polyClipped
Region of interest clipped.
void addPoint(unsigned int n, const vpPoint &P)
Class for generating random numbers with uniform probability density.