40 #include <visp3/core/vpPolygon.h>
41 #include <visp3/mbt/vpMbtDistanceKltPoints.h>
42 #include <visp3/me/vpMeTracker.h>
44 #if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
46 #if defined(VISP_HAVE_CLIPPER)
47 #include <clipper.hpp>
50 #if defined(__APPLE__) && defined(__MACH__)
51 #include <TargetConditionals.h>
59 : H(), N(), N_cur(), invd0(1.), cRc0_0n(), initPoints(std::map<int,
vpImagePoint>()),
60 curPoints(std::map<int,
vpImagePoint>()), curPointsInd(std::map<int, int>()), nbPointsCur(0), nbPointsInit(0),
61 minNbPoint(4), enoughPoints(false), dt(1.), d0(1.), cam(), isTrackedKltPoints(true), polygon(NULL),
62 hiddenface(NULL), useScanLine(false)
86 initPoints = std::map<int, vpImagePoint>();
87 curPoints = std::map<int, vpImagePoint>();
88 curPointsInd = std::map<int, int>();
89 std::vector<vpImagePoint> roi;
92 for (
unsigned int i = 0; i < static_cast<unsigned int>(_tracker.
getNbFeatures()); i++) {
103 (
unsigned int)x_tmp < hiddenface->getMbScanLineRenderer().getPrimitiveIDs().getWidth() &&
116 curPointsInd[(int)
id] = (
int)i;
120 curPointsInd[id] = (int)i;
125 nbPointsInit = (
unsigned int)initPoints.size();
126 nbPointsCur = (
unsigned int)curPoints.size();
128 if (nbPointsCur >= minNbPoint)
131 enoughPoints =
false;
159 curPoints = std::map<int, vpImagePoint>();
160 curPointsInd = std::map<int, int>();
162 for (
unsigned int i = 0; i < static_cast<unsigned int>(_tracker.
getNbFeatures()); i++) {
164 if (isTrackedFeature((
int)
id) &&
vpMeTracker::inMask(mask, (
unsigned int)y, (
unsigned int)x)) {
166 curPoints[(int)
id] =
vpImagePoint(
static_cast<double>(y),
static_cast<double>(x));
167 curPointsInd[(int)
id] = (
int)i;
169 curPoints[id] =
vpImagePoint(
static_cast<double>(y),
static_cast<double>(x));
170 curPointsInd[id] = (int)i;
175 nbPointsCur = (
unsigned int)curPoints.size();
177 if (nbPointsCur >= minNbPoint)
180 enoughPoints =
false;
197 unsigned int index_ = 0;
199 std::map<int, vpImagePoint>::const_iterator iter = curPoints.begin();
200 for (; iter != curPoints.end(); ++iter) {
202 double i_cur(iter->second.get_i()), j_cur(iter->second.get_j());
204 double x_cur(0), y_cur(0);
213 computeP_mu_t(x0, y0, x0_transform, y0_transform, H);
215 double invZ = compute_1_over_Z(x_cur, y_cur);
217 _J[2 * index_][0] = -invZ;
218 _J[2 * index_][1] = 0;
219 _J[2 * index_][2] = x_cur * invZ;
220 _J[2 * index_][3] = x_cur * y_cur;
221 _J[2 * index_][4] = -(1 + x_cur * x_cur);
222 _J[2 * index_][5] = y_cur;
224 _J[2 * index_ + 1][0] = 0;
225 _J[2 * index_ + 1][1] = -invZ;
226 _J[2 * index_ + 1][2] = y_cur * invZ;
227 _J[2 * index_ + 1][3] = (1 + y_cur * y_cur);
228 _J[2 * index_ + 1][4] = -y_cur * x_cur;
229 _J[2 * index_ + 1][5] = -x_cur;
231 _R[2 * index_] = (x0_transform - x_cur);
232 _R[2 * index_ + 1] = (y0_transform - y_cur);
237 double vpMbtDistanceKltPoints::compute_1_over_Z(
double x,
double y)
239 double num = cRc0_0n[0] * x + cRc0_0n[1] * y + cRc0_0n[2];
240 double den = -(d0 - dt);
256 inline void vpMbtDistanceKltPoints::computeP_mu_t(
double x_in,
double y_in,
double &x_out,
double &y_out,
259 double p_mu_t_2 = x_in * _cHc0[2][0] + y_in * _cHc0[2][1] + _cHc0[2][2];
261 if (fabs(p_mu_t_2) < std::numeric_limits<double>::epsilon()) {
267 x_out = (x_in * _cHc0[0][0] + y_in * _cHc0[0][1] + _cHc0[0][2]) / p_mu_t_2;
268 y_out = (x_in * _cHc0[1][0] + y_in * _cHc0[1][1] + _cHc0[1][2]) / p_mu_t_2;
294 vpGEMM(ctransc0, N, -invd0, cRc0, 1.0, cHc0, VP_GEMM_B_T);
317 for (
unsigned int i = 0; i < 3; i += 1) {
318 dt += ctransc0[i] * (N_cur[i]);
329 bool vpMbtDistanceKltPoints::isTrackedFeature(
int _id)
339 std::map<int, vpImagePoint>::iterator iter = initPoints.find(_id);
340 if (iter != initPoints.end())
356 #
if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
361 unsigned char nb,
unsigned int shiftBorder)
363 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
364 int width = mask.cols;
365 int height = mask.rows;
367 int width = mask->width;
368 int height = mask->height;
371 int i_min, i_max, j_min, j_max;
372 std::vector<vpImagePoint> roi;
375 double shiftBorder_d = (double)shiftBorder;
377 #if defined(VISP_HAVE_CLIPPER)
378 std::vector<vpImagePoint> roi_offset;
380 ClipperLib::Path path;
381 for (std::vector<vpImagePoint>::const_iterator it = roi.begin(); it != roi.end(); ++it) {
382 path.push_back(ClipperLib::IntPoint((ClipperLib::cInt)it->get_u(), (ClipperLib::cInt)it->get_v()));
385 ClipperLib::Paths solution;
386 ClipperLib::ClipperOffset co;
387 co.AddPath(path, ClipperLib::jtRound, ClipperLib::etClosedPolygon);
388 co.Execute(solution, -shiftBorder_d);
391 if (!solution.empty()) {
392 size_t index_max = 0;
394 if (solution.size() > 1) {
398 for (
size_t i = 0; i < solution.size(); i++) {
399 std::vector<vpImagePoint> corners;
401 for (
size_t j = 0; j < solution[i].size(); j++) {
402 corners.push_back(
vpImagePoint((
double)(solution[i][j].Y), (
double)(solution[i][j].X)));
406 if (polygon_area.
getArea() > max_area) {
407 max_area = polygon_area.
getArea();
413 for (
size_t i = 0; i < solution[index_max].size(); i++) {
414 roi_offset.push_back(
vpImagePoint((
double)(solution[index_max][i].Y), (
double)(solution[index_max][i].X)));
424 #if defined(VISP_HAVE_CLIPPER)
431 if (i_min > height) {
434 if (i_max > height) {
444 #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
445 for (
int i = i_min; i < i_max; i++) {
446 double i_d = (double)i;
448 for (
int j = j_min; j < j_max; j++) {
449 double j_d = (double)j;
451 #if defined(VISP_HAVE_CLIPPER)
454 mask.ptr<uchar>(i)[j] = nb;
457 if (shiftBorder != 0) {
462 mask.at<
unsigned char>(i, j) = nb;
466 mask.at<
unsigned char>(i, j) = nb;
473 unsigned char *ptrData = (
unsigned char *)mask->imageData + i_min * mask->widthStep + j_min;
474 for (
int i = i_min; i < i_max; i++) {
475 double i_d = (double)i;
476 for (
int j = j_min; j < j_max; j++) {
477 double j_d = (double)j;
478 if (shiftBorder != 0) {
495 ptrData += mask->widthStep - j_max + j_min;
510 std::map<int, vpImagePoint> tmp;
511 std::map<int, int> tmp2;
512 unsigned int nbSupp = 0;
516 std::map<int, vpImagePoint>::const_iterator iter = curPoints.begin();
517 for (; iter != curPoints.end(); ++iter) {
518 if (_w[k] > threshold_outlier && _w[k + 1] > threshold_outlier) {
520 tmp[iter->first] =
vpImagePoint(iter->second.get_i(), iter->second.get_j());
521 tmp2[iter->first] = curPointsInd[iter->first];
525 initPoints.erase(iter->first);
534 if (nbPointsCur >= minNbPoint)
537 enoughPoints =
false;
548 std::map<int, vpImagePoint>::const_iterator iter = curPoints.begin();
549 for (; iter != curPoints.end(); ++iter) {
552 iP.
set_i(
static_cast<double>(iter->second.get_i()));
553 iP.
set_j(
static_cast<double>(iter->second.get_j()));
559 std::stringstream ss;
572 std::map<int, vpImagePoint>::const_iterator iter = curPoints.begin();
573 for (; iter != curPoints.end(); ++iter) {
576 iP.
set_i(
static_cast<double>(iter->second.get_i()));
577 iP.
set_j(
static_cast<double>(iter->second.get_j()));
583 std::stringstream ss;
591 bool displayFullModel)
593 std::vector<std::vector<double> > models =
getModelForDisplay(camera, displayFullModel);
595 for (
size_t i = 0; i < models.size(); i++) {
605 bool displayFullModel)
607 std::vector<std::vector<double> > models =
getModelForDisplay(camera, displayFullModel);
609 for (
size_t i = 0; i < models.size(); i++) {
624 std::vector<std::vector<double> > features;
626 std::map<int, vpImagePoint>::const_iterator iter = curPoints.begin();
627 for (; iter != curPoints.end(); ++iter) {
630 iP.
set_i(
static_cast<double>(iter->second.get_i()));
631 iP.
set_j(
static_cast<double>(iter->second.get_j()));
637 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
638 std::vector<double> params = {1,
641 std::vector<double> params;
643 params.push_back(iP.
get_i());
644 params.push_back(iP.
get_j());
645 params.push_back(iP2.
get_i());
646 params.push_back(iP2.
get_j());
647 params.push_back(
static_cast<double>(
id));
649 features.push_back(params);
664 bool displayFullModel)
666 std::vector<std::vector<double> > models;
669 std::vector<std::pair<vpPoint, unsigned int> > roi;
672 for (
unsigned int j = 0; j < roi.size(); j += 1) {
681 std::vector<std::pair<vpPoint, vpPoint> > linesLst;
686 linesLst.push_back(std::make_pair(roi[j].first, roi[(j + 1) % roi.size()].first));
688 for (
unsigned int i = 0; i < linesLst.size(); i++) {
689 linesLst[i].first.project();
690 linesLst[i].second.project();
694 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
695 std::vector<double> params = {0,
698 std::vector<double> params;
700 params.push_back(ip1.
get_i());
701 params.push_back(ip1.
get_j());
702 params.push_back(ip2.
get_i());
703 params.push_back(ip2.
get_j());
705 models.push_back(params);
714 #elif !defined(VISP_BUILD_SHARED_LIBS)
717 void dummy_vpMbtDistanceKltPoints(){};
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
vpColVector & normalize()
Class to define RGB colors available for display functionnalities.
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 displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
error that can be emited by ViSP classes.
@ divideByZeroError
Division by zero.
Implementation of an homogeneous matrix and operations on such kind of matrices.
void extract(vpRotationMatrix &R) const
Implementation of an homography and operations on homographies.
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
void set_ij(double ii, double jj)
Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented in OpenCV. Thus to enable this ...
int getNbFeatures() const
Get the number of current features.
void getFeature(const int &index, long &id, float &x, float &y) const
static int round(double x)
Implementation of a matrix and operations on matrices.
vpMbScanLine & getMbScanLineRenderer()
void computeScanLineQuery(const vpPoint &a, const vpPoint &b, std::vector< std::pair< vpPoint, vpPoint > > &lines, const bool &displayResults=false)
void updateMask(cv::Mat &mask, unsigned char _nb=255, unsigned int _shiftBorder=0)
void displayPrimitive(const vpImage< unsigned char > &_I)
bool useScanLine
Use scanline rendering.
void computeInteractionMatrixAndResidu(vpColVector &_R, vpMatrix &_J)
virtual ~vpMbtDistanceKltPoints()
std::vector< std::vector< double > > getFeaturesForDisplay()
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
void init(const vpKltOpencv &_tracker, const vpImage< bool > *mask=NULL)
unsigned int computeNbDetectedCurrent(const vpKltOpencv &_tracker, const vpImage< bool > *mask=NULL)
std::vector< std::vector< double > > getModelForDisplay(const vpCameraParameters &cam, bool displayFullModel=false)
void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false)
vpMbtPolygon * polygon
Pointer to the polygon that define a face.
void removeOutliers(const vpColVector &weight, const double &threshold_outlier)
void computeHomography(const vpHomogeneousMatrix &_cTc0, vpHomography &cHc0)
virtual bool isVisible(const vpHomogeneousMatrix &cMo, double alpha, const bool &modulo=false, const vpCameraParameters &cam=vpCameraParameters(), unsigned int width=0, unsigned int height=0)
static bool inMask(const vpImage< bool > *mask, unsigned int i, unsigned int j)
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
This class defines the container for a plane geometrical structure.
vpColVector getNormal() const
vpPoint & getPoint(const unsigned int _index)
void getRoiClipped(const vpCameraParameters &cam, std::vector< vpImagePoint > &roi)
void getPolygonClipped(std::vector< std::pair< vpPoint, unsigned int > > &poly)
static void getMinMaxRoi(const std::vector< vpImagePoint > &roi, int &i_min, int &i_max, int &j_min, int &j_max)
Defines a generic 2D polygon.
void buildFrom(const std::vector< vpImagePoint > &corners, const bool create_convex_hull=false)
bool isInside(const vpImagePoint &iP, const PointInPolygonMethod &method=PnPolyRayCasting) const
Implementation of a rotation matrix and operations on such kind of matrices.
Class that consider the case of a translation vector.