$search
00001 00010 #ifndef _RECOGNIZER_3D_ 00011 #define _RECOGNIZER_3D_ 00012 00013 #include <blort/Recognizer3D/DetectGPUSIFT.hh> 00014 #include <blort/Recognizer3D/ODetect3D.hh> 00015 #include <blort/Recognizer3D/Object3D.hh> 00016 #include <blort/Recognizer3D/ModelObject3D.hh> 00017 #include <blort/Recognizer3D/KeypointDescriptor.hh> 00018 #include <blort/Recognizer3D/PoseCv.hh> 00019 #include <blort/TomGine/tgModel.h> 00020 #include <blort/TomGine/tgPose.h> 00021 #include <string> 00022 #include <opencv2/core/core.hpp> 00023 #include <sensor_msgs/CameraInfo.h> 00024 00025 namespace blortRecognizer{ 00026 00030 struct CameraParameter { 00031 int w,h; 00032 float fx, fy; 00033 float cx, cy; 00034 float k1, k2, k3; 00035 float p1, p2; 00036 CameraParameter(){ w=h=fx=fy=cx=cy=k1=k2=k3=p1=p2=0; } 00037 CameraParameter(const sensor_msgs::CameraInfo& cam_info); 00038 }; 00039 00040 struct Siftex{ 00041 vec3 pos; 00042 vec3 normal; 00043 vec3 viewray; 00044 }; 00045 00047 class Recognizer3D{ 00048 00049 public: 00054 Recognizer3D(const CameraParameter& camParam, std::string config_root = "", 00055 bool display = false); 00056 ~Recognizer3D(); 00057 00058 void setCameraParameter(const blortRecognizer::CameraParameter& camParam); 00059 00060 void setGaussian(int kernel, float stdDeviation){ m_gauss_kernel = kernel; m_gauss_dev = stdDeviation; } 00061 00067 bool recognize(IplImage* tFrame, TomGine::tgPose& pose, float &conf); 00068 00073 bool learnSifts(IplImage* tFrame, const TomGine::tgModel& model, const TomGine::tgPose& pose); 00074 00078 bool loadModelFromFile(const std::string sift_file); 00079 00083 bool saveModelToFile(const char* sift_file); 00084 00088 void getSifts(std::vector<Siftex>& sl){ sl = m_siftexlist; } 00089 00093 void getLastSifts(std::vector<Siftex> &sl){ sl = m_lastsiftexlist; } 00094 00095 //BENCE 00096 unsigned int getCodeBookSize(); 00097 cv::Mat getImage(){ return display_image; } 00098 void setDoUndistort(bool enable){ do_undistort = enable; } 00099 00100 static void Convert(P::PoseCv& p1, TomGine::tgPose& p2); 00101 00102 void setNNThreshold(double nn_threshold); 00103 void setRansacNPointsToMatch(unsigned int n); 00104 cv::Mat getDebugImage(){ return debug_image; } 00105 00106 private: 00107 Recognizer3D(); 00108 00109 P::DetectGPUSIFT sift; 00110 P::ModelObject3D m_sift_model_learner; 00111 P::ODetect3D m_detect; 00112 P::Object3D m_sift_model; 00113 00114 P::Array<P::KeypointDescriptor*> m_image_keys; 00115 00116 std::vector<Siftex> m_lastsiftexlist; 00117 std::vector<Siftex> m_siftexlist; 00118 00119 CvMat *pIntrinsicDistort; 00120 CvMat *pDistortion; 00121 CvMat *C; 00122 CvMat *pMapX, *pMapY; 00123 CameraParameter m_cp; 00124 00125 bool m_model_loaded; 00126 bool m_display; 00127 00128 int m_gauss_kernel; 00129 float m_gauss_dev; 00130 00131 //BENCE 00132 cv::Mat display_image; 00133 cv::Mat debug_image; // usually empty 00134 bool do_undistort; 00135 std::string config_root; 00136 //cv::Ptr<pal_blort::CvDetect3D> cv_detect; 00137 }; 00138 00139 } 00140 00141 #endif /* _RECOGNIZER_3D_ */