utilities.hpp
Go to the documentation of this file.
00001 #pragma once
00002 
00003 #ifndef __BLORT_UTILITIES_HPP__
00004 #define __BLORT_UTILITIES_HPP__
00005 
00006 #include <blort/GLWindow/GLWindow.h>
00007 #include <blort/Tracker/Tracker.h>
00008 #include <blort/TomGine/tgCamera.h>
00009 #include <blort/TomGine/tgMathlib.h>
00010 #include <blort/Recognizer3D/Recognizer3D.h>
00011 #include <blort/Tracker/CDataFile.h>
00012 #include <stdexcept>
00013 #include <string>
00014 #include <blort/blort/pal_util.h>
00015 
00016 // Converts pose from Camera coordinates to world space
00017 void ConvertCam2World(const TomGine::tgPose& pC, const TomGine::tgPose& camPose, TomGine::tgPose& pW){
00018 
00019     pW = camPose.Transpose() * pC;
00020 }
00021 
00022 void ConvertCv2tgPoseWorld(     const cv::Mat_<double> &R, const cv::Mat_<double> &T, 
00023                                 const TomGine::tgPose &camPose, TomGine::tgPose &pose)
00024 {
00025     mat3 r;
00026     vec3 t, rvec;
00027     TomGine::tgPose pose_cam_coord, camPoseT;
00028     camPoseT = camPose.Transpose();
00029 
00030     t.x = T(0,0);
00031     t.y = T(1,0);
00032     t.z = T(2,0);
00033     rvec.x = R(0,0);
00034     rvec.y = R(1,0);
00035     rvec.z = R(2,0);
00036     r.fromRotVector(rvec);
00037     r = r.transpose();
00038 
00039     pose_cam_coord.SetPose(r,t);
00040 
00041     pose = camPoseT * pose_cam_coord;
00042 }
00043 
00044 void GetRecognizerParameter(blortRecognizer::CameraParameter& params, 
00045                             const TomGine::tgCamera::Parameter& camPar )
00046 {
00047     params.w = camPar.width;
00048     params.h = camPar.height;
00049     params.fx = camPar.fx;
00050     params.fy = camPar.fy;
00051     params.cx = camPar.cx;
00052     params.cy = camPar.cy;
00053     params.k1 = camPar.k1;
00054     params.k2 = camPar.k2;
00055     params.k3 = camPar.k3;
00056     params.p1 = camPar.p1;
00057     params.p2 = camPar.p2;
00058 }
00059 
00060 void GetRecognizerParameter(    blortRecognizer::CameraParameter& params,
00061                                 const char* cam_cal_file)
00062 {
00063     CDataFile camCDF, poseCDF, iniCDF;
00064 
00065     // Load calibration and ini files
00066     if(!camCDF.Load(cam_cal_file)){
00067         char errmsg[128];
00068         sprintf(errmsg, "[utilities::GetTrackingParameter] Can not open cam_cal file '%s'", cam_cal_file);
00069         throw std::runtime_error(errmsg);
00070     }
00071 
00072     params.w = camCDF.GetInt("w");
00073     params.h = camCDF.GetInt("h");
00074     params.fx = camCDF.GetFloat("fx");
00075     params.fy = camCDF.GetFloat("fy");
00076     params.cx = camCDF.GetFloat("cx");
00077     params.cy = camCDF.GetFloat("cy");
00078     params.k1 = camCDF.GetFloat("k1");
00079     params.k2 = camCDF.GetFloat("k2");
00080     params.k3 = 0.0f;
00081     params.p1 = camCDF.GetFloat("p1");
00082     params.p2 = camCDF.GetFloat("p2");
00083 }
00084 
00085 void getCamPoseCvCheckerboard(const cv::Mat_<double> &R, const cv::Mat_<double> &T, TomGine::tgPose &camPose)
00086 {
00087     vec3 r, t;
00088     mat3 Rm;
00089     cv::Mat_<double> cv_Rm = cv::Mat_<double> ( 3,3 );
00090     cv::Mat_<double> cv_rvec = cv::Mat_<double> ( 3,1 );
00091     cv::Mat_<double> cv_tvec = cv::Mat_<double> ( 3,1 );
00092     cv::Rodrigues(R, cv_Rm);
00093     cv::transpose(cv_Rm, cv_Rm);
00094     cv_tvec = -cv_Rm * T;                               // t = -R^T * t + t
00095     cv::Rodrigues(cv_Rm, cv_rvec);
00096 
00097     r.x = cv_rvec(0,0);
00098     r.y = cv_rvec(1,0);
00099     r.z = cv_rvec(2,0);
00100     t.x = cv_tvec(0,0);
00101     t.y = cv_tvec(1,0);
00102     t.z = cv_tvec(2,0);
00103     Rm.fromRotVector(r);
00104 
00105     camPose.SetPose(Rm,t);
00106 }
00107 
00108 void getCamPose(const char* pose_cal_file, TomGine::tgPose& camPose){
00109     CDataFile cdfParams;
00110     if(!cdfParams.Load(pose_cal_file)){
00111         char errmsg[128];
00112         sprintf(errmsg, "[utilities::getCamPose] Can not open pose_cal file '%s'", pose_cal_file);
00113         throw std::runtime_error(errmsg);
00114     }
00115 
00116     vec3 t, r;
00117     mat3 R;
00118 
00119     std::string pose = cdfParams.GetString("pose");
00120     sscanf( pose.c_str(), "[%f %f %f] [%f %f %f]",
00121             &(t.x), &(t.y), &(t.z), &(r.x), &(r.y), &(r.z) );
00122 
00123     R.fromRotVector(r);
00124 
00125     camPose.SetPose(R,t);
00126 }
00127 
00128 void GetCameraParameterCV(  TomGine::tgCamera::Parameter& camPar, 
00129                             CvMat *pImgSize,
00130                             CvMat *pIntrinsicDistort,
00131                             CvMat *pDistortions,
00132                             const cv::Mat &R, const cv::Mat &T)
00133 {
00134 
00135     if(pImgSize){
00136         camPar.width = cvmGet(pImgSize, 0,0);
00137         camPar.height = cvmGet(pImgSize, 1,0);
00138     }else{
00139         throw std::runtime_error("[utilities::GetCameraParameterCV] Wrong format for CvMat pImgSize");
00140     }
00141 
00142     if(pIntrinsicDistort){
00143         camPar.fx = cvmGet(pIntrinsicDistort, 0,0);
00144         camPar.fy = cvmGet(pIntrinsicDistort, 1,1);
00145         camPar.cx = cvmGet(pIntrinsicDistort, 0,2);
00146         camPar.cy = cvmGet(pIntrinsicDistort, 1,2);
00147     }else{
00148         throw std::runtime_error("[utilities::GetCameraParameterCV] Wrong format for CvMat pIntrinsicDistort");
00149     }
00150 
00151     if(pDistortions){
00152         camPar.k1 = cvmGet(pDistortions, 0,0);
00153         camPar.k2 = cvmGet(pDistortions, 1,0);
00154         camPar.k3 = cvmGet(pDistortions, 2,0);
00155         //camPar.k4 = cvmGet(pIntrinsicDistort, 3,0);
00156         camPar.p1 = 0.0f;
00157         camPar.p2 = 0.0f;
00158     }else{
00159         throw std::runtime_error("[utilities::GetCameraParameterCV] Wrong format for CvMat pDistortions");
00160     }
00161 
00162     camPar.zFar = 5.0f;
00163     camPar.zNear = 0.1f;
00164 
00165     // Invert pose of calibration pattern to get pose of camera
00166     vec3 r;
00167     cv::Mat Rm = cv::Mat( 3,3, CV_64F );
00168     cv::Mat rvec = cv::Mat( 3,1, CV_64F );
00169     cv::Mat tvec = cv::Mat( 3,1, CV_64F );
00170 
00171     cv::Rodrigues(R, Rm);
00172 
00173     cv::transpose(Rm, Rm);
00174 
00175     tvec = -Rm * T;                             // t = -R^T * t
00176 
00177     cv::Rodrigues(Rm, rvec);
00178 
00179     r.x = (float)rvec.at<double>(0,0);
00180     r.y = (float)rvec.at<double>(1,0);
00181     r.z = (float)rvec.at<double>(2,0);
00182     camPar.pos.x = tvec.at<double>(0,0);
00183     camPar.pos.y = tvec.at<double>(1,0);
00184     camPar.pos.z = tvec.at<double>(2,0);
00185 
00186     camPar.rot.fromRotVector(r);
00187 }
00188 
00189 void setCameraPose(TomGine::tgCamera::Parameter& camPar, const char* pose_cal_file)
00190 {
00191     CDataFile poseCDF;
00192     if(!poseCDF.Load(pose_cal_file)){
00193         char errmsg[128];
00194         sprintf(errmsg, "[utilities::GetTrackingParameter] Can not open pose_cal file '%s'", pose_cal_file);
00195         throw std::runtime_error(errmsg);
00196     }
00197 
00198     // Pose
00199     vec3 p, r;
00200     std::string pose = poseCDF.GetString("pose");
00201     sscanf( pose.c_str(), "[%f %f %f] [%f %f %f]", &(p.x), &(p.y), &(p.z), &(r.x), &(r.y), &(r.z) );
00202     //printf("%s\n", pose.c_str());
00203     //printf("%f %f %f, %f %f %f\n", p.x, p.y, p.z, r.x, r.y, r.z);
00204     camPar.pos.x = p.x;
00205     camPar.pos.y = p.y;
00206     camPar.pos.z = p.z;
00207     camPar.rot.fromRotVector(r);
00208 }
00209 
00210 void GetCameraParameter( TomGine::tgCamera::Parameter& camPar, const char* cam_cal_file, const char* pose_cal_file)
00211 {
00212     CDataFile camCDF, poseCDF;
00213 
00214     // Load calibration and ini files
00215     if(!camCDF.Load(cam_cal_file)){
00216         char errmsg[128];
00217         sprintf(errmsg, "[utilities::GetTrackingParameter] Can not open cam_cal file '%s'", cam_cal_file);
00218         throw std::runtime_error(errmsg);
00219     }
00220 
00221     if(!poseCDF.Load(pose_cal_file)){
00222         char errmsg[128];
00223         sprintf(errmsg, "[utilities::GetTrackingParameter] Can not open pose_cal file '%s'", pose_cal_file);
00224         throw std::runtime_error(errmsg);
00225     }
00226 
00227     // Camera Parameters
00228     camPar.width = camCDF.GetInt("w");
00229     camPar.height = camCDF.GetInt("h");
00230     camPar.fx = camCDF.GetFloat("fx");
00231     camPar.fy = camCDF.GetFloat("fy");
00232     camPar.cx = camCDF.GetFloat("cx");
00233     camPar.cy = camCDF.GetFloat("cy");
00234     camPar.k1 = camCDF.GetFloat("k1");
00235     camPar.k2 = camCDF.GetFloat("k2");
00236     camPar.k3 = 0.0f;
00237     camPar.p1 = camCDF.GetFloat("p1");
00238     camPar.p2 = camCDF.GetFloat("p2");
00239     camPar.zFar = 5.0f;
00240     camPar.zNear = 0.1f;
00241 
00242     // Pose
00243     vec3 p, r;
00244     std::string pose = poseCDF.GetString("pose");
00245     sscanf( pose.c_str(), "[%f %f %f] [%f %f %f]", &(p.x), &(p.y), &(p.z), &(r.x), &(r.y), &(r.z) );
00246     //printf("%s\n", pose.c_str());
00247     //printf("%f %f %f, %f %f %f\n", p.x, p.y, p.z, r.x, r.y, r.z);
00248     camPar.pos.x = p.x;
00249     camPar.pos.y = p.y;
00250     camPar.pos.z = p.z;
00251     camPar.rot.fromRotVector(r);
00252 }
00253 
00254 void GetTrackingParameter( Tracking::Tracker::Parameter& params, const char* ini_file, std::string config_root = ".")
00255 {
00256     CDataFile iniCDF;
00257 
00258     if(!iniCDF.Load(ini_file)){
00259         char errmsg[128];
00260         sprintf(errmsg, "[utilities::GetTrackingParameter] Can not open tracking_ini file '%s'", ini_file);
00261         throw std::runtime_error(errmsg);
00262     }
00263 
00264     // Tracking
00265     // Constraints
00266     params.variation.r.x = iniCDF.GetFloat("r.x", "Constraints") * Tracking::pi/180.0f;
00267     params.variation.r.y = iniCDF.GetFloat("r.y", "Constraints") * Tracking::pi/180.0f;
00268     params.variation.r.z = iniCDF.GetFloat("r.z", "Constraints") * Tracking::pi/180.0f;
00269     params.variation.t.x        = iniCDF.GetFloat("t.x", "Constraints");
00270     params.variation.t.y        = iniCDF.GetFloat("t.y", "Constraints");
00271     params.variation.t.z        = iniCDF.GetFloat("t.z", "Constraints");
00272     params.variation.z          = iniCDF.GetFloat("z", "Constraints");
00273 
00274     // Performance
00275     params.num_recursions = iniCDF.GetInt("recursions", "Performance");
00276     params.num_particles = iniCDF.GetInt("particles", "Performance");
00277     params.hypotheses_trials = iniCDF.GetInt("hypotheses", "Performance");
00278     params.convergence = iniCDF.GetInt("convergence", "Performance");
00279 
00280     // Resource Path
00281     params.modelPath = pal_blort::addRoot(iniCDF.GetString("ModelPath", "ResourcePath"), config_root);
00282     params.texturePath = pal_blort::addRoot(iniCDF.GetString("TexturePath", "ResourcePath"), config_root);
00283     params.shaderPath = pal_blort::addRoot(iniCDF.GetString("ShaderPath", "ResourcePath"), config_root);
00284 
00285     // Other
00286     params.edge_tolerance = iniCDF.GetFloat("EdgeMatchingTolerance", "Other") * Tracking::pi/180.0f;
00287     params.minTexGrabAngle = iniCDF.GetFloat("MinTextureGrabAngle", "Other") * Tracking::pi/180.0f;
00288     params.num_spreadings =  iniCDF.GetInt("NumberOfSpreadings", "Other");
00289     params.max_kernel_size = iniCDF.GetInt("MaxKernelSize", "Other");
00290 
00291     params.model_sobel_th = iniCDF.GetFloat("ModelSobelThreshold", "Other");
00292     params.image_sobel_th = iniCDF.GetFloat("ImageSobelThreshold", "Other");
00293     params.pred_no_convergence = iniCDF.GetFloat("PredictorNoConvergence", "Other");
00294 
00295     params.c_th_base = iniCDF.GetFloat("BaseThreshold", "Qualitative");
00296     params.c_th_min = iniCDF.GetFloat("MinThreshold", "Qualitative");
00297     params.c_th_fair = iniCDF.GetFloat("FairThreshold", "Qualitative");
00298     params.c_th_lost = iniCDF.GetFloat("LostThreshold", "Qualitative");
00299 
00300     params.c_mv_not = iniCDF.GetFloat("NoMovementThreshold", "Movement");
00301     params.c_mv_slow = iniCDF.GetFloat("SlowMovementThreshold", "Movement");
00302 }
00303 
00304 void GetPlySiftFilenames(const char* ini_file, std::string &ply_file, std::string &sift_file, std::string &model_name)
00305 {
00306     CDataFile iniCDF;
00307 
00308     if(!iniCDF.Load(ini_file)){
00309         char errmsg[128];
00310         sprintf(errmsg, "[utilities::GetTrackingParameter] Can not open tracking_ini file '%s'", ini_file);
00311         throw std::runtime_error(errmsg);
00312     }
00313 
00314     ply_file = iniCDF.GetString("ModelPath", "ResourcePath");
00315     ply_file.append(iniCDF.GetString("Model", "Files"));
00316     sift_file = iniCDF.GetString("SiftPath", "ResourcePath");
00317     sift_file.append(iniCDF.GetString("SiftModel", "Files"));
00318 
00319     model_name = iniCDF.GetString("Model", "Files");
00320     model_name = model_name.substr(0, model_name.find("."));
00321 
00322 }
00323 
00324 bool InputControl(Tracking::Tracker* tracker, blortGLWindow::Event& event, std::string config_root =""){
00325 
00326     switch (event.type)
00327     {
00328     case blortGLWindow::TMGL_Press:
00329         switch (event.input)
00330         {
00331         case blortGLWindow::TMGL_Escape:
00332         case blortGLWindow::TMGL_q:
00333             return false;
00334             break;
00335         case blortGLWindow::TMGL_1: //1
00336             tracker->setKernelSize(0);
00337             printf("Kernel size: %d\n", (int)0);
00338             break;
00339         case blortGLWindow::TMGL_2: //2
00340             tracker->setKernelSize(1);
00341             printf("Kernel size: %d\n", (int)1);
00342             break;
00343         case blortGLWindow::TMGL_3: //3
00344             tracker->setKernelSize(2);
00345             printf("Kernel size: %d\n", (int)2);
00346             break;
00347         case blortGLWindow::TMGL_4: //4
00348             tracker->setEdgeShader();
00349             break;
00350         case blortGLWindow::TMGL_5: //5
00351             tracker->setColorShader();
00352             break;
00353         case blortGLWindow::TMGL_e: //e
00354             tracker->setEdgesImageFlag( !tracker->getEdgesImageFlag() );
00355             break;
00356         case blortGLWindow::TMGL_i: //i
00357             tracker->printStatistics();
00358             break;
00359         case blortGLWindow::TMGL_l: //l
00360             tracker->setLockFlag( !tracker->getLockFlag() );
00361             break;
00362         case blortGLWindow::TMGL_m: //m
00363             tracker->setModelModeFlag( tracker->getModelModeFlag()+1 );
00364             break;
00365         case blortGLWindow::TMGL_p: //p
00366             tracker->setDrawParticlesFlag( !tracker->getDrawParticlesFlag() );
00367             break;
00368         case blortGLWindow::TMGL_r: //r
00369             tracker->reset();
00370             break;
00371         case blortGLWindow::TMGL_s: //s
00372             tracker->saveModels(pal_blort::addRoot("Resources/ply/", config_root).c_str());
00373             break;
00374         case blortGLWindow::TMGL_t: //t
00375             tracker->textureFromImage(true);
00376             break;
00377         case blortGLWindow::TMGL_u: //u
00378             tracker->untextureModels();
00379             break;
00380         default:
00381             break;
00382         }
00383         break;
00384         case blortGLWindow::TMGL_None:
00385         break;
00386         case blortGLWindow::TMGL_Release:
00387         break;
00388         case blortGLWindow::TMGL_Motion:
00389         break;
00390         case blortGLWindow::TMGL_Expose:
00391         break;
00392         case blortGLWindow::TMGL_Quit:
00393         break;
00394     }
00395     return true;
00396 }
00397 
00398 CvMat* load_xml(const char* filename){
00399     char errmsg[128];
00400     CvMat *mat = (CvMat*)cvLoad(filename);
00401     if(mat==NULL){
00402         sprintf(errmsg, "[utilities::load_xml] cvLoad cannot open xml file '%s'\n", filename);
00403         throw std::runtime_error(errmsg);
00404     }
00405     return mat;
00406 }
00407 
00408 
00409 
00410 #endif // __BLORT_UTILITIES_HPP__


blort
Author(s): Michael Zillich, Thomas Mörwald, Johann Prankl, Andreas Richtsfeld, Bence Magyar (ROS version)
autogenerated on Thu Jan 2 2014 11:38:26