42 #include <visp/vpMbEdgeKltTracker.h>
44 #ifdef VISP_HAVE_OPENCV
94 unsigned int i = (
unsigned int)
scales.size();
145 unsigned int i = (
unsigned int)
scales.size();
180 unsigned int nbrow = 0;
181 for(std::list<vpMbtDistanceLine*>::iterator it=
lines[lvl].begin(); it!=
lines[lvl].end(); ++it){
187 for(std::list<vpMbtDistanceCylinder*>::const_iterator it=
cylinders[lvl].begin(); it!=
cylinders[lvl].end(); ++it){
280 #ifdef VISP_HAVE_XML2
284 vpTRACE(
"You need the libXML2 to read the config file %s", configFile);
309 const unsigned int lvl)
319 for(std::list<vpMbtDistanceLine*>::const_iterator it=
lines[lvl].begin(); it!=
lines[lvl].end(); ++it){
327 for(std::list<vpMbtDistanceCylinder*>::const_iterator it=
cylinders[lvl].begin(); it!=
cylinders[lvl].end(); ++it){
374 for(std::list<vpMbtDistanceLine*>::const_iterator it=
lines[lvl].begin(); it!=
lines[lvl].end(); ++it){
378 std::list<vpMeSite>::iterator itListLine;
381 for (
unsigned int i=0 ; i < l->
nbFeature ; i++){
408 for(std::list<vpMbtDistanceCylinder*>::const_iterator it=
cylinders[lvl].begin(); it!=
cylinders[lvl].end(); ++it){
411 std::list<vpMeSite>::iterator itListCyl1;
412 std::list<vpMeSite>::iterator itListCyl2;
443 for(
unsigned int i=cy->
nbFeaturel1 ; i < cy->nbFeature ; i++){
485 if(nbrow < 4 && nbInfos < 4){
493 double residu_1 = -1;
494 unsigned int iter = 0;
510 J_klt.
resize(2*nbInfos,6);
516 vpRobust robust_mbt(0), robust_klt(0);
521 double factorMBT = 1.0;
522 double factorKLT = 1.0;
530 double residuMBT = 0;
531 double residuKLT = 0;
533 while( ((
int)((residu - residu_1)*1e8) !=0 ) && (iter<
maxIter) ){
541 unsigned int shift = 0;
554 w.
resize(nbrow + 2*nbInfos);
563 robust_klt.
resize(2*nbInfos);
565 w_true.
resize(nbrow + 2*nbInfos);
571 for(
unsigned int i = 0; i < R_mbt.
getRows(); i++)
572 residuMBT += fabs(R_mbt[i]);
575 robust_mbt.setIteration(iter);
584 for(
unsigned int i = 0; i < R_klt.
getRows(); i++)
585 residuKLT += fabs(R_klt[i]);
596 unsigned int cpt = 0;
597 while(cpt< (nbrow+2*nbInfos)){
598 if(cpt<(
unsigned)nbrow){
599 w[cpt] = ((w_mbt[cpt] * factor[cpt]) * factorMBT) ;
602 w[cpt] = (w_klt[cpt-nbrow] * factorKLT);
615 for (
unsigned int i = 0; i < static_cast<unsigned int>(R->
getRows()); i++){
619 w_true[i] = w[i]*w[i];
622 for (
unsigned int j = 0; j < 6; j += 1){
628 residu = sqrt(num/den);
659 unsigned int nbInfos;
660 unsigned int nbFaceUsed;
693 unsigned int i = (
unsigned int)
scales.size();
729 for(std::list<vpMbtDistanceLine*>::const_iterator it=
lines[lvl].begin(); it!=
lines[lvl].end(); ++it){
746 std::list<vpMeSite>::const_iterator itListLine;
750 for (
unsigned int i=0 ; i < l->
nbFeature ; i++){
759 for(std::list<vpMbtDistanceCylinder*>::const_iterator it=
cylinders[lvl].begin(); it!=
cylinders[lvl].end(); ++it){
764 std::list<vpMeSite>::const_iterator itCyl1;
765 std::list<vpMeSite>::const_iterator itCyl2;
771 for(
unsigned int i=0 ; i < cy->
nbFeature ; i++){
774 if(i<cy->nbFeaturel1) {
799 for(std::list<vpMbtDistanceLine*>::const_iterator it=
lines[lvl].begin(); it!=
lines[lvl].end(); ++it){
802 for (
unsigned int i=0 ; i < l->
nbFeature ; i++){
803 for (
unsigned int j=0; j < 6 ; j++){
804 L[n+i][j] = l->
L[i][j];
805 error[n+i] = l->
error[i];
811 for(std::list<vpMbtDistanceCylinder*>::const_iterator it=
cylinders[lvl].begin(); it!=
cylinders[lvl].end(); ++it){
814 for(
unsigned int i=0 ; i < cy->
nbFeature ; i++){
815 for(
unsigned int j=0; j < 6 ; j++){
816 L[n+i][j] = cy->
L[i][j];
817 error[n+i] = cy->
error[i];
879 const vpColor& col ,
const unsigned int thickness,
const bool displayFullModel)
883 for (
unsigned int i = 0; i <
scales.size(); i += 1){
887 l->
display(I,cMo, cam, col, thickness, displayFullModel);
891 (*it)->display(I, cMo, cam, col, thickness);
904 #ifdef VISP_HAVE_OGRE
922 const vpColor& col ,
const unsigned int thickness,
const bool displayFullModel)
926 for (
unsigned int i = 0; i <
scales.size(); i += 1){
930 l->
display(I,cMo, cam, col, thickness, displayFullModel);
934 (*it)->display(I, cMo, cam, col, thickness);
947 #ifdef VISP_HAVE_OGRE
953 #endif //VISP_HAVE_OPENCV