36#include <visp3/core/vpPolygon.h>
37#include <visp3/mbt/vpMbtDistanceKltCylinder.h>
38#include <visp3/mbt/vpMbtDistanceKltPoints.h>
40#if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_VIDEO)
42#if defined(VISP_HAVE_CLIPPER)
46#if defined(__APPLE__) && defined(__MACH__)
47#include <TargetConditionals.h>
55 : c0Mo(), p1Ext(), p2Ext(), cylinder(), circle1(), circle2(), initPoints(), initPoints3D(), curPoints(),
56 curPointsInd(), nbPointsCur(0), nbPointsInit(0), minNbPoint(4), enoughPoints(false), cam(),
57 isTrackedKltCylinder(true), listIndicesCylinderBBox(), hiddenface(NULL), useScanLine(false)
111 initPoints = std::map<int, vpImagePoint>();
112 initPoints3D = std::map<int, vpPoint>();
113 curPoints = std::map<int, vpImagePoint>();
114 curPointsInd = std::map<int, int>();
116 for (
unsigned int i = 0; i < static_cast<unsigned int>(_tracker.
getNbFeatures()); i++) {
119 _tracker.
getFeature((
int)i,
id, x_tmp, y_tmp);
124 if ((
unsigned int)y_tmp < hiddenface->getMbScanLineRenderer().getPrimitiveIDs().getHeight() &&
125 (
unsigned int)x_tmp < hiddenface->getMbScanLineRenderer().getPrimitiveIDs().getWidth()) {
134 std::vector<vpImagePoint> roi;
147 double xm = 0, ym = 0;
149 double Z = computeZ(xm, ym);
151#ifdef TARGET_OS_IPHONE
154 curPointsInd[(int)
id] = (
int)i;
158 curPointsInd[id] = (int)i;
165#ifdef TARGET_OS_IPHONE
166 initPoints3D[(int)
id] = p;
168 initPoints3D[id] = p;
176 if (nbPointsCur >= minNbPoint)
179 enoughPoints =
false;
198 curPoints = std::map<int, vpImagePoint>();
199 curPointsInd = std::map<int, int>();
201 for (
unsigned int i = 0; i < static_cast<unsigned int>(_tracker.
getNbFeatures()); i++) {
203 if (isTrackedFeature((
int)
id)) {
204#ifdef TARGET_OS_IPHONE
205 curPoints[(int)
id] =
vpImagePoint(
static_cast<double>(y),
static_cast<double>(x));
206 curPointsInd[(int)
id] = (
int)i;
208 curPoints[id] =
vpImagePoint(
static_cast<double>(y),
static_cast<double>(x));
209 curPointsInd[id] = (int)i;
215 if (nbPointsCur >= minNbPoint)
218 enoughPoints =
false;
233 std::map<int, vpImagePoint> tmp;
234 std::map<int, int> tmp2;
235 unsigned int nbSupp = 0;
239 std::map<int, vpImagePoint>::const_iterator iter = curPoints.begin();
240 for (; iter != curPoints.end(); ++iter) {
241 if (_w[k] > threshold_outlier && _w[k + 1] > threshold_outlier) {
243 tmp[iter->first] =
vpImagePoint(iter->second.get_i(), iter->second.get_j());
244 tmp2[iter->first] = curPointsInd[iter->first];
248 initPoints.erase(iter->first);
255 curPoints = std::map<int, vpImagePoint>();
256 curPointsInd = std::map<int, int>();
260 if (nbPointsCur >= minNbPoint)
263 enoughPoints =
false;
281 unsigned int index_ = 0;
285 std::map<int, vpImagePoint>::const_iterator iter = curPoints.begin();
286 for (; iter != curPoints.end(); ++iter) {
288 double i_cur(iter->second.get_i()), j_cur(iter->second.get_j());
290 double x_cur(0), y_cur(0);
297 double x0_transform(p0.
get_x()), y0_transform(p0.
get_y());
299 double Z = computeZ(x_cur, y_cur);
301 if (
vpMath::isNaN(Z) || Z < std::numeric_limits<double>::epsilon()) {
307 _J[2 * index_][0] = 0;
308 _J[2 * index_][1] = 0;
309 _J[2 * index_][2] = 0;
310 _J[2 * index_][3] = 0;
311 _J[2 * index_][4] = 0;
312 _J[2 * index_][5] = 0;
314 _J[2 * index_ + 1][0] = 0;
315 _J[2 * index_ + 1][1] = 0;
316 _J[2 * index_ + 1][2] = 0;
317 _J[2 * index_ + 1][3] = 0;
318 _J[2 * index_ + 1][4] = 0;
319 _J[2 * index_ + 1][5] = 0;
321 _R[2 * index_] = (x0_transform - x_cur);
322 _R[2 * index_ + 1] = (y0_transform - y_cur);
325 double invZ = 1.0 / Z;
327 _J[2 * index_][0] = -invZ;
328 _J[2 * index_][1] = 0;
329 _J[2 * index_][2] = x_cur * invZ;
330 _J[2 * index_][3] = x_cur * y_cur;
331 _J[2 * index_][4] = -(1 + x_cur * x_cur);
332 _J[2 * index_][5] = y_cur;
334 _J[2 * index_ + 1][0] = 0;
335 _J[2 * index_ + 1][1] = -invZ;
336 _J[2 * index_ + 1][2] = y_cur * invZ;
337 _J[2 * index_ + 1][3] = (1 + y_cur * y_cur);
338 _J[2 * index_ + 1][4] = -y_cur * x_cur;
339 _J[2 * index_ + 1][5] = -x_cur;
341 _R[2 * index_] = (x0_transform - x_cur);
342 _R[2 * index_ + 1] = (y0_transform - y_cur);
355bool vpMbtDistanceKltCylinder::isTrackedFeature(
int _id)
357 std::map<int, vpImagePoint>::iterator iter = initPoints.find(_id);
358 if (iter != initPoints.end())
375 unsigned char nb,
unsigned int shiftBorder)
377 int width = mask.cols;
378 int height = mask.rows;
383 int i_min, i_max, j_min, j_max;
384 std::vector<vpImagePoint> roi;
387 double shiftBorder_d = (double)shiftBorder;
388#if defined(VISP_HAVE_CLIPPER)
389 std::vector<vpImagePoint> roi_offset;
391 ClipperLib::Path path;
392 for (std::vector<vpImagePoint>::const_iterator it = roi.begin(); it != roi.end(); ++it) {
393 path.push_back(ClipperLib::IntPoint((ClipperLib::cInt)it->get_u(), (ClipperLib::cInt)it->get_v()));
396 ClipperLib::Paths solution;
397 ClipperLib::ClipperOffset co;
398 co.AddPath(path, ClipperLib::jtRound, ClipperLib::etClosedPolygon);
399 co.Execute(solution, -shiftBorder_d);
402 if (!solution.empty()) {
403 size_t index_max = 0;
405 if (solution.size() > 1) {
409 for (
size_t i = 0; i < solution.size(); i++) {
410 std::vector<vpImagePoint> corners;
412 for (
size_t j = 0; j < solution[i].size(); j++) {
413 corners.push_back(
vpImagePoint((
double)(solution[i][j].Y), (
double)(solution[i][j].X)));
417 if (polygon_area.
getArea() > max_area) {
418 max_area = polygon_area.
getArea();
424 for (
size_t i = 0; i < solution[index_max].size(); i++) {
425 roi_offset.push_back(
vpImagePoint((
double)(solution[index_max][i].Y), (
double)(solution[index_max][i].X)));
435#if defined(VISP_HAVE_CLIPPER)
442 if (i_min > height) {
445 if (i_max > height) {
455 for (
int i = i_min; i < i_max; i++) {
456 double i_d = (double)i;
458 for (
int j = j_min; j < j_max; j++) {
459 double j_d = (double)j;
461#if defined(VISP_HAVE_CLIPPER)
464 mask.ptr<uchar>(i)[j] = nb;
467 if (shiftBorder != 0) {
473 mask.at<
unsigned char>(i, j) = nb;
477 mask.at<
unsigned char>(i, j) = nb;
494 std::map<int, vpImagePoint>::const_iterator iter = curPoints.begin();
495 for (; iter != curPoints.end(); ++iter) {
498 iP.
set_i(
static_cast<double>(iter->second.get_i()));
499 iP.
set_j(
static_cast<double>(iter->second.get_j()));
505 std::stringstream ss;
518 std::map<int, vpImagePoint>::const_iterator iter = curPoints.begin();
519 for (; iter != curPoints.end(); ++iter) {
522 iP.
set_i(
static_cast<double>(iter->second.get_i()));
523 iP.
set_j(
static_cast<double>(iter->second.get_j()));
529 std::stringstream ss;
541 for (
size_t i = 0; i < models.size(); i++) {
554 for (
size_t i = 0; i < models.size(); i++) {
569 std::vector<std::vector<double> > features;
571 std::map<int, vpImagePoint>::const_iterator iter = curPoints.begin();
572 for (; iter != curPoints.end(); ++iter) {
575 iP.
set_i(
static_cast<double>(iter->second.get_i()));
576 iP.
set_j(
static_cast<double>(iter->second.get_j()));
582#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
583 std::vector<double> params = {1,
586 std::vector<double> params;
588 params.push_back(iP.
get_i());
589 params.push_back(iP.
get_j());
590 params.push_back(iP2.
get_i());
591 params.push_back(iP2.
get_j());
592 params.push_back(
static_cast<double>(
id));
594 features.push_back(params);
611 std::vector<std::vector<double> > models;
623 std::cout <<
"Problem projection circle 1";
628 std::cout <<
"Problem projection circle 2";
641 double i11, i12, i21, i22, j11, j12, j21, j22;
656#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
657 std::vector<double> params1 = {0,
659 models.push_back(params1);
661 std::vector<double> params2 = {0,
664 std::vector<double> params1, params2;
665 params1.push_back(0);
666 params1.push_back(ip11.
get_i());
667 params1.push_back(ip11.
get_j());
668 params1.push_back(ip12.
get_i());
669 params1.push_back(ip12.
get_j());
671 params2.push_back(0);
672 params2.push_back(ip21.
get_i());
673 params2.push_back(ip21.
get_j());
674 params2.push_back(ip22.
get_i());
675 params2.push_back(ip22.
get_j());
677 models.push_back(params1);
678 models.push_back(params2);
688double vpMbtDistanceKltCylinder::computeZ(
const double &x,
const double &y)
701#elif !defined(VISP_BUILD_SHARED_LIBS)
704void dummy_vpMbtDistanceKltCylinder(){};
Generic class defining intrinsic camera parameters.
void changeFrame(const vpHomogeneousMatrix &noMo, vpColVector &noP) const
void setWorldCoordinates(const vpColVector &oP)
static void computeIntersectionPoint(const vpCircle &circle, const vpCameraParameters &cam, const double &rho, const double &theta, double &i, double &j)
Implementation of column vector and the associated operations.
Class to define RGB colors available for display functionalities.
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const
double computeZ(double x, double y) const
void setWorldCoordinates(const vpColVector &oP)
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)
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 ...
void set_ij(double ii, double jj)
Definition of the vpImage class member functions.
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 bool isNaN(double value)
static int round(double x)
Implementation of a matrix and operations on matrices.
std::vector< PolygonType * > & getPolygon()
vpMbScanLine & getMbScanLineRenderer()
vpMbtDistanceKltCylinder()
void buildFrom(const vpPoint &p1, const vpPoint &p2, const double &r)
void removeOutliers(const vpColVector &weight, const double &threshold_outlier)
unsigned int computeNbDetectedCurrent(const vpKltOpencv &_tracker)
std::vector< std::vector< double > > getFeaturesForDisplay()
void computeInteractionMatrixAndResidu(const vpHomogeneousMatrix &cMc0, vpColVector &_R, vpMatrix &_J)
virtual ~vpMbtDistanceKltCylinder()
bool useScanLine
Use scanline rendering.
void updateMask(cv::Mat &mask, unsigned char _nb=255, unsigned int _shiftBorder=0)
void init(const vpKltOpencv &_tracker, const vpHomogeneousMatrix &cMo)
std::vector< std::vector< double > > getModelForDisplay(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
void displayPrimitive(const vpImage< unsigned char > &_I)
std::vector< int > listIndicesCylinderBBox
Pointer to the polygon that define a face.
void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false)
vpMbHiddenFaces< vpMbtPolygon > * hiddenface
Pointer to the list of faces.
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 &u, const double &v, double &x, double &y)
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_y() const
Get the point y coordinate in the image plane.
double get_oZ() const
Get the point oZ coordinate in the object frame.
double get_x() const
Get the point x coordinate in the image plane.
double get_oY() const
Get the point oY coordinate in the object frame.
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const
void setWorldCoordinates(double oX, double oY, double oZ)
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