39 #include <visp/vpMbKltTracker.h>
41 #ifdef VISP_HAVE_OPENCV
98 bool reInitialisation =
false;
102 #ifdef VISP_HAVE_OGRE
126 IplImage* mask = cvCreateImage(cvSize((
int)I.
getWidth(), (int)I.
getHeight()), IPL_DEPTH_8U, 1);
129 for (
unsigned int i = 0; i <
faces.
size(); i += 1){
130 if(
faces[i]->isVisible())
136 for (
unsigned int i = 0; i <
faces.
size(); i += 1){
137 if(
faces[i]->isVisible()){
142 cvReleaseImage(&mask);
155 cvReleaseImage(&
cur);
187 #ifdef VISP_HAVE_OGRE
199 std::vector<vpImagePoint>
202 std::vector<vpImagePoint> kltPoints;
220 std::map<int, vpImagePoint>
223 std::map<int, vpImagePoint> kltPoints;
258 for (
unsigned int i = 0; i <
faces.
size(); i += 1){
259 faces[i]->setCameraParameters(cam);
276 #ifndef VISP_HAVE_OGRE
278 std::cout <<
"WARNING: ViSP dosen't have Ogre3D, basic visibility test will be used. setOgreVisibilityTest() set to false." << std::endl;
296 bool reInitialisation =
false;
300 #ifdef VISP_HAVE_OGRE
307 if(reInitialisation){
308 std::cout <<
"WARNING: Visibility changed, must reinitialise to update pose" << std::endl;
322 CvPoint2D32f* initial_guess = NULL;
323 initial_guess = (CvPoint2D32f*)cvAlloc((
unsigned int)
tracker.
getMaxFeatures()*
sizeof(initial_guess[0]));
325 for (
unsigned int i = 0; i <
faces.
size(); i += 1){
326 if(
faces[i]->isVisible() &&
faces[i]->hasEnoughPoints()){
334 float invDc = 1.0f / plan.
getD();
338 vpGEMM(cdtc, Nc, -invDc, cdRc, 1.0, cdHc, VP_GEMM_B_T);
345 std::map<int, vpImagePoint>::const_iterator iter =
faces[i]->getCurrentPoints().begin();
346 for( ; iter !=
faces[i]->getCurrentPoints().end(); iter++){
348 cdp[0] = iter->second.get_j(); cdp[1] = iter->second.get_i(); cdp[2] = 1.0;
350 double p_mu_t_2 = cdp[0] * cdGc[2][0] + cdp[1] * cdGc[2][1] + cdGc[2][2];
352 if( fabs(p_mu_t_2) < std::numeric_limits<double>::epsilon()){
358 cdp[0] = (cdp[0] * cdGc[0][0] + cdp[1] * cdGc[0][1] + cdGc[0][2]) / p_mu_t_2;
359 cdp[1] = (cdp[0] * cdGc[1][0] + cdp[1] * cdGc[1][1] + cdGc[1][2]) / p_mu_t_2;
362 initial_guess[(
faces[i]->getCurrentPointsInd())[iter->first]].x = (
float)cdp[0];
363 initial_guess[(
faces[i]->getCurrentPointsInd())[iter->first]].y = (
float)cdp[1];
370 if(initial_guess) cvFree(&initial_guess);
371 initial_guess = NULL;
387 if( corners.size() > 2){
392 for(
unsigned int j = 0; j < corners.size(); j++) {
421 for (
unsigned int i = 0; i <
faces.
size(); i += 1){
422 if(
faces[i]->isVisible()){
425 if(
faces[i]->hasEnoughPoints()){
426 nbInfos +=
faces[i]->getNbPointsCur();
441 bool reInitialisation =
false;
443 unsigned int initialNumber = 0;
444 unsigned int currentNumber = 0;
445 unsigned int shift = 0;
446 for (
unsigned int i = 0; i <
faces.
size(); i += 1){
447 if(
faces[i]->isVisible()){
448 initialNumber +=
faces[i]->getInitialNumberPoint();
449 if(
faces[i]->hasEnoughPoints()){
452 shift += 2*
faces[i]->getNbPointsCur();
454 currentNumber +=
faces[i]->getNbPointsCur();
464 double value =
percentGood * (double)initialNumber;
465 if((
double)currentNumber < value){
467 reInitialisation =
true;
473 #ifdef VISP_HAVE_OGRE
509 double normRes_1 = -1;
510 unsigned int iter = 0;
513 J.
resize(2*nbInfos, 6, 0);
515 while( ((
int)((normRes - normRes_1)*1e8) != 0 ) && (iter<
maxIter) ){
517 unsigned int shift = 0;
518 for (
unsigned int i = 0; i <
faces.
size(); i += 1){
519 if(
faces[i]->isVisible() &&
faces[i]->hasEnoughPoints()){
524 faces[i]->computeInteractionMatrixAndResidu(subR, subJ);
526 std::cerr <<
"exception while tracking face " << i << std::endl;
530 shift += 2*
faces[i]->getNbPointsCur();
551 for (
unsigned int i = 0; i < static_cast<unsigned int>(R.
getRows()); i += 1){
552 w_true = w[i] * w[i];
558 for(
unsigned int i=0; i<static_cast<unsigned int>(R.
getRows()); i++){
559 for(
unsigned int j=0; j<6; j++){
593 unsigned int nbInfos;
594 unsigned int nbFaceUsed;
597 if(nbInfos < 4 || nbFaceUsed == 0){
672 #ifdef VISP_HAVE_XML2
687 std::cout <<
" *********** Parsing XML for MBT KLT Tracker ************ " << std::endl;
689 xmlp.
parse(configFile);
711 vpTRACE(
"You need the libXML2 to read the config file %s", configFile);
727 const vpColor& col ,
const unsigned int thickness,
const bool displayFullModel)
729 for (
unsigned int i = 0; i <
faces.
size(); i += 1){
730 if(displayFullModel ||
faces[i]->isVisible())
732 faces[i]->changeFrame(cMo);
733 std::vector<vpImagePoint> roi =
faces[i]->getRoi(cam);
734 for (
unsigned int j = 0; j <
faces[i]->getNbPoint(); j += 1){
737 ip2 = roi[(j+1)%
faces[i]->getNbPoint()];
742 faces[i]->displayPrimitive(I);
749 #ifdef VISP_HAVE_OGRE
767 const vpColor& col ,
const unsigned int thickness,
const bool displayFullModel)
769 for (
unsigned int i = 0; i <
faces.
size(); i += 1){
770 if(displayFullModel ||
faces[i]->isVisible())
772 faces[i]->changeFrame(cMo);
773 std::vector<vpImagePoint> roi =
faces[i]->getRoi(cam);
774 for (
unsigned int j = 0; j <
faces[i]->getNbPoint(); j += 1){
777 ip2 = roi[(j+1)%
faces[i]->getNbPoint()];
782 faces[i]->displayPrimitive(I);
789 #ifdef VISP_HAVE_OGRE
806 unsigned int nbTotalPoints = 0;
807 for (
unsigned int i = 0; i <
faces.
size(); i += 1){
808 if(
faces[i]->isVisible()){
809 nbTotalPoints +=
faces[i]->getNbPointsCur();
813 if(nbTotalPoints < 10){
814 std::cerr <<
"test tracking failed (too few points to realise a good tracking)." << std::endl;
816 "test tracking failed (too few points to realise a good tracking).");
820 #endif //VISP_HAVE_OPENCV