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)
43 #include <clipper.hpp>
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(nullptr), useScanLine(false)
110 initPoints = std::map<int, vpImagePoint>();
111 initPoints3D = std::map<int, vpPoint>();
112 curPoints = std::map<int, vpImagePoint>();
113 curPointsInd = std::map<int, int>();
115 for (
unsigned int i = 0; i < static_cast<unsigned int>(_tracker.
getNbFeatures()); i++) {
118 _tracker.
getFeature((
int)i, id, x_tmp, y_tmp);
124 (
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];
249 initPoints.erase(iter->first);
256 curPoints = std::map<int, vpImagePoint>();
257 curPointsInd = std::map<int, int>();
261 if (nbPointsCur >= minNbPoint)
264 enoughPoints =
false;
282 unsigned int index_ = 0;
286 std::map<int, vpImagePoint>::const_iterator iter = curPoints.begin();
287 for (; iter != curPoints.end(); ++iter) {
289 double i_cur(iter->second.get_i()), j_cur(iter->second.get_j());
291 double x_cur(0), y_cur(0);
298 double x0_transform(p0.
get_x()), y0_transform(p0.
get_y());
300 double Z = computeZ(x_cur, y_cur);
302 if (
vpMath::isNaN(Z) || Z < std::numeric_limits<double>::epsilon()) {
308 _J[2 * index_][0] = 0;
309 _J[2 * index_][1] = 0;
310 _J[2 * index_][2] = 0;
311 _J[2 * index_][3] = 0;
312 _J[2 * index_][4] = 0;
313 _J[2 * index_][5] = 0;
315 _J[2 * index_ + 1][0] = 0;
316 _J[2 * index_ + 1][1] = 0;
317 _J[2 * index_ + 1][2] = 0;
318 _J[2 * index_ + 1][3] = 0;
319 _J[2 * index_ + 1][4] = 0;
320 _J[2 * index_ + 1][5] = 0;
322 _R[2 * index_] = (x0_transform - x_cur);
323 _R[2 * index_ + 1] = (y0_transform - y_cur);
327 double invZ = 1.0 / Z;
329 _J[2 * index_][0] = -invZ;
330 _J[2 * index_][1] = 0;
331 _J[2 * index_][2] = x_cur * invZ;
332 _J[2 * index_][3] = x_cur * y_cur;
333 _J[2 * index_][4] = -(1 + x_cur * x_cur);
334 _J[2 * index_][5] = y_cur;
336 _J[2 * index_ + 1][0] = 0;
337 _J[2 * index_ + 1][1] = -invZ;
338 _J[2 * index_ + 1][2] = y_cur * invZ;
339 _J[2 * index_ + 1][3] = (1 + y_cur * y_cur);
340 _J[2 * index_ + 1][4] = -y_cur * x_cur;
341 _J[2 * index_ + 1][5] = -x_cur;
343 _R[2 * index_] = (x0_transform - x_cur);
344 _R[2 * index_ + 1] = (y0_transform - y_cur);
357 bool vpMbtDistanceKltCylinder::isTrackedFeature(
int _id)
359 std::map<int, vpImagePoint>::iterator iter = initPoints.find(_id);
360 if (iter != initPoints.end())
377 unsigned char nb,
unsigned int shiftBorder)
379 int width = mask.cols;
380 int height = mask.rows;
385 int i_min, i_max, j_min, j_max;
386 std::vector<vpImagePoint> roi;
389 double shiftBorder_d = (double)shiftBorder;
390 #if defined(VISP_HAVE_CLIPPER)
391 std::vector<vpImagePoint> roi_offset;
393 ClipperLib::Path path;
394 for (std::vector<vpImagePoint>::const_iterator it = roi.begin(); it != roi.end(); ++it) {
395 path.push_back(ClipperLib::IntPoint((ClipperLib::cInt)it->get_u(), (ClipperLib::cInt)it->get_v()));
398 ClipperLib::Paths solution;
399 ClipperLib::ClipperOffset co;
400 co.AddPath(path, ClipperLib::jtRound, ClipperLib::etClosedPolygon);
401 co.Execute(solution, -shiftBorder_d);
404 if (!solution.empty()) {
405 size_t index_max = 0;
407 if (solution.size() > 1) {
411 for (
size_t i = 0; i < solution.size(); i++) {
412 std::vector<vpImagePoint> corners;
414 for (
size_t j = 0; j < solution[i].size(); j++) {
415 corners.push_back(
vpImagePoint((
double)(solution[i][j].Y), (
double)(solution[i][j].X)));
419 if (polygon_area.
getArea() > max_area) {
420 max_area = polygon_area.
getArea();
426 for (
size_t i = 0; i < solution[index_max].size(); i++) {
427 roi_offset.push_back(
vpImagePoint((
double)(solution[index_max][i].Y), (
double)(solution[index_max][i].X)));
438 #if defined(VISP_HAVE_CLIPPER)
445 if (i_min > height) {
448 if (i_max > height) {
458 for (
int i = i_min; i < i_max; i++) {
459 double i_d = (double)i;
461 for (
int j = j_min; j < j_max; j++) {
462 double j_d = (double)j;
464 #if defined(VISP_HAVE_CLIPPER)
467 mask.ptr<uchar>(i)[j] = nb;
470 if (shiftBorder != 0) {
476 mask.at<
unsigned char>(i, j) = nb;
481 mask.at<
unsigned char>(i, j) = nb;
498 std::map<int, vpImagePoint>::const_iterator iter = curPoints.begin();
499 for (; iter != curPoints.end(); ++iter) {
502 iP.
set_i(
static_cast<double>(iter->second.get_i()));
503 iP.
set_j(
static_cast<double>(iter->second.get_j()));
509 std::stringstream ss;
522 std::map<int, vpImagePoint>::const_iterator iter = curPoints.begin();
523 for (; iter != curPoints.end(); ++iter) {
526 iP.
set_i(
static_cast<double>(iter->second.get_i()));
527 iP.
set_j(
static_cast<double>(iter->second.get_j()));
533 std::stringstream ss;
545 for (
size_t i = 0; i < models.size(); i++) {
558 for (
size_t i = 0; i < models.size(); i++) {
573 std::vector<std::vector<double> > features;
575 std::map<int, vpImagePoint>::const_iterator iter = curPoints.begin();
576 for (; iter != curPoints.end(); ++iter) {
579 iP.
set_i(
static_cast<double>(iter->second.get_i()));
580 iP.
set_j(
static_cast<double>(iter->second.get_j()));
586 std::vector<double> params = { 1,
589 features.push_back(params);
606 std::vector<std::vector<double> > models;
619 std::cout <<
"Problem projection circle 1";
625 std::cout <<
"Problem projection circle 2";
638 double i11, i12, i21, i22, j11, j12, j21, j22;
653 std::vector<double> params1 = { 0,
655 models.push_back(params1);
657 std::vector<double> params2 = { 0,
660 models.push_back(params1);
661 models.push_back(params2);
671 double vpMbtDistanceKltCylinder::computeZ(
const double &x,
const double &y)
684 #elif !defined(VISP_BUILD_SHARED_LIBS)
687 void dummy_vpMbtDistanceKltCylinder() { };
Generic class defining intrinsic camera parameters.
void setWorldCoordinates(const vpColVector &oP) override
void projection() override
void changeFrame(const vpHomogeneousMatrix &noMo, vpColVector &noP) const override
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 projection() override
void setWorldCoordinates(const vpColVector &oP) override
double computeZ(double x, double y) const
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const override
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)
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.
vpMbScanLine & getMbScanLineRenderer()
std::vector< PolygonType * > & getPolygon()
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.
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const override
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 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