52 #include <visp/vpMatrix.h>
53 #include <visp/vpMath.h>
54 #include <visp/vpColVector.h>
55 #include <visp/vpPoint.h>
56 #include <visp/vpPose.h>
57 #include <visp/vpDisplay.h>
58 #include <visp/vpDisplayOpenCV.h>
59 #include <visp/vpDisplayX.h>
60 #include <visp/vpDisplayGDI.h>
61 #include <visp/vpPixelMeterConversion.h>
62 #include <visp/vpCameraParameters.h>
63 #include <visp/vpColor.h>
64 #include <visp/vpIoTools.h>
65 #include <visp/vpException.h>
66 #include <visp/vpImageIo.h>
67 #include <visp/vpMbTracker.h>
68 #include <visp/vpMatrixException.h>
69 #include <visp/vpIoTools.h>
74 #include <Inventor/nodes/SoSeparator.h>
75 #include <Inventor/VRMLnodes/SoVRMLIndexedFaceSet.h>
76 #include <Inventor/VRMLnodes/SoVRMLIndexedLineSet.h>
77 #include <Inventor/VRMLnodes/SoVRMLCoordinate.h>
78 #include <Inventor/actions/SoWriteAction.h>
79 #include <Inventor/actions/SoSearchAction.h>
80 #include <Inventor/misc/SoChildList.h>
81 #include <Inventor/actions/SoGetMatrixAction.h>
82 #include <Inventor/actions/SoGetPrimitiveCountAction.h>
83 #include <Inventor/actions/SoToVRML2Action.h>
84 #include <Inventor/VRMLnodes/SoVRMLGroup.h>
85 #include <Inventor/VRMLnodes/SoVRMLTransform.h>
86 #include <Inventor/VRMLnodes/SoVRMLShape.h>
139 std::string ext =
".init";
140 std::string str_pose =
"";
141 size_t pos = (
unsigned int)initFile.rfind(ext);
144 std::fstream finitpos ;
146 char s[FILENAME_MAX];
148 if( pos == initFile.size()-ext.size() && pos != 0)
149 str_pose = initFile.substr(0,pos) +
".0.pos";
151 str_pose = initFile +
".0.pos";
153 finitpos.open(str_pose.c_str() ,std::ios::in) ;
154 sprintf(s,
"%s", str_pose.c_str());
159 if(finitpos.fail() ){
160 std::cout <<
"cannot read " << s << std::endl <<
"cMo set to identity" << std::endl;
164 for (
unsigned int i = 0; i < 6; i += 1){
165 finitpos >> init_pos[i];
171 std::cout <<
"last_cMo : "<<std::endl << last_cMo <<std::endl;
178 std::cout <<
"No modification : left click " << std::endl;
179 std::cout <<
"Modify initial pose : right click " << std::endl ;
182 "left click to validate, right click to modify initial pose",
210 if( pos == initFile.size()-ext.size() && pos != 0)
211 sprintf(s,
"%s", initFile.c_str());
213 sprintf(s,
"%s.init", initFile.c_str());
215 std::cout <<
"filename " << s << std::endl ;
216 finit.open(s,std::ios::in) ;
218 std::cout <<
"cannot read " << s << std::endl;
223 if( pos == initFile.size()-ext.size() && pos != 0)
224 dispF = initFile.substr(0,pos) +
".ppm";
226 dispF = initFile +
".ppm";
228 sprintf(s,
"%s", dispF.c_str());
232 #if defined VISP_HAVE_X11
234 #elif defined VISP_HAVE_GDI
236 #elif defined VISP_HAVE_OPENCV
242 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
243 d.
init(Iref,10,500,
"Where to initialize...") ;
253 std::cout <<
"number of points " << n << std::endl ;
255 for (
unsigned int i=0 ; i < n ; i++){
265 bool isWellInit =
false;
269 for(
unsigned int i=0 ; i< n ; i++)
271 std::cout <<
"Click on point " << i+1 << std::endl ;
280 std::cout <<
"click sur point " << ip << std::endl;
303 "left click to validate, right click to re initialize object",
335 std::cout <<
"cMo : "<<std::endl <<
cMo <<std::endl;
349 const std::string &displayFile)
356 for (
unsigned int i=0 ; i < points3D_list.size() ; i++)
357 P[i].setWorldCoordinates(points3D_list[i].get_oX(),points3D_list[i].get_oY(),points3D_list[i].get_oZ()) ;
361 #if defined VISP_HAVE_X11
363 #elif defined VISP_HAVE_GDI
365 #elif defined VISP_HAVE_OPENCV
369 if(displayFile !=
""){
371 std::cout << displayFile.c_str() << std::endl;
373 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
374 d.
init(Iref,10,500,
"Where to initialize...") ;
383 bool isWellInit =
false;
386 for(
unsigned int i=0 ; i< points3D_list.size() ; i++)
388 std::cout <<
"Click on point " << i+1 << std::endl ;
397 std::cout <<
"Click on point " << ip << std::endl;
420 "left click to validate, right click to re initialize object",
467 char s[FILENAME_MAX];
470 std::string ext =
".init";
471 size_t pos = initFile.rfind(ext);
473 if( pos == initFile.size()-ext.size() && pos != 0)
474 sprintf(s,
"%s", initFile.c_str());
476 sprintf(s,
"%s.init", initFile.c_str());
478 std::cout <<
"filename " << s << std::endl ;
479 finit.open(s,std::ios::in) ;
481 std::cout <<
"cannot read " << s << std::endl;
488 std::cout <<
"number of points " << size << std::endl ;
492 for(
unsigned int i=0 ; i< size ; i++)
505 vpERROR_TRACE(
"vpMbTracker::initFromPoints(), Number of 2D points different to the number of 3D points." );
507 for(
unsigned int i=0 ; i< size ; i++)
545 const std::vector<vpPoint> &points3D_list )
547 if(points2D_list.size() != points3D_list.size())
548 vpERROR_TRACE(
"vpMbTracker::initFromPoints(), Number of 2D points different to the number of 3D points." );
550 size_t size = points3D_list.size();
554 for(
size_t i=0 ; i< size ; i++)
556 P[i].
setWorldCoordinates(points3D_list[i].get_oX(),points3D_list[i].get_oY(),points3D_list[i].get_oZ()) ;
601 char s[FILENAME_MAX];
605 std::string ext =
".pos";
606 size_t pos = initFile.rfind(ext);
608 if( pos == initFile.size()-ext.size() && pos != 0)
609 sprintf(s,
"%s", initFile.c_str());
611 sprintf(s,
"%s.pos", initFile.c_str());
613 std::cout <<
"filename " << s << std::endl ;
614 finit.open(s,std::ios::in) ;
616 std::cout <<
"cannot read " << s << std::endl;
620 for (
unsigned int i = 0; i < 6; i += 1){
621 finit >> init_pos[i];
660 std::fstream finitpos ;
661 char s[FILENAME_MAX];
663 sprintf(s,
"%s", filename.c_str());
664 finitpos.open(s, std::ios::out) ;
667 finitpos << init_pos;
697 std::string::const_iterator it;
700 it = modelFile.end();
701 if((*(it-1) ==
'o' && *(it-2) ==
'a' && *(it-3) ==
'c' && *(it-4) ==
'.') ||
702 (*(it-1) ==
'O' && *(it-2) ==
'A' && *(it-3) ==
'C' && *(it-4) ==
'.') ){
705 else if((*(it-1) ==
'l' && *(it-2) ==
'r' && *(it-3) ==
'w' && *(it-4) ==
'.') ||
706 (*(it-1) ==
'L' && *(it-2) ==
'R' && *(it-3) ==
'W' && *(it-4) ==
'.') ){
755 #ifdef VISP_HAVE_COIN
759 SbBool ok = in.openFile(modelFile.c_str());
760 SoSeparator *sceneGraph;
761 SoVRMLGroup *sceneGraphVRML2;
768 if(!in.isFileVRML2())
770 sceneGraph = SoDB::readAll(&in);
771 if (sceneGraph == NULL) { }
774 SoToVRML2Action tovrml2;
775 tovrml2.apply(sceneGraph);
777 sceneGraphVRML2 =tovrml2.getVRML2SceneGraph();
778 sceneGraphVRML2->ref();
783 sceneGraphVRML2 = SoDB::readAllVRML(&in);
784 if (sceneGraphVRML2 == NULL) { }
785 sceneGraphVRML2->ref();
791 unsigned int indexFace = 0;
794 sceneGraphVRML2->unref();
796 vpERROR_TRACE(
"coin not detected with ViSP, cannot load model : %s", modelFile.c_str());
830 std::ifstream fileId;
831 fileId.exceptions ( std::ifstream::failbit | std::ifstream::eofbit );
832 fileId.open (modelFile.c_str(), std::ifstream::in);
834 std::cout <<
"cannot read CAO model file: " << modelFile << std::endl;
842 while( (fileId.get(c)!=NULL)&&(c ==
'#')) fileId.ignore(256,
'\n');
848 fileId >> caoVersion;
851 std::cout <<
"in vpMbEdgeTracker::loadCAOModel -> Bad parameter header file : use V0, V1, ...";
853 "in vpMbEdgeTracker::loadCAOModel -> Bad parameter header file : use V0, V1, ...");
856 while( (fileId.get(c)!=NULL)&&(c!=
'\n')) ;
857 while( (fileId.get(c)!=NULL)&&(c ==
'#')) fileId.ignore(256,
'\n') ;
861 unsigned int caoNbrPoint;
862 fileId >> caoNbrPoint;
863 std::cout <<
"> " << caoNbrPoint <<
" points" << std::endl;
866 caoPoints =
new vpPoint[caoNbrPoint];
876 for(
unsigned int k=0; k < caoNbrPoint; k++){
880 if (caoVersion == 2){
885 if(k != caoNbrPoint-1){
886 fileId.ignore(256,
'\n');
891 while( (fileId.get(c)!=NULL)&&(c!=
'\n')) ;
892 while( (fileId.get(c)!=NULL)&&(c ==
'#')) fileId.ignore(256,
'\n');
896 unsigned int caoNbrLine;
897 fileId >> caoNbrLine;
898 unsigned int *caoLinePoints = NULL;
899 std::cout <<
"> " << caoNbrLine <<
" lines" << std::endl;
901 caoLinePoints =
new unsigned int[2*caoNbrLine];
903 unsigned int index1, index2;
905 for(
unsigned int k=0; k < caoNbrLine ; k++){
909 caoLinePoints[2*k] = index1;
910 caoLinePoints[2*k+1] = index2;
912 if(index1 < caoNbrPoint && index2 < caoNbrPoint){
913 std::vector<vpPoint> extremities;
914 extremities.push_back(caoPoints[index1]);
915 extremities.push_back(caoPoints[index2]);
919 vpTRACE(
" line %d has wrong coordinates.", k);
922 if(k != caoNbrLine-1){
923 fileId.ignore(256,
'\n');
927 while( (fileId.get(c)!=NULL)&&(c!=
'\n')) ;
928 while( (fileId.get(c)!=NULL)&&(c ==
'#')) fileId.ignore(256,
'\n');
934 unsigned int caoNbrPolygonLine;
935 fileId >> caoNbrPolygonLine;
936 std::cout <<
"> " << caoNbrPolygonLine <<
" polygon line" << std::endl;
938 for(
unsigned int k = 0;k < caoNbrPolygonLine; k++){
939 unsigned int nbLinePol;
941 std::vector<vpPoint> corners;
942 for(
unsigned int i = 0; i < nbLinePol; i++){
944 corners.push_back(caoPoints[caoLinePoints[2*index]]);
946 if(k != caoNbrPolygonLine-1){
947 fileId.ignore(256,
'\n');
952 while( (fileId.get(c)!=NULL)&&(c!=
'\n')) ;
953 while( (fileId.get(c)!=NULL)&&(c ==
'#')) fileId.ignore(256,
'\n');
957 unsigned int caoNbrPolygonPoint;
958 fileId >> caoNbrPolygonPoint;
959 std::cout <<
"> " << caoNbrPolygonPoint <<
" polygon point" << std::endl;
960 for(
unsigned int k = 0;k < caoNbrPolygonPoint; k++){
962 fileId >> nbPointPol;
963 std::vector<vpPoint> corners;
964 for(
int i = 0; i < nbPointPol; i++){
966 corners.push_back(caoPoints[index]);
968 if(k != caoNbrPolygonPoint-1){
969 fileId.ignore(256,
'\n');
974 unsigned int caoNbCylinder;
976 while( (fileId.get(c)!=NULL)&&(c!=
'\n')) ;
977 while( (fileId.get(c)!=NULL)&&(c ==
'#')) fileId.ignore(256,
'\n');
982 delete[] caoLinePoints;
989 fileId >> caoNbCylinder;
990 std::cout <<
"> " << caoNbCylinder <<
" cylinder" << std::endl;
991 for(
unsigned int k=0; k<caoNbCylinder; ++k){
993 unsigned int indexP1, indexP2;
997 initCylinder(caoPoints[indexP1], caoPoints[indexP2], radius);
1001 std::cerr <<
"Cannot get the number of cylinders. Defaulting to zero." << std::endl;
1006 delete[] caoLinePoints;
1007 }
catch(std::ifstream::failure e){
1008 std::cerr <<
"Cannot read line!" << std::endl;
1017 #ifdef VISP_HAVE_COIN
1029 SoVRMLTransform *sceneGraphVRML2Trasnform =
dynamic_cast<SoVRMLTransform *
>(sceneGraphVRML2);
1030 if(sceneGraphVRML2Trasnform){
1031 float rx, ry, rz, rw;
1032 sceneGraphVRML2Trasnform->rotation.getValue().getValue(rx,ry,rz,rw);
1037 tx = sceneGraphVRML2Trasnform->translation.getValue()[0];
1038 ty = sceneGraphVRML2Trasnform->translation.getValue()[1];
1039 tz = sceneGraphVRML2Trasnform->translation.getValue()[2];
1044 sx = sceneGraphVRML2Trasnform->scale.getValue()[0];
1045 sy = sceneGraphVRML2Trasnform->scale.getValue()[1];
1046 sz = sceneGraphVRML2Trasnform->scale.getValue()[2];
1049 for(
unsigned int i = 0 ; i < 3 ; i++)
1051 for(
unsigned int i = 0 ; i < 3 ; i++)
1053 for(
unsigned int i = 0 ; i < 3 ; i++)
1057 transform = transform * transformCur;
1060 int nbShapes = sceneGraphVRML2->getNumChildren();
1066 for (
int i = 0; i < nbShapes; i++)
1069 child = sceneGraphVRML2->getChild(i);
1071 if (child->getTypeId() == SoVRMLGroup::getClassTypeId()){
1072 extractGroup((SoVRMLGroup*)child, transform_recursive, indexFace);
1075 if (child->getTypeId() == SoVRMLTransform::getClassTypeId()){
1076 extractGroup((SoVRMLTransform*)child, transform_recursive, indexFace);
1079 if (child->getTypeId() == SoVRMLShape::getClassTypeId()){
1080 SoChildList * child2list = child->getChildren();
1081 for (
int j = 0; j < child2list->getLength(); j++)
1083 if (((SoNode*)child2list->get(j))->getTypeId() == SoVRMLIndexedFaceSet::getClassTypeId())
1085 SoVRMLIndexedFaceSet * face_set;
1086 face_set = (SoVRMLIndexedFaceSet*)child2list->get(j);
1087 if(!strncmp(face_set->getName().getString(),
"cyl",3)){
1093 if (((SoNode*)child2list->get(j))->getTypeId() == SoVRMLIndexedLineSet::getClassTypeId())
1095 SoVRMLIndexedLineSet * line_set;
1096 line_set = (SoVRMLIndexedLineSet*)child2list->get(j);
1115 std::vector<vpPoint> corners;
1120 int indexListSize = face_set->coordIndex.getNum();
1124 SoVRMLCoordinate *coord;
1126 for (
int i = 0; i < indexListSize; i++)
1128 if (face_set->coordIndex[i] == -1)
1130 if(corners.size() > 1)
1139 coord = (SoVRMLCoordinate *)(face_set->coord.getValue());
1140 int index = face_set->coordIndex[i];
1141 pointTransformed[0]=coord->point[index].getValue()[0];
1142 pointTransformed[1]=coord->point[index].getValue()[1];
1143 pointTransformed[2]=coord->point[index].getValue()[2];
1144 pointTransformed[3] = 1.0;
1146 pointTransformed = transform * pointTransformed;
1149 corners.push_back(pt);
1169 std::vector<vpPoint> corners_c1, corners_c2;
1170 SoVRMLCoordinate* coords = (SoVRMLCoordinate *)face_set->coord.getValue();
1172 unsigned int indexListSize = (
unsigned int)coords->point.getNum();
1174 if(indexListSize % 2 == 1){
1175 std::cout <<
"Not an even number of points when extracting a cylinder." << std::endl;
1178 corners_c1.resize(indexListSize / 2);
1179 corners_c2.resize(indexListSize / 2);
1185 for(
int i=0; i<coords->point.getNum(); ++i){
1186 pointTransformed[0]=coords->point[i].getValue()[0];
1187 pointTransformed[1]=coords->point[i].getValue()[1];
1188 pointTransformed[2]=coords->point[i].getValue()[2];
1189 pointTransformed[3] = 1.0;
1191 pointTransformed = transform * pointTransformed;
1195 if(i < (
int)corners_c1.size()){
1196 corners_c1[(
unsigned int)i] = pt;
1198 corners_c2[(
unsigned int)i-corners_c1.size()] = pt;
1206 dist[0] = p1.
get_oX() - corners_c1[0].get_oX();
1207 dist[1] = p1.
get_oY() - corners_c1[0].get_oY();
1208 dist[2] = p1.
get_oZ() - corners_c1[0].get_oZ();
1209 double radius_c1 = sqrt(dist.
sumSquare());
1210 dist[0] = p2.
get_oX() - corners_c2[0].get_oX();
1211 dist[1] = p2.
get_oY() - corners_c2[0].get_oY();
1212 dist[2] = p2.
get_oZ() - corners_c2[0].get_oZ();
1213 double radius_c2 = sqrt(dist.sumSquare());
1215 if(std::fabs(radius_c1 - radius_c2) > (std::numeric_limits<double>::epsilon() *
vpMath::maximum(radius_c1, radius_c2))){
1216 std::cout <<
"Radius from the two circles of the cylinders are different." << std::endl;
1237 std::cout <<
"Cannot extract center of gravity of empty set." << std::endl;
1245 for(
unsigned int i=0; i<pts.size(); ++i){
1247 oY += pts[i].get_oY();
1248 oZ += pts[i].get_oZ();
1265 std::vector<vpPoint> corners;
1268 int indexListSize = line_set->coordIndex.getNum();
1270 SbVec3f point(0,0,0);
1272 SoVRMLCoordinate *coord;
1274 unsigned int indexFace = 0;
1276 for (
int i = 0; i < indexListSize; i++)
1278 if (line_set->coordIndex[i] == -1)
1280 if(corners.size() > 1)
1289 coord = (SoVRMLCoordinate *)(line_set->coord.getValue());
1290 int index = line_set->coordIndex[i];
1291 point[0]=coord->point[index].getValue()[0];
1292 point[1]=coord->point[index].getValue()[1];
1293 point[2]=coord->point[index].getValue()[2];
1296 corners.push_back(pt);
1322 "Incorrect matrices size in computeJTR.");
1326 const unsigned int N = interaction.
getRows();
1328 for (
unsigned int i = 0; i < 6; i += 1){
1330 for (
unsigned int j = 0; j < N; j += 1){
1331 ssum += interaction[j][i] * error[j];