00001 #ifndef Impala_Core_Tracking_KalmanTemplate_h
00002 #define Impala_Core_Tracking_KalmanTemplate_h
00003
00004 #include "Core/Array/Pattern/PatGenConv2d.h"
00005 #include "Core/Array/Pattern/PatBinaryPixOp.h"
00006
00007 #include "Core/Tracking/ObjectRepresentation.h"
00008 #include "Core/Tracking/SearcherAroundLastPos.h"
00009 #include "Core/Tracking/FunctorKalman.h"
00010
00011 namespace Impala
00012 {
00013 namespace Core
00014 {
00015 namespace Tracking
00016 {
00017
00020 class KalmanTemplate : public ObjectRepresentation
00021 {
00022 public:
00023 KalmanTemplate()
00024 {
00025 mTemplate = new Array::Array2dVec3Real64(0, 0, 0, 0);
00026 mScratch = new Array::Array2dVec3UInt8(0, 0, 0, 0);
00027 mTemplatePrediction = Array::ArrayClone<Array::Array2dVec3Real64,
00028 Array::Array2dVec3Real64>(mTemplate);
00029 }
00030
00031 virtual ~KalmanTemplate()
00032 {
00033 delete mScratch;
00034 delete mTemplate;
00035 delete mTemplatePrediction;
00036 }
00037
00041 void Update( Array::Array2dVec3Real64& image, const Position& pos)
00042 {
00043 Array::Array2dVec3Real64* temp =
00044 new Array::Array2dVec3Real64(pos.size.x, pos.size.y, 0, 0);
00045 Array::Pattern::PatSet(temp, &image, pos.translation.x, pos.translation.y,
00046 pos.size.x, pos.size.y, 0, 0);
00047 Array::Pattern::PatBinaryPixOp(mTemplatePrediction, mTemplate,
00048 temp, mKalman);
00049 delete temp;
00050 mKalman.UpdateKalman();
00051 if(!mKalman.mOcclusion)
00052 {
00053 Array::Pattern::PatSet(mTemplate, mTemplatePrediction, 0, 0,
00054 mTemplate->CW(), mTemplate->CH(), 0, 0);
00055 }
00056
00057 Array::Pattern::PatSet(mScratch,mTemplate,0,0,pos.size.x, pos.size.y,0,0);
00058 }
00059
00061 double Score( Array::Array2dVec3Real64& image, const Position& pos)
00062 {
00063 Array::Trait::FuncBpoRobustMahalanobis dif;
00064 for(int i=0 ; i<3 ; i++)
00065 dif.mSigma[i] = mKalman.mSigmaG[i] + mKalman.mSigmaI[i];
00066 Array::Pattern::PatBinaryPixOp(mTemplatePrediction, &image, mTemplate, dif);
00067 return dif.mError;
00068 }
00069
00074 void Reset( Array::Array2dVec3Real64& image, const Position& pos)
00075 {
00076 ILOG_DEBUG("Reset with size = (" << pos.size.x << "," << pos.size.y << ")");
00077 delete mTemplate;
00078 mTemplate = new Array::Array2dVec3Real64(pos.size.x, pos.size.y, 0, 0);
00079 Array::Pattern::PatSet(mTemplate, &image,
00080 pos.translation.x, pos.translation.y,
00081 pos.size.x, pos.size.y, 0, 0);
00082 delete mTemplatePrediction;
00083 mTemplatePrediction =
00084 new Array::Array2dVec3Real64(pos.size.x, pos.size.y, 0, 0);
00085 Array::Pattern::PatSet(mTemplatePrediction, &image,
00086 pos.translation.x, pos.translation.y,
00087 pos.size.x, pos.size.y, 0, 0);
00088 mKalman.Reset(pos.size.x, pos.size.y);
00089 delete mScratch;
00090 mScratch = new Array::Array2dVec3UInt8(pos.size.x, pos.size.y, 0, 0);
00091 Array::Pattern::PatSet(mScratch,mTemplate,0,0,pos.size.x, pos.size.y,0,0);
00092 }
00093
00095 Array::Array2dVec3Real64* GetImage()
00096 {
00097 ILOG_DEBUG("object image requested");
00098 return mTemplate;
00099 }
00100
00101 Array::Array2dVec3Real64* mTemplate;
00102 Array::Array2dVec3Real64* mTemplatePrediction;
00103 Array::Trait::FuncKalmanColor mKalman;
00104 Array::Array2dVec3UInt8* mScratch;
00105 ILOG_CLASS;
00106 };
00107
00108 ILOG_VAR_INIT(KalmanTemplate, Impala.Core.Tracking);
00109
00110 }
00111 }
00112 }
00113
00114 #endif //Impala_Core_Tracking_KalmanTemplate_h
00115