42 #include <visp/vpMbKltTracker.h>
44 #ifdef VISP_HAVE_OPENCV
104 bool reInitialisation =
false;
108 #ifdef VISP_HAVE_OGRE
134 IplImage* mask = cvCreateImage(cvSize((
int)I.
getWidth(), (int)I.
getHeight()), IPL_DEPTH_8U, 1);
137 for (
unsigned int i = 0; i <
faces.
size(); i += 1){
138 if(
faces[i]->isVisible())
144 for (
unsigned int i = 0; i <
faces.
size(); i += 1){
145 if(
faces[i]->isVisible()){
150 cvReleaseImage(&mask);
163 cvReleaseImage(&
cur);
197 #ifdef VISP_HAVE_OGRE
209 std::vector<vpImagePoint>
212 std::vector<vpImagePoint> kltPoints;
230 std::map<int, vpImagePoint>
233 std::map<int, vpImagePoint> kltPoints;
268 for (
unsigned int i = 0; i <
faces.
size(); i += 1){
269 faces[i]->setCameraParameters(cam);
286 #ifndef VISP_HAVE_OGRE
288 std::cout <<
"WARNING: ViSP doesn't have Ogre3D, basic visibility test will be used. setOgreVisibilityTest() set to false." << std::endl;
306 bool reInitialisation =
false;
310 #ifdef VISP_HAVE_OGRE
317 if(reInitialisation){
318 std::cout <<
"WARNING: Visibility changed, must reinitialize to update pose" << std::endl;
332 CvPoint2D32f* initial_guess = NULL;
333 initial_guess = (CvPoint2D32f*)cvAlloc((
unsigned int)
tracker.
getMaxFeatures()*
sizeof(initial_guess[0]));
335 for (
unsigned int i = 0; i <
faces.
size(); i += 1){
336 if(
faces[i]->isVisible() &&
faces[i]->hasEnoughPoints()){
344 double invDc = 1.0 / plan.
getD();
348 vpGEMM(cdtc, Nc, -invDc, cdRc, 1.0, cdHc, VP_GEMM_B_T);
355 std::map<int, vpImagePoint>::const_iterator iter =
faces[i]->getCurrentPoints().begin();
356 for( ; iter !=
faces[i]->getCurrentPoints().end(); iter++){
358 cdp[0] = iter->second.get_j(); cdp[1] = iter->second.get_i(); cdp[2] = 1.0;
360 double p_mu_t_2 = cdp[0] * cdGc[2][0] + cdp[1] * cdGc[2][1] + cdGc[2][2];
362 if( fabs(p_mu_t_2) < std::numeric_limits<double>::epsilon()){
368 cdp[0] = (cdp[0] * cdGc[0][0] + cdp[1] * cdGc[0][1] + cdGc[0][2]) / p_mu_t_2;
369 cdp[1] = (cdp[0] * cdGc[1][0] + cdp[1] * cdGc[1][1] + cdGc[1][2]) / p_mu_t_2;
372 initial_guess[(
faces[i]->getCurrentPointsInd())[iter->first]].x = (
float)cdp[0];
373 initial_guess[(
faces[i]->getCurrentPointsInd())[iter->first]].y = (
float)cdp[1];
380 if(initial_guess) cvFree(&initial_guess);
381 initial_guess = NULL;
397 vpTRACE(
"Far clipping value cannot be inferior than near clipping value. Far clipping won't be considered.");
399 vpTRACE(
"Far clipping value cannot be inferior than 0. Far clipping won't be considered.");
403 for (
unsigned int i = 0; i <
faces.
size(); i ++){
418 vpTRACE(
"Near clipping value cannot be superior than far clipping value. Near clipping won't be considered.");
420 vpTRACE(
"Near clipping value cannot be inferior than 0. Near clipping won't be considered.");
424 for (
unsigned int i = 0; i <
faces.
size(); i ++){
441 for (
unsigned int i = 0; i <
faces.
size(); i ++)
454 if( corners.size() > 2){
457 polygon->
setNbPoint((
unsigned int)corners.size());
459 for(
unsigned int j = 0; j < corners.size(); j++) {
497 for (
unsigned int i = 0; i <
faces.
size(); i += 1){
498 if(
faces[i]->isVisible()){
501 if(
faces[i]->hasEnoughPoints()){
502 nbInfos +=
faces[i]->getNbPointsCur();
517 bool reInitialisation =
false;
519 unsigned int initialNumber = 0;
520 unsigned int currentNumber = 0;
521 unsigned int shift = 0;
522 for (
unsigned int i = 0; i <
faces.
size(); i += 1){
523 if(
faces[i]->isVisible()){
524 initialNumber +=
faces[i]->getInitialNumberPoint();
525 if(
faces[i]->hasEnoughPoints()){
528 shift += 2*
faces[i]->getNbPointsCur();
530 currentNumber +=
faces[i]->getNbPointsCur();
540 double value =
percentGood * (double)initialNumber;
541 if((
double)currentNumber < value){
543 reInitialisation =
true;
549 #ifdef VISP_HAVE_OGRE
585 double normRes_1 = -1;
586 unsigned int iter = 0;
589 J.
resize(2*nbInfos, 6, 0);
591 while( ((
int)((normRes - normRes_1)*1e8) != 0 ) && (iter<
maxIter) ){
593 unsigned int shift = 0;
594 for (
unsigned int i = 0; i <
faces.
size(); i += 1){
595 if(
faces[i]->isVisible() &&
faces[i]->hasEnoughPoints()){
600 faces[i]->computeInteractionMatrixAndResidu(subR, subJ);
602 std::cerr <<
"exception while tracking face " << i << std::endl;
606 shift += 2*
faces[i]->getNbPointsCur();
627 for (
unsigned int i = 0; i < static_cast<unsigned int>(R.
getRows()); i += 1){
628 w_true = w[i] * w[i];
634 for(
unsigned int i=0; i<static_cast<unsigned int>(R.
getRows()); i++){
635 for(
unsigned int j=0; j<6; j++){
669 unsigned int nbInfos;
670 unsigned int nbFaceUsed;
673 if(nbInfos < 4 || nbFaceUsed == 0){
751 #ifdef VISP_HAVE_XML2
766 std::cout <<
" *********** Parsing XML for MBT KLT Tracker ************ " << std::endl;
768 xmlp.
parse(configFile);
799 vpTRACE(
"You need the libXML2 to read the config file %s", configFile);
815 const vpColor& col ,
const unsigned int thickness,
const bool displayFullModel)
822 for (
unsigned int i = 0; i <
faces.
size(); i += 1){
823 if(displayFullModel ||
faces[i]->isVisible())
825 faces[i]->changeFrame(cMo);
826 faces[i]->computeRoiClipped(c);
827 std::vector<std::pair<vpImagePoint,unsigned int> > roi;
828 faces[i]->getRoiClipped(c, roi);
830 for (
unsigned int j = 0; j < roi.size(); j += 1){
839 ip2 = roi[(j+1)%roi.size()].first;
846 faces[i]->displayPrimitive(I);
851 #ifdef VISP_HAVE_OGRE
869 const vpColor& col ,
const unsigned int thickness,
const bool displayFullModel)
876 for (
unsigned int i = 0; i <
faces.
size(); i += 1){
877 if(displayFullModel ||
faces[i]->isVisible())
879 faces[i]->changeFrame(cMo);
880 faces[i]->computeRoiClipped(c);
881 std::vector<std::pair<vpImagePoint,unsigned int> > roi;
882 faces[i]->getRoiClipped(c, roi);
884 for (
unsigned int j = 0; j < roi.size(); j += 1){
893 ip2 = roi[(j+1)%roi.size()].first;
900 faces[i]->displayPrimitive(I);
905 #ifdef VISP_HAVE_OGRE
922 unsigned int nbTotalPoints = 0;
923 for (
unsigned int i = 0; i <
faces.
size(); i += 1){
924 if(
faces[i]->isVisible()){
925 nbTotalPoints +=
faces[i]->getNbPointsCur();
929 if(nbTotalPoints < 10){
930 std::cerr <<
"test tracking failed (too few points to realize a good tracking)." << std::endl;
932 "test tracking failed (too few points to realize a good tracking).");
936 #endif //VISP_HAVE_OPENCV