00001
00009 #ifndef P_DOBJECT_3D_HH
00010 #define P_DOBJECT_3D_HH
00011
00012 #include <opencv/cv.h>
00013 #include <SiftGPU.h>
00014 #include <blort/Recognizer3D/KeyClusterPair.hh>
00015 #include <blort/Recognizer3D/Definitions.hh>
00016 #include <blort/Recognizer3D/Geometry.hh>
00017 #include <blort/Recognizer3D/Math.hh>
00018 #include <blort/Recognizer3D/Object3D.hh>
00019 #include <blort/Recognizer3D/PoseCv.hh>
00020 #include <blort/Recognizer3D/Vector3.hh>
00021
00022 namespace P
00023 {
00024
00025 class ODetect3D
00026 {
00027 public:
00028 class Point3D
00029 {
00030 public:
00031 float x;
00032 float y;
00033 float z;
00034 };
00035 class Point2D
00036 {
00037 public:
00038 float x;
00039 float y;
00040 };
00041
00042 private:
00043 IplImage *dbg;
00044
00045 CvMat *cameraMatrix;
00046 CvMat *distCoeffs;
00047
00048 SiftMatchGPU *matcher;
00049 int matcherSize;
00050
00051 Array<KeypointDescriptor*> inlier;
00052
00053 void DeletePairs(Array<KeyClusterPair*> &matches);
00054 void MatchKeypoints2(Array<KeypointDescriptor *> &keys, Array<CodebookEntry *> &cb,
00055 Array<KeyClusterPair*> &matches);
00056 void MatchKeypoints(Array<KeypointDescriptor *> &keys, Array<CodebookEntry *> &cb,
00057 Array<KeyClusterPair* > &matches);
00058 void MatchKeypointsGPU(Array<KeypointDescriptor *> &keys, Array<CodebookEntry *> &cb,
00059 Array<KeyClusterPair* > &matches);
00060 void FitModelRANSAC(Array<KeyClusterPair*> &matches, PoseCv &pose, unsigned &numInl);
00061 void GetRandIdx(unsigned size, unsigned num, P::Array<unsigned> &idx);
00062 void GetInlier(Array<KeyClusterPair*> &matches, PoseCv &pose, int &inl);
00063 bool GetBestCorner(PoseCv &pose, KeypointDescriptor *k, CodebookEntry *cbe,
00064 Point3D &pos, double &minDist);
00065 void RefinePoseLS(Array<KeyClusterPair*> &matches, PoseCv &pose, unsigned &inl, double &err);
00066 void ComputeConfidence(Array<KeypointDescriptor *> &keys, unsigned &numInl, Object3D &object);
00067
00068
00069 inline void ProjectPoint2Image(double xc, double yc, double zc,
00070 CvMat *C, double &xi, double &yi);
00071 inline void CopyPoseCv(PoseCv &in, PoseCv &out);
00072
00073
00074 Array<KeyClusterPair*> matches;
00075 double nn_far_enough_threshold;
00076 unsigned int n_points_to_match;
00077
00078 public:
00079 ODetect3D();
00080 ~ODetect3D();
00081
00082 bool Detect(Array<KeypointDescriptor *> &keys, Object3D &object);
00083
00084 void SetCameraParameter(CvMat *C);
00085
00086 void SetDebugImage(IplImage *img){dbg = img;}
00087
00088 void DrawInlier(IplImage *img, CvScalar col);
00089
00090
00091 void setNNThreshold(double nn_threshold)
00092 {
00093 nn_far_enough_threshold = nn_threshold;
00094 }
00095
00096 void setNPointsToMatch(unsigned int n)
00097 {
00098 n_points_to_match = n;
00099 }
00100 };
00101
00102
00107 inline void ODetect3D::ProjectPoint2Image(double xc, double yc, double zc,
00108 CvMat *C, double &xi, double &yi)
00109 {
00110 xi = C->data.fl[0] * xc/zc + C->data.fl[2];
00111 yi = C->data.fl[4] * yc/zc + C->data.fl[5];
00112 }
00113
00114 inline void ODetect3D::CopyPoseCv(PoseCv &in, PoseCv &out)
00115 {
00116 cvCopy(in.R, out.R);
00117 cvCopy(in.t, out.t);
00118 cvCopy(in.n, out.n);
00119 }
00120
00121
00122 }
00123
00124 #endif
00125
