44 #include <visp3/core/vpConfig.h>
46 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
48 #if defined(VISP_HAVE_MODULE_KLT) && defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)
50 #include <visp3/core/vpTrackingException.h>
51 #include <visp3/core/vpVelocityTwistMatrix.h>
52 #include <visp3/mbt/vpMbEdgeKltMultiTracker.h>
59 m_thresholdMBT(2.), m_mapOfCameraTransformationMatrix(), m_referenceCameraName(
"Camera"), m_nbrow(0),
60 m_L_hybridMulti(), m_error_hybridMulti(), m_w_hybridMulti(), m_weightedError_hybridMulti()
76 m_thresholdKLT(2.), m_thresholdMBT(2.), m_mapOfCameraTransformationMatrix(), m_referenceCameraName(
"Camera"),
77 m_nbrow(0), m_L_hybridMulti(), m_error_hybridMulti(), m_w_hybridMulti(), m_weightedError_hybridMulti()
82 }
else if (nbCameras == 1) {
85 }
else if (nbCameras == 2) {
91 m_referenceCameraName =
"Camera1";
93 for (
unsigned int i = 1; i <= nbCameras; i++) {
102 m_referenceCameraName = m_mapOfKltTrackers.begin()->first;
116 m_thresholdKLT(2.), m_thresholdMBT(2.), m_mapOfCameraTransformationMatrix(), m_referenceCameraName(cameraNames.front()),
117 m_nbrow(0), m_L_hybridMulti(), m_error_hybridMulti(), m_w_hybridMulti(), m_weightedError_hybridMulti()
141 std::cerr <<
"There are less than 4 KLT features, set factorKLT = 1. !" << std::endl;
146 std::cerr <<
"There are less than 4 moving edges, set factorMBT = 1. !" << std::endl;
161 std::map<std::string, vpVelocityTwistMatrix> mapOfVelocityTwist;
166 mapOfVelocityTwist[it->first] = cVo;
171 double residu_1 = -1;
172 unsigned int iter = 0;
182 while (((
int)((residu - residu_1) * 1e8) != 0) && (iter <
m_maxIter)) {
185 bool reStartFromLastIncrement =
false;
188 if (reStartFromLastIncrement) {
192 if (!reStartFromLastIncrement) {
224 for (
unsigned int j = 0; j < 6; j++) {
230 residu = sqrt(num / den);
258 for (std::map<std::string, vpMbKltTracker *>::const_iterator it =
m_mapOfKltTrackers.begin();
268 "computeVVSInteractionMatrixAndR"
269 "esidu() should not be called!");
274 std::map<std::string, vpVelocityTwistMatrix> &mapOfVelocityTwist)
276 unsigned int startIdx = 0;
279 for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it =
m_mapOfEdgeTrackers.begin();
297 for (std::map<std::string, vpMbKltTracker *>::const_iterator it =
m_mapOfKltTrackers.begin();
311 klt->
ctTc0 = c_curr_tTc_curr0;
326 unsigned int startIdx = 0;
328 for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it =
m_mapOfEdgeTrackers.begin();
339 for (std::map<std::string, vpMbKltTracker *>::const_iterator it =
m_mapOfKltTrackers.begin();
364 bool displayFullModel)
369 for (std::map<std::string, vpMbKltTracker *>::const_iterator it_klt =
m_mapOfKltTrackers.begin();
372 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it_pts = it_klt->second->kltPolygons.begin();
373 it_pts != it_klt->second->kltPolygons.end(); ++it_pts) {
380 for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it_cyl = it_klt->second->kltCylinders.begin();
381 it_cyl != it_klt->second->kltCylinders.end(); ++it_cyl) {
402 unsigned int thickness,
bool displayFullModel)
409 for (std::map<std::string, vpMbKltTracker *>::const_iterator it_klt =
m_mapOfKltTrackers.begin();
412 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it_pts = it_klt->second->kltPolygons.begin();
413 it_pts != it_klt->second->kltPolygons.end(); ++it_pts) {
420 for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it_cyl = it_klt->second->kltCylinders.begin();
421 it_cyl != it_klt->second->kltCylinders.end(); ++it_cyl) {
446 const vpColor &color,
unsigned int thickness,
bool displayFullModel)
454 for (std::map<std::string, vpMbKltTracker *>::const_iterator it_klt =
m_mapOfKltTrackers.begin();
456 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it_pts = it_klt->second->kltPolygons.begin();
457 it_pts != it_klt->second->kltPolygons.end(); ++it_pts) {
468 for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it_cyl = it_klt->second->kltCylinders.begin();
469 it_cyl != it_klt->second->kltCylinders.end(); ++it_cyl) {
501 const vpColor &color,
unsigned int thickness,
bool displayFullModel)
509 for (std::map<std::string, vpMbKltTracker *>::const_iterator it_klt =
m_mapOfKltTrackers.begin();
511 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it_pts = it_klt->second->kltPolygons.begin();
512 it_pts != it_klt->second->kltPolygons.end(); ++it_pts) {
523 for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it_cyl = it_klt->second->kltCylinders.begin();
524 it_cyl != it_klt->second->kltCylinders.end(); ++it_cyl) {
551 const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
552 const std::map<std::string, vpCameraParameters> &mapOfCameraParameters,
553 const vpColor &col,
unsigned int thickness,
bool displayFullModel)
560 for (std::map<std::string, vpMbKltTracker *>::const_iterator it_klt =
m_mapOfKltTrackers.begin();
563 std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img = mapOfImages.find(it_klt->first);
564 if (it_img != mapOfImages.end()) {
565 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it_pts = it_klt->second->kltPolygons.begin();
566 it_pts != it_klt->second->kltPolygons.end(); ++it_pts) {
573 for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it_cyl = it_klt->second->kltCylinders.begin();
574 it_cyl != it_klt->second->kltCylinders.end(); ++it_cyl) {
595 const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
596 const std::map<std::string, vpCameraParameters> &mapOfCameraParameters,
597 const vpColor &col,
unsigned int thickness,
bool displayFullModel)
604 for (std::map<std::string, vpMbKltTracker *>::const_iterator it_klt =
m_mapOfKltTrackers.begin();
607 std::map<std::string, const vpImage<vpRGBa> *>::const_iterator it_img = mapOfImages.find(it_klt->first);
608 if (it_img != mapOfImages.end()) {
609 for (std::list<vpMbtDistanceKltPoints *>::const_iterator it_pts = it_klt->second->kltPolygons.begin();
610 it_pts != it_klt->second->kltPolygons.end(); ++it_pts) {
617 for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it_cyl = it_klt->second->kltCylinders.begin();
618 it_cyl != it_klt->second->kltCylinders.end(); ++it_cyl) {
674 std::map<std::string, vpCameraParameters> &mapOfCameraParameters)
const
677 mapOfCameraParameters.clear();
692 std::map<std::string, vpMbKltTracker *>::const_iterator it =
m_mapOfKltTrackers.find(cameraName);
696 return it->second->getClipping();
698 std::cerr <<
"Cannot find camera: " << cameraName << std::endl;
706 std::cerr <<
"Return the wrong faces reference !" << std::endl;
707 std::cerr <<
"Use vpMbEdgeKltMultiTracker::getEdgeFaces or "
708 "vpMbEdgeKltMultiTracker::getKltFaces instead !"
758 std::cerr <<
"Use vpMbEdgeKltMultiTracker::getEdgeMultiNbPolygon or "
759 "vpMbEdgeKltMultiTracker::getKltMultiNbPolygon instead !"
820 mapOfCameraPoses.clear();
833 #ifdef VISP_HAVE_MODULE_GUI
845 const std::string &displayFile)
935 const std::string &initFile1,
const std::string &initFile2,
936 bool displayHelp,
bool firstCameraIsReference)
942 std::map<std::string, vpMbKltTracker *>::const_iterator it_klt =
m_mapOfKltTrackers.begin();
943 if (firstCameraIsReference) {
945 it_klt->second->getPose(c2Mo);
947 it_klt->second->getPose(c2Mo);
984 const std::string &initFile,
bool displayHelp)
989 std::map<std::string, vpHomogeneousMatrix> mapOfCameraPoses;
1026 const std::map<std::string, std::string> &mapOfInitFiles,
1032 std::map<std::string, vpHomogeneousMatrix> mapOfCameraPoses;
1039 #endif //#ifdef VISP_HAVE_MODULE_GUI
1042 const std::string &)
1044 std::cerr <<
"The method initCircle is not used in vpMbEdgeKltMultiTracker !" << std::endl;
1048 const std::string &)
1050 std::cerr <<
"The method initCylinder is not used in vpMbEdgeKltMultiTracker !" << std::endl;
1055 std::cerr <<
"The method initFaceFromCorners is not used in "
1056 "vpMbEdgeKltMultiTracker !"
1062 std::cerr <<
"The method initFaceFromLines is not used in "
1063 "vpMbEdgeKltMultiTracker !"
1092 char s[FILENAME_MAX];
1096 std::string ext =
".pos";
1097 size_t pos = initFile.rfind(ext);
1099 if (pos == initFile.size() - ext.size() && pos != 0)
1100 sprintf(s,
"%s", initFile.c_str());
1102 sprintf(s,
"%s.pos", initFile.c_str());
1104 finit.open(s, std::ios::in);
1106 std::cerr <<
"cannot read " << s << std::endl;
1110 for (
unsigned int i = 0; i < 6; i += 1) {
1111 finit >> init_pos[i];
1161 bool firstCameraIsReference)
1188 const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses)
1199 unsigned int nbrows = 0;
1202 for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it =
m_mapOfEdgeTrackers.begin();
1305 bool firstCameraIsReference)
1367 for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it =
m_mapOfEdgeTrackers.begin();
1382 bool newvisibleface =
false;
1383 for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it =
m_mapOfEdgeTrackers.begin();
1386 edge->
visibleFace(*mapOfImages[it->first], it->second->m_cMo, newvisibleface);
1390 for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it =
m_mapOfEdgeTrackers.begin();
1396 mapOfImages[it->first]->getHeight());
1400 for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it =
m_mapOfEdgeTrackers.begin();
1406 for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it =
m_mapOfEdgeTrackers.begin();
1409 edge->
initMovingEdge(*mapOfImages[it->first], it->second->m_cMo);
1462 bool firstCameraIsReference)
1479 const std::string &cad_name,
1480 const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
1549 bool firstCameraIsReference)
1554 if (firstCameraIsReference) {
1583 const std::map<std::string, vpCameraParameters> &mapOfCameraParameters)
1588 for (std::map<std::string, vpCameraParameters>::const_iterator it = mapOfCameraParameters.begin();
1589 it != mapOfCameraParameters.end(); ++it) {
1612 it->second = cameraTransformationMatrix;
1614 std::cerr <<
"Cannot find camera: " << cameraName <<
" !" << std::endl;
1626 const std::map<std::string, vpHomogeneousMatrix> &mapOfTransformationMatrix)
1717 #ifdef VISP_HAVE_OGRE
1810 const std::string &name)
1840 const std::string &name)
1896 for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it =
m_mapOfEdgeTrackers.begin();
1898 it->second->setOgreVisibilityTest(v);
1901 #ifdef VISP_HAVE_OGRE
1902 for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it =
m_mapOfEdgeTrackers.begin();
1904 it->second->faces.getOgreContext()->setWindowName(
"Multi Edge MBT Hybrid (" + it->first +
")");
1909 for (std::map<std::string, vpMbKltTracker *>::const_iterator it =
m_mapOfKltTrackers.begin();
1911 it->second->setOgreVisibilityTest(v);
1914 #ifdef VISP_HAVE_OGRE
1915 for (std::map<std::string, vpMbKltTracker *>::const_iterator it =
m_mapOfKltTrackers.begin();
1917 it->second->faces.getOgreContext()->setWindowName(
"Multi KLT MBT Hybrid (" + it->first +
")");
1945 std::cerr <<
"This method requires only 1 camera !" << std::endl;
1950 it_edge->second->setPose(I, cMo);
1951 it_klt->second->setPose(I, cMo);
1972 std::cerr <<
"This method requires only 1 camera !" << std::endl;
1978 it_edge->second->setPose(
m_I, cMo);
1979 it_klt->second->setPose(
m_I, cMo);
2003 bool firstCameraIsReference)
2035 const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses)
2053 for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it =
m_mapOfEdgeTrackers.begin();
2055 it->second->setProjectionErrorComputation(flag);
2094 std::cerr <<
"The method vpMbEdgeKltMultiTracker::testTracking is not used !" << std::endl;
2112 std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
2116 std::stringstream ss;
2132 std::cout <<
"Not supported interface, this class is deprecated." << std::endl;
2146 std::map<std::string, vpMbKltTracker *>::const_iterator it =
m_mapOfKltTrackers.begin();
2147 std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
2149 mapOfImages[it->first] = &I1;
2152 mapOfImages[it->first] = &I2;
2155 std::stringstream ss;
2156 ss <<
"Require two cameras ! There are " <<
m_mapOfKltTrackers.size() <<
" cameras !";
2175 for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge =
m_mapOfEdgeTrackers.begin();
2177 std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img = mapOfImages.find(it_edge->first);
2179 if (it_img == mapOfImages.end()) {
2185 for (std::map<std::string, vpMbKltTracker *>::const_iterator it_klt =
m_mapOfKltTrackers.begin();
2187 std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img = mapOfImages.find(it_klt->first);
2189 if (it_img == mapOfImages.end()) {
2213 for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it =
m_mapOfEdgeTrackers.begin();
2220 std::cerr <<
"Error in moving edge tracking" << std::endl;
2226 #elif !defined(VISP_BUILD_SHARED_LIBS)
2229 void dummy_vpMbEdgeKltMultiTracker(){}
2230 #endif // VISP_HAVE_OPENCV
2231 #elif !defined(VISP_BUILD_SHARED_LIBS)
2234 void dummy_vpMbEdgeKltMultiTracker(){}
2235 #endif //#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)