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>
109 #ifdef VISP_HAVE_COIN
111 std::string version = COIN_VERSION;
112 unsigned int major, minor, patch;
115 if ( (major << 16 | minor << 8 | patch) >= (2 << 16 | 4 << 8 | 0) )
117 else if ( (major << 16 | minor << 8 | patch) >= (2 << 16 | 0 << 8 | 0) )
155 std::string ext =
".init";
156 std::string str_pose =
"";
157 unsigned int pos = initFile.rfind(ext);
160 std::fstream finitpos ;
162 char s[FILENAME_MAX];
164 if( pos == initFile.size()-ext.size() && pos != 0)
165 str_pose = initFile.substr(0,pos) +
".0.pos";
167 str_pose = initFile +
".0.pos";
169 finitpos.open(str_pose.c_str() ,std::ios::in) ;
170 sprintf(s,
"%s", str_pose.c_str());
175 if(finitpos.fail() ){
176 std::cout <<
"cannot read " << s << std::endl <<
"cMo set to identity" << std::endl;
180 for (
unsigned int i = 0; i < 6; i += 1){
181 finitpos >> init_pos[i];
187 std::cout <<
"last_cMo : "<<std::endl << last_cMo <<std::endl;
194 std::cout <<
"No modification : left click " << std::endl;
195 std::cout <<
"Modify initial pose : right click " << std::endl ;
198 "left click to validate, right click to modify initial pose",
226 if( pos == initFile.size()-ext.size() && pos != 0)
227 sprintf(s,
"%s", initFile.c_str());
229 sprintf(s,
"%s.init", initFile.c_str());
231 std::cout <<
"filename " << s << std::endl ;
232 finit.open(s,std::ios::in) ;
234 std::cout <<
"cannot read " << s << std::endl;
239 if( pos == initFile.size()-ext.size() && pos != 0)
240 dispF = initFile.substr(0,pos) +
".ppm";
242 dispF = initFile +
".ppm";
244 sprintf(s,
"%s", dispF.c_str());
248 #if defined VISP_HAVE_X11
250 #elif defined VISP_HAVE_GDI
252 #elif defined VISP_HAVE_OPENCV
258 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
259 d.
init(Iref,10,500,
"Where to initialize...") ;
269 std::cout <<
"number of points " << n << std::endl ;
271 for (
unsigned int i=0 ; i < n ; i++){
281 bool isWellInit =
false;
285 for(
unsigned int i=0 ; i< n ; i++)
287 std::cout <<
"Click on point " << i+1 << std::endl ;
296 std::cout <<
"click sur point " << ip << std::endl;
319 "left click to validate, right click to re initialize object",
351 std::cout <<
"cMo : "<<std::endl <<
cMo <<std::endl;
365 const std::string &displayFile)
372 for (
unsigned int i=0 ; i < points3D_list.size() ; i++)
373 P[i].setWorldCoordinates(points3D_list[i].get_oX(),points3D_list[i].get_oY(),points3D_list[i].get_oZ()) ;
377 #if defined VISP_HAVE_X11
379 #elif defined VISP_HAVE_GDI
381 #elif defined VISP_HAVE_OPENCV
385 if(displayFile !=
""){
387 std::cout << displayFile.c_str() << std::endl;
389 #if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
390 d.
init(Iref,10,500,
"Where to initialize...") ;
399 bool isWellInit =
false;
402 for(
unsigned int i=0 ; i< points3D_list.size() ; i++)
404 std::cout <<
"Click on point " << i+1 << std::endl ;
413 std::cout <<
"Click on point " << ip << std::endl;
436 "left click to validate, right click to re initialize object",
483 char s[FILENAME_MAX];
486 std::string ext =
".init";
487 unsigned int pos = initFile.rfind(ext);
489 if( pos == initFile.size()-ext.size() && pos != 0)
490 sprintf(s,
"%s", initFile.c_str());
492 sprintf(s,
"%s.init", initFile.c_str());
494 std::cout <<
"filename " << s << std::endl ;
495 finit.open(s,std::ios::in) ;
497 std::cout <<
"cannot read " << s << std::endl;
504 std::cout <<
"number of points " << size << std::endl ;
508 for(
unsigned int i=0 ; i< size ; i++)
521 vpERROR_TRACE(
"vpMbTracker::initFromPoints(), Number of 2D points different to the number of 3D points." );
523 for(
unsigned int i=0 ; i< size ; i++)
561 const std::vector<vpPoint> &points3D_list )
563 if(points2D_list.size() != points3D_list.size())
564 vpERROR_TRACE(
"vpMbTracker::initFromPoints(), Number of 2D points different to the number of 3D points." );
566 unsigned int size = points3D_list.size();
570 for(
unsigned int i=0 ; i< size ; i++)
572 P[i].
setWorldCoordinates(points3D_list[i].get_oX(),points3D_list[i].get_oY(),points3D_list[i].get_oZ()) ;
617 char s[FILENAME_MAX];
621 std::string ext =
".pos";
622 unsigned int pos = initFile.rfind(ext);
624 if( pos == initFile.size()-ext.size() && pos != 0)
625 sprintf(s,
"%s", initFile.c_str());
627 sprintf(s,
"%s.pos", initFile.c_str());
629 std::cout <<
"filename " << s << std::endl ;
630 finit.open(s,std::ios::in) ;
632 std::cout <<
"cannot read " << s << std::endl;
636 for (
unsigned int i = 0; i < 6; i += 1){
637 finit >> init_pos[i];
676 std::fstream finitpos ;
677 char s[FILENAME_MAX];
679 sprintf(s,
"%s", filename.c_str());
680 finitpos.open(s, std::ios::out) ;
683 finitpos << init_pos;
700 std::string::const_iterator it;
703 it = modelFile.end();
704 if((*(it-1) ==
'o' && *(it-2) ==
'a' && *(it-3) ==
'c' && *(it-4) ==
'.') ||
705 (*(it-1) ==
'O' && *(it-2) ==
'A' && *(it-3) ==
'C' && *(it-4) ==
'.') ){
708 else if((*(it-1) ==
'l' && *(it-2) ==
'r' && *(it-3) ==
'w' && *(it-4) ==
'.') ||
709 (*(it-1) ==
'L' && *(it-2) ==
'R' && *(it-3) ==
'W' && *(it-4) ==
'.') ){
747 #ifdef VISP_HAVE_COIN
753 SbBool ok = in.openFile(modelFile.c_str());
754 SoSeparator *sceneGraph;
755 SoVRMLGroup *sceneGraphVRML2;
762 if(!in.isFileVRML2())
764 sceneGraph = SoDB::readAll(&in);
765 if (sceneGraph == NULL) { }
768 SoToVRML2Action tovrml2;
769 tovrml2.apply(sceneGraph);
771 sceneGraphVRML2 =tovrml2.getVRML2SceneGraph();
772 sceneGraphVRML2->ref();
777 sceneGraphVRML2 = SoDB::readAllVRML(&in);
778 if (sceneGraphVRML2 == NULL) { }
779 sceneGraphVRML2->ref();
785 unsigned int indexFace = 0;
788 sceneGraphVRML2->unref();
790 vpERROR_TRACE(
"coin not detected with ViSP, cannot load model : %s", modelFile.c_str());
824 std::ifstream fileId;
825 fileId.exceptions ( std::ifstream::failbit | std::ifstream::eofbit );
826 fileId.open (modelFile.c_str(), std::ifstream::in);
828 std::cout <<
"cannot read CAO model file: " << modelFile << std::endl;
836 while( (fileId.get(c)!=NULL)&&(c ==
'#')) fileId.ignore(256,
'\n');
842 fileId >> caoVersion;
845 std::cout <<
"in vpMbEdgeTracker::loadCAOModel -> Bad parameter header file : use V0, V1, ...";
847 "in vpMbEdgeTracker::loadCAOModel -> Bad parameter header file : use V0, V1, ...");
850 while( (fileId.get(c)!=NULL)&&(c!=
'\n')) ;
851 while( (fileId.get(c)!=NULL)&&(c ==
'#')) fileId.ignore(256,
'\n') ;
855 unsigned int caoNbrPoint;
856 fileId >> caoNbrPoint;
857 std::cout <<
"> " << caoNbrPoint <<
" points" << std::endl;
860 caoPoints =
new vpPoint[caoNbrPoint];
870 for(
unsigned int k=0; k < caoNbrPoint; k++){
874 if (caoVersion == 2){
879 if(k != caoNbrPoint-1){
880 fileId.ignore(256,
'\n');
885 while( (fileId.get(c)!=NULL)&&(c!=
'\n')) ;
886 while( (fileId.get(c)!=NULL)&&(c ==
'#')) fileId.ignore(256,
'\n');
890 unsigned int caoNbrLine;
891 fileId >> caoNbrLine;
892 unsigned int *caoLinePoints = NULL;
893 std::cout <<
"> " << caoNbrLine <<
" lines" << std::endl;
895 caoLinePoints =
new unsigned int[2*caoNbrLine];
897 unsigned int index1, index2;
899 for(
unsigned int k=0; k < caoNbrLine ; k++){
903 caoLinePoints[2*k] = index1;
904 caoLinePoints[2*k+1] = index2;
906 if(index1 < caoNbrPoint && index2 < caoNbrPoint){
907 std::vector<vpPoint> extremities;
908 extremities.push_back(caoPoints[index1]);
909 extremities.push_back(caoPoints[index2]);
913 vpTRACE(
" line %d has wrong coordinates.", k);
916 if(k != caoNbrLine-1){
917 fileId.ignore(256,
'\n');
921 while( (fileId.get(c)!=NULL)&&(c!=
'\n')) ;
922 while( (fileId.get(c)!=NULL)&&(c ==
'#')) fileId.ignore(256,
'\n');
928 unsigned int caoNbrPolygonLine;
929 fileId >> caoNbrPolygonLine;
930 std::cout <<
"> " << caoNbrPolygonLine <<
" polygon line" << std::endl;
932 for(
unsigned int k = 0;k < caoNbrPolygonLine; k++){
933 unsigned int nbLinePol;
935 std::vector<vpPoint> corners;
936 for(
unsigned int i = 0; i < nbLinePol; i++){
938 corners.push_back(caoPoints[caoLinePoints[2*index]]);
940 if(k != caoNbrPolygonLine-1){
941 fileId.ignore(256,
'\n');
946 while( (fileId.get(c)!=NULL)&&(c!=
'\n')) ;
947 while( (fileId.get(c)!=NULL)&&(c ==
'#')) fileId.ignore(256,
'\n');
951 unsigned int caoNbrPolygonPoint;
952 fileId >> caoNbrPolygonPoint;
953 std::cout <<
"> " << caoNbrPolygonPoint <<
" polygon point" << std::endl;
954 for(
unsigned int k = 0;k < caoNbrPolygonPoint; k++){
956 fileId >> nbPointPol;
957 std::vector<vpPoint> corners;
958 for(
int i = 0; i < nbPointPol; i++){
960 corners.push_back(caoPoints[index]);
962 if(k != caoNbrPolygonPoint-1){
963 fileId.ignore(256,
'\n');
968 unsigned int caoNbCylinder;
970 while( (fileId.get(c)!=NULL)&&(c!=
'\n')) ;
971 while( (fileId.get(c)!=NULL)&&(c ==
'#')) fileId.ignore(256,
'\n');
976 delete[] caoLinePoints;
983 fileId >> caoNbCylinder;
984 std::cout <<
"> " << caoNbCylinder <<
" cylinder" << std::endl;
985 for(
unsigned int k=0; k<caoNbCylinder; ++k){
987 unsigned int indexP1, indexP2;
991 initCylinder(caoPoints[indexP1], caoPoints[indexP2], radius);
995 std::cerr <<
"Cannot get the number of cylinders. Defaulting to zero." << std::endl;
1000 delete[] caoLinePoints;
1001 }
catch(std::ifstream::failure e){
1002 std::cerr <<
"Cannot read line!" << std::endl;
1011 #ifdef VISP_HAVE_COIN
1023 SoVRMLTransform *sceneGraphVRML2Trasnform =
dynamic_cast<SoVRMLTransform *
>(sceneGraphVRML2);
1024 if(sceneGraphVRML2Trasnform){
1025 float rx, ry, rz, rw;
1026 sceneGraphVRML2Trasnform->rotation.getValue().getValue(rx,ry,rz,rw);
1031 tx = sceneGraphVRML2Trasnform->translation.getValue()[0];
1032 ty = sceneGraphVRML2Trasnform->translation.getValue()[1];
1033 tz = sceneGraphVRML2Trasnform->translation.getValue()[2];
1038 sx = sceneGraphVRML2Trasnform->scale.getValue()[0];
1039 sy = sceneGraphVRML2Trasnform->scale.getValue()[1];
1040 sz = sceneGraphVRML2Trasnform->scale.getValue()[2];
1043 for(
unsigned int i = 0 ; i < 3 ; i++)
1045 for(
unsigned int i = 0 ; i < 3 ; i++)
1047 for(
unsigned int i = 0 ; i < 3 ; i++)
1051 transform = transform * transformCur;
1054 int nbShapes = sceneGraphVRML2->getNumChildren();
1060 for (
int i = 0; i < nbShapes; i++)
1063 child = sceneGraphVRML2->getChild(i);
1065 if (child->getTypeId() == SoVRMLGroup::getClassTypeId()){
1066 extractGroup((SoVRMLGroup*)child, transform_recursive, indexFace);
1069 if (child->getTypeId() == SoVRMLTransform::getClassTypeId()){
1070 extractGroup((SoVRMLTransform*)child, transform_recursive, indexFace);
1073 if (child->getTypeId() == SoVRMLShape::getClassTypeId()){
1074 SoChildList * child2list = child->getChildren();
1075 for (
int j = 0; j < child2list->getLength(); j++)
1077 if (((SoNode*)child2list->get(j))->getTypeId() == SoVRMLIndexedFaceSet::getClassTypeId())
1079 SoVRMLIndexedFaceSet * face_set;
1080 face_set = (SoVRMLIndexedFaceSet*)child2list->get(j);
1081 if(!strncmp(face_set->getName().getString(),
"cyl",3)){
1087 if (((SoNode*)child2list->get(j))->getTypeId() == SoVRMLIndexedLineSet::getClassTypeId())
1089 SoVRMLIndexedLineSet * line_set;
1090 line_set = (SoVRMLIndexedLineSet*)child2list->get(j);
1109 std::vector<vpPoint> corners;
1114 int indexListSize = face_set->coordIndex.getNum();
1118 SoVRMLCoordinate *coord;
1120 for (
int i = 0; i < indexListSize; i++)
1122 if (face_set->coordIndex[i] == -1)
1124 if(corners.size() > 1)
1133 coord = (SoVRMLCoordinate *)(face_set->coord.getValue());
1134 int index = face_set->coordIndex[i];
1135 pointTransformed[0]=coord->point[index].getValue()[0];
1136 pointTransformed[1]=coord->point[index].getValue()[1];
1137 pointTransformed[2]=coord->point[index].getValue()[2];
1138 pointTransformed[3] = 1.0;
1140 pointTransformed = transform * pointTransformed;
1143 corners.push_back(pt);
1163 std::vector<vpPoint> corners_c1, corners_c2;
1164 SoVRMLCoordinate* coords = (SoVRMLCoordinate *)face_set->coord.getValue();
1166 unsigned int indexListSize = (
unsigned int)coords->point.getNum();
1168 if(indexListSize % 2 == 1){
1169 std::cout <<
"Not an even number of points when extracting a cylinder." << std::endl;
1172 corners_c1.resize(indexListSize / 2);
1173 corners_c2.resize(indexListSize / 2);
1179 for(
int i=0; i<coords->point.getNum(); ++i){
1180 pointTransformed[0]=coords->point[i].getValue()[0];
1181 pointTransformed[1]=coords->point[i].getValue()[1];
1182 pointTransformed[2]=coords->point[i].getValue()[2];
1183 pointTransformed[3] = 1.0;
1185 pointTransformed = transform * pointTransformed;
1189 if(i < (
int)corners_c1.size()){
1190 corners_c1[(
unsigned int)i] = pt;
1192 corners_c2[(
unsigned int)i-corners_c1.size()] = pt;
1200 dist[0] = p1.
get_oX() - corners_c1[0].get_oX();
1201 dist[1] = p1.
get_oY() - corners_c1[0].get_oY();
1202 dist[2] = p1.
get_oZ() - corners_c1[0].get_oZ();
1203 double radius_c1 = sqrt(dist.
sumSquare());
1204 dist[0] = p2.
get_oX() - corners_c2[0].get_oX();
1205 dist[1] = p2.
get_oY() - corners_c2[0].get_oY();
1206 dist[2] = p2.
get_oZ() - corners_c2[0].get_oZ();
1207 double radius_c2 = sqrt(dist.sumSquare());
1209 if(std::fabs(radius_c1 - radius_c2) > (std::numeric_limits<double>::epsilon() *
vpMath::maximum(radius_c1, radius_c2))){
1210 std::cout <<
"Radius from the two circles of the cylinders are different." << std::endl;
1231 std::cout <<
"Cannot extract center of gravity of empty set." << std::endl;
1239 for(
unsigned int i=0; i<pts.size(); ++i){
1241 oY += pts[i].get_oY();
1242 oZ += pts[i].get_oZ();
1259 std::vector<vpPoint> corners;
1262 int indexListSize = line_set->coordIndex.getNum();
1264 SbVec3f point(0,0,0);
1266 SoVRMLCoordinate *coord;
1268 unsigned int indexFace = 0;
1270 for (
int i = 0; i < indexListSize; i++)
1272 if (line_set->coordIndex[i] == -1)
1274 if(corners.size() > 1)
1283 coord = (SoVRMLCoordinate *)(line_set->coord.getValue());
1284 int index = line_set->coordIndex[i];
1285 point[0]=coord->point[index].getValue()[0];
1286 point[1]=coord->point[index].getValue()[1];
1287 point[2]=coord->point[index].getValue()[2];
1290 corners.push_back(pt);
1316 "Incorrect matrices size in computeJTR.");
1320 const unsigned int N = interaction.
getRows();
1322 for (
unsigned int i = 0; i < 6; i += 1){
1324 for (
unsigned int j = 0; j < N; j += 1){
1325 ssum += interaction[j][i] * error[j];