40 #include <visp/vpMbtKltPolygon.h>
42 #ifdef VISP_HAVE_OPENCV
55 initPoints = std::map<int, vpImagePoint>();
56 curPoints = std::map<int, vpImagePoint>();
57 curPointsInd = std::map<int, int>();
82 initPoints = std::map<int, vpImagePoint>();
83 curPoints = std::map<int, vpImagePoint>();
84 curPointsInd = std::map<int, int>();
86 for (
unsigned int i = 0; i < static_cast<unsigned int>(_tracker.
getNbFeatures()); i ++){
91 if(isInside(
getRoi(cam), y_tmp, x_tmp)){
94 curPointsInd[id] = (int)i;
124 curPoints = std::map<int, vpImagePoint>();
125 curPointsInd = std::map<int, int>();
127 for (
unsigned int i = 0; i < static_cast<unsigned int>(_tracker.
getNbFeatures()); i++){
129 if(isTrackedFeature(
id)){
130 curPoints[id] =
vpImagePoint(static_cast<double>(y),static_cast<double>(x));
131 curPointsInd[id] = (int)i;
136 if(nbPointsCur >= minNbPoint) enoughPoints =
true;
137 else enoughPoints =
false;
155 unsigned int index = 0;
157 std::map<int, vpImagePoint>::const_iterator iter = curPoints.begin();
158 for( ; iter != curPoints.end(); iter++){
160 double i_cur(iter->second.get_i()), j_cur(iter->second.get_j());
162 double x_cur(0), y_cur(0);
169 double x0_transform, y0_transform ;
170 computeP_mu_t(x0, y0, x0_transform, y0_transform, H );
172 double invZ = compute_1_over_Z(x_cur, y_cur);
174 _J[2*
index][0] = - invZ;
176 _J[2*
index][2] = x_cur * invZ;
177 _J[2*
index][3] = x_cur * y_cur;
178 _J[2*
index][4] = -(1+x_cur*x_cur);
179 _J[2*
index][5] = y_cur;
181 _J[2*index+1][0] = 0;
182 _J[2*index+1][1] = - invZ;
183 _J[2*index+1][2] = y_cur * invZ;
184 _J[2*index+1][3] = (1+y_cur*y_cur);
185 _J[2*index+1][4] = - y_cur * x_cur;
186 _J[2*index+1][5] = - x_cur;
188 _R[2*
index] = (x0_transform - x_cur);
189 _R[2*index+1] = (y0_transform - y_cur);
195 vpMbtKltPolygon::compute_1_over_Z(
const double x,
const double y)
197 double num = cRc0_0n[0] * x + cRc0_0n[1] * y + cRc0_0n[2];
198 double den = -(d0 - dt);
215 vpMbtKltPolygon::computeP_mu_t(
const double x_in,
const double y_in,
double& x_out,
double& y_out,
const vpMatrix& _cHc0)
217 double p_mu_t_2 = x_in * _cHc0[2][0] + y_in * _cHc0[2][1] + _cHc0[2][2];
219 if( fabs(p_mu_t_2) < std::numeric_limits<double>::epsilon()){
225 x_out = (x_in * _cHc0[0][0] + y_in * _cHc0[0][1] + _cHc0[0][2]) / p_mu_t_2;
226 y_out = (x_in * _cHc0[1][0] + y_in * _cHc0[1][1] + _cHc0[1][2]) / p_mu_t_2;
251 vpGEMM(ctransc0, N, -invd0, cRc0, 1.0, _cHc0, VP_GEMM_B_T);
252 _cHc0 /= _cHc0[2][2];
274 for (
unsigned int i = 0; i < 3; i += 1){
275 dt += ctransc0[i] * (N_cur[i]);
287 vpMbtKltPolygon::isTrackedFeature(
const int _id)
297 std::map<int, vpImagePoint>::iterator iter = initPoints.find(_id);
298 if(iter != initPoints.end())
314 int width = _mask->width;
315 int i_min, i_max, j_min, j_max;
316 std::vector<vpImagePoint> roi =
getRoi(cam);
320 if(i_min > _mask->height){
323 if(i_max > _mask->height){
324 i_max = _mask->height;
326 if(j_min > _mask->width){
329 if(j_max > _mask->width){
330 j_max = _mask->width;
332 double shiftBorder_d = (double) _shiftBorder;
333 char* ptrData = _mask->imageData + i_min*width+j_min;
334 for(
int i=i_min; i< i_max; i++){
335 double i_d = (double) i;
336 for(
int j=j_min; j< j_max; j++){
337 double j_d = (double) j;
338 if(_shiftBorder != 0){
339 if( vpMbtKltPolygon::isInside(roi, i_d, j_d)
340 && vpMbtKltPolygon::isInside(roi, i_d+shiftBorder_d, j_d+shiftBorder_d)
341 && vpMbtKltPolygon::isInside(roi, i_d-shiftBorder_d, j_d+shiftBorder_d)
342 && vpMbtKltPolygon::isInside(roi, i_d+shiftBorder_d, j_d-shiftBorder_d)
343 && vpMbtKltPolygon::isInside(roi, i_d-shiftBorder_d, j_d-shiftBorder_d) ){
351 if(vpMbtKltPolygon::isInside(roi, i, j)){
359 ptrData += width - j_max + j_min;
373 std::map<int, vpImagePoint> tmp;
374 std::map<int, int> tmp2;
375 unsigned int nbSupp = 0;
379 std::map<int, vpImagePoint>::const_iterator iter = curPoints.begin();
380 for( ; iter != curPoints.end(); iter++){
381 if(_w[k] > threshold_outlier && _w[k+1] > threshold_outlier){
383 tmp[iter->first] =
vpImagePoint(iter->second.get_i(), iter->second.get_j());
384 tmp2[iter->first] = curPointsInd[iter->first];
389 initPoints.erase(iter->first);
396 curPoints = std::map<int, vpImagePoint>();
397 curPointsInd = std::map<int, int>();
401 if(nbPointsCur >= minNbPoint) enoughPoints =
true;
402 else enoughPoints =
false;
414 std::map<int, vpImagePoint>::const_iterator iter = curPoints.begin();
415 for( ; iter != curPoints.end(); iter++){
418 iP.
set_i(static_cast<double>(iter->second.get_i()));
419 iP.
set_j(static_cast<double>(iter->second.get_j()));
426 sprintf(ide,
"%ld", static_cast<long int>(
id));
439 std::map<int, vpImagePoint>::const_iterator iter = curPoints.begin();
440 for( ; iter != curPoints.end(); iter++){
443 iP.
set_i(static_cast<double>(iter->second.get_i()));
444 iP.
set_j(static_cast<double>(iter->second.get_j()));
451 sprintf(ide,
"%ld", static_cast<long int>(
id));
460 bool vpMbtKltPolygon::intersect(
const vpImagePoint& p1,
const vpImagePoint& p2,
const double i_test,
const double j_test,
const double i,
const double j)
465 double ex = j - j_test;
466 double ey = i - i_test;
469 int den = (int)(dx * ey - dy * ex) ;
472 t = -( p1.
get_j() * ey - j_test*ey - ex * p1.
get_i() + ex * i_test ) / den;
473 u = -( -dx*p1.
get_i() + dx * i_test + dy * p1.
get_j() - dy * j_test ) / den;
478 return ( t >=0 && t < 1 && u >= 0 && u < 1);
481 bool vpMbtKltPolygon::isInside(
const std::vector<vpImagePoint>& roi,
const double i,
const double j)
483 double i_test = 1000000.;
484 double j_test = 1000000.;
485 unsigned int nbInter = 0;
486 bool computeAgain =
true;
489 computeAgain =
false;
490 for(
unsigned int k=0; k< roi.size(); k++){
492 if(vpMbtKltPolygon::intersect(roi[k], roi[(k+1)%roi.size()], i, j, i_test, j_test)){
508 return ((nbInter%2) == 1);
511 #endif // VISP_HAVE_OPENCV
void set_j(const double j)
Definition of the vpMatrix class.
void init(const vpKltOpencv &_tracker)
The class provides a data structure for the homogeneous matrices as well as a set of operations on th...
void set_i(const double i)
int index
Index of the polygon. Cannot be unsigned int because deafult value is -1.
error that can be emited by ViSP classes.
void displayPrimitive(const vpImage< unsigned char > &_I)
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
Point coordinates conversion from pixel coordinates to normalized coordinates in meter...
void removeOutliers(const vpColVector &weight, const double &threshold_outlier)
static int round(const double x)
The vpRotationMatrix considers the particular case of a rotation matrix.
This class aims to compute the homography wrt.two images.
unsigned int computeNbDetectedCurrent(const vpKltOpencv &_tracker)
void computeInteractionMatrixAndResidu(vpColVector &_R, vpMatrix &_J)
void getFeature(int index, int &id, float &x, float &y) const
virtual ~vpMbtKltPolygon()
void updateMask(IplImage *_mask, unsigned int _nb=255, unsigned int _shiftBorder=0)
virtual void displayCross(const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)=0
std::vector< vpImagePoint > getRoi(const vpCameraParameters &_cam)
void extract(vpRotationMatrix &R) const
void computeHomography(const vpHomogeneousMatrix &_cTc0, vpHomography &cHc0)
vpColVector getNormal() const
Class that provides a data structure for the column vectors as well as a set of operations on these v...
virtual void displayCharString(const vpImagePoint &ip, const char *text, const vpColor &color=vpColor::green)=0
Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker implemented with OpenCV.
int getNbFeatures() const
Get the current number of features.
static void getMinMaxRoi(const std::vector< vpImagePoint > &roi, int &i_min, int &i_max, int &j_min, int &j_max)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
This class defines the container for a plane geometrical structure.
bool isvisible
flag to specify whether the face is visible or not
vpColVector & normalize()
normalise the vector
Class that consider the case of a translation vector.
vpPoint * p
corners in the object frame