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(nullptr), isTrackedLine(true), isTrackedLineWithVisibility(true), wmean(1), featureline(),
59 poly(), useScanLine(false), meline(), line(nullptr), p1(nullptr), p2(nullptr), L(), error(), nbFeature(), nbFeatureTotal(0),
60 Reinit(false), hiddenface(nullptr), Lindex_polygon(), Lindex_polygon_tracked(), isvisible(false)
71 for (
unsigned int i = 0; i <
meline.size(); i++)
124 double norm = sqrt(A * A + B * B + C * C);
125 plane.
setA(A / norm);
126 plane.
setB(B / norm);
127 plane.
setC(C / norm);
128 plane.
setD(D / norm);
148 buildPlane(P1, P2, P3, plane1);
149 buildPlane(P1, P2, P4, plane2);
165 if (
line ==
nullptr) {
187 if (std::fabs((V1 - V2).sumSquare()) > std::numeric_limits<double>::epsilon()) {
189 V3[0] = double(rand_gen.
next() % 1000) / 100;
190 V3[1] = double(rand_gen.
next() % 1000) / 100;
191 V3[2] = double(rand_gen.
next() % 1000) / 100;
198 vpPoint P3(V3[0], V3[1], V3[2]);
199 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++)
284 if (
meline[i] !=
nullptr) {
307 for (
unsigned int i = 0; i <
meline.size(); i++) {
327 std::vector<std::pair<vpPoint, vpPoint> > linesLst;
336 if (linesLst.size() == 0) {
352 while (theta > M_PI) {
355 while (theta < -M_PI) {
359 if (theta < -M_PI / 2.0)
360 theta = -theta - 3 * M_PI / 2.0;
362 theta = M_PI / 2.0 - theta;
364 for (
unsigned int i = 0; i < linesLst.size(); i++) {
367 linesLst[i].first.project();
368 linesLst[i].second.project();
373 vpMbtMeLine *melinePt =
new vpMbtMeLine;
374 melinePt->setMask(*mask);
377 melinePt->setInitRange(0);
381 melinePt->jmin = (int)ip1.
get_j() - marge;
382 melinePt->jmax = (int)ip2.
get_j() + marge;
385 melinePt->jmin = (int)ip2.
get_j() - marge;
386 melinePt->jmax = (int)ip1.
get_j() + marge;
389 melinePt->imin = (int)ip1.
get_i() - marge;
390 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());
429 for (
size_t i = 0; i <
meline.size(); i++) {
436 for (
size_t i = 0; i <
meline.size(); i++) {
469 std::vector<std::pair<vpPoint, vpPoint> > linesLst;
478 if (linesLst.size() !=
meline.size() || linesLst.size() == 0) {
479 for (
size_t i = 0; i <
meline.size(); i++) {
496 for (
size_t j = 0; j <
meline.size(); j++) {
512 while (theta > M_PI) {
515 while (theta < -M_PI) {
519 if (theta < -M_PI / 2.0)
520 theta = -theta - 3 * M_PI / 2.0;
522 theta = M_PI / 2.0 - theta;
525 for (
unsigned int i = 0; i < linesLst.size(); i++) {
528 linesLst[i].first.project();
529 linesLst[i].second.project();
552 meline[i]->updateParameters(I, ip1, ip2, rho, theta);
558 for (
size_t j = 0; j <
meline.size(); j++) {
572 for (
size_t i = 0; i <
meline.size(); i++) {
598 for (
size_t i = 0; i <
meline.size(); i++) {
626 bool displayFullModel)
628 std::vector<std::vector<double> > models =
631 for (
size_t i = 0; i < models.size(); i++) {
651 bool displayFullModel)
653 std::vector<std::vector<double> > models =
656 for (
size_t i = 0; i < models.size(); i++) {
679 for (
size_t i = 0; i <
meline.size(); i++) {
680 if (
meline[i] !=
nullptr) {
688 for (
size_t i = 0; i <
meline.size(); i++) {
689 if (
meline[i] !=
nullptr) {
701 std::vector<std::vector<double> > features;
703 for (
size_t i = 0; i <
meline.size(); i++) {
704 vpMbtMeLine *me_l =
meline[i];
705 if (me_l !=
nullptr) {
706 for (std::list<vpMeSite>::const_iterator it = me_l->getMeList().begin(); it != me_l->getMeList().end(); ++it) {
708 std::vector<double> params = { 0,
711 features.push_back(params);
733 bool displayFullModel)
735 std::vector<std::vector<double> > models;
737 if ((
isvisible && isTrackedLine) || displayFullModel) {
756 std::vector<std::pair<vpPoint, vpPoint> > linesLst;
764 for (
unsigned int i = 0; i < linesLst.size(); i++) {
765 linesLst[i].first.project();
766 linesLst[i].second.project();
771 std::vector<double> params = { 0,
774 models.push_back(params);
792 for (
size_t i = 0; i <
meline.size(); i++) {
795 std::list<vpMeSite> &me_site_list =
meline[i]->getMeList();
796 me_site_list.clear();
816 double rho = featureline.
getRho();
817 double theta = featureline.
getTheta();
819 double co = cos(theta);
820 double si = sin(theta);
822 double mx = 1.0 / cam.
get_px();
823 double my = 1.0 / cam.
get_py();
833 for (
size_t i = 0; i <
meline.size(); i++) {
834 for (std::list<vpMeSite>::const_iterator it =
meline[i]->getMeList().begin();
835 it !=
meline[i]->getMeList().end(); ++it) {
842 alpha_ = x * si - y * co;
845 double *Ltheta = H[1];
847 for (
unsigned int k = 0; k < 6; k++) {
848 L[j][k] = (Lrho[k] + alpha_ * Ltheta[k]);
850 error[j] = rho - (x * co + y * si);
859 for (
size_t i = 0; i <
meline.size(); i++) {
860 for (std::list<vpMeSite>::const_iterator it =
meline[i]->getMeList().begin();
861 it !=
meline[i]->getMeList().end(); ++it) {
862 for (
unsigned int k = 0; k < 6; k++) {
889 for (
size_t i = 0; i <
meline.size(); i++) {
890 for (std::list<vpMeSite>::const_iterator it =
meline[i]->getMeList().begin(); it !=
meline[i]->getMeList().end();
895 if (i_ < 0 || j_ < 0) {
899 if (((
unsigned int)i_ >(I.
getHeight() - threshold)) || (
unsigned int)i_ < threshold ||
900 ((
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) override
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 ...
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 projection() override
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const override
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.
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)
bool initMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, bool doNotTrack, const vpImage< bool > *mask=nullptr)
std::vector< std::vector< double > > getModelForDisplay(unsigned int width, unsigned int height, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, bool displayFullModel=false)
virtual ~vpMbtDistanceLine()
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 reinitMovingEdge(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpImage< bool > *mask=nullptr)
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.
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const override
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.
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.