Menu
▾
▴
robotflow-cvs — CVS changes are sent to this mailing list.
You can subscribe to this list here.
| 2004 |
Jan
|
Feb
|
Mar
|
Apr
|
May
(59) |
Jun
(40) |
Jul
(59) |
Aug
(81) |
Sep
(14) |
Oct
(9) |
Nov
(22) |
Dec
(1) |
|---|---|---|---|---|---|---|---|---|---|---|---|---|
| 2005 |
Jan
(25) |
Feb
(3) |
Mar
(27) |
Apr
(14) |
May
(15) |
Jun
(112) |
Jul
(44) |
Aug
(7) |
Sep
(18) |
Oct
(34) |
Nov
(17) |
Dec
(20) |
| 2006 |
Jan
(12) |
Feb
|
Mar
(1) |
Apr
|
May
|
Jun
(3) |
Jul
(1) |
Aug
|
Sep
|
Oct
|
Nov
(1) |
Dec
(11) |
|
From: Pierre M. <sid...@us...> - 2005-06-02 16:43:26
|
Update of /cvsroot/robotflow/RobotFlow/Vision/include In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv7819 Modified Files: VisualFeatureDesc.h Log Message: Added validity flag. Index: VisualFeatureDesc.h =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Vision/include/VisualFeatureDesc.h,v retrieving revision 1.4 retrieving revision 1.5 diff -C2 -d -r1.4 -r1.5 *** VisualFeatureDesc.h 1 Jun 2005 03:34:01 -0000 1.4 --- VisualFeatureDesc.h 2 Jun 2005 16:43:17 -0000 1.5 *************** *** 27,32 **** { e_VISUALDESCRIPTOR_histogram = 0, ! e_VISUALDESCRIPTOR_statistics, ! e_VISUALDESCRIPTOR_point, e_VISUALDESCRIPTOR_unknown } e_VISUALDESCRIPTOR_type; --- 27,31 ---- { e_VISUALDESCRIPTOR_histogram = 0, ! e_VISUALDESCRIPTOR_integral, e_VISUALDESCRIPTOR_unknown } e_VISUALDESCRIPTOR_type; *************** *** 137,140 **** --- 136,144 ---- } + virtual bool GetValidity() const + { + throw new FD::GeneralException("Exception in VisualFeatureDesc::GetValidity: cannot use base class routine.",__FILE__,__LINE__); + } + e_VISUALDESCRIPTOR_type GetType() const { return m_descType; } *************** *** 154,157 **** --- 158,166 ---- } + virtual void SetValidity(bool i_flag) + { + throw new FD::GeneralException("Exception in VisualFeatureDesc::SetValidity: cannot use base class routine.",__FILE__,__LINE__); + } + private: e_VISUALDESCRIPTOR_type m_descType; |
|
From: Pierre M. <sid...@us...> - 2005-06-02 16:42:53
|
Update of /cvsroot/robotflow/RobotFlow/Vision/include In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv7576 Modified Files: VisualTarget.h Log Message: Fixed multi-cue similarity method. Index: VisualTarget.h =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Vision/include/VisualTarget.h,v retrieving revision 1.4 retrieving revision 1.5 diff -C2 -d -r1.4 -r1.5 *** VisualTarget.h 26 May 2005 19:37:29 -0000 1.4 --- VisualTarget.h 2 Jun 2005 16:42:43 -0000 1.5 *************** *** 200,204 **** } ! double sim = 1.0; for (int i=0; i<m_numDesc; i++) { --- 200,204 ---- } ! double sim = 0.0; for (int i=0; i<m_numDesc; i++) { *************** *** 207,211 **** } ! sim *= m_cueWeights[i]*(*m_targetDesc)[i]->Similarity((*i_desc)[i]->GetCstFeatures(), (*i_desc)[i]->GetSize()); } --- 207,211 ---- } ! sim += m_cueWeights[i]*(*m_targetDesc)[i]->Similarity((*i_desc)[i]->GetCstFeatures(), (*i_desc)[i]->GetSize()); } *************** *** 220,224 **** int i; ! double sim = 1.0, cueSim, sumCueSim = 0.0; for (i=0; i<m_numDesc; i++) { --- 220,224 ---- int i; ! double sim = 0.0, cueSim, sumCueSim = 0.0; for (i=0; i<m_numDesc; i++) { *************** *** 229,233 **** m_tmpCueProb[i] = (*m_targetDesc)[i]->Similarity((*i_desc)[i]->GetCstFeatures(), (*i_desc)[i]->GetSize()); sumCueSim += m_tmpCueProb[i]; ! sim *= m_cueWeights[i]*m_tmpCueProb[i]; } --- 229,239 ---- m_tmpCueProb[i] = (*m_targetDesc)[i]->Similarity((*i_desc)[i]->GetCstFeatures(), (*i_desc)[i]->GetSize()); sumCueSim += m_tmpCueProb[i]; ! sim += m_cueWeights[i]*m_tmpCueProb[i]; ! std::cout << "Cue Weights [" << i << "]=" << m_cueWeights[i] << " Current Cue Prob=" << m_tmpCueProb[i] << std::endl; ! } ! ! if (sumCueSim == 0.0) { ! std::cerr << "WARNING: VisualTarget::SimilarityWCueAdapt: cannot adapt cue weights with a total probability of 0.0." << std::endl; ! return sim; } *************** *** 237,246 **** sumCueSim = 0.0; - if (sumCueSimInv == 0.0) { - sumCueSimInv = 0.0; - } - for (i=0; i<m_numDesc; i++) { ! m_cueWeights[i] = i_rate*m_tmpCueProb[i]*sumCueSim + rateInv*m_cueWeights[i]; sumCueSim += m_cueWeights[i]; } --- 243,248 ---- sumCueSim = 0.0; for (i=0; i<m_numDesc; i++) { ! m_cueWeights[i] = i_rate*m_tmpCueProb[i]*sumCueSimInv + rateInv*m_cueWeights[i]; sumCueSim += m_cueWeights[i]; } *************** *** 305,314 **** VisualROI *GetROI() { ! return &(*m_refROI); } const VisualROI *GetCstROI() const { ! return (const VisualROI *)(&(*m_refROI)); } --- 307,316 ---- VisualROI *GetROI() { ! return m_refROI.get(); } const VisualROI *GetCstROI() const { ! return (const VisualROI *)(m_refROI.get()); } |
|
From: Pierre M. <sid...@us...> - 2005-06-02 16:42:08
|
Update of /cvsroot/robotflow/RobotFlow/Vision/include In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv7001 Modified Files: VisualHistogramDesc.h Log Message: Added validity flag. Index: VisualHistogramDesc.h =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Vision/include/VisualHistogramDesc.h,v retrieving revision 1.5 retrieving revision 1.6 diff -C2 -d -r1.5 -r1.6 *** VisualHistogramDesc.h 1 Jun 2005 03:34:48 -0000 1.5 --- VisualHistogramDesc.h 2 Jun 2005 16:41:59 -0000 1.6 *************** *** 16,21 **** */ ! #ifndef _VISUALHISTOGRAMFEATURE_H_ ! #define _VISUALHISTOGRAMFEATURE_H_ #include "Object.h" --- 16,21 ---- */ ! #ifndef _VISUALHISTOGRAMDESC_H_ ! #define _VISUALHISTOGRAMDESC_H_ #include "Object.h" *************** *** 32,35 **** --- 32,37 ---- e_VISUALHIST_BhattacharyyaCoeff = 0, e_VISUALHIST_BhattacharyyaDist, + e_VISUALHIST_HistIntersection, + e_VISUALHIST_EuclideanDist, // TODO: add other similarity methods e_VISUALHIST_unknownSimilarity *************** *** 55,58 **** --- 57,61 ---- : VisualFeatureDesc<BinType>(e_VISUALDESCRIPTOR_histogram), m_simType(e_VISUALHIST_unknownSimilarity), + m_valid(false), m_normFlag(false), m_numDimensions(0), *************** *** 68,71 **** --- 71,75 ---- : VisualFeatureDesc<BinType>(e_VISUALDESCRIPTOR_histogram), m_simType(i_simType), + m_valid(true), m_normFlag(i_normFlag), m_numDimensions(i_numDimensions), *************** *** 99,102 **** --- 103,107 ---- { this->SetType(i_ref.GetType()); + m_valid = i_ref.m_valid; m_simType = i_ref.m_simType; m_normFlag = i_ref.m_normFlag; *************** *** 143,146 **** --- 148,152 ---- this->SetType(i_ref.GetType()); m_simType = i_ref.m_simType; + m_valid = i_ref.m_valid; m_normFlag = i_ref.m_normFlag; *************** *** 195,198 **** --- 201,205 ---- out << "<TotalBins " << m_totalBins << " >" << std::endl; out << "<SimilarityType " << (int)m_simType << " >" << std::endl; + out << "<Validity " << (int)m_valid << " >" << std::endl; out << "<NormFlag " << (int)m_normFlag << " >" << std::endl; out << "<LastMaxVal " << m_lastMaxVal << " >" << std::endl; *************** *** 261,264 **** --- 268,276 ---- SetSimilarityFct(); } + else if (tag == "Validity") { + int val; + in >> val; + m_valid = bool(val); + } else if (tag == "NormFlag") { int val; *************** *** 320,323 **** --- 332,340 ---- } + if (!m_valid) { + // Invalid descriptor, output maximal distance + return 0.0; + } + // Use appropriate function return (this->*m_similarityFct)(i_candidate); *************** *** 754,757 **** --- 771,779 ---- } + bool GetValidity() const + { + return m_valid; + } + // WARNING: this function modifies only the histogram bins void SetSize(unsigned int i_size) *************** *** 797,800 **** --- 819,827 ---- } } + + void SetValidity(bool i_flag) + { + m_valid = i_flag; + } private: *************** *** 807,810 **** --- 834,848 ---- m_similarityFct = &VisualHistogramDesc::BhattacharyyaDist; } + else if (m_simType == e_VISUALHIST_HistIntersection) { + if (m_normFlag) { + m_similarityFct = &VisualHistogramDesc::HistogramIntersection; + } + else { + m_similarityFct = &VisualHistogramDesc::HistogramIntersectionWNorm; + } + } + else if (m_simType == e_VISUALHIST_EuclideanDist) { + m_similarityFct = &VisualHistogramDesc::EuclideanDist; + } else { m_similarityFct = NULL; *************** *** 849,852 **** --- 887,970 ---- } + double HistogramIntersection(const BinType *i_candidateBins) const + { + const BinType *p_curBins = (const BinType *)m_bins; + const BinType *p_candidateBins = i_candidateBins; + double sum = 0.0; + + for (int b=0; b<m_totalBins; b++) { + if (*p_curBins < *p_candidateBins) { + sum += *p_curBins; + } + else { + sum += *p_candidateBins; + } + + p_curBins++; + p_candidateBins++; + } + + return 1.0-sum; + } + + double HistogramIntersectionWNorm(const BinType *i_candidateBins) const + { + const BinType *p_curBins = (const BinType *)m_bins; + const BinType *p_candidateBins = i_candidateBins; + BinType curHistSum = (BinType)0; + BinType candHistSum = (BinType)0; + BinType curNormVal; + BinType candNormVal; + double sum = 0.0; + + for (int b=0; b<m_totalBins; b++) { + curHistSum += *p_curBins; + candHistSum += *p_candidateBins; + + p_curBins++; + p_candidateBins++; + } + + p_curBins = (const BinType *)m_bins; + p_candidateBins = i_candidateBins; + + for (int b=0; b<m_totalBins; b++) { + curNormVal = (*p_curBins)/curHistSum; + candNormVal = (*p_candidateBins)/candHistSum; + + if (curNormVal < candNormVal) { + sum += curNormVal; + } + else { + sum += candNormVal; + } + + p_curBins++; + p_candidateBins++; + } + + return 1.0-sum; + } + + double EuclideanDist(const BinType *i_candidateBins) const + { + int b; + BinType diff; + double dist = 0.0; + const BinType *p_curBins = (const BinType *)m_bins; + const BinType *p_candidateBins = i_candidateBins; + + for (int b=0; b<m_totalBins; b++) { + diff = (*p_curBins) - (*p_candidateBins); + dist += diff*diff; + p_curBins++; + p_candidateBins++; + } + + dist = sqrt(dist); + + return 1.0-dist; + } + void AdaptNorm(const BinType *i_candidateBins, double i_rate) { *************** *** 905,908 **** --- 1023,1029 ---- e_VISUALHIST_similarityType m_simType; + // Validity flag in cases where ROI was invalid for features extraction + bool m_valid; + // Flag to normalize histogram bins bool m_normFlag; |
|
From: Pierre M. <sid...@us...> - 2005-06-01 03:34:58
|
Update of /cvsroot/robotflow/RobotFlow/Vision/include In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv20981 Modified Files: VisualHistogramDesc.h Log Message: Fixed some std namespace errors Index: VisualHistogramDesc.h =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Vision/include/VisualHistogramDesc.h,v retrieving revision 1.4 retrieving revision 1.5 diff -C2 -d -r1.4 -r1.5 *** VisualHistogramDesc.h 26 May 2005 19:37:29 -0000 1.4 --- VisualHistogramDesc.h 1 Jun 2005 03:34:48 -0000 1.5 *************** *** 191,220 **** { int b, d; ! out << "<VisualHistogramDesc " << endl; ! out << "<NumDimensions " << m_numDimensions << " >" << endl; ! out << "<TotalBins " << m_totalBins << " >" << endl; ! out << "<SimilarityType " << (int)m_simType << " >" << endl; ! out << "<NormFlag " << (int)m_normFlag << " >" << endl; ! out << "<LastMaxVal " << m_lastMaxVal << " >" << endl; ! out << "<NumBins " << endl; for (d=0; d<m_numDimensions; d++) { out << m_numBins[d] << " "; } ! out << " >" << endl; ! out << "<BinsWidth " << endl; for (d=0; d<m_numDimensions; d++) { out << m_binsWidth[d] << " "; } ! out << " >" << endl; ! out << "<Bins " << endl; for (b=0; b<m_totalBins; ++b) { out << m_bins[b] << " "; } ! out << " >" << endl; ! out << " >" << endl; } --- 191,220 ---- { int b, d; ! out << "<VisualHistogramDesc " << std::endl; ! out << "<NumDimensions " << m_numDimensions << " >" << std::endl; ! out << "<TotalBins " << m_totalBins << " >" << std::endl; ! out << "<SimilarityType " << (int)m_simType << " >" << std::endl; ! out << "<NormFlag " << (int)m_normFlag << " >" << std::endl; ! out << "<LastMaxVal " << m_lastMaxVal << " >" << std::endl; ! out << "<NumBins " << std::endl; for (d=0; d<m_numDimensions; d++) { out << m_numBins[d] << " "; } ! out << " >" << std::endl; ! out << "<BinsWidth " << std::endl; for (d=0; d<m_numDimensions; d++) { out << m_binsWidth[d] << " "; } ! out << " >" << std::endl; ! out << "<Bins " << std::endl; for (b=0; b<m_totalBins; ++b) { out << m_bins[b] << " "; } ! out << " >" << std::endl; ! out << " >" << std::endl; } *************** *** 889,893 **** inline double EpanechKernel(double i_dist) const { ! return max( 0.0, k_VISUALHIST_2DIVPI * (1.0 - i_dist) ); } --- 889,893 ---- inline double EpanechKernel(double i_dist) const { ! return std::max( 0.0, k_VISUALHIST_2DIVPI * (1.0 - i_dist) ); } |
|
From: Pierre M. <sid...@us...> - 2005-06-01 03:34:12
|
Update of /cvsroot/robotflow/RobotFlow/Vision/include In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv20773 Modified Files: VisualFeatureDesc.h Log Message: Removed unecessary cout in some methods Index: VisualFeatureDesc.h =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Vision/include/VisualFeatureDesc.h,v retrieving revision 1.3 retrieving revision 1.4 diff -C2 -d -r1.3 -r1.4 *** VisualFeatureDesc.h 12 Apr 2005 20:09:05 -0000 1.3 --- VisualFeatureDesc.h 1 Jun 2005 03:34:01 -0000 1.4 *************** *** 77,81 **** VisualFeatureDesc(const VisualFeatureDesc<FeatBaseType> &i_ref) { - cout << "Using VisualFeatureDesc::copy constructor" << endl; m_descType = i_ref.m_descType; } --- 77,80 ---- *************** *** 88,92 **** virtual VisualFeatureDesc<FeatBaseType> & operator =(const VisualFeatureDesc<FeatBaseType> &i_ref) { - cout << "Using VisualFeatureDesc::operator=" << endl; // Avoid self assignment if (&i_ref != this) { --- 87,90 ---- *************** *** 99,103 **** virtual VisualFeatureDesc<FeatBaseType>* clone() const { - cout << "Using VisualFeatureDesc::clone" << endl; return new VisualFeatureDesc<FeatBaseType>(*this); } --- 97,100 ---- |
|
From: Pierre M. <sid...@us...> - 2005-06-01 03:23:48
|
Update of /cvsroot/robotflow/RobotFlow/Vision/src In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv14574 Added Files: RGB5552RGB24.cc Log Message: RGB555 to RGB24 color space conversion. --- NEW FILE: RGB5552RGB24.cc --- /* Copyright (C) 2004 Pierre Moisan (Pie...@US...) This library is free software; you can redistribute it and/or modify it under the terms of the GNU Lesser General Public License as published by the Free Software Foundation; either version 2.1 of the License, or (at your option) any later version. This library is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public License along with this library; if not, write to the Free Software Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #ifndef _RGB5552RGB24_CC_ #define _RGB5552RGB24_CC_ #include "BufferedNode.h" #include "Image.h" using namespace std; using namespace FD; namespace RobotFlow { class RGB5552RGB24; DECLARE_NODE(RGB5552RGB24) /*Node * * @name RGB5552RGB24 * @category RobotFlow:Vision * @description Simple conversion from RGB555 color format to RGB24 color format. * * @input_name RGB555IMAGE * @input_type Image * @input_description RGB555 image * * @output_name RGB24IMAGE * @output_type IMAGE * @output_description RGB24 image * END*/ class RGB5552RGB24 : public BufferedNode { int m_imageInID; int m_imageOutID; unsigned char m_red_conversion_table[32768]; unsigned char m_green_conversion_table[32768]; unsigned char m_blue_conversion_table[32768]; public: RGB5552RGB24(string nodeName, ParameterSet params) : BufferedNode(nodeName, params) { m_imageInID = addInput("RGB555IMAGE"); m_imageOutID = addOutput("RGB24IMAGE"); build_table(); } void calculate(int output_id, int count, Buffer &out) { Image &input_image = object_cast<Image>(getInput(m_imageInID,count)); int width = input_image.get_width(); int height = input_image.get_height(); unsigned short* input_data = (unsigned short*) input_image.get_data(); //creating new image (rgb24) Image *output_image = Image::alloc(width,height,3); unsigned char* output_data = output_image->get_data(); //copying data for (unsigned int i = 0; i < width * height; i++) { *output_data++ = m_blue_conversion_table[*input_data]; *output_data++ = m_green_conversion_table[*input_data]; *output_data++ = m_red_conversion_table[*input_data]; *input_data++; } out[count] = ObjectRef(output_image); }//calculate private: void build_table () { for (unsigned int i = 0; i < 32768; i++) { float red = ((float)((i >> 10) & 0x1f)) / 31.0; float green = ((float)((i >> 5) & 0x1f)) / 31.0; float blue = ((float)(i & 0x1f)) / 31.0; m_red_conversion_table[i] = (unsigned char) (red * 255.0); m_green_conversion_table[i] = (unsigned char) (green * 255.0); m_blue_conversion_table[i] = (unsigned char) (blue * 255.0); } }//build_table }; } #endif |
|
From: Pierre M. <sid...@us...> - 2005-05-26 19:53:04
|
Update of /cvsroot/robotflow/RobotFlow/Vision/include In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv8489 Modified Files: Makefile.am Log Message: Removedan invalid file in OpenCV list. Index: Makefile.am =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Vision/include/Makefile.am,v retrieving revision 1.10 retrieving revision 1.11 diff -C2 -d -r1.10 -r1.11 *** Makefile.am 26 May 2005 19:43:22 -0000 1.10 --- Makefile.am 26 May 2005 19:52:50 -0000 1.11 *************** *** 13,17 **** PFPredictionModel.h \ PFUtilityFct.h \ - PKLTScaleAdaption.h \ VisualFeatureDesc.h \ VisualHistogramDesc.h \ --- 13,16 ---- |
|
From: Pierre M. <sid...@us...> - 2005-05-26 19:51:31
|
Update of /cvsroot/robotflow/RobotFlow/Vision/src In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv8204 Modified Files: Makefile.am Log Message: Added new files for particle filter. Index: Makefile.am =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Vision/src/Makefile.am,v retrieving revision 1.30 retrieving revision 1.31 diff -C2 -d -r1.30 -r1.31 *** Makefile.am 12 Apr 2005 19:34:03 -0000 1.30 --- Makefile.am 26 May 2005 19:51:18 -0000 1.31 *************** *** 8,17 **** if WITH_OPENCV OPENCV_SOURCES = ColorHistExtraction.cc \ MeanShiftTracker.cc \ VisualFeatureDesc.cc \ VisualHistogramDesc.cc \ VisualROI.cc \ VisualTarget.cc \ ! VisualTargetManager.cc else OPENCV_SOURCES = --- 8,24 ---- if WITH_OPENCV OPENCV_SOURCES = ColorHistExtraction.cc \ + DrawPFParticle.cc \ MeanShiftTracker.cc \ + PFGenericParticle.cc \ + PFGenericParticleFilter.cc \ + PFPMRandomWalk.cc \ + PFUtilityFct.cc \ + PFState2VisualROI.cc \ VisualFeatureDesc.cc \ VisualHistogramDesc.cc \ VisualROI.cc \ VisualTarget.cc \ ! VisualTargetManager.cc \ ! VisualTargetPFInterface.cc else OPENCV_SOURCES = *************** *** 73,77 **** Scale.cc \ ScalePrint.cc \ ! RGB242RGB15.cc $(OPENCV_SOURCES) --- 80,85 ---- Scale.cc \ ScalePrint.cc \ ! RGB242RGB15.cc \ ! RGB5552RGB24.cc $(OPENCV_SOURCES) |
|
From: Pierre M. <sid...@us...> - 2005-05-26 19:49:47
|
Update of /cvsroot/robotflow/RobotFlow/Vision/src In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv7826 Added Files: DrawPFParticle.cc PFGenericParticle.cc PFGenericParticleFilter.cc PFPMRandomWalk.cc PFState2VisualROI.cc PFUtilityFct.cc VisualTargetPFInterface.cc Log Message: Generic particle filter implementation for visual tracking purposes --- NEW FILE: PFGenericParticleFilter.cc --- /* Copyright (C) 2005 Pierre Moisan (Pie...@US...) This library is free software; you can redistribute it and/or modify it under the terms of the GNU Lesser General Public License as published by the Free Software Foundation; either version 2.1 of the License, or (at your option) any later version. This library is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public License along with this library; if not, write to the Free Software Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "PFGenericParticleFilter.h" using namespace std; using namespace FD; namespace RobotFlow { DECLARE_NODE(PFGenericParticleFilter) /*Node * * @name PFGenericParticleFilter * @category RobotFlow:Vision:Tracking * @description Generic implementation of a particle filter. * * @parameter_name NUM_PARTICLES * @parameter_type int * @parameter_value 200 * @parameter_description Number of particles (samples) to use. * * @parameter_name PARTICLE_STATE_SIZE * @parameter_type int * @parameter_value 4 * @parameter_description Particle state size. * * @input_name INIT_VARIANCE * @input_type Vector<float> * @input_description Variance for initialization of each of the particle state. * * @input_name REFERENCE_MEAN_STATE * @input_type PFGenericParticle * @input_description Current reference mean state to track. * * @input_name PREDICT_COMPLETED * @input_type bool * @input_description Flag indicating the success of the prediction. * * @input_name PARTICLE_LIKELIHOOD * @input_type float * @input_description Resulting measurement likelihood for current particle. * * @output_name FILTERING_FINISHED * @output_type bool * @output_description Flag indicating that the tracking has completed. * * @output_name CURRENT_PARTICLE * @output_type PFGenericParticle * @output_description Current particle (sample) to apply prediction model. * * @output_name TRACKED_MEAN_STATE * @output_type PFGenericParticle * @output_description Resulting mean state after particle filtering. * END*/ PFGenericParticleFilter::PFGenericParticleFilter() : PFParticleFilter(e_PF_GenericFilter), m_init(false), m_initPF(false), m_initSamples(false), m_finished(false), m_curSampleIdx(-1), m_numSamples(0), m_sampleStateSize(0), m_samples(NULL), m_tmpSamples(NULL), m_cumulWeight(NULL), m_initVariance(NULL) { } PFGenericParticleFilter::PFGenericParticleFilter(unsigned int i_numSamples, unsigned int i_sampleStateSize, const Vector<float> *i_initVariance) : PFParticleFilter(e_PF_GenericFilter), m_init(false), m_initPF(false), m_initSamples(false), m_finished(false), m_curSampleIdx(-1), m_numSamples(i_numSamples), m_sampleStateSize(i_sampleStateSize), m_samples(NULL), m_tmpSamples(NULL), m_cumulWeight(NULL), m_initVariance(NULL) { Initialize(i_initVariance); } PFGenericParticleFilter::PFGenericParticleFilter(string nodeName, ParameterSet params) : PFParticleFilter(nodeName, params), m_init(false), m_initPF(false), m_initSamples(false), m_finished(false), m_curSampleIdx(-1), m_numSamples(0), m_sampleStateSize(0), m_samples(NULL), m_tmpSamples(NULL), m_cumulWeight(NULL), m_initVariance(NULL) { SetType(e_PF_GenericFilter); m_initVarianceInID = addInput("INIT_VARIANCE"); m_refMeanStateInID = addInput("REFERENCE_MEAN_STATE"); m_predictInID = addInput("PREDICT_COMPLETED"); m_likelihoodInID = addInput("PARTICLE_LIKELIHOOD"); m_finishedOutID = addOutput("FILTERING_FINISHED"); m_particleOutID = addOutput("CURRENT_PARTICLE"); m_meanStateOutID = addOutput("TRACKED_MEAN_STATE"); m_numSamples = dereference_cast<int>(parameters.get("NUM_PARTICLES")); m_sampleStateSize = dereference_cast<int>(parameters.get("PARTICLE_STATE_SIZE")); } PFGenericParticleFilter::~PFGenericParticleFilter() { if (m_initVariance) { m_initVariance->destroy(); } delete [] m_samples; delete [] m_tmpSamples; delete [] m_cumulWeight; } // Modified BufferedNode request method to support cyclic node connection void PFGenericParticleFilter::request(int output_id, const ParameterSet &req) { if (req.exist("LOOKAHEAD")) { outputs[output_id].lookAhead = max(outputs[output_id].lookAhead,dereference_cast<int> (req.get("LOOKAHEAD"))); } if (req.exist("LOOKBACK")) { outputs[output_id].lookBack = max(outputs[output_id].lookBack,dereference_cast<int> (req.get("LOOKBACK"))); } if (req.exist("INORDER")) { inOrder = true; } int outputLookAhead=0, outputLookBack=0; outputLookAhead=max(outputLookAhead, outputs[output_id].lookAhead); outputLookBack =max(outputLookBack, outputs[output_id].lookBack); if (output_id == m_finishedOutID) { // TRACKING_FINISHED output does not required any inputs return; } else if (output_id == m_particleOutID) { // CURRENT_PARTICLE output does not required any inputs return; } else if (output_id == m_meanStateOutID) { ParameterSet myReq, myReq2, myReq3, myReq4; myReq.add("LOOKAHEAD", ObjectRef(Int::alloc(inputsCache[m_initVarianceInID].lookAhead+outputLookAhead))); myReq.add("LOOKBACK", ObjectRef(Int::alloc(inputsCache[m_initVarianceInID].lookBack+outputLookBack))); inputs[m_initVarianceInID].node->request(inputs[m_initVarianceInID].outputID,myReq); myReq2.add("LOOKAHEAD", ObjectRef(Int::alloc(inputsCache[m_refMeanStateInID].lookAhead+outputLookAhead))); myReq2.add("LOOKBACK", ObjectRef(Int::alloc(inputsCache[m_refMeanStateInID].lookBack+outputLookBack))); inputs[m_refMeanStateInID].node->request(inputs[m_refMeanStateInID].outputID,myReq2); myReq3.add("LOOKAHEAD", ObjectRef(Int::alloc(inputsCache[m_predictInID].lookAhead+outputLookAhead))); myReq3.add("LOOKBACK", ObjectRef(Int::alloc(inputsCache[m_predictInID].lookBack+outputLookBack))); inputs[m_predictInID].node->request(inputs[m_predictInID].outputID,myReq3); myReq4.add("LOOKAHEAD", ObjectRef(Int::alloc(inputsCache[m_likelihoodInID].lookAhead+outputLookAhead))); myReq4.add("LOOKBACK", ObjectRef(Int::alloc(inputsCache[m_likelihoodInID].lookBack+outputLookBack))); inputs[m_likelihoodInID].node->request(inputs[m_likelihoodInID].outputID,myReq4); } else { throw new GeneralException ("PFGenericParticleFilter::request : unknown output ID.",__FILE__,__LINE__); } } void PFGenericParticleFilter::calculate(int output_id, int count, Buffer &out) { try { if (output_id == m_finishedOutID) { (*outputs[m_finishedOutID].buffer)[count] = ObjectRef(Bool::alloc(!m_finished)); if (m_finished) { m_initPF = false; m_finished = false; } } else if (output_id == m_particleOutID) { (*outputs[m_particleOutID].buffer)[count] = ObjectRef(m_curSample); } else if (output_id == m_meanStateOutID){ if (!m_initPF) { // First get reference mean state to track ObjectRef meanStateRef = getInput(m_refMeanStateInID, count); if (meanStateRef->isNil()) { // Tracking failed of has not yet started m_initSamples = false; // Invalid target, output nilObject (*outputs[m_meanStateOutID].buffer)[count] = ObjectRef(nilObject); return; } m_refMeanState = RCPtr<PFGenericParticle>(meanStateRef); // Get INIT_VARIANCE only once for intialization if (!m_init) { ObjectRef varObjRef = getInput(m_initVarianceInID, count); if (!varObjRef->isNil()) { RCPtr<Vector<float> > varRef = varObjRef; Initialize(&(*varRef)); } else { throw new GeneralException ("PFGenericParticleFilter::calculate : invalid (nilObject) INIT_VARIANCE input.",__FILE__,__LINE__); } } if (!m_initSamples) { InitSamples(); } m_curSampleIdx = 0; m_likelihoodsSum = 0.f; m_initPF = true; } // Get a reference to current particle m_curSample = RCPtr<PFGenericParticle>(m_samples[m_curSampleIdx]); // // Prediction step // Apply prediction to current particle // bool predictCompleted = dereference_cast<bool>(getInput(m_predictInID, count)); if (!predictCompleted) { throw new GeneralException ("PFGenericParticleFilter::calculate : prediction failed on current particle.",__FILE__,__LINE__); } // // Measurement step // Compute likelihood of the current particle // float likelihood = dereference_cast<float>(getInput(m_likelihoodInID, count)); if (likelihood < 0.f || likelihood > 1.f) { throw new GeneralException ("PFGenericParticleFilter::calculate : invalid likelihood for current particle (should be between [0.0, 1.0]).",__FILE__,__LINE__); } // Assign likelihood to current particle's weight m_curSample->SetWeight(likelihood); m_likelihoodsSum += likelihood; /* cout << "Current particle info: likelihood=" << likelihood << endl; float *p_refState = m_refMeanState->GetState(); for (int i=0; i<m_sampleStateSize; i++) { cout << "State[" << i << "]=" << *p_refState++ << endl; } */ m_curSampleIdx++; // Verify if we have done every particle if (m_curSampleIdx >= m_numSamples) { m_finished = true; } if (!m_finished) { (*outputs[m_meanStateOutID].buffer)[count] = ObjectRef(nilObject); } else { // // Update step // Propagate densities and update samples // Update(); // Compute mean state ComputeMeanState(); // // Resampling step // Resample(); // Output mean state (*outputs[m_meanStateOutID].buffer)[count] = ObjectRef(m_outMeanState); } } } catch (BaseException *e) { throw e->add(new GeneralException("Exception in PFGenericParticleFilter::calculate:",__FILE__,__LINE__)); } } void PFGenericParticleFilter::Initialize(const Vector<float> *i_variance) { try { if (m_sampleStateSize != i_variance->size()) { throw new GeneralException ("PFGenericParticleFilter::Initialize : particle state size differs from variance vector size.",__FILE__,__LINE__); } m_initVariance = Vector<float>::alloc(m_sampleStateSize); for (int i=0; i<m_sampleStateSize; i++) { (*m_initVariance)[i] = (*i_variance)[i]; } m_samples = new RCPtr<PFGenericParticle>[m_numSamples]; m_tmpSamples = new RCPtr<PFGenericParticle>[m_numSamples]; for (int s=0; s<m_numSamples; s++) { m_samples[s] = RCPtr<PFGenericParticle>(new PFGenericParticle(m_sampleStateSize)); m_tmpSamples[s] = RCPtr<PFGenericParticle>(new PFGenericParticle(m_sampleStateSize)); } m_cumulWeight = new float[m_numSamples]; m_outMeanState = RCPtr<PFGenericParticle>(new PFGenericParticle(m_sampleStateSize)); m_init = true; } catch (BaseException *e) { throw e->add(new GeneralException("Exception in PFGenericParticleFilter::Initialize:",__FILE__,__LINE__)); } } void PFGenericParticleFilter::InitSamples() { int i, s; float *p_sampleState, *p_refState; if (!m_samples) { throw new GeneralException ("PFGenericParticleFilter::InitSamples : impossible to initialize samples without proper memory allocation.",__FILE__,__LINE__); } if (m_refMeanState->GetType() == e_PFP_Unknown) { throw new GeneralException ("PFGenericParticleFilter::InitSamples : cannot use particle with unknown type.",__FILE__,__LINE__); } for (s=0; s<m_numSamples; s++) { p_sampleState = m_samples[s]->GetState(); p_refState = m_refMeanState->GetState(); for (i=0; i<m_sampleStateSize; i++) { *p_sampleState++ = (*p_refState++)*(1.f + (*m_initVariance)[i]*(PFUTIL_randn()-0.5f)); } } m_initSamples = true; } void PFGenericParticleFilter::Update() { int s; float *p_cumulWeight, curWeight; float cumulSum=1e-30, weightSum=1e-30, weightSumInv, weightBias; if (!m_samples) { throw new GeneralException ("PFGenericParticleFilter::Update : impossible to update with uninitialized samples.",__FILE__,__LINE__); } p_cumulWeight = m_cumulWeight; /* if (m_likelihoodsSum != 0.f) { likelihoodsSumInv = 1.f/m_likelihoodsSum; } else { likelihoodsSumInv = 1.f; } */ for (s=0; s<m_numSamples; s++) { // Normalize weights //curWeight = m_samples[s]->GetWeight()*likelihoodsSumInv; curWeight = m_samples[s]->GetWeight(); m_samples[s]->SetWeight(curWeight); cumulSum += curWeight; // Compute cumulative weights *p_cumulWeight++ = cumulSum; weightSum += cumulSum; } weightSumInv = 1.f/weightSum; weightBias = (1.f-m_likelihoodsSum)/m_numSamples; p_cumulWeight = m_cumulWeight; for (s=0; s<m_numSamples; s++) { *p_cumulWeight = weightSumInv*(*p_cumulWeight)*m_likelihoodsSum + weightBias; p_cumulWeight++; } p_cumulWeight = m_cumulWeight; weightSum = 1e-30; for (s=0; s<m_numSamples; s++) { curWeight = m_samples[s]->GetWeight()*(*p_cumulWeight); m_samples[s]->SetWeight(curWeight); weightSum += curWeight; p_cumulWeight++; } weightSumInv = 1.f/weightSum; for (s=0; s<m_numSamples; s++) { curWeight = m_samples[s]->GetWeight()*weightSumInv; m_samples[s]->SetWeight(curWeight); } } void PFGenericParticleFilter::ComputeMeanState() { int i, s; float *p_sampleState, *p_meanState, likelihood; if (!m_samples) { throw new GeneralException ("PFGenericParticleFilter::ComputeMeanState : impossible to compute mean state with uninitialized samples.",__FILE__,__LINE__); } p_meanState = m_outMeanState->GetState(); for (i=0; i<m_sampleStateSize; i++) { *p_meanState++ = 0.f; } // Perform a weighted sum to get the mean state for (s=0; s<m_numSamples; s++) { likelihood = m_samples[s]->GetWeight(); p_sampleState = m_samples[s]->GetState(); p_meanState = m_outMeanState->GetState(); for (i=0; i<m_sampleStateSize; i++) { *p_meanState += (*p_sampleState++)*likelihood; p_meanState++; } } } void PFGenericParticleFilter::Resample() { int s; float effectiveN = 0.f, curWeight, sizeInv, sumWeights; if (!m_samples) { throw new GeneralException ("PFGenericParticleFilter::Resample : impossible to resample with uninitialized samples.",__FILE__,__LINE__); } for (s=0; s<m_numSamples; s++) { curWeight = m_samples[s]->GetWeight(); effectiveN += curWeight * curWeight; } effectiveN = 1.f/(effectiveN*m_numSamples); if (effectiveN > 0.6f) { // Current distribution modelled by the particles set has a sufficient density return; } CopyTmpSamples(); m_cumulWeight[0] = m_tmpSamples[0]->GetWeight(); for (s=1; s<m_numSamples; s++) { m_cumulWeight[s] = m_cumulWeight[s-1]+m_samples[s]->GetWeight(); } sumWeights = 0.f; for (s=0; s<m_numSamples; s++) { float r = PFUTIL_ran(); int j=PFUTIL_find_range(r, m_cumulWeight, m_numSamples); *(m_samples[s]) = *(m_tmpSamples[j]); m_samples[s]->SetWeight(1.f); sumWeights += m_samples[s]->GetWeight(); } sumWeights = 1.f/sumWeights; for (s=0; s<m_numSamples; s++) { curWeight = m_samples[s]->GetWeight(); m_samples[s]->SetWeight(curWeight*sumWeights); } } // // Private methods // void PFGenericParticleFilter::CopyTmpSamples() { int s; for (s=0; s<m_numSamples; s++) { *(m_tmpSamples[s]) = *(m_samples[s]); } } } --- NEW FILE: PFUtilityFct.cc --- // // Utility functions mainly for random numbers generation // Copyright (C) 2004 Jean-Marc Valin // #include "PFUtilityFct.h" using namespace std; namespace RobotFlow { static float PFUTIL_logtable2[PFUTIL_FLOGLOOKUP2SIZE]; void PFUTIL_build_flog_table() { static bool init=false; if (!init) { PFUTIL_FloatManip m; for (int i=0;i<PFUTIL_FLOGLOOKUP2SIZE;i++) { m.i = (i<<PFUTIL_FLOGLOOKUP2SHIFT) | 0x3f800000; PFUTIL_logtable2[i]=log(m.f); } init=true; } } float PFUTIL_flog(const float in) { PFUTIL_build_flog_table(); PFUTIL_FloatManip m; m.f = in; //Extract the mantissa and perform lookup for log(mantissa) float tb = PFUTIL_logtable2[(m.i & 0x007fffff)>>PFUTIL_FLOGLOOKUP2SHIFT]; //Extract the exponent int id = (m.i>>23)-127; return id*M_LN2 + tb; } float PFUTIL_randn() { static bool avail = false; static float cached=0.f; if (avail) { avail = false; return cached; } else { float U1, U2, S, x; do { U1 = float(rand())/float(RAND_MAX); U2 = float(rand())/float(RAND_MAX); U1 = 2*U1-1; U2 = 2*U2-1; S = U1*U1 + U2*U2; } while (S >= 1 || S == 0.0f); float tmp = sqrt(-2 * PFUTIL_flog(S) / S); cached = tmp * U1; avail = true; return tmp * U2; } } float PFUTIL_ran() { const unsigned int jflone = 0x3f800000; const unsigned int jflmsk = 0x007fffff; static int idum = 10000; union {int i; float f;} ran; idum = 1664525*idum + 1013904223; ran.i = jflone | (jflmsk & idum); ran.f -= 1; return ran.f; } int PFUTIL_find_range(float i_x, const float *i_cumul, int i_cumulSize) { int low=0; int high=i_cumulSize-1; int cur = high>>1; while (1) { if (i_cumul[cur] > i_x) { if (cur == 0 || i_cumul[cur-1] <= i_x) { break; } else { high = cur-1; cur = (low+cur) >> 1; } } else { low = cur; cur = (high+cur+1) >> 1; if (cur==high) { break; } } } return cur; } } --- NEW FILE: PFGenericParticle.cc --- /* Copyright (C) 2005 Pierre Moisan (Pie...@US...) This library is free software; you can redistribute it and/or modify it under the terms of the GNU Lesser General Public License as published by the Free Software Foundation; either version 2.1 of the License, or (at your option) any later version. This library is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public License along with this library; if not, write to the Free Software Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "PFGenericParticle.h" using namespace std; using namespace FD; namespace RobotFlow { DECLARE_TYPE(PFGenericParticle) PFGenericParticle::PFGenericParticle() : PFParticle(e_PFP_Generic), m_stateSize(0), m_state(NULL), m_weight(-1) { } PFGenericParticle::PFGenericParticle(unsigned int i_stateSize) : PFParticle(e_PFP_Generic), m_stateSize(i_stateSize), m_state(NULL), m_weight(-1) { m_state = new float [m_stateSize]; } PFGenericParticle::PFGenericParticle(const PFGenericParticle &i_ref) { this->SetType(i_ref.GetType()); m_stateSize = i_ref.m_stateSize; m_state = new float [m_stateSize]; m_weight = i_ref.m_weight; for (int i=0; i<m_stateSize; i++) { m_state[i] = i_ref.m_state[i]; } } PFGenericParticle::~PFGenericParticle() { delete [] m_state; } PFGenericParticle & PFGenericParticle::operator =(const PFGenericParticle &i_ref) { // Avoid self assignment if (&i_ref != this) { this->SetType(i_ref.GetType()); if (m_stateSize != i_ref.m_stateSize) { m_stateSize = i_ref.m_stateSize; delete m_state; m_state = new float [m_stateSize]; } if (m_state && i_ref.m_state) { for (int i=0; i<m_stateSize; i++) { m_state[i] = i_ref.m_state[i]; } } m_weight = i_ref.m_weight; } return *this; } PFGenericParticle* PFGenericParticle::clone() const { return new PFGenericParticle(*this); } // Default routine to print a PFGenericParticle object to an output stream void PFGenericParticle::printOn(ostream &out) const { throw new GeneralException("Exception in PFGenericParticle::printOn: routine not yet implemented.",__FILE__,__LINE__); } // Default routine to read a PFGenericParticle object from an input stream void PFGenericParticle::readFrom(istream &in) { throw new GeneralException("Exception in PFGenericParticle::readFrom: routine not yet implemented.",__FILE__,__LINE__); } unsigned int PFGenericParticle::GetStateSize() const { return m_stateSize; } float *PFGenericParticle::GetState() { return m_state; } const float *PFGenericParticle::GetCstState() const { return (const float *) m_state; } float PFGenericParticle::GetStateIdx(int i_idx) const { if (i_idx < 0 || i_idx >= m_stateSize) { throw new GeneralException("Exception in PFGenericParticle::GetStateIdx: invalid state index.",__FILE__,__LINE__); } return m_state[i_idx]; } float PFGenericParticle::GetWeight() const { return m_weight; } void PFGenericParticle::SetStateSize(unsigned int i_size) { if (i_size != m_stateSize) { m_stateSize = i_size; delete [] m_state; m_state = new float[m_stateSize]; } } // Assumes that given ptr has the same size as current particle void PFGenericParticle::SetState(const float *i_state) { for (int i=0; i<m_stateSize; i++) { m_state[i] = i_state[i]; } } void PFGenericParticle::SetStateIdx(int i_idx, float i_val) { if (i_idx < 0 || i_idx >= m_stateSize) { throw new GeneralException("Exception in PFGenericParticle::SetStateIdx: invalid state index.",__FILE__,__LINE__); } m_state[i_idx] = i_val; } void PFGenericParticle::SetWeight(float i_weight) { m_weight = i_weight; } } --- NEW FILE: VisualTargetPFInterface.cc --- /* Copyright (C) 2005 Pierre Moisan (Pie...@US...) This library is free software; you can redistribute it and/or modify it under the terms of the GNU Lesser General Public License as published by the Free Software Foundation; either version 2.1 of the License, or (at your option) any later version. This library is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public License along with this library; if not, write to the Free Software Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #ifndef _VISUALROI2PFSTATE_CC_ #define _VISUALROI2PFSTATE_CC_ #include "BufferedNode.h" #include "VisualTarget.h" #include "PFGenericParticle.h" using namespace std; using namespace FD; namespace RobotFlow { class VisualTargetPFInterface; DECLARE_NODE(VisualTargetPFInterface) /*Node * * @name VisualTargetPFInterface * @category RobotFlow:Vision:Tracking * @description Interface for tracking a VisualTarget using a particle filter framework. * * @parameter_name USE_SCALE * @parameter_type bool * @parameter_value true * @parameter_description Flag indicating to use the ROI scale (width and height) in the particle's state. * * @parameter_name USE_ROTATION_ANGLE * @parameter_type bool * @parameter_value false * @parameter_description Flag indicating to use the ROI rotation angle in the particle's state. * * @input_name IN_TARGET * @input_type VisualTarget<double> * @input_description Current VisualTarget to convert. * * @input_name IN_FEATURES_VECTOR * @input_type Vector<VisualFeatureDesc<double> *> * @input_description Features descriptors vector (to compute a particle likelihood). * * @input_name IN_MEAN_STATE * @input_type PFGenericParticle * @input_description Mean state particle resulting from partcile filter tracking. * * @input_name PREPROCESS_COMPLETED * @input_type Vector<int> * @input_description Flags indicating the completion of the features extraction preprocessing stage. * * @input_name FILTERING_FINISHED * @input_type bool * @input_description Flags indicating the completion of the particle filtering stage. * * @output_name OUT_PARTICLE * @output_type PFGenericParticle * @output_description Resulting particle with appropriate state. * * @output_name OUT_LIKELIHOOD * @output_type float * @output_description Current particle likelihood. * * @output_name OUT_TARGET * @output_type VisualTarget<double> * @output_description Resulting tracked target. * * @output_name TRACKING_FINISHED * @output_type bool * @output_description Flag indicating that the tracking has completed. * END*/ class VisualTargetPFInterface : public BufferedNode { public: VisualTargetPFInterface::VisualTargetPFInterface(string nodeName, ParameterSet params) : BufferedNode(nodeName, params), m_needNewTarget(true) { m_targetInID = addInput("IN_TARGET"); m_featuresInID = addInput("IN_FEATURES_VECTOR"); m_meanParticleInID = addInput("IN_MEAN_STATE"); m_ppCompletedInID = addInput("PREPROCESS_COMPLETED"); m_filtFinishedInID = addInput("FILTERING_FINISHED"); m_particleOutID = addOutput("OUT_PARTICLE"); m_likelihoodOutID = addOutput("OUT_LIKELIHOOD"); m_targetOutID = addOutput("OUT_TARGET"); m_finishedOutID = addOutput("TRACKING_FINISHED"); m_useScale = dereference_cast<bool>(parameters.get("USE_SCALE")); m_useAngle = dereference_cast<bool>(parameters.get("USE_ROTATION_ANGLE")); // First 2 elements of state is center position x,y m_stateSize = 2; if (m_useScale) { // Add scale state which is described by the half-width and half-height of ROI m_stateSize += 2; } if (m_useAngle) { // Add rotation angle state m_stateSize += 1; } // Allocate particle m_curSample = RCPtr<PFGenericParticle>(new PFGenericParticle(m_stateSize)); } VisualTargetPFInterface::~VisualTargetPFInterface() { } // Modified BufferedNode request method to support cyclic node connection void VisualTargetPFInterface::request(int output_id, const ParameterSet &req) { if (req.exist("LOOKAHEAD")) { outputs[output_id].lookAhead = max(outputs[output_id].lookAhead,dereference_cast<int> (req.get("LOOKAHEAD"))); } if (req.exist("LOOKBACK")) { outputs[output_id].lookBack = max(outputs[output_id].lookBack,dereference_cast<int> (req.get("LOOKBACK"))); } if (req.exist("INORDER")) { inOrder = true; } int outputLookAhead=0, outputLookBack=0; outputLookAhead=max(outputLookAhead, outputs[output_id].lookAhead); outputLookBack =max(outputLookBack, outputs[output_id].lookBack); if (output_id == m_particleOutID) { // OUT_PARTICLE output does not required any inputs (if flow is respected) return; } else if (output_id == m_likelihoodOutID) { // Likelihood output requires features vector ParameterSet myReq; myReq.add("LOOKAHEAD", ObjectRef(Int::alloc(inputsCache[m_featuresInID].lookAhead+outputLookAhead))); myReq.add("LOOKBACK", ObjectRef(Int::alloc(inputsCache[m_featuresInID].lookBack+outputLookBack))); inputs[m_featuresInID].node->request(inputs[m_featuresInID].outputID,myReq); return; } else if (output_id == m_targetOutID) { ParameterSet myReq, myReq2, myReq3; /* myReq.add("LOOKAHEAD", ObjectRef(Int::alloc(inputsCache[m_targetInID].lookAhead+outputLookAhead))); myReq.add("LOOKBACK", ObjectRef(Int::alloc(inputsCache[m_targetInID].lookBack+outputLookBack))); inputs[m_targetInID].node->request(inputs[m_targetInID].outputID,myReq); */ myReq2.add("LOOKAHEAD", ObjectRef(Int::alloc(inputsCache[m_meanParticleInID].lookAhead+outputLookAhead))); myReq2.add("LOOKBACK", ObjectRef(Int::alloc(inputsCache[m_meanParticleInID].lookBack+outputLookBack))); inputs[m_meanParticleInID].node->request(inputs[m_meanParticleInID].outputID,myReq2); myReq3.add("LOOKAHEAD", ObjectRef(Int::alloc(inputsCache[m_ppCompletedInID].lookAhead+outputLookAhead))); myReq3.add("LOOKBACK", ObjectRef(Int::alloc(inputsCache[m_ppCompletedInID].lookBack+outputLookBack))); inputs[m_ppCompletedInID].node->request(inputs[m_ppCompletedInID].outputID,myReq3); } else if (output_id == m_finishedOutID) { ParameterSet myReq, myReq2; myReq.add("LOOKAHEAD", ObjectRef(Int::alloc(inputsCache[m_filtFinishedInID].lookAhead+outputLookAhead))); myReq.add("LOOKBACK", ObjectRef(Int::alloc(inputsCache[m_filtFinishedInID].lookBack+outputLookBack))); inputs[m_filtFinishedInID].node->request(inputs[m_filtFinishedInID].outputID,myReq); myReq2.add("LOOKAHEAD", ObjectRef(Int::alloc(inputsCache[m_targetInID].lookAhead+outputLookAhead))); myReq2.add("LOOKBACK", ObjectRef(Int::alloc(inputsCache[m_targetInID].lookBack+outputLookBack))); inputs[m_targetInID].node->request(inputs[m_targetInID].outputID,myReq2); } else { throw new GeneralException ("VisualTargetPFInterface::request : unknown output ID.",__FILE__,__LINE__); } } void VisualTargetPFInterface::calculate(int output_id, int count, Buffer &out) { try { if (output_id == m_finishedOutID) { if (m_needNewTarget) { // Get current target ObjectRef targetObjRef = getInput(m_targetInID, count); if (!targetObjRef->isNil()) { m_curTarget = RCPtr<VisualTarget<double> >(targetObjRef); if (!m_curTarget->GetCstROI()) { throw new GeneralException ("VisualTargetPFInterface::calculate : current VisualTarget has not a valid VisualROI member.",__FILE__,__LINE__); } // Force features preprocessing since we should be // starting a tracking iteration RCPtr<Vector<int> > preprocessRef = getInput(m_ppCompletedInID, count); // Set current sample according to target's ROI const VisualROI *roiRef = m_curTarget->GetCstROI(); float *p_state = m_curSample->GetState(); // Set position *p_state++ = (float)(roiRef->GetXCen()); *p_state++ = (float)(roiRef->GetYCen()); if (m_useScale) { // Add scale state *p_state++ = (float)(roiRef->GetHSX()); *p_state++ = (float)(roiRef->GetHSY()); } if (m_useAngle) { // Add rotation angle state *p_state++ = (float)(roiRef->GetAngle()); } m_needNewTarget = false; } } if (m_needNewTarget) { (*outputs[m_finishedOutID].buffer)[count] = ObjectRef(Bool::alloc(false)); } else { // See if particle filtering has finished bool filtFinished = dereference_cast<bool>(getInput(m_filtFinishedInID, count)); (*outputs[m_finishedOutID].buffer)[count] = ObjectRef(Bool::alloc(filtFinished)); } } else if (output_id == m_likelihoodOutID) { // Get features vector ObjectRef featObjRef = getInput(m_featuresInID, count); if (!featObjRef->isNil()) { RCPtr<Vector<VisualFeatureDesc<double> *> > featRef = featObjRef; // Use current reference target to estimate the // likelihood of the current features vector float sim = m_curTarget->Similarity(featRef.get()); float likelihood = exp(20.0*(sim-1.f)); //cout << "Likelihood = " << likelihood << endl; // Output current likehood (*outputs[m_likelihoodOutID].buffer)[count] = ObjectRef(Float::alloc(likelihood)); } else { throw new GeneralException ("VisualTargetPFInterface::calculate : invalid (nilObject) IN_FEATURES_VECTOR input.",__FILE__,__LINE__); } } else if (output_id == m_particleOutID) { // Simply output current particle reference that should be valid (*outputs[m_particleOutID].buffer)[count] = ObjectRef(m_curSample); } else if (output_id == m_targetOutID) { if (m_needNewTarget) { // Output nilObject since tracking is just starting (*outputs[m_targetOutID].buffer)[count] = nilObject; } else { // Get mean state particle ObjectRef particleObjRef = getInput(m_meanParticleInID, count); if (!particleObjRef->isNil()) { RCPtr<PFGenericParticle> meanParticleRef = particleObjRef; VisualROI *curROI = m_curTarget->GetROI(); // Set current target's ROI according to mean state particle const float *p_state = meanParticleRef->GetCstState(); // Set position curROI->SetXCen((int)(*p_state++)); curROI->SetYCen((int)(*p_state++)); if (m_useScale) { // Set scale curROI->SetHSX((int)(*p_state++)); curROI->SetHSY((int)(*p_state++)); } if (m_useAngle) { // Set rotation angle state curROI->SetAngle((int)(*p_state++)); } // Set flag to indicate we need a new target m_needNewTarget = true; // Output resulting target (*outputs[m_targetOutID].buffer)[count] = ObjectRef(m_curTarget); } else { // Output nilObject since tracking is not finished (*outputs[m_targetOutID].buffer)[count] = nilObject; } } } } catch (BaseException *e) { throw e->add(new GeneralException("Exception in VisualTargetPFInterface::calculate:",__FILE__,__LINE__)); } } private: // Input IDs int m_targetInID; int m_featuresInID; int m_meanParticleInID; int m_ppCompletedInID; int m_filtFinishedInID; // Output IDs int m_particleOutID; int m_likelihoodOutID; int m_targetOutID; int m_finishedOutID; // Parameters bool m_useScale; bool m_useAngle; unsigned int m_stateSize; bool m_needNewTarget; RCPtr<VisualTarget<double> > m_curTarget; RCPtr<PFGenericParticle> m_curSample; }; } #endif --- NEW FILE: PFPMRandomWalk.cc --- /* Copyright (C) 2005 Pierre Moisan (Pie...@US...) This library is free software; you can redistribute it and/or modify it under the terms of the GNU Lesser General Public License as published by the Free Software Foundation; either version 2.1 of the License, or (at your option) any later version. This library is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public License along with this library; if not, write to the Free Software Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #include "PFPMRandomWalk.h" using namespace std; using namespace FD; namespace RobotFlow { DECLARE_NODE(PFPMRandomWalk) DECLARE_TYPE(PFPMRandomWalk) /*Node * * @name PFPMRandomWalk * @category RobotFlow:Vision:Tracking * @description Random walk prediction model for particle filters. * * @input_name NOISE_VARIANCE * @input_type Vector<float> * @input_description Noise variance for each of the particle state. * * @input_name CURRENT_PARTICLE * @input_type PFGenericParticle * @input_description Current particle (sample) to apply prediction model. * * @output_name PREDICT_COMPLETED * @output_type bool * @output_description Flag indicating the success of the prediction. * END*/ PFPMRandomWalk::PFPMRandomWalk() : PFPredictionModel(e_PFPM_RandomWalk), m_init(false), m_stateSize(0), m_noiseVariance(NULL) { } PFPMRandomWalk::PFPMRandomWalk(const Vector<float> *i_variance) : PFPredictionModel(e_PFPM_RandomWalk), m_init(false), m_stateSize(0), m_noiseVariance(NULL) { Initialize(i_variance); } PFPMRandomWalk::PFPMRandomWalk(string nodeName, ParameterSet params) : PFPredictionModel(nodeName, params), m_init(false), m_stateSize(0), m_noiseVariance(NULL) { SetType(e_PFPM_RandomWalk); m_varianceInID = addInput("NOISE_VARIANCE"); m_particleInID = addInput("CURRENT_PARTICLE"); m_completedOutID = addOutput("PREDICT_COMPLETED"); } PFPMRandomWalk::~PFPMRandomWalk() { if (m_noiseVariance) { m_noiseVariance->destroy(); } } void PFPMRandomWalk::calculate(int output_id, int count, Buffer &out) { try { // Get NOISE_VARIANCE only once for intialization if (!m_init) { ObjectRef varObjRef = getInput(m_varianceInID, count); if (!varObjRef->isNil()) { RCPtr<Vector<float> > varRef = varObjRef; Initialize(&(*varRef)); } else { throw new GeneralException ("PFPMRandomWalk::calculate : invalid (nilObject) NOISE_VARIANCE input.",__FILE__,__LINE__); } } // Get current particle to apply prediction model to ObjectRef sampleObjRef = getInput(m_particleInID, count); if (!sampleObjRef->isNil()) { RCPtr<PFGenericParticle> sampleRef = sampleObjRef; // Particle is modified by reference Predict(sampleRef.get()); } else { throw new GeneralException ("PFPMRandomWalk::calculate : invalid (nilObject) CURRENT_PARTICLE input.",__FILE__,__LINE__); } // Output true indicating prediction was correctly applied (*outputs[m_completedOutID].buffer)[count] = ObjectRef(Bool::alloc(true)); } catch (BaseException *e) { throw e->add(new GeneralException("Exception in PFPMRandomWalk::calculate:",__FILE__,__LINE__)); } } void PFPMRandomWalk::Predict(PFParticle *io_sample) { try { float *p_state; // Check for current particle sanity if (m_stateSize != io_sample->GetStateSize()) { throw new GeneralException ("PFPMRandomWalk::Predict : current particle state size differs from variance vector size.",__FILE__,__LINE__); } p_state = io_sample->GetState(); if (!p_state) { throw new GeneralException ("PFPMRandomWalk::Predict : current particle has an uninitialized state.",__FILE__,__LINE__); } for (int i=0; i<m_stateSize; i++) { *p_state = *p_state * (1.f + (PFUTIL_randn()-0.5f)*(*m_noiseVariance)[i]); p_state++; } } catch (BaseException *e) { throw e->add(new GeneralException("Exception in PFPMRandomWalk::Predict:",__FILE__,__LINE__)); } } void PFPMRandomWalk::Initialize(const Vector<float> *i_variance) { try { m_stateSize = i_variance->size(); m_noiseVariance = Vector<float>::alloc(m_stateSize); for (int i=0; i<m_stateSize; i++) { (*m_noiseVariance)[i] = (*i_variance)[i]; } m_init = true; } catch (BaseException *e) { throw e->add(new GeneralException("Exception in PFPMRandomWalk::Initialize:",__FILE__,__LINE__)); } } } --- NEW FILE: DrawPFParticle.cc --- /* Copyright (C) 2005 Pierre Moisan (Pie...@US...) This library is free software; you can redistribute it and/or modify it under the terms of the GNU Lesser General Public License as published by the Free Software Foundation; either version 2.1 of the License, or (at your option) any later version. This library is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public License along with this library; if not, write to the Free Software Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #ifndef _DRAWPFPARTICLE_CC_ #define _DRAWPFPARTICLE_CC_ #include "BufferedNode.h" #include "VisualROI.h" #include "Image.h" using namespace std; using namespace FD; namespace RobotFlow { class DrawPFParticle; DECLARE_NODE(DrawPFParticle) /*Node * * @name DrawPFParticle * @category RobotFlow:Vision:Tracking * @description Draws particle state as a visual ROI in current image. * * @parameter_name FRAME_WIDTH * @parameter_type int * @parameter_value 320 * @parameter_description Video frame width. * * @parameter_name FRAME_HEIGHT * @parameter_type int * @parameter_value 240 * @parameter_description Video frame height. * * @parameter_name NUM_CHANNELS * @parameter_type int * @parameter_value 3 * @parameter_description Number of channels in video frame. * * @input_name IN_IMAGE * @input_type Image * @input_description Current image to use. * * @input_name IN_ROI * @input_type VisualROI * @input_description Current particle to draw. * * @output_name OUT_IMAGE * @output_type Image * @output_description Resulting image with particles drawn. * * @output_name OUT_ROI * @output_type VisualROI * @output_description Current ROI. * * @output_name PREPROCESS_COMPLETED * @output_type int * @output_description Output to force preprocessing. * END*/ class DrawPFParticle : public BufferedNode { public: DrawPFParticle::DrawPFParticle(string nodeName, ParameterSet params) : BufferedNode(nodeName, params) { m_imageInID = addInput("IN_IMAGE"); m_roiInID = addInput("IN_ROI"); m_imageOutID = addOutput("OUT_IMAGE"); m_roiOutID = addOutput("OUT_ROI"); m_ppCompletedOutID = addOutput("PREPROCESS_COMPLETED"); m_width = dereference_cast<int>(parameters.get("FRAME_WIDTH")); m_height = dereference_cast<int>(parameters.get("FRAME_HEIGHT")); m_numChannels = dereference_cast<int>(parameters.get("NUM_CHANNELS")); m_numPixels = m_width*m_height; m_numBytesInFrame = m_numPixels*m_numChannels; m_curImage = RCPtr<Image>(Image::alloc(m_width, m_height, m_numChannels)); m_color = new unsigned char[m_numChannels]; for (int c=0; c<m_numChannels; c++) { m_color[c] = 128; } } DrawPFParticle::~DrawPFParticle() { delete [] m_color; } // Modified BufferedNode request method to support cyclic node connection void DrawPFParticle::request(int output_id, const ParameterSet &req) { if (req.exist("LOOKAHEAD")) { outputs[output_id].lookAhead = max(outputs[output_id].lookAhead,dereference_cast<int> (req.get("LOOKAHEAD"))); } if (req.exist("LOOKBACK")) { outputs[output_id].lookBack = max(outputs[output_id].lookBack,dereference_cast<int> (req.get("LOOKBACK"))); } if (req.exist("INORDER")) { inOrder = true; } int outputLookAhead=0, outputLookBack=0; outputLookAhead=max(outputLookAhead, outputs[output_id].lookAhead); outputLookBack =max(outputLookBack, outputs[output_id].lookBack); if (output_id == m_imageOutID) { return; } else if (output_id == m_roiOutID) { ParameterSet myReq; myReq.add("LOOKAHEAD", ObjectRef(Int::alloc(inputsCache[m_roiInID].lookAhead+outputLookAhead))); myReq.add("LOOKBACK", ObjectRef(Int::alloc(inputsCache[m_roiInID].lookBack+outputLookBack))); inputs[m_roiInID].node->request(inputs[m_roiInID].outputID,myReq); } else if (output_id == m_ppCompletedOutID) { ParameterSet myReq; myReq.add("LOOKAHEAD", ObjectRef(Int::alloc(inputsCache[m_imageInID].lookAhead+outputLookAhead))); myReq.add("LOOKBACK", ObjectRef(Int::alloc(inputsCache[m_imageInID].lookBack+outputLookBack))); inputs[m_imageInID].node->request(inputs[m_imageInID].outputID, myReq); } else { throw new GeneralException ("DrawPFParticle::request : unknown output ID.",__FILE__,__LINE__); } } void DrawPFParticle::calculate(int output_id, int count, Buffer &out) { try { if (output_id == m_roiOutID) { // Get current particle to convert ObjectRef roiObjRef = getInput(m_roiInID, count); if (!roiObjRef->isNil()) { RCPtr<VisualROI> roiRef = RCPtr<VisualROI>(roiObjRef); // Draw particle roiRef->DrawROI(m_width, m_height, m_numChannels, m_curImage->get_data(), m_color); } // Output the input ROI (*outputs[m_roiOutID].buffer)[count] = roiObjRef; } else if (output_id == m_ppCompletedOutID) { // Get a new copy of current frame RCPtr<Image> imageRef = getInput(m_imageInID, count); // Verify input image sanity if (imageRef->get_width() != m_width || imageRef->get_height() != m_height || imageRef->get_pixelsize() != m_numChannels) { throw new GeneralException ("DrawPFParticle::calculate : image parameters do not correspond to given input.",__FILE__,__LINE__); } // Copy input image memcpy(m_curImage->get_data(), imageRef->get_data(), m_numBytesInFrame); (*outputs[m_ppCompletedOutID].buffer)[count] = ObjectRef(Int::alloc(1)); } else if (output_id == m_imageOutID) { // Allocate output image Image *imageOut = Image::alloc(m_width, m_height, m_numChannels); // Copy image memcpy(imageOut->get_data(), m_curImage->get_data(), m_numBytesInFrame); (*outputs[m_imageOutID].buffer)[count] = ObjectRef(imageOut); } } catch (BaseException *e) { throw e->add(new GeneralException("Exception in DrawPFParticle::calculate:",__FILE__,__LINE__)); } } private: // Input IDs int m_imageInID; int m_roiInID; // Output IDs int m_imageOutID; int m_ppCompletedOutID; int m_roiOutID; // Parameters int m_width; int m_height; int m_numChannels; int m_numPixels; int m_numBytesInFrame; unsigned char *m_color; RCPtr<Image> m_curImage; }; } #endif --- NEW FILE: PFState2VisualROI.cc --- /* Copyright (C) 2005 Pierre Moisan (Pie...@US...) This library is free software; you can redistribute it and/or modify it under the terms of the GNU Lesser General Public License as published by the Free Software Foundation; either version 2.1 of the License, or (at your option) any later version. This library is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public License along with this library; if not, write to the Free Software Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #ifndef _PFSTATE2VISUALROI_CC_ #define _PFSTATE2VISUALROI_CC_ #include "BufferedNode.h" #include "VisualROI.h" #include "PFGenericParticle.h" using namespace std; using namespace FD; namespace RobotFlow { class PFState2VisualROI; DECLARE_NODE(PFState2VisualROI) /*Node * * @name PFState2VisualROI * @category RobotFlow:Vision:Tracking * @description Conversion from a particle filter sample (dynamic state) to a VisualROI object. * * @parameter_name USE_SCALE * @parameter_type bool * @parameter_value true * @parameter_description Flag indicating to use the ROI scale (width and height) in the particle's state. * * @parameter_name USE_ROTATION_ANGLE * @parameter_type bool * @parameter_value false * @parameter_description Flag indicating to use the ROI rotation angle in the particle's state. * * @input_name IN_PARTICLE * @input_type PFGenericParticle * @input_description Current ROI to convert. * * @output_name OUT_ROI * @output_type VisualROI * @output_description Resulting particle with appropriate state. * END*/ class PFState2VisualROI : public BufferedNode { public: PFState2VisualROI::PFState2VisualROI(string nodeName, ParameterSet params) : BufferedNode(nodeName, params) { m_particleInID = addInput("IN_PARTICLE"); m_roiOutID = addOutput("OUT_ROI"); m_useScale = dereference_cast<bool>(parameters.get("USE_SCALE")); m_useAngle = dereference_cast<bool>(parameters.get("USE_ROTATION_ANGLE")); // First 2 elements of state is center position x,y m_stateSize = 2; if (m_useScale) { // Add scale state which is described by the half-width and half-height of ROI m_stateSize += 2; } if (m_useAngle) { // Add rotation angle state m_stateSize += 1; } // Allocate ROI m_curROI = RCPtr<VisualROI>(new VisualROI()); m_curROI->SetType(e_VISUALROI_elliptical); } PFState2VisualROI::~PFState2VisualROI() { } void PFState2VisualROI::calculate(int output_id, int count, Buffer &out) { try { // Get current particle to convert ObjectRef particleObjRef = getInput(m_particleInID, count); if (!particleObjRef->isNil()) { RCPtr<PFGenericParticle> particleRef = RCPtr<PFGenericParticle>(particleObjRef); // Set ROI according to particle state const float *p_state = particleRef->GetCstState(); // Set position m_curROI->SetXCen((int)(*p_state++)); m_curROI->SetYCen((int)(*p_state++)); if (m_useScale) { // Set scale m_curROI->SetHSX((int)(*p_state++)); m_curROI->SetHSY((int)(*p_state++)); } if (m_useAngle) { // Set rotation angle state m_curROI->SetAngle((int)(*p_state++)); } // Output particle (*outputs[m_roiOutID].buffer)[count] = ObjectRef(m_curROI); } else { throw new GeneralException ("PFState2VisualROI::calculate : invalid (nilObject) IN_PARTICLE input.",__FILE__,__LINE__); } } catch (BaseException *e) { throw e->add(new GeneralException("Exception in PFState2VisualROI::calculate:",__FILE__,__LINE__)); } } private: // Input IDs int m_particleInID; // Output IDs int m_roiOutID; // Parameters bool m_useScale; bool m_useAngle; unsigned int m_stateSize; RCPtr<VisualROI> m_curROI; }; } #endif |
|
From: Pierre M. <sid...@us...> - 2005-05-26 19:45:22
|
Update of /cvsroot/robotflow/RobotFlow/Vision/src In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv6687 Modified Files: VisualROI.cc VisualTargetManager.cc Log Message: Now using RCPtr to produce reference output instead of copying member data. Index: VisualTargetManager.cc =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Vision/src/VisualTargetManager.cc,v retrieving revision 1.4 retrieving revision 1.5 diff -C2 -d -r1.4 -r1.5 *** VisualTargetManager.cc 12 Apr 2005 19:58:10 -0000 1.4 --- VisualTargetManager.cc 26 May 2005 19:45:09 -0000 1.5 *************** *** 125,129 **** // VisualTargetManager::VisualTargetManager() - : m_target(NULL) { --- 125,128 ---- *************** *** 134,139 **** // VisualTargetManager::VisualTargetManager(std::string nodeName, ParameterSet params) ! : BufferedNode(nodeName, params), ! m_target(NULL) { m_imageInID = addInput("IMAGE_IN"); --- 133,137 ---- // VisualTargetManager::VisualTargetManager(std::string nodeName, ParameterSet params) ! : BufferedNode(nodeName, params) { m_imageInID = addInput("IMAGE_IN"); *************** *** 171,176 **** m_curImage = cvCreateImage(imgSize, IPL_DEPTH_8U, m_numChannels); ! m_target = new VisualTarget<double>(); ! m_refTarget = RCPtr<VisualTarget<double> >(m_target); m_roiColor[0] = 255; --- 169,173 ---- m_curImage = cvCreateImage(imgSize, IPL_DEPTH_8U, m_numChannels); ! m_refTarget = RCPtr<VisualTarget<double> >(new VisualTarget<double>()); m_roiColor[0] = 255; *************** *** 181,185 **** VisualTargetManager::~VisualTargetManager() { - delete m_target; cvReleaseImage(&m_curImage); } --- 178,181 ---- *************** *** 261,265 **** try { if (output_id == m_imageOutID) { - fflush(stdout); // Get current image RCPtr<Image> imageRef = getInput(m_imageInID, count); --- 257,260 ---- *************** *** 274,280 **** memcpy(m_curImage->imageData, imageRef->get_data(), m_numBytesInFrame); ! if (m_target->IsValid()) { // Draw current target's ROI in current image ! m_target->GetCstROI()->DrawROI(m_curImage, (const unsigned char *)m_roiColor); // Copy current image for output --- 269,275 ---- memcpy(m_curImage->imageData, imageRef->get_data(), m_numBytesInFrame); ! if (m_refTarget->IsValid()) { // Draw current target's ROI in current image ! m_refTarget->GetCstROI()->DrawROI(m_curImage, (const unsigned char *)m_roiColor); // Copy current image for output *************** *** 300,311 **** } else if (output_id == m_roiOutID) { ! if (m_target->IsValid()) { // Output directly the reference since it should // only be used as a constant reference ! //VisualROI *outROI = new VisualROI(*(m_target->GetCstROI())); ! //(*outputs[m_roiOutID].buffer)[count] = ObjectRef(outROI); ! //(*outputs[m_roiOutID].buffer)[count] = ObjectRef(RCPtr<VisualROI>(m_target->GetROI())); ! static RCPtr<VisualROI> refROI = RCPtr<VisualROI>(m_target->GetROI()); ! (*outputs[m_roiOutID].buffer)[count] = refROI; } else { --- 295,302 ---- } else if (output_id == m_roiOutID) { ! if (m_refTarget->IsValid()) { // Output directly the reference since it should // only be used as a constant reference ! (*outputs[m_roiOutID].buffer)[count] = m_refTarget->GetROIRCPtr(); } else { *************** *** 315,324 **** } else if (output_id == m_targetOutID) { ! if (m_target->IsValid()) { // Output directly the reference since it should // only be used as a constant reference - //VisualTarget<double> *outTarget = new VisualTarget<double>(*m_target); - //(*outputs[m_targetOutID].buffer)[count] = ObjectRef(outTarget); - static RCPtr<VisualTarget<double> > refTarget = RCPtr<VisualTarget<double> >(m_target); (*outputs[m_targetOutID].buffer)[count] = m_refTarget; } --- 306,312 ---- } else if (output_id == m_targetOutID) { ! if (m_refTarget->IsValid()) { // Output directly the reference since it should // only be used as a constant reference (*outputs[m_targetOutID].buffer)[count] = m_refTarget; } *************** *** 329,339 **** } else if (output_id == m_targetProbOutID) { ! if (m_target->GetCurrentAge() < -10) { // Target is too old to track // Invalidate it ! m_target->SetValidity(false); } ! if (!m_target->IsValid()) { // Try to initialize a new target // Get detected ROI --- 317,327 ---- } else if (output_id == m_targetProbOutID) { ! if (m_refTarget->GetCurrentAge() < -10) { // Target is too old to track // Invalidate it ! m_refTarget->SetValidity(false); } ! if (!m_refTarget->IsValid()) { // Try to initialize a new target // Get detected ROI *************** *** 343,348 **** if (!roiRef->isNil()) { // Initialize target at current ROI ! m_target->SetROI(&(object_cast<VisualROI>(roiRef))); ! m_target->SetValidity(true); if (!m_ppCompleted) { --- 331,336 ---- if (!roiRef->isNil()) { // Initialize target at current ROI ! m_refTarget->SetROI(&(object_cast<VisualROI>(roiRef))); ! m_refTarget->SetValidity(true); if (!m_ppCompleted) { *************** *** 354,369 **** // Get features for current ROI and use them // as the target feature model ! ObjectRef featVecRef = getInput(m_featVecInID, count); ! if (featVecRef->isNil()) { throw new GeneralException ("VisualTargetManager::calculate : cannot initialize target with null input FEATURES_VECTOR.",__FILE__,__LINE__); } ! ! m_target->SetDescriptorsVec(&(object_cast<Vector<VisualFeatureDesc<double> *> >(featVecRef))); ! m_target->InitCueWeights(); ! m_target->InitAges(); } } ! if (m_target->IsValid()) { double sim = 0.0; bool targetMatch = false; --- 342,357 ---- // Get features for current ROI and use them // as the target feature model ! ObjectRef featVecObjRef = getInput(m_featVecInID, count); ! if (featVecObjRef->isNil()) { throw new GeneralException ("VisualTargetManager::calculate : cannot initialize target with null input FEATURES_VECTOR.",__FILE__,__LINE__); } ! RCPtr<Vector<VisualFeatureDesc<double> *> > featVecRef = featVecObjRef; ! m_refTarget->SetDescriptorsVec(&(*featVecRef)); ! m_refTarget->InitCueWeights(); ! m_refTarget->InitAges(); } } ! if (m_refTarget->IsValid()) { double sim = 0.0; bool targetMatch = false; *************** *** 382,386 **** if (!targetRef->isNil()) { ! *m_target = object_cast<VisualTarget<double> >(targetRef); // Get features to evalute the likelihood of the current --- 370,374 ---- if (!targetRef->isNil()) { ! *m_refTarget = object_cast<VisualTarget<double> >(targetRef); // Get features to evalute the likelihood of the current *************** *** 391,395 **** } ! sim = m_target->SimilarityWCueAdapt( &(object_cast<Vector<VisualFeatureDesc<double> *> >(featVecRef)), m_cueAdaptRate); --- 379,383 ---- } ! sim = m_refTarget->SimilarityWCueAdapt( &(object_cast<Vector<VisualFeatureDesc<double> *> >(featVecRef)), m_cueAdaptRate); *************** *** 397,403 **** targetMatch = (sim >= m_targetMatchThres); ! m_target->AgeTarget(targetMatch); if (sim > m_targetAdaptThres) { ! m_target->Adapt(&(object_cast<Vector<VisualFeatureDesc<double> *> >(featVecRef)), m_targetAdaptRate); } --- 385,391 ---- targetMatch = (sim >= m_targetMatchThres); ! m_refTarget->AgeTarget(targetMatch); if (sim > m_targetAdaptThres) { ! m_refTarget->Adapt(&(object_cast<Vector<VisualFeatureDesc<double> *> >(featVecRef)), m_targetAdaptRate); } *************** *** 419,424 **** } else if (output_id == m_targetDXOutID) { ! if (m_target->IsValid()) { ! float deltaX = (float)(m_target->GetCstROI()->GetXCen()) - m_imgXCen; (*outputs[m_targetDXOutID].buffer)[count] = ObjectRef(Float::alloc(deltaX)); } --- 407,412 ---- } else if (output_id == m_targetDXOutID) { ! if (m_refTarget->IsValid()) { ! float deltaX = (float)(m_refTarget->GetCstROI()->GetXCen()) - m_imgXCen; (*outputs[m_targetDXOutID].buffer)[count] = ObjectRef(Float::alloc(deltaX)); } *************** *** 428,433 **** } else if (output_id == m_targetDYOutID) { ! if (m_target->IsValid()) { ! float deltaY = m_imgYCen - (float)(m_target->GetCstROI()->GetYCen()); (*outputs[m_targetDYOutID].buffer)[count] = ObjectRef(Float::alloc(deltaY)); } --- 416,421 ---- } else if (output_id == m_targetDYOutID) { ! if (m_refTarget->IsValid()) { ! float deltaY = m_imgYCen - (float)(m_refTarget->GetCstROI()->GetYCen()); (*outputs[m_targetDYOutID].buffer)[count] = ObjectRef(Float::alloc(deltaY)); } Index: VisualROI.cc =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Vision/src/VisualROI.cc,v retrieving revision 1.4 retrieving revision 1.5 diff -C2 -d -r1.4 -r1.5 *** VisualROI.cc 12 Apr 2005 19:58:10 -0000 1.4 --- VisualROI.cc 26 May 2005 19:45:09 -0000 1.5 *************** *** 163,166 **** --- 163,226 ---- } + void VisualROI::DrawROI(int i_width, int i_height, int i_numChannels, + unsigned char *io_pixels, const unsigned char *i_color) const + { + if (!io_pixels) { + throw new GeneralException ("VisualROI::DrawROI : invalid pixels reference.",__FILE__,__LINE__); + } + + if (!i_color) { + throw new GeneralException ("VisualROI::DrawROI : invalid color reference.",__FILE__,__LINE__); + } + + if (!m_perim) { + throw new GeneralException ("VisualROI::DrawROI : cannot draw ROI with uninitialized region data.",__FILE__,__LINE__); + } + + unsigned char *p_pixels = io_pixels; + const short *p_perim = this->GetCstPerim(); + int imgWidth = i_width; + int imgHeight = i_height; + int numChannels = i_numChannels; + int i, c, x, y; + short deltaX, deltaY; + bool broken = true; + + // Start at the top center of the region + x = m_xCen; + y = m_yCen; + p_pixels += numChannels*(y*imgWidth + x); + + // Overlay region of interest + for (i=m_perimLength; i>0; i--) { + deltaX = *p_perim++; + deltaY = *p_perim++; + x += deltaX; + y += deltaY; + + // Draw only if region is visible + if (y>0 && y<imgHeight && x>0 && x<imgWidth) { + if (!broken) { + // Relative position + p_pixels += numChannels*(deltaY*imgWidth + deltaX); + } + else { + // Absolute position + p_pixels = (unsigned char *)(io_pixels + numChannels*(y*imgWidth + x)); + broken = false; + } + + for (c=0; c<numChannels; c++) { + *p_pixels++ = i_color[c]; + } + + p_pixels -= numChannels; + } + else { + broken = true; + } + } + } + void VisualROI::Reset(int i_hsX, int i_hsY, int i_angle) { *************** *** 259,262 **** --- 319,327 ---- } + void VisualROI::SetType(e_VISUALROI_type i_type) + { + m_type = i_type; + } + void VisualROI::SetXCen(int i_xCen) { |
|
From: Pierre M. <sid...@us...> - 2005-05-26 19:44:32
|
Update of /cvsroot/robotflow/RobotFlow/Vision/src In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv6061 Modified Files: ColorHistExtraction.cc Log Message: Removed unused VisualROI member Index: ColorHistExtraction.cc =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Vision/src/ColorHistExtraction.cc,v retrieving revision 1.4 retrieving revision 1.5 diff -C2 -d -r1.4 -r1.5 *** ColorHistExtraction.cc 12 Apr 2005 19:58:10 -0000 1.4 --- ColorHistExtraction.cc 26 May 2005 19:44:09 -0000 1.5 *************** *** 93,97 **** m_numChannels(-1), m_init(false), - m_roi(NULL), m_colorHistogram(NULL) { --- 93,96 ---- *************** *** 108,112 **** m_numChannels(i_numChannels), m_init(false), - m_roi(NULL), m_colorHistogram(NULL) { --- 107,110 ---- *************** *** 128,132 **** : VisualFeaturesExtraction<double>(nodeName, params), m_init(false), - m_roi(NULL), m_colorHistogram(NULL) { --- 126,129 ---- *************** *** 149,153 **** ColorHistExtraction::~ColorHistExtraction() { - delete m_roi; delete m_colorHistogram; --- 146,149 ---- *************** *** 354,358 **** m_colorHistogram = new VisualHistogramDesc<double, unsigned char>(e_VISUALHIST_BhattacharyyaCoeff, true, m_numChannels, i_numBins); - m_roi = new VisualROI(); m_init = true; --- 350,353 ---- |
|
From: Pierre M. <sid...@us...> - 2005-05-26 19:43:32
|
Update of /cvsroot/robotflow/RobotFlow/Vision/include In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv5794 Modified Files: Makefile.am Log Message: Added new files for particle filter. Index: Makefile.am =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Vision/include/Makefile.am,v retrieving revision 1.9 retrieving revision 1.10 diff -C2 -d -r1.9 -r1.10 *** Makefile.am 12 Apr 2005 19:32:46 -0000 1.9 --- Makefile.am 26 May 2005 19:43:22 -0000 1.10 *************** *** 5,15 **** OPENCV_FILES = VisualFeaturesExtraction.h \ ColorHistExtraction.h \ - VisualTracker.h \ MeanShiftTracker.h \ VisualFeatureDesc.h \ VisualHistogramDesc.h \ VisualROI.h \ VisualTarget.h \ ! VisualTargetManager.h else OPENCV_FILES = --- 5,23 ---- OPENCV_FILES = VisualFeaturesExtraction.h \ ColorHistExtraction.h \ MeanShiftTracker.h \ + PFGenericParticle.h \ + PFGenericParticleFilter.h \ + PFParticle.h \ + PFParticleFilter.h \ + PFPMRandomWalk.h \ + PFPredictionModel.h \ + PFUtilityFct.h \ + PKLTScaleAdaption.h \ VisualFeatureDesc.h \ VisualHistogramDesc.h \ VisualROI.h \ VisualTarget.h \ ! VisualTargetManager.h \ ! VisualTracker.h else OPENCV_FILES = |
Update of /cvsroot/robotflow/RobotFlow/Vision/include In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv5344 Added Files: PFGenericParticle.h PFGenericParticleFilter.h PFPMRandomWalk.h PFParticle.h PFParticleFilter.h PFPredictionModel.h PFUtilityFct.h Log Message: Generic particle filter implementation for visual tracking purposes --- NEW FILE: PFParticle.h --- /* Copyright (C) 2005 Pierre Moisan (Pie...@US...) This library is free software; you can redistribute it and/or modify it under the terms of the GNU Lesser General Public License as published by the Free Software Foundation; either version 2.1 of the License, or (at your option) any later version. This library is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public License along with this library; if not, write to the Free Software Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #ifndef _PFPARTICLE_H_ #define _PFPARTICLE_H_ #include "Object.h" #include <iostream> namespace RobotFlow { typedef enum { e_PFP_Generic = 0, e_PFP_Unknown } e_PFP_type; // // This should be an abstract base class. Since many containers (like Vector) // do not allow abstract functions, each abstract function throws an exception // to avoid the direct use of the base class "abstract" routines. // class PFParticle : public FD::Object { friend std::ostream &operator<<(std::ostream &o_out, const PFParticle &i_ref) { try { i_ref.printOn(o_out); // Enable cascading return o_out; } catch (FD::BaseException *e) { throw e->add(new FD::GeneralException("Exception in PFParticle::operator<<:",__FILE__,__LINE__)); } } friend std::istream &operator>>(std::istream &i_in, PFParticle &o_ref) { try { o_ref.readFrom(i_in); // Enable cascading return i_in; } catch (FD::BaseException *e) { throw e->add(new FD::GeneralException("Exception in PFParticle::operator>>:",__FILE__,__LINE__)); } } public: PFParticle() : m_pfpType(e_PFP_Unknown) { } PFParticle(e_PFP_type i_pfpType) : m_pfpType(i_pfpType) { } PFParticle(const PFParticle &i_ref) { m_pfpType = i_ref.m_pfpType; } virtual ~PFParticle() { } virtual PFParticle & operator =(const PFParticle &i_ref) { // Avoid self assignment if (&i_ref != this) { this->m_pfpType = i_ref.m_pfpType; } return *this; } virtual PFParticle* clone() const { return new PFParticle(*this); } // Default routine to print a PFParticle object to an output stream virtual void printOn(std::ostream &out) const { throw new FD::GeneralException("Exception in PFParticle::printOn: cannot use base class routine.",__FILE__,__LINE__); } // Default routine to read a PFParticle object from an input stream virtual void readFrom(std::istream &in) { throw new FD::GeneralException("Exception in PFParticle::readFrom: cannot use base class routine.",__FILE__,__LINE__); } e_PFP_type GetType() const { return m_pfpType; } void SetType(e_PFP_type i_type) { m_pfpType = i_type; } virtual unsigned int GetStateSize() const { throw new FD::GeneralException("Exception in PFParticle::GetStateSize: cannot use base class routine.",__FILE__,__LINE__); } virtual float *GetState() { throw new FD::GeneralException("Exception in PFParticle::GetState: cannot use base class routine.",__FILE__,__LINE__); } virtual const float *GetCstState() const { throw new FD::GeneralException("Exception in PFParticle::GetCstState: cannot use base class routine.",__FILE__,__LINE__); } virtual float GetStateIdx(int i_idx) const { throw new FD::GeneralException("Exception in PFParticle::GetStateIdx: cannot use base class routine.",__FILE__,__LINE__); } virtual float GetWeight() const { throw new FD::GeneralException("Exception in PFParticle::GetWeight: cannot use base class routine.",__FILE__,__LINE__); } virtual void SetStateSize(unsigned int i_size) { throw new FD::GeneralException("Exception in PFParticle::SetStateSize: cannot use base class routine.",__FILE__,__LINE__); } virtual void SetState(const float *i_state) { throw new FD::GeneralException("Exception in PFParticle::SetState: cannot use base class routine.",__FILE__,__LINE__); } virtual void SetStateIdx(int i_idx, float i_val) { throw new FD::GeneralException("Exception in PFParticle::SetStateIdx: cannot use base class routine.",__FILE__,__LINE__); } virtual void SetWeight(float i_weight) { throw new FD::GeneralException("Exception in PFParticle::SetWeight: cannot use base class routine.",__FILE__,__LINE__); } private: e_PFP_type m_pfpType; }; } #endif --- NEW FILE: PFPMRandomWalk.h --- /* Copyright (C) 2005 Pierre Moisan (Pie...@US...) This library is free software; you can redistribute it and/or modify it under the terms of the GNU Lesser General Public License as published by the Free Software Foundation; either version 2.1 of the License, or (at your option) any later version. This library is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public License along with this library; if not, write to the Free Software Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #ifndef _PFPMRANDOMWALK_H_ #define _PFPMRANDOMWALK_H_ #include "PFPredictionModel.h" #include "PFGenericParticle.h" #include "PFUtilityFct.h" #include "Vector.h" namespace RobotFlow { // // Random walk prediction model for particle filters // class PFPMRandomWalk : public PFPredictionModel { public: PFPMRandomWalk(); PFPMRandomWalk(const FD::Vector<float> *i_variance); PFPMRandomWalk(std::string nodeName, FD::ParameterSet params); virtual ~PFPMRandomWalk(); virtual void calculate(int output_id, int count, FD::Buffer &out); virtual void Predict(PFParticle *io_sample); void Initialize(const FD::Vector<float> *i_variance); private: // BufferedNode inputs int m_varianceInID; int m_particleInID; // BufferedNode outputs int m_completedOutID; bool m_init; unsigned int m_stateSize; FD::Vector<float> *m_noiseVariance; }; } #endif --- NEW FILE: PFPredictionModel.h --- /* Copyright (C) 2005 Pierre Moisan (Pie...@US...) This library is free software; you can redistribute it and/or modify it under the terms of the GNU Lesser General Public License as published by the Free Software Foundation; either version 2.1 of the License, or (at your option) any later version. This library is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public License along with this library; if not, write to the Free Software Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #ifndef _PFPREDICTIONMODEL_H_ #define _PFPREDICTIONMODEL_H_ #include "BufferedNode.h" #include "PFParticle.h" namespace RobotFlow { typedef enum { e_PFPM_RandomWalk = 0, e_PFPM_Unknown } e_PFPM_type; // // Abstract base class for particle filter prediction models // class PFPredictionModel : public FD::BufferedNode { public: PFPredictionModel() : m_modelType(e_PFPM_Unknown) { } PFPredictionModel(e_PFPM_type i_modelType) : m_modelType(i_modelType) { } PFPredictionModel(std::string nodeName, FD::ParameterSet params) : FD::BufferedNode(nodeName, params) { } virtual ~PFPredictionModel() { } e_PFPM_type GetType() const { return m_modelType; } void SetType(e_PFPM_type i_type) { m_modelType = i_type; } virtual void calculate(int output_id, int count, FD::Buffer &out) = 0; virtual void Predict(PFParticle *io_sample) = 0; private: e_PFPM_type m_modelType; }; } #endif --- NEW FILE: PFGenericParticle.h --- /* Copyright (C) 2005 Pierre Moisan (Pie...@US...) This library is free software; you can redistribute it and/or modify it under the terms of the GNU Lesser General Public License as published by the Free Software Foundation; either version 2.1 of the License, or (at your option) any later version. This library is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public License along with this library; if not, write to the Free Software Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #ifndef _PFGENERICPARTICLE_H_ #define _PFGENERICPARTICLE_H_ #include "PFParticle.h" #include <iostream> namespace RobotFlow { class PFGenericParticle : public PFParticle { public: PFGenericParticle(); PFGenericParticle(unsigned int i_stateSize); PFGenericParticle(const PFGenericParticle &i_ref); virtual ~PFGenericParticle(); virtual PFGenericParticle & operator =(const PFGenericParticle &i_ref); virtual PFGenericParticle* clone() const; // Default routine to print a PFGenericParticle object to an output stream virtual void printOn(std::ostream &out) const; // Default routine to read a PFGenericParticle object from an input stream virtual void readFrom(std::istream &in); virtual unsigned int GetStateSize() const; virtual float *GetState(); virtual float GetStateIdx(int i_idx) const; virtual const float *GetCstState() const; virtual float GetWeight() const; virtual void SetStateSize(unsigned int i_size); virtual void SetState(const float *i_state); virtual void SetStateIdx(int i_idx, float i_val); virtual void SetWeight(float i_weight); private: int m_stateSize; float *m_state; float m_weight; }; } #endif --- NEW FILE: PFGenericParticleFilter.h --- /* Copyright (C) 2005 Pierre Moisan (Pie...@US...) This library is free software; you can redistribute it and/or modify it under the terms of the GNU Lesser General Public License as published by the Free Software Foundation; either version 2.1 of the License, or (at your option) any later version. This library is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public License along with this library; if not, write to the Free Software Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #ifndef _PFGENERICPARTICLEFILTER_H_ #define _PFGENERICPARTICLEFILTER_H_ #include "PFParticleFilter.h" #include "PFGenericParticle.h" namespace RobotFlow { // // Generic implementation of a particle filter // For more information, refer to the following publication // M. S. Arulampalam, S. Maskell, N. Gordon, T. Clapp. // "A Tutorial on Particle Filters for On-line Nonlinear/Non-Gaussian // Bayesian Tracking" (2001) // class PFGenericParticleFilter : public PFParticleFilter { public: PFGenericParticleFilter(); PFGenericParticleFilter(unsigned int i_numSamples, unsigned int i_sampleStateSize, const FD::Vector<float> *i_initVariance); PFGenericParticleFilter(std::string nodeName, FD::ParameterSet params); virtual ~PFGenericParticleFilter(); // Default routine to print a PFGenericParticleFilter object to an output stream void printOn(std::ostream &out) const { throw new FD::GeneralException("Exception in PFGenericParticleFilter::printOn: method not yet implemented.",__FILE__,__LINE__); } // Default routine to read a PFGenericParticleFilter object from an input stream void readFrom(std::istream &in) { throw new FD::GeneralException("Exception in PFGenericParticleFilter::printOn: method not yet implemented.",__FILE__,__LINE__); } virtual void request(int output_id, const FD::ParameterSet &req); void calculate(int output_id, int count, FD::Buffer &out); void Initialize(const FD::Vector<float> *i_variance); void InitSamples(); void Update(); void ComputeMeanState(); void Resample(); private: void CopyTmpSamples(); private: // Input IDs (for BufferedNode) int m_initVarianceInID; int m_refMeanStateInID; int m_predictInID; int m_likelihoodInID; // Output IDs (for BufferedNode) int m_finishedOutID; int m_particleOutID; int m_meanStateOutID; bool m_init; bool m_initPF; bool m_initSamples; bool m_finished; int m_curSampleIdx; unsigned int m_numSamples; unsigned int m_sampleStateSize; FD::RCPtr<PFGenericParticle> *m_samples; FD::RCPtr<PFGenericParticle> *m_tmpSamples; float m_likelihoodsSum; float *m_cumulWeight; FD::Vector<float> *m_initVariance; FD::RCPtr<PFGenericParticle> m_curSample; FD::RCPtr<PFGenericParticle> m_refMeanState; FD::RCPtr<PFGenericParticle> m_outMeanState; }; } #endif --- NEW FILE: PFParticleFilter.h --- /* Copyright (C) 2005 Pierre Moisan (Pie...@US...) This library is free software; you can redistribute it and/or modify it under the terms of the GNU Lesser General Public License as published by the Free Software Foundation; either version 2.1 of the License, or (at your option) any later version. This library is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public License along with this library; if not, write to the Free Software Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #ifndef _PFPARTICLEFILTER_H_ #define _PFPARTICLEFILTER_H_ #include "VisualTracker.h" #include "PFParticle.h" #include "PFUtilityFct.h" namespace RobotFlow { typedef enum { e_PF_GenericFilter = 0, e_PF_Unknown } e_PF_type; // // Abstract base class for particle filter // class PFParticleFilter : public VisualTracker { public: PFParticleFilter() : m_filterType(e_PF_Unknown) { } PFParticleFilter(e_PF_type i_modelType) : m_filterType(i_modelType) { } PFParticleFilter(std::string nodeName, FD::ParameterSet params) : VisualTracker(nodeName, params) { } virtual ~PFParticleFilter() { } // Default routine to print a PFParticleFilter object to an output stream void printOn(std::ostream &out) const = 0; // Default routine to read a PFParticleFilter object from an input stream void readFrom(std::istream &in) = 0; void calculate(int output_id, int count, FD::Buffer &out) = 0; e_PF_type GetType() const { return m_filterType; } void SetType(e_PF_type i_type) { m_filterType = i_type; } private: e_PF_type m_filterType; }; } #endif --- NEW FILE: PFUtilityFct.h --- #ifndef _PFUTILITYFCT_H_ #define _PFUTILITYFCT_H_ #include <stdlib.h> #include <math.h> namespace RobotFlow { // // Utility functions mainly for random numbers generation // Copyright (C) 2004 Jean-Marc Valin // #define PFUTIL_FLOGLOOKUP2SIZE 256 #define PFUTIL_FLOGLOOKUP2SHIFT 15 union PFUTIL_FloatManip { float f; unsigned int i; }; void PFUTIL_build_flog_table(); float PFUTIL_flog(const float in); float PFUTIL_randn(); float PFUTIL_ran(); int PFUTIL_find_range(float i_x, const float *i_cumul, int i_cumulSize); } #endif |
|
From: Pierre M. <sid...@us...> - 2005-05-26 19:39:18
|
Update of /cvsroot/robotflow/RobotFlow/Vision/include In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv4960 Modified Files: ColorHistExtraction.h Log Message: Removed unused VisualROI member Index: ColorHistExtraction.h =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Vision/include/ColorHistExtraction.h,v retrieving revision 1.3 retrieving revision 1.4 diff -C2 -d -r1.3 -r1.4 *** ColorHistExtraction.h 12 Apr 2005 20:09:04 -0000 1.3 --- ColorHistExtraction.h 26 May 2005 19:39:03 -0000 1.4 *************** *** 125,131 **** // Initialization flag bool m_init; - - // Current region of interest - VisualROI *m_roi; // Histogram descriptor for region of interest --- 125,128 ---- |
|
From: Pierre M. <sid...@us...> - 2005-05-26 19:37:40
|
Update of /cvsroot/robotflow/RobotFlow/Vision/include In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv4696 Modified Files: VisualHistogramDesc.h VisualROI.h VisualTarget.h VisualTargetManager.h Log Message: Now using RCPtr to produce reference output instead of copying member data. Index: VisualROI.h =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Vision/include/VisualROI.h,v retrieving revision 1.3 retrieving revision 1.4 diff -C2 -d -r1.3 -r1.4 *** VisualROI.h 12 Apr 2005 20:09:06 -0000 1.3 --- VisualROI.h 26 May 2005 19:37:29 -0000 1.4 *************** *** 54,57 **** --- 54,60 ---- void DrawROI(IplImage *io_frame, const unsigned char *i_color) const; + + void DrawROI(int i_width, int i_height, int i_numChannels, + unsigned char *io_pixels, const unsigned char *i_color) const; void Reset(int i_hsX, int i_hsY, int i_angle); *************** *** 85,88 **** --- 88,93 ---- const unsigned char *GetCstMask() const; + void SetType(e_VISUALROI_type i_type); + void SetXCen(int i_xCen); Index: VisualTargetManager.h =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Vision/include/VisualTargetManager.h,v retrieving revision 1.3 retrieving revision 1.4 diff -C2 -d -r1.3 -r1.4 *** VisualTargetManager.h 12 Apr 2005 20:09:06 -0000 1.3 --- VisualTargetManager.h 26 May 2005 19:37:29 -0000 1.4 *************** *** 90,94 **** bool m_ppCompleted; - VisualTarget<double> *m_target; FD::RCPtr<VisualTarget<double> > m_refTarget; //RCPtr<VisualROI> m_refROI; --- 90,93 ---- Index: VisualHistogramDesc.h =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Vision/include/VisualHistogramDesc.h,v retrieving revision 1.3 retrieving revision 1.4 diff -C2 -d -r1.3 -r1.4 *** VisualHistogramDesc.h 12 Apr 2005 20:09:06 -0000 1.3 --- VisualHistogramDesc.h 26 May 2005 19:37:29 -0000 1.4 *************** *** 94,97 **** --- 94,100 ---- VisualHistogramDesc(const VisualHistogramDesc<BinType,FeatType> &i_ref) + : m_numBins(NULL), + m_binsWidth(NULL), + m_bins(NULL) { this->SetType(i_ref.GetType()); Index: VisualTarget.h =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Vision/include/VisualTarget.h,v retrieving revision 1.3 retrieving revision 1.4 diff -C2 -d -r1.3 -r1.4 *** VisualTarget.h 12 Apr 2005 20:09:06 -0000 1.3 --- VisualTarget.h 26 May 2005 19:37:29 -0000 1.4 *************** *** 35,42 **** VisualTarget() : m_valid(false), m_id(0), m_activeAge(0), m_passiveAge(0), m_curDescIdx(0), ! m_roi(NULL), m_numDesc(-1), m_targetDesc(NULL), m_cueWeights(NULL), m_tmpCueProb(NULL) { ! } --- 35,42 ---- VisualTarget() : m_valid(false), m_id(0), m_activeAge(0), m_passiveAge(0), m_curDescIdx(0), ! m_refROI(NULL), m_numDesc(-1), m_targetDesc(NULL), m_cueWeights(NULL), m_tmpCueProb(NULL) { ! } *************** *** 44,51 **** FD::Vector<VisualFeatureDesc<FeatBaseType> *> *i_targetDesc) : m_valid(true), m_id(i_id), m_activeAge(0), m_passiveAge(0), m_curDescIdx(0), ! m_roi(NULL), m_targetDesc(NULL), m_cueWeights(NULL), ! m_tmpCueProb(NULL) { ! m_roi = new VisualROI(*i_roi); m_numDesc = i_targetDesc->size(); m_targetDesc = new FD::Vector<VisualFeatureDesc<FeatBaseType> *>(m_numDesc); --- 44,50 ---- FD::Vector<VisualFeatureDesc<FeatBaseType> *> *i_targetDesc) : m_valid(true), m_id(i_id), m_activeAge(0), m_passiveAge(0), m_curDescIdx(0), ! m_targetDesc(NULL), m_cueWeights(NULL), m_tmpCueProb(NULL) { ! m_refROI = FD::RCPtr<VisualROI>(new VisualROI(*i_roi)); m_numDesc = i_targetDesc->size(); m_targetDesc = new FD::Vector<VisualFeatureDesc<FeatBaseType> *>(m_numDesc); *************** *** 61,65 **** VisualTarget::VisualTarget(const VisualTarget& i_ref) :m_id(-1), m_activeAge(-1), m_passiveAge(-1), m_curDescIdx(0), ! m_numDesc(-1), m_roi(NULL), m_targetDesc(NULL), m_cueWeights(NULL), m_tmpCueProb(NULL) { --- 60,64 ---- VisualTarget::VisualTarget(const VisualTarget& i_ref) :m_id(-1), m_activeAge(-1), m_passiveAge(-1), m_curDescIdx(0), ! m_numDesc(-1), m_targetDesc(NULL), m_cueWeights(NULL), m_tmpCueProb(NULL) { *************** *** 70,83 **** m_passiveAge = i_ref.m_passiveAge; m_curDescIdx = i_ref.m_curDescIdx; ! if (i_ref.m_roi) { ! if (this->m_roi) { ! *(this->m_roi) = *(i_ref.m_roi); } else { ! this->m_roi = new VisualROI(*(i_ref.m_roi)); } } else { ! this->m_roi = NULL; } if (i_ref.m_targetDesc) { --- 69,82 ---- m_passiveAge = i_ref.m_passiveAge; m_curDescIdx = i_ref.m_curDescIdx; ! if (!i_ref.m_refROI.isNil()) { ! if (!this->m_refROI.isNil()) { ! *(this->m_refROI) = *(i_ref.m_refROI); } else { ! this->m_refROI = FD::RCPtr<VisualROI>(new VisualROI(*(i_ref.m_refROI))); } } else { ! std::cout << "VisualTarget::VisualTarget: reference to copy has NULL ROI" << std::endl; } if (i_ref.m_targetDesc) { *************** *** 100,111 **** } ! virtual ~VisualTarget() { - delete m_roi; - for (int i=0; i<m_numDesc; i++) { delete (*m_targetDesc)[i]; } - delete m_targetDesc; delete [] m_cueWeights; --- 99,107 ---- } ! ~VisualTarget() { for (int i=0; i<m_numDesc; i++) { delete (*m_targetDesc)[i]; } delete m_targetDesc; delete [] m_cueWeights; *************** *** 113,117 **** } ! VisualTarget<FeatBaseType> & operator =(const VisualTarget<FeatBaseType> &i_ref) { try { --- 109,113 ---- } ! VisualTarget<FeatBaseType> & operator =(VisualTarget<FeatBaseType> &i_ref) { try { *************** *** 123,136 **** this->m_passiveAge = i_ref.m_passiveAge; this->m_curDescIdx = i_ref.m_curDescIdx; ! if (i_ref.m_roi) { ! if (this->m_roi) { ! *(this->m_roi) = *(i_ref.m_roi); } else { ! this->m_roi = new VisualROI(*(i_ref.m_roi)); } } else { ! this->m_roi = NULL; } if (i_ref.m_targetDesc) { --- 119,132 ---- this->m_passiveAge = i_ref.m_passiveAge; this->m_curDescIdx = i_ref.m_curDescIdx; ! if (!i_ref.m_refROI.isNil()) { ! if (!this->m_refROI.isNil()) { ! *(this->m_refROI) = *(i_ref.m_refROI); } else { ! this->m_refROI = FD::RCPtr<VisualROI>(new VisualROI(*(i_ref.m_refROI))); } } else { ! std::cout << "VisualTarget::VisualTarget: reference to copy has NULL ROI" << std::endl; } if (i_ref.m_targetDesc) { *************** *** 309,318 **** VisualROI *GetROI() { ! return m_roi; } const VisualROI *GetCstROI() const { ! return (const VisualROI *)m_roi; } --- 305,319 ---- VisualROI *GetROI() { ! return &(*m_refROI); } const VisualROI *GetCstROI() const { ! return (const VisualROI *)(&(*m_refROI)); ! } ! ! FD::RCPtr<VisualROI> GetROIRCPtr() ! { ! return m_refROI; } *************** *** 402,410 **** void SetROI(VisualROI *i_roi) { ! if (m_roi) { ! *m_roi = *i_roi; } else { ! m_roi = new VisualROI(*i_roi); } } --- 403,411 ---- void SetROI(VisualROI *i_roi) { ! if (!m_refROI.isNil()) { ! *m_refROI = *i_roi; } else { ! m_refROI = FD::RCPtr<VisualROI>(new VisualROI(*i_roi)); } } *************** *** 445,452 **** { // Reset the number of descriptors ! SetNumDescriptors(i_descVec->size()); ! ! for (int i=0; i<m_numDesc; i++) { ! (*m_targetDesc)[i] = (*i_descVec)[i]->clone(); } } --- 446,457 ---- { // Reset the number of descriptors ! if (i_descVec->size() > 0 ) { ! this->SetNumDescriptors(i_descVec->size()); ! for (int i=0; i<m_numDesc; i++) { ! (*m_targetDesc)[i] = (*i_descVec)[i]->clone(); ! } ! } ! else { ! throw new FD::GeneralException("Exception in VisualTarget::SetDescriptorsVec: input feature vector must have at least one element.",__FILE__,__LINE__); } } *************** *** 491,495 **** int m_activeAge; int m_passiveAge; ! VisualROI *m_roi; int m_numDesc; int m_curDescIdx; --- 496,500 ---- int m_activeAge; int m_passiveAge; ! FD::RCPtr<VisualROI> m_refROI; int m_numDesc; int m_curDescIdx; |
|
From: Dominic L. <ma...@us...> - 2005-05-17 18:18:38
|
Update of /cvsroot/robotflow/RobotFlow/Vision/src In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv18651 Modified Files: CvBiModalTest.cc Log Message: looking at stddev Index: CvBiModalTest.cc =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Vision/src/CvBiModalTest.cc,v retrieving revision 1.2 retrieving revision 1.3 diff -C2 -d -r1.2 -r1.3 *** CvBiModalTest.cc 17 May 2005 14:54:29 -0000 1.2 --- CvBiModalTest.cc 17 May 2005 18:18:28 -0000 1.3 *************** *** 6,16 **** #include "Image.h" #include "highgui.h" ! ! #include <cstdio> ! #include <cstdlib> ! #include <image.h> ! #include <misc.h> ! #include <pnmfile.h> ! #include "segment-image.h" #include <math.h> #include <sys/timeb.h> --- 6,11 ---- #include "Image.h" #include "highgui.h" ! #include <stdio.h> ! #include <stdlib.h> #include <math.h> #include <sys/timeb.h> *************** *** 247,253 **** } ! if (mean_dist > min_dist /*&& ! std1sum < std_max && ! std2sum < std_max */ ) { bimodal = true; } --- 242,255 ---- } ! ! ! //TEST CONDITIONS ! if (mean_dist > min_dist && ! (stddev1.val[0] < mean_dist / std_max) && ! (stddev1.val[1] < mean_dist / std_max) && ! (stddev1.val[2] < mean_dist / std_max) && ! (stddev2.val[0] < mean_dist / std_max) && ! (stddev2.val[1] < mean_dist / std_max) && ! (stddev2.val[2] < mean_dist / std_max)) { bimodal = true; } *************** *** 340,343 **** --- 342,346 ---- cvReleaseMemStorage(&storage); + ftime(&t2); |
|
From: Dominic L. <ma...@us...> - 2005-05-17 14:54:40
|
Update of /cvsroot/robotflow/RobotFlow/Vision/src In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv1349 Modified Files: CvBiModalTest.cc Log Message: Extracting symbols Index: CvBiModalTest.cc =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Vision/src/CvBiModalTest.cc,v retrieving revision 1.1 retrieving revision 1.2 diff -C2 -d -r1.1 -r1.2 *** CvBiModalTest.cc 17 May 2005 14:06:51 -0000 1.1 --- CvBiModalTest.cc 17 May 2005 14:54:29 -0000 1.2 *************** *** 1,4 **** ! #ifndef _CVFINDCONTOURS_CC_ ! #define _CVFINDCONTOURS_CC_ #include "BufferedNode.h" --- 1,4 ---- ! #ifndef _CVBIMODALTEST_CC_ ! #define _CVBIMODALTEST_CC_ #include "BufferedNode.h" *************** *** 52,55 **** --- 52,59 ---- * @output_description Image with contours drawed. * + * @output_name BINARIZED_IMG + * @output_type Image + * @output_description binarized image + * END*/ *************** *** 61,64 **** --- 65,76 ---- IplImage *m_mask; IplImage *m_edges; + IplImage *m_gray; + IplImage *m_binarized; + + int m_imageInID; + int m_imageOutID; + int m_binImageOutID; + double m_minDist; + double m_maxStd; public: *************** *** 69,72 **** --- 81,85 ---- m_imageInID = addInput("IN_IMAGE"); m_imageOutID = addOutput("CONTOUR_IMG"); + m_binImageOutID = addOutput("BINARIZED_IMG"); m_minDist = dereference_cast<float>(parameters.get("MIN_DIST")); *************** *** 79,83 **** m_mask = cvCreateImage( cvSize(320,240), IPL_DEPTH_8U, 1 ); m_edges = cvCreateImage( cvSize(320,240), IPL_DEPTH_8U, 1 ); ! } --- 92,97 ---- m_mask = cvCreateImage( cvSize(320,240), IPL_DEPTH_8U, 1 ); m_edges = cvCreateImage( cvSize(320,240), IPL_DEPTH_8U, 1 ); ! m_gray = cvCreateImage( cvSize(320,240), IPL_DEPTH_8U, 1 ); ! m_binarized = cvCreateImage( cvSize(320,240), IPL_DEPTH_8U, 1 ); } *************** *** 87,94 **** cvReleaseImage(&m_mask); cvReleaseImage(&m_edges); ! } ! bool processRect(CvRect rect, IplImage *src, IplImage *dest, double std_max, double min_dist) { //Set Region of interest --- 101,117 ---- cvReleaseImage(&m_mask); cvReleaseImage(&m_edges); ! cvReleaseImage(&m_gray); ! cvReleaseImage(&m_binarized); } ! void binarize(CvRect rect, IplImage *src, IplImage *dest, double threshold) { ! cvSetImageROI(src,rect); ! cvSetImageROI(dest,rect); ! cvThreshold(src,dest,threshold,255,CV_THRESH_BINARY); ! ! } ! ! ! bool processRect(CvRect rect, IplImage *src, IplImage *dest, double std_max, double min_dist, double *threshold = 0) { //Set Region of interest *************** *** 156,160 **** if (dist1 <= dist2) { //DIST1 SMALLER, SET MASK PROPERLY FOR MEAN FUNCTION TO BE CALCULATED PROPERLY ! *maskPtr = 1; nb[0]++; } else { --- 179,184 ---- if (dist1 <= dist2) { //DIST1 SMALLER, SET MASK PROPERLY FOR MEAN FUNCTION TO BE CALCULATED PROPERLY ! *maskPtr = 1; ! nb[0]++; } else { *************** *** 218,222 **** stddev2.val[2] * stddev2.val[2]); ! if (mean_dist > min_dist /*&& --- 242,249 ---- stddev2.val[2] * stddev2.val[2]); ! //BINARIZATION THRESHOLD CALCULATION (IF REQUIRED) ! if (threshold) { ! *threshold = (mean1.val[0] + mean2.val[0] + mean1.val[1] + mean2.val[1] + mean1.val[2] + mean2.val[2]) / 6.0; ! } if (mean_dist > min_dist /*&& *************** *** 238,246 **** //copy source image memcpy(m_src->imageData, imageRef->get_data(), imageRef->get_size()); ! ! ! ! ! //reset ROI cvResetImageROI(m_src); --- 265,269 ---- //copy source image memcpy(m_src->imageData, imageRef->get_data(), imageRef->get_size()); ! //reset ROI cvResetImageROI(m_src); *************** *** 248,254 **** --- 271,282 ---- cvResetImageROI(m_mask); cvResetImageROI(m_edges); + cvResetImageROI(m_gray); + cvResetImageROI(m_binarized); cvCopy(m_src,m_dest,NULL); + //clear binarized image + memset(m_binarized->imageData,0xFF,320*240); + struct timeb t1, t2; ftime(&t1); *************** *** 277,283 **** */ //EDGE MODE ! cvCvtColor(m_src, m_edges, CV_BGR2GRAY); ! cvSmooth(m_edges,m_edges,CV_GAUSSIAN,3); ! cvCanny(m_edges, m_edges, 128, 255, 3); CvMemStorage* storage = cvCreateMemStorage(0); --- 305,311 ---- */ //EDGE MODE ! cvCvtColor(m_src, m_gray, CV_BGR2GRAY); ! cvSmooth(m_gray,m_gray,CV_GAUSSIAN,3); ! cvCanny(m_gray, m_edges, 128, 255, 3); CvMemStorage* storage = cvCreateMemStorage(0); *************** *** 290,302 **** CvRect rect = cvBoundingRect(contour,0); ! ! if (processRect(rect,m_src,m_dest,m_maxStd,m_minDist)) { ! CvScalar color = CV_RGB(255,0,0); ! CvPoint p1,p2; ! p1.x = rect.x; ! p1.y = rect.y; ! p2.x = rect.x + rect.width; ! p2.y = rect.y + rect.height; ! cvRectangle(m_dest,p1,p2,color); } } --- 318,339 ---- CvRect rect = cvBoundingRect(contour,0); ! double threshold; ! ! if (processRect(rect,m_src,m_dest,m_maxStd,m_minDist, &threshold)) { ! ! //filter too big regions ! if (rect.width < 100 & ! rect.height < 100) { ! CvScalar color = CV_RGB(255,0,0); ! CvPoint p1,p2; ! p1.x = rect.x; ! p1.y = rect.y; ! p2.x = rect.x + rect.width; ! p2.y = rect.y + rect.height; ! cvRectangle(m_dest,p1,p2,color); ! ! //void binarize(CvRect rect, IplImage *src, IplImage *dest, double threshold) { ! binarize(rect,m_gray,m_binarized,threshold); ! } } } *************** *** 310,319 **** Image *m_outImg = Image::alloc( imageRef->get_width(), imageRef->get_height(), 3); //Copy destination image (convert from CV format) ! memcpy(m_outImg->get_data(), m_dest->imageData, m_outImg->get_size()); ! ! if( output_id == m_imageOutID ) { ! out[count] = ObjectRef(m_outImg); ! } } catch (BaseException *e) { --- 347,360 ---- Image *m_outImg = Image::alloc( imageRef->get_width(), imageRef->get_height(), 3); + Image *m_binImg = Image::alloc( imageRef->get_width(), imageRef->get_height(), 1); + //Copy destination image (convert from CV format) ! memcpy(m_outImg->get_data(), m_dest->imageData, m_outImg->get_size()); ! (*outputs[m_imageOutID].buffer)[count] = ObjectRef(m_outImg); ! ! ! memcpy(m_binImg->get_data(), m_binarized->imageData,m_binImg->get_size()); ! (*outputs[m_binImageOutID].buffer)[count] = ObjectRef(m_binImg); ! } catch (BaseException *e) { *************** *** 322,331 **** } ! private: ! int m_imageInID; ! int m_imageOutID; ! ! double m_minDist; ! double m_maxStd; }; --- 363,367 ---- } ! }; |
|
From: Dominic L. <ma...@us...> - 2005-05-17 14:07:04
|
Update of /cvsroot/robotflow/RobotFlow/Vision/src In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv23572 Added Files: CvBiModalTest.cc Log Message: working version of the bi modal color model --- NEW FILE: CvBiModalTest.cc --- #ifndef _CVFINDCONTOURS_CC_ #define _CVFINDCONTOURS_CC_ #include "BufferedNode.h" #include "cv.h" #include "Image.h" #include "highgui.h" #include <cstdio> #include <cstdlib> #include <image.h> #include <misc.h> #include <pnmfile.h> #include "segment-image.h" #include <math.h> #include <sys/timeb.h> using namespace std; using namespace FD; namespace RobotFlow { class CvBiModalTest; DECLARE_NODE(CvBiModalTest) /*Node * * @name CvBiModalTest * @category RobotFlow:Vision:OpenCV * @description Determine the contours in the input image. * * @input_name IN_IMAGE * @input_type Image * @input_description Current color frame to process. * * @parameter_name MAX_STD * @parameter_type float * @parameter_value 0 * @parameter_description Low threshold used for edge searching. * * @parameter_name MIN_DIST * @parameter_type float * @parameter_value 255 * @parameter_description High threshold used for edge searching. * * @output_name CONTOUR_IMG * @output_type Image * @output_description Image with contours drawed. * END*/ class CvBiModalTest : public BufferedNode { private: IplImage *m_src; IplImage *m_dest; IplImage *m_mask; IplImage *m_edges; public: CvBiModalTest(string nodeName, ParameterSet params) : BufferedNode(nodeName, params) { m_imageInID = addInput("IN_IMAGE"); m_imageOutID = addOutput("CONTOUR_IMG"); m_minDist = dereference_cast<float>(parameters.get("MIN_DIST")); m_maxStd = dereference_cast<float>(parameters.get("MAX_STD")); //Allocate memory m_src = cvCreateImage( cvSize(320,240), IPL_DEPTH_8U, 3 ); m_dest = cvCreateImage( cvSize(320,240), IPL_DEPTH_8U, 3 ); m_mask = cvCreateImage( cvSize(320,240), IPL_DEPTH_8U, 1 ); m_edges = cvCreateImage( cvSize(320,240), IPL_DEPTH_8U, 1 ); } ~CvBiModalTest() { cvReleaseImage(&m_src); cvReleaseImage(&m_dest); cvReleaseImage(&m_mask); cvReleaseImage(&m_edges); } bool processRect(CvRect rect, IplImage *src, IplImage *dest, double std_max, double min_dist) { //Set Region of interest cvSetImageROI(src,rect); cvSetImageROI(m_mask,rect); bool bimodal = false; int x1,y1,x2,y2; x1 = rect.x; y1 = rect.y; x2 = rect.x + rect.width; y2 = rect.y + rect.height; CvScalar mean0,mean1,mean2; CvScalar stddev0,stddev1,stddev2; //Get Mean, std dev cvAvgSdv(src,&mean0,&stddev0); //cerr<<"CV made (mean0): "<<mean0.val[0]<<" "<<mean0.val[1]<<" "<<mean0.val[2]<<endl; //cerr<<"CV made (stddev0): "<<stddev0.val[0]<<" "<<stddev0.val[1]<<" "<<stddev0.val[2]<<endl; //cerr<<"JM TEST "<<sqrt(stddev0.val[0] * stddev0.val[0] + // stddev0.val[1] * stddev0.val[1] + // stddev0.val[2] * stddev0.val[2])<<endl; //LOOK FOR STDDEV1 IF WE SHOULD CONTINUE... //BRIGHTER mean1.val[0] = min(mean0.val[0] + 1,255.0); mean1.val[1] = min(mean0.val[1] + 1,255.0); mean1.val[2] = min(mean0.val[2] + 1,255.0); //DARKER mean2.val[0] = max(mean0.val[0] - 1,0.0); mean2.val[1] = max(mean0.val[1] - 1,0.0); mean2.val[2] = max(mean0.val[2] - 1,0.0); int nb[2] = {0 ,0 }; //calculate euclidian distance for (int row = y1; row < y2; row++) { char *basePtr = &m_src->imageData[(row * m_src->widthStep) + (rect.x * m_src->nChannels)]; char *imgPtr = basePtr; char *maskPtr = &m_mask->imageData[(row * m_mask->widthStep) + (rect.x * m_mask->nChannels)]; for (; imgPtr < basePtr + (m_src->nChannels * rect.width); imgPtr += m_src->nChannels, maskPtr += m_mask->nChannels) { //get image data double Red = (unsigned char) imgPtr[0]; double Green = (unsigned char) imgPtr[1]; double Blue = (unsigned char) imgPtr[2]; double dist1 = (Red - mean1.val[0]) * (Red - mean1.val[0]) + (Green - mean1.val[1]) * (Green - mean1.val[1]) + (Blue - mean1.val[2]) * (Blue - mean1.val[2]); double dist2 = (Red - mean2.val[0]) * (Red - mean2.val[0]) + (Green - mean2.val[1]) * (Green - mean2.val[1]) + (Blue - mean2.val[2]) * (Blue - mean2.val[2]); if (dist1 <= dist2) { //DIST1 SMALLER, SET MASK PROPERLY FOR MEAN FUNCTION TO BE CALCULATED PROPERLY *maskPtr = 1; nb[0]++; } else { //DIST2 SMALLER, SET MASK PROPERLY FOR MEAN FUNCTION TO BE CALCULATED PROPERLY *maskPtr = 0; nb[1]++; } } } //CALCULATE NEW AVG AND STD FOR NEWLY FOUND PIXELS cvAvgSdv(src,&mean1,&stddev1,m_mask); //INVERT MASK for (int row = y1; row < y2; row++) { char *basePtr = &m_mask->imageData[(row * m_mask->widthStep) + (rect.x * m_mask->nChannels)]; char *maskPtr = basePtr; for (; maskPtr < basePtr + (m_mask->nChannels * rect.width); maskPtr += m_mask->nChannels) { if (*maskPtr == 1) { *maskPtr = 0; } else { *maskPtr = 1; } } } //TODO STDDEV ON NO ELEMENT ? //CALCULATE NEW AVG AND STD FOR NEWLY FOUND PIXELS cvAvgSdv(src,&mean2,&stddev2,m_mask); //cerr<<"CV made (mean1): "<<mean1.val[0]<<" "<<mean1.val[1]<<" "<<mean1.val[2]<<endl; //cerr<<"CV made (stddev1): "<<stddev1.val[0]<<" "<<stddev1.val[1]<<" "<<stddev1.val[2]<<endl; //cerr<<"JM TEST "<<sqrt(stddev1.val[0] * stddev1.val[0] + // stddev1.val[1] * stddev1.val[1] + // stddev1.val[2] * stddev1.val[2])<<endl; //cerr<<"CV made (mean2): "<<mean2.val[0]<<" "<<mean2.val[1]<<" "<<mean2.val[2]<<endl; //cerr<<"CV made (stddev2): "<<stddev2.val[0]<<" "<<stddev2.val[1]<<" "<<stddev2.val[2]<<endl; //cerr<<"JM TEST "<<sqrt(stddev2.val[0] * stddev2.val[0] + // stddev2.val[1] * stddev2.val[1] + // stddev2.val[2] * stddev2.val[2])<<endl; //cerr<<"nbsample " <<nb[0]<<" "<<nb[1]<<endl; //DISTANCE ENTRE LES 2 MOYENNES SOIT SUFFISANTE //STD SOIT PETIT PAR RAPPORT A LA MOYENNE double mean_dist = sqrt((mean1.val[0] - mean2.val[0]) * (mean1.val[0] - mean2.val[0]) + (mean1.val[1] - mean2.val[1]) * (mean1.val[1] - mean2.val[1]) + (mean1.val[2] - mean2.val[2]) * (mean1.val[2] - mean2.val[2])); double std1sum = sqrt(stddev1.val[0] * stddev1.val[0]+ stddev1.val[1] * stddev1.val[1] + stddev1.val[2] * stddev1.val[2]); double std2sum = sqrt(stddev2.val[0] * stddev2.val[0]+ stddev2.val[1] * stddev2.val[1] + stddev2.val[2] * stddev2.val[2]); if (mean_dist > min_dist /*&& std1sum < std_max && std2sum < std_max */ ) { bimodal = true; } return bimodal; } void calculate(int output_id, int count, Buffer &out) { try { RCPtr<Image> imageRef = getInput(m_imageInID, count); //copy source image memcpy(m_src->imageData, imageRef->get_data(), imageRef->get_size()); //reset ROI cvResetImageROI(m_src); cvResetImageROI(m_dest); cvResetImageROI(m_mask); cvResetImageROI(m_edges); cvCopy(m_src,m_dest,NULL); struct timeb t1, t2; ftime(&t1); //TILE MODE /* for (int y = 0; y < 240; y+= 16) { for (int x = 0; x < 320; x+= 16) { CvRect rect; rect.x = x; rect.y = y; rect.width = 16; rect.height = 16; if (processRect(rect,m_src,m_dest,m_maxStd,m_minDist)) { CvScalar color = CV_RGB(255,0,0); CvPoint p1,p2; p1.x = rect.x; p1.y = rect.y; p2.x = rect.x + rect.width; p2.y = rect.y + rect.height; cvRectangle(m_dest,p1,p2,color); } } } */ //EDGE MODE cvCvtColor(m_src, m_edges, CV_BGR2GRAY); cvSmooth(m_edges,m_edges,CV_GAUSSIAN,3); cvCanny(m_edges, m_edges, 128, 255, 3); CvMemStorage* storage = cvCreateMemStorage(0); CvSeq* contour = 0; cvFindContours(m_edges, storage, &contour, sizeof(CvContour), CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE ); for( ; contour != 0; contour = contour->h_next ) { CvRect rect = cvBoundingRect(contour,0); if (processRect(rect,m_src,m_dest,m_maxStd,m_minDist)) { CvScalar color = CV_RGB(255,0,0); CvPoint p1,p2; p1.x = rect.x; p1.y = rect.y; p2.x = rect.x + rect.width; p2.y = rect.y + rect.height; cvRectangle(m_dest,p1,p2,color); } } cvReleaseMemStorage(&storage); ftime(&t2); double timeDiff=(t2.time-t1.time)+((t2.millitm-t1.millitm)/1000.0); cout << "Total run time (sec): " << timeDiff << endl; Image *m_outImg = Image::alloc( imageRef->get_width(), imageRef->get_height(), 3); //Copy destination image (convert from CV format) memcpy(m_outImg->get_data(), m_dest->imageData, m_outImg->get_size()); if( output_id == m_imageOutID ) { out[count] = ObjectRef(m_outImg); } } catch (BaseException *e) { throw e->add(new GeneralException("Exception in CvBiModalTest::calculate:",__FILE__,__LINE__)); } } private: int m_imageInID; int m_imageOutID; double m_minDist; double m_maxStd; }; }//namespace RobotFlow #endif |
|
From: Dominic L. <ma...@us...> - 2005-05-04 19:23:12
|
Update of /cvsroot/robotflow/RobotFlow/Vision/src In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv22743 Modified Files: Image.cc Log Message: Avoid printing warning messages Index: Image.cc =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Vision/src/Image.cc,v retrieving revision 1.17 retrieving revision 1.18 diff -C2 -d -r1.17 -r1.18 *** Image.cc 29 Mar 2005 15:20:47 -0000 1.17 --- Image.cc 4 May 2005 19:23:04 -0000 1.18 *************** *** 287,290 **** --- 287,305 ---- } + METHODDEF(void) + output_message (j_common_ptr cinfo) + { + //cerr<<"output_message"<<endl; + } + + + /* Conditionally emit a trace or warning message */ + METHODDEF (void) + emit_message (j_common_ptr cinfo, int msg_level) { + + //cerr<<"emit_message level "<<msg_level<<endl; + //TODO DO something with warning messages + } + int Image::jpeg_compress(unsigned char* buffer, int max_size, int quality) { *************** *** 292,297 **** struct jpeg_compress_struct cinfo; struct jpeg_error_mgr jerr; ! cinfo.err = jpeg_std_error(&jerr); jpeg_create_compress(&cinfo); --- 307,313 ---- struct jpeg_compress_struct cinfo; struct jpeg_error_mgr jerr; ! cinfo.err = jpeg_std_error(&jerr); + jpeg_create_compress(&cinfo); *************** *** 395,399 **** /* We set up the normal JPEG error routines, then override error_exit. */ ! cinfo.err = jpeg_std_error(&jerr); /* Now we can initialize the JPEG decompression object. */ --- 411,422 ---- /* We set up the normal JPEG error routines, then override error_exit. */ ! jpeg_std_error(&jerr); ! ! //change the output message handler ! //jerr.output_message = output_message; ! jerr.emit_message = emit_message; ! //jerr.trace_level = -10; ! cinfo.err = &jerr; ! /* Now we can initialize the JPEG decompression object. */ |
|
From: Dominic L. <ma...@us...> - 2005-05-04 19:22:49
|
Update of /cvsroot/robotflow/RobotFlow/Devices/include In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv22557/include Modified Files: SNCRZ30.h Log Message: waiting on socket, adding speed configuration Index: SNCRZ30.h =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Devices/include/SNCRZ30.h,v retrieving revision 1.11 retrieving revision 1.12 diff -C2 -d -r1.11 -r1.12 *** SNCRZ30.h 29 Mar 2005 15:20:38 -0000 1.11 --- SNCRZ30.h 4 May 2005 19:22:35 -0000 1.12 *************** *** 95,98 **** --- 95,99 ---- int m_panSpeed; int m_tiltSpeed; + int m_transmitSpeed; bool m_continuous; bool m_wait_reply; |
|
From: Dominic L. <ma...@us...> - 2005-05-04 19:22:49
|
Update of /cvsroot/robotflow/RobotFlow/Devices/src In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv22557/src Modified Files: SNCRZ30.cc Log Message: waiting on socket, adding speed configuration Index: SNCRZ30.cc =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Devices/src/SNCRZ30.cc,v retrieving revision 1.22 retrieving revision 1.23 diff -C2 -d -r1.22 -r1.23 *** SNCRZ30.cc 29 Mar 2005 15:20:39 -0000 1.22 --- SNCRZ30.cc 4 May 2005 19:22:36 -0000 1.23 *************** *** 82,85 **** --- 82,90 ---- * @parameter_description Continuously grab images ? * + * @parameter_name TRANSMIT_SPEED + * @parameter_type int + * @parameter_value 0 + * @parameter_description transfer rate FPS [0=FASTEST] + * * @parameter_name WAIT_REPLY * @parameter_type bool *************** *** 141,149 **** m_wait_reply = dereference_cast<bool>(parameters.get("WAIT_REPLY")); m_inquiryPosition = dereference_cast<bool>(parameters.get("INQUIRY_POSITION")); ! //initialize camera position m_currentZoom = 0; m_currentTilt = 0; m_currentPan = 0; } --- 146,156 ---- m_wait_reply = dereference_cast<bool>(parameters.get("WAIT_REPLY")); m_inquiryPosition = dereference_cast<bool>(parameters.get("INQUIRY_POSITION")); ! m_transmitSpeed = dereference_cast<int>(parameters.get("TRANSMIT_SPEED")); ! //initialize camera position m_currentZoom = 0; m_currentTilt = 0; m_currentPan = 0; + } *************** *** 308,312 **** //send periodic images on image socket... ostringstream my_stream; ! my_stream << "GET http://"<<m_host<<":"<<m_port<<"/image?speed=0 HTTP/1.1\r\n"<<endl; int sent_size = m_imageSocket.send_packet((unsigned char*)my_stream.str().c_str(), my_stream.str().size()); //cerr<<"sent request size "<<sent_size<<endl; --- 315,319 ---- //send periodic images on image socket... ostringstream my_stream; ! my_stream << "GET http://"<<m_host<<":"<<m_port<<"/image?speed="<<m_transmitSpeed<<" HTTP/1.1\r\n"<<endl; int sent_size = m_imageSocket.send_packet((unsigned char*)my_stream.str().c_str(), my_stream.str().size()); //cerr<<"sent request size "<<sent_size<<endl; *************** *** 622,643 **** //read data on socket ! char buffer[32768]; int size = m_imageSocket.recv_packet((unsigned char*)buffer,32768); //cerr<<"SNCRZ30::update_data got size :"<<size<<endl; - - int last_image_num = process_image_data(buffer,size); ! //reading all images in the buffer ! while(1) { ! ! int temp_image_num = process_image_data(NULL,0); ! ! if (temp_image_num == last_image_num){ ! break; } else { ! last_image_num = temp_image_num; ! } } } --- 629,650 ---- //read data on socket ! static char buffer[32768]; ! static int current_image_num = 0; + //read data int size = m_imageSocket.recv_packet((unsigned char*)buffer,32768); //cerr<<"SNCRZ30::update_data got size :"<<size<<endl; ! if (size > 0) { ! //process data ! int last_image_num = process_image_data(buffer,size); ! ! //look for a new image ! if (last_image_num > current_image_num) { ! current_image_num = last_image_num; } else { ! update_data(); ! } } } |
|
From: Dominic L. <ma...@us...> - 2005-04-26 14:26:22
|
Update of /cvsroot/robotflow/RobotFlow/Devices/src In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv24902 Modified Files: IMU400CC_200.cc SerialDriver.cc Log Message: working device Index: SerialDriver.cc =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Devices/src/SerialDriver.cc,v retrieving revision 1.4 retrieving revision 1.5 diff -C2 -d -r1.4 -r1.5 *** SerialDriver.cc 30 Mar 2005 19:11:02 -0000 1.4 --- SerialDriver.cc 26 Apr 2005 14:25:48 -0000 1.5 *************** *** 121,124 **** --- 121,125 ---- void End_session(int fd) { + tcflush(fd,TCIOFLUSH); close(fd); } Index: IMU400CC_200.cc =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Devices/src/IMU400CC_200.cc,v retrieving revision 1.1 retrieving revision 1.2 diff -C2 -d -r1.1 -r1.2 *** IMU400CC_200.cc 25 Apr 2005 19:51:13 -0000 1.1 --- IMU400CC_200.cc 26 Apr 2005 14:25:48 -0000 1.2 *************** *** 55,58 **** --- 55,63 ---- * @parameter_value /dev/ttyUSB0 * + * @parameter_name BIAS_VALUES + * @parameter_type int + * @parameter_value 0 + * @parameter_description The number of values to calculate the bias on gyro rate before processing. + * * @output_name X_ACCEL * @output_type float *************** *** 110,114 **** #define IMU_RR_PRECISION 200.0 #define IMU_TIME_PRECISION 0.79E-6 ! #define IMU_GRAVITY_ACCEL_VALUE 9.81 class IMU400_DATA { --- 115,119 ---- #define IMU_RR_PRECISION 200.0 #define IMU_TIME_PRECISION 0.79E-6 ! #define IMU_GRAVITY_ACCEL_VALUE 9.80 class IMU400_DATA { *************** *** 190,193 **** --- 195,199 ---- //parameters string m_serial_port; + int m_bias_values; //Internal variables *************** *** 337,341 **** pthread_mutex_lock(&m_recv_list_lock); ! if (m_recv_list.size() < 1000) { m_recv_list.push_back(data); } --- 343,347 ---- pthread_mutex_lock(&m_recv_list_lock); ! if (m_recv_list.size() < max(1000,m_bias_values)) { m_recv_list.push_back(data); } *************** *** 449,456 **** //parameters m_serial_port = object_cast<String>(parameters.get("SERIAL_PORT")); ! ! //open serial port ! m_fd = Port_initialisation(m_serial_port.c_str(),BAUDRATE_38400); ! cerr<<"Open serial port : "<<m_serial_port<<" fd : "<<m_fd<<endl; } --- 455,460 ---- //parameters m_serial_port = object_cast<String>(parameters.get("SERIAL_PORT")); ! m_bias_values = dereference_cast<int>(parameters.get("BIAS_VALUES")); ! } *************** *** 463,506 **** int timeout = 0; ! //verify if serial port opened ! if (m_fd >= 0) { ! ! cerr<<"STARTING POLLED MODE"<<endl; ! message[0] = 'P'; ! write(m_fd, message,1); timeout = 0; do { //PING MESSAGE message[0] = 'R'; ! write(m_fd, message,1); ! //READ DEVICE ANSWER ! read(m_fd,message,1); ! usleep(50000); ! cerr<<"GOT PING ANSWER : "<<message[0]<<endl; ! } while(message[0] != 'H' && timeout++ < 100); if (message[0] != 'H') { throw new GeneralException("No device found",__FILE__,__LINE__); } ! ! message[0] = 'z'; //ZERO ! message[1] = 0xC8; ! write(m_fd, message,2); ! cerr<<"WAITING FOR ZERO"<<endl; ! read(m_fd,message,1); ! cerr<<"GOT ZERO MESSAGE : "<<message[0]<<endl; ! ! sleep(10);//wait 10 seconds do { message[0] = 'c'; //SCALED MODE MESSAGE write(m_fd, message,1); ! read(m_fd,message,1); ! cerr<<"GOT SCALED MODE ANSWER : "<<message[0]<<endl; ! } while (message[0] != 'C'); ! cerr<<"STARTING CONTINUOUS MODE"<<endl; message[0] = 'C'; //CONTINUOUS MODE START write(m_fd, message,1); --- 467,540 ---- int timeout = 0; ! //open serial port ! m_fd = Port_initialisation(m_serial_port.c_str(),BAUDRATE_38400); ! cerr<<"IMU400CC_200 : Open serial port (38400): "<<m_serial_port<<" fd : "<<m_fd<<endl; ! ! cerr<<"IMU400CC_200 : STARTING POLLED MODE"<<endl; ! message[0] = 'P'; ! write(m_fd, message,1); ! ! //TODO MAKE AUTOMATIC BAUD RATE DETECTION WORK... ! //message[0] = 'b'; ! //write(m_fd,message,1); ! //End_session(m_fd); ! //reopen serial port ! //m_fd = Port_initialisation(m_serial_port.c_str(),BAUDRATE_57600); ! //cerr<<"Open serial port (57600): "<<m_serial_port<<" fd : "<<m_fd<<endl; ! ! //verify if serial port opened ! if (m_fd >= 0) { timeout = 0; + do { //PING MESSAGE message[0] = 'R'; ! write(m_fd, message,1); ! ! //READ DEVICE ANSWER (TIMEOUT 1 SEC ON READ) ! if (read(m_fd,message,1) > 0) { ! cerr<<"IMU400CC_200 : GOT PING ANSWER : "<<message[0]<<endl; ! } ! } while(message[0] != 'H' && timeout++ < 10); if (message[0] != 'H') { + End_session(m_fd); + m_fd = -1; throw new GeneralException("No device found",__FILE__,__LINE__); } + + bool zero_sent=false; + timeout = 0; + do { + if (!zero_sent) { + message[0] = 'z'; //ZERO + message[1] = 0xFF; + write(m_fd, message,2); + zero_sent = true; + } + cerr<<"IMU400CC_200 : WAITING FOR ZERO"<<endl; + if (read(m_fd,message,1) > 0) { + cerr<<"IMU400CC_200 : GOT ZERO MESSAGE : "<<message[0]<<endl; + } + } while ( message[0] != 'Z' && timeout++ < 10); ! //wait until unit zeroed ! cerr<<"IMU400CC_200 : WAITING 25SEC UNTIL UNIT IS CALIBRATED..."<<endl; ! sleep(25); ! ! timeout = 0; do { + cerr<<"IMU400CC_200 : SENDING SCALED MODE"<<endl; message[0] = 'c'; //SCALED MODE MESSAGE write(m_fd, message,1); ! ! if (read(m_fd,message,1) > 0) { ! cerr<<"IMU400CC_200 : GOT SCALED MODE ANSWER : "<<message[0]<<endl; ! } ! } while (message[0] != 'C' && timeout++ < 10); ! cerr<<"IMU400CC_200 : STARTING CONTINUOUS MODE"<<endl; message[0] = 'C'; //CONTINUOUS MODE START write(m_fd, message,1); *************** *** 519,556 **** pthread_create(&m_recvThread, NULL, imu400_recv_thread, this); ! ! //bias calib values over 1000 samples ! while(1) { ! pthread_mutex_lock(&m_recv_list_lock); ! if (m_recv_list.size() == 1000) break; ! pthread_mutex_unlock(&m_recv_list_lock); ! } ! ! ! float count = 0; ! ! //calculate bias ! for (list<IMU400_DATA>::iterator iter = m_recv_list.begin(); ! iter != m_recv_list.end(); iter++) { ! m_yaw_bias += iter->m_yaw_rate; ! m_pitch_bias += iter->m_pitch_rate; ! m_roll_bias += iter->m_roll_rate; ! ! count += 1.0; ! } ! ! if (count > 0) { ! m_yaw_bias /= count; ! m_pitch_bias /= count; ! m_roll_bias /= count; } - cerr<<"BIASES : "<<m_yaw_bias<<" "<<m_pitch_bias<<" "<<m_roll_bias<<endl; - - //clear list - m_recv_list.resize(0); - - //very important to - pthread_mutex_unlock(&m_recv_list_lock); } } --- 553,592 ---- pthread_create(&m_recvThread, NULL, imu400_recv_thread, this); ! if (m_bias_values > 0) { ! ! //bias calib values over m_bias_values samples ! while(1) { ! pthread_mutex_lock(&m_recv_list_lock); ! if (m_recv_list.size() >= m_bias_values) break; ! pthread_mutex_unlock(&m_recv_list_lock); ! } ! ! float count = 0; ! ! //calculate bias ! for (list<IMU400_DATA>::iterator iter = m_recv_list.begin(); ! iter != m_recv_list.end(); iter++) { ! ! m_yaw_bias += iter->m_yaw_rate; ! m_pitch_bias += iter->m_pitch_rate; ! m_roll_bias += iter->m_roll_rate; ! ! count += 1.0; ! } ! ! if (count > 0) { ! m_yaw_bias /= count; ! m_pitch_bias /= count; ! m_roll_bias /= count; ! } ! cerr<<"BIASES : "<<m_yaw_bias<<" "<<m_pitch_bias<<" "<<m_roll_bias<<endl; ! ! //clear list ! m_recv_list.resize(0); ! ! //very important to ! pthread_mutex_unlock(&m_recv_list_lock); } } } *************** *** 633,660 **** unsigned char message[IMU_PACKET_SIZE]; cerr << "IMU400CC_200 : destructor" << endl; - cerr << "IMU400CC_200 : waiting for send & receive threads" << endl; - pthread_mutex_unlock(&m_send_list_lock); // unlock all - pthread_mutex_unlock(&m_recv_list_lock); - - m_killed = 1; - pseudosem_post(&m_semaphore); - - pthread_join(m_sendThread,NULL); - cerr << "IMU400CC_200 : Sending_thread killed (joined)" << endl; - - pthread_join(m_recvThread,NULL); - cerr << "IMU400CC_200 : Receiving_thread killed (joined)" << endl; - - pseudosem_destroy(&m_semaphore); - pthread_mutex_destroy(&m_send_list_lock); - pthread_mutex_destroy(&m_recv_list_lock); - ! cerr<<"STARTING POLLED MODE"<<endl; ! message[0] = 'P'; ! write(m_fd, message,1); ! sleep(1); ! ! End_session(m_fd); cerr << "IMU400CC_200 (done!)" << endl; --- 669,699 ---- unsigned char message[IMU_PACKET_SIZE]; cerr << "IMU400CC_200 : destructor" << endl; ! if (m_fd > 0) { ! cerr << "IMU400CC_200 : waiting for send & receive threads" << endl; ! pthread_mutex_unlock(&m_send_list_lock); // unlock all ! pthread_mutex_unlock(&m_recv_list_lock); ! ! m_killed = 1; ! pseudosem_post(&m_semaphore); ! ! pthread_join(m_sendThread,NULL); ! cerr << "IMU400CC_200 : Sending_thread killed (joined)" << endl; ! ! pthread_join(m_recvThread,NULL); ! cerr << "IMU400CC_200 : Receiving_thread killed (joined)" << endl; ! ! pseudosem_destroy(&m_semaphore); ! pthread_mutex_destroy(&m_send_list_lock); ! pthread_mutex_destroy(&m_recv_list_lock); ! ! ! cerr<<"IMU400CC_200 : STARTING POLLED MODE"<<endl; ! message[0] = 'P'; ! write(m_fd, message,1); ! sleep(1); ! ! End_session(m_fd); ! } cerr << "IMU400CC_200 (done!)" << endl; |
|
From: Dominic L. <ma...@us...> - 2005-04-25 19:51:41
|
Update of /cvsroot/robotflow/RobotFlow/Devices/src In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv6380 Modified Files: Makefile.am Added Files: IMU400CC_200.cc Log Message: first working driver Index: Makefile.am =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Devices/src/Makefile.am,v retrieving revision 1.12 retrieving revision 1.13 diff -C2 -d -r1.12 -r1.13 *** Makefile.am 30 Mar 2005 19:10:59 -0000 1.12 --- Makefile.am 25 Apr 2005 19:51:13 -0000 1.13 *************** *** 14,18 **** CANON_VCC4.cc \ SNCRZ30.cc \ ! SNCRZ30RS232.cc libDevices_la_LDFLAGS = -release $(LT_RELEASE) --- 14,19 ---- CANON_VCC4.cc \ SNCRZ30.cc \ ! SNCRZ30RS232.cc \ ! IMU400CC_200.cc libDevices_la_LDFLAGS = -release $(LT_RELEASE) --- NEW FILE: IMU400CC_200.cc --- /* Copyright (C) 2005 Dominic Letourneau This library is free software; you can redistribute it and/or modify it under the terms of the GNU Lesser General Public License as published by the Free Software Foundation; either version 2.1 of the License, or (at your option) any later version. This library is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public License along with this library; if not, write to the Free Software Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ #ifndef _IMU400CC_200_CC_ #define _IMU400CC_200_CC_ #include "BufferedNode.h" #include "SerialDriver.h" #include "BaseException.h" #include "misc.h" #include <fcntl.h> #include <stdio.h> #include <string.h> #include <sys/types.h> #include <unistd.h> #include <signal.h> #include <termios.h> #include <stdlib.h> #include <list> #include <pthread.h> #include <string> #include <sched.h> #include "pseudosem.h" using namespace std; using namespace FD; namespace RobotFlow { class IMU400CC_200; DECLARE_NODE(IMU400CC_200) /*Node * @name IMU400CC_200 * @category RobotFlow:Devices * @description Crossbow IMU400CC-200 driver. * * @parameter_name SERIAL_PORT * @parameter_type string * @parameter_value /dev/ttyUSB0 * * @output_name X_ACCEL * @output_type float * @output_description X ACCELERATION (G force) * * @output_name Y_ACCEL * @output_type float * @output_description Y ACCELERATION (G force) * * @output_name Z_ACCEL * @output_type float * @output_description Z ACCELERATION (G force) * * @output_name YAW_RATE * @output_type float * @output_description YAW RATE (deg/sec) * * @output_name PITCH_RATE * @output_type float * @output_description PITCH RATE (deg/sec) * * @output_name ROLL_RATE * @output_type float * @output_description ROLL_RATE (deg/sec) * * @output_name X * @output_type float * @output_description X position (meters) * * @output_name Y * @output_type float * @output_description Y position (meters) * * @output_name Z * @output_type float * @output_description Z position (meters) * * @output_name YAW * @output_type float * @output_description YAW (deg) * * @output_name PITCH * @output_type float * @output_description PITCH (deg) * * @output_name ROLL * @output_type float * @output_description ROLL (deg) * END*/ #define IMU_PACKET_SIZE 18 #define IMU_GR_PRECISION 10.0 #define IMU_RR_PRECISION 200.0 #define IMU_TIME_PRECISION 0.79E-6 #define IMU_GRAVITY_ACCEL_VALUE 9.81 class IMU400_DATA { public: IMU400_DATA() { m_pitch_rate = 0; m_roll_rate = 0; m_yaw_rate = 0; m_x_accel = 0; m_y_accel = 0; m_z_accel = 0; m_time = 0; m_temperature = 0; } IMU400_DATA(const IMU400_DATA &cpy) { m_pitch_rate = cpy.m_pitch_rate; m_roll_rate = cpy.m_roll_rate; m_yaw_rate = cpy.m_yaw_rate; m_x_accel = cpy.m_x_accel; m_y_accel = cpy.m_y_accel; m_z_accel = cpy.m_z_accel; m_time = cpy.m_time; m_temperature = cpy.m_temperature; } IMU400_DATA& operator=(const IMU400_DATA &cpy) { m_pitch_rate = cpy.m_pitch_rate; m_roll_rate = cpy.m_roll_rate; m_yaw_rate = cpy.m_yaw_rate; m_x_accel = cpy.m_x_accel; m_y_accel = cpy.m_y_accel; m_z_accel = cpy.m_z_accel; m_time = cpy.m_time; m_temperature = cpy.m_temperature; } void print(ostream &out) { out <<"RATE "<<m_roll_rate <<","<<m_pitch_rate<<","<<m_yaw_rate<<endl; out <<"ACCEL "<<m_x_accel<<","<<m_y_accel<<","<<m_z_accel<<endl; out <<"TEMP "<<m_temperature<<" TIME "<<m_time<<endl; } //VARIABLES float m_pitch_rate; float m_roll_rate; float m_yaw_rate; float m_x_accel; float m_y_accel; float m_z_accel; float m_time; float m_temperature; }; class IMU400CC_200 : public BufferedNode { friend void* imu400_recv_thread (void *dev); friend void* imu400_send_thread (void *dev); private: //outputs int m_xAccelID; int m_yAccelID; int m_zAccelID; int m_yawRateID; int m_pitchRateID; int m_rollRateID; int m_xPosID; int m_yPosID; int m_zPosID; int m_yawID; int m_pitchID; int m_rollID; //parameters string m_serial_port; //Internal variables int m_fd; int m_killed; pthread_t m_sendThread; pthread_t m_recvThread; pthread_mutex_t m_send_list_lock; // Used to block the access to the sending list pthread_mutex_t m_recv_list_lock; // Used to block the access to the reception list pseudosem_t m_semaphore; // Used to tell the sending thread that it can try to send something list<string> m_send_list; list<IMU400_DATA> m_recv_list; IMU400_DATA m_actual_IMU_data; float m_x_vel; float m_y_vel; float m_z_vel; float m_x_pos; float m_y_pos; float m_z_pos; float m_yaw; float m_pitch; float m_roll; float m_yaw_bias; float m_pitch_bias; float m_roll_bias; void SendPacket() { string to_send; //lock pthread_mutex_lock(&m_send_list_lock); if (!m_send_list.empty()) { //Copy and Delete first element to_send = m_send_list.front(); m_send_list.pop_front(); //unlock here to avoid blocking on write (long) pthread_mutex_unlock(&m_send_list_lock); if(write(m_fd, to_send.c_str(),to_send.size()) < 0) { perror("IMU400CC_200::Send():write():"); throw new GeneralException("IMU400CC_200::Send():write() error",__FILE__,__LINE__); } }//if !empty else { //unlock pthread_mutex_unlock(&m_send_list_lock); } } void processPacket(unsigned char packet[IMU_PACKET_SIZE], ostream &out) { short roll_rate = 0; short pitch_rate = 0; short yaw_rate = 0; short x_accel = 0; short y_accel = 0; short z_accel = 0; unsigned short temperature = 0; unsigned short time = 0; static unsigned short old_time=65535;//free running counter starts at 65535 unsigned short temp1,temp2 = 0; //float f_roll_rate = 0; //float f_pitch_rate = 0; //float f_yaw_rate = 0; //float f_x_accel = 0; //float f_y_accel = 0; //float f_z_accel = 0; //float f_temperature = 0; IMU400_DATA data; //ROLL RATE temp1 = packet[1]; temp2 = packet[2]; roll_rate = (temp1<<8 | temp2); data.m_roll_rate = (float)roll_rate * IMU_RR_PRECISION * 1.5 / 32768.0; data.m_roll_rate -= m_roll_bias; //PITCH RATE temp1 = packet[3]; temp2 = packet[4]; pitch_rate = (temp1<<8 | temp2); data.m_pitch_rate = (float)pitch_rate * IMU_RR_PRECISION * 1.5 / 32768.0; data.m_pitch_rate -= m_pitch_bias; //YAW RATE temp1 = packet[5]; temp2 = packet[6]; yaw_rate = (temp1<<8 | temp2); data.m_yaw_rate = (float)yaw_rate * IMU_RR_PRECISION * 1.5 / 32768.0; data.m_yaw_rate -= m_yaw_bias; //X ACCEL temp1 = packet[7]; temp2 = packet[8]; x_accel = (temp1<<8 | temp2); data.m_x_accel = (float)x_accel * IMU_GR_PRECISION * 1.5 * IMU_GRAVITY_ACCEL_VALUE / 32768.0; //Y ACCEL temp1 = packet[9]; temp2 = packet[10]; y_accel = (temp1<<8 | temp2); data.m_y_accel = (float)y_accel * IMU_GR_PRECISION * 1.5 * IMU_GRAVITY_ACCEL_VALUE / 32768.0; //Z ACCEL temp1 = packet[11]; temp2 = packet[12]; z_accel = (temp1<<8 | temp2); data.m_z_accel = (float)z_accel * IMU_GR_PRECISION * 1.5 * IMU_GRAVITY_ACCEL_VALUE / 32768.0; //TEMPERATURE temp1 = packet[13]; temp2 = packet[14]; temperature = (temp1<<8 | temp2); data.m_temperature = (((float) temperature * 5.0 / 4096.0) - 1.375) * 44.44; //TIME temp1 = packet[15]; temp2 = packet[16]; time = (temp1<<8 | temp2); if (time < old_time) { //delta time float new_time = old_time - time; data.m_time = new_time * IMU_TIME_PRECISION; } else { float new_time = 65535 - time + old_time; data.m_time = new_time * IMU_TIME_PRECISION; } //data.print(cout); pthread_mutex_lock(&m_recv_list_lock); if (m_recv_list.size() < 1000) { m_recv_list.push_back(data); } else { cerr<<"IMU400CC_200 : ERROR receive list full, discarding first element"<<endl; m_recv_list.pop_front(); m_recv_list.push_back(data); } //store actual data m_actual_IMU_data = data; pthread_mutex_unlock(&m_recv_list_lock); //store old time old_time = time; } bool Checksum(unsigned char packet[IMU_PACKET_SIZE]) { unsigned int checksum = 0; for (int i = 1; i < IMU_PACKET_SIZE - 1; i++) { checksum += packet[i]; } if ((checksum & 0xFF) == packet[IMU_PACKET_SIZE -1 ]) { //cerr<<"CHECKSUM OK!"<<endl; return true; } else { cerr<<"CHECKSUM ERROR"<<endl; cerr<<"CALC: "<<checksum<<" packet : "<<(unsigned int) packet[IMU_PACKET_SIZE -1 ]<<endl; return false; } } void ReceivePacket() { unsigned char recv_message[IMU_PACKET_SIZE]; int cnt = 0; do { //waiting for packet header 0xFF while(!m_killed) { recv_message[0] = 0; cnt = 0; if ( (cnt+=read(m_fd, &recv_message[0], 1 )) < 0 ) { perror( "IMU400CC_200:ReceivePacket():" ); throw new GeneralException( "Cannot read packet",__FILE__,__LINE__ ); } if (recv_message[0] == 0xFF) { //cerr<<"got packet header 0xFF"<<endl; break; } } //cerr<<"reading the rest of the message"<<endl; while(cnt != IMU_PACKET_SIZE && !m_killed) { if ( (cnt+=read(m_fd, &recv_message[cnt], IMU_PACKET_SIZE - cnt )) < 0 ) { perror( "IMU400CC_200:ReceivePacket():" ); throw new GeneralException("Cannot read packet",__FILE__,__LINE__); } } } while(!m_killed && !Checksum(recv_message)); processPacket(recv_message,cerr); //PUT MESSAGE IN RECV LIST //m_recv_list.push_back(string((char*)recv_message,IMU_PACKET_SIZE)); } public: IMU400CC_200(string nodeName, ParameterSet params) : BufferedNode(nodeName, params) , m_killed(0), m_x_pos(0), m_y_pos(0), m_z_pos(0), m_yaw(0), m_pitch(0), m_roll(0), m_x_vel(0), m_y_vel(0), m_z_vel(0), m_yaw_bias(0), m_pitch_bias(0), m_roll_bias(0) { //inputs //outputs m_xAccelID = addOutput("X_ACCEL"); m_yAccelID = addOutput("Y_ACCEL"); m_zAccelID = addOutput("Z_ACCEL"); m_yawRateID = addOutput("YAW_RATE"); m_pitchRateID = addOutput("PITCH_RATE"); m_rollRateID = addOutput("ROLL_RATE"); m_xPosID = addOutput("X"); m_yPosID = addOutput("Y"); m_zPosID = addOutput("Z"); m_yawID = addOutput("YAW"); m_pitchID = addOutput("PITCH"); m_rollID = addOutput("ROLL"); //parameters m_serial_port = object_cast<String>(parameters.get("SERIAL_PORT")); //open serial port m_fd = Port_initialisation(m_serial_port.c_str(),BAUDRATE_38400); cerr<<"Open serial port : "<<m_serial_port<<" fd : "<<m_fd<<endl; } void initialize() { BufferedNode::initialize(); unsigned char message[IMU_PACKET_SIZE]; int timeout = 0; //verify if serial port opened if (m_fd >= 0) { cerr<<"STARTING POLLED MODE"<<endl; message[0] = 'P'; write(m_fd, message,1); timeout = 0; do { //PING MESSAGE message[0] = 'R'; write(m_fd, message,1); //READ DEVICE ANSWER read(m_fd,message,1); usleep(50000); cerr<<"GOT PING ANSWER : "<<message[0]<<endl; } while(message[0] != 'H' && timeout++ < 100); if (message[0] != 'H') { throw new GeneralException("No device found",__FILE__,__LINE__); } message[0] = 'z'; //ZERO message[1] = 0xC8; write(m_fd, message,2); cerr<<"WAITING FOR ZERO"<<endl; read(m_fd,message,1); cerr<<"GOT ZERO MESSAGE : "<<message[0]<<endl; sleep(10);//wait 10 seconds do { message[0] = 'c'; //SCALED MODE MESSAGE write(m_fd, message,1); read(m_fd,message,1); cerr<<"GOT SCALED MODE ANSWER : "<<message[0]<<endl; } while (message[0] != 'C'); cerr<<"STARTING CONTINUOUS MODE"<<endl; message[0] = 'C'; //CONTINUOUS MODE START write(m_fd, message,1); //message send list list mutex pthread_mutex_init(&m_send_list_lock,NULL); //message recv list list mutex pthread_mutex_init(&m_recv_list_lock ,NULL); //semaphore to wake up send thread when something is ready. pseudosem_init(&m_semaphore,0,0); //creating recv & send threads pthread_create(&m_sendThread, NULL, imu400_send_thread, this); pthread_create(&m_recvThread, NULL, imu400_recv_thread, this); //bias calib values over 1000 samples while(1) { pthread_mutex_lock(&m_recv_list_lock); if (m_recv_list.size() == 1000) break; pthread_mutex_unlock(&m_recv_list_lock); } float count = 0; //calculate bias for (list<IMU400_DATA>::iterator iter = m_recv_list.begin(); iter != m_recv_list.end(); iter++) { m_yaw_bias += iter->m_yaw_rate; m_pitch_bias += iter->m_pitch_rate; m_roll_bias += iter->m_roll_rate; count += 1.0; } if (count > 0) { m_yaw_bias /= count; m_pitch_bias /= count; m_roll_bias /= count; } cerr<<"BIASES : "<<m_yaw_bias<<" "<<m_pitch_bias<<" "<<m_roll_bias<<endl; //clear list m_recv_list.resize(0); //very important to pthread_mutex_unlock(&m_recv_list_lock); } } void Add_message_to_send(const string &msg_to_send) { //wait for the mutex pthread_mutex_lock(&m_send_list_lock); // add the new item into the list m_send_list.push_back(msg_to_send); // Unlock the mutex pthread_mutex_unlock(&m_send_list_lock); // Something new is in the list pseudosem_post(&m_semaphore); } void calculate(int output_id, int count, Buffer &out) { IMU400_DATA data; pthread_mutex_lock(&m_recv_list_lock); for (list<IMU400_DATA>::iterator iter = m_recv_list.begin(); iter != m_recv_list.end(); iter++) { //X m_x_vel += iter->m_x_accel * iter->m_time; m_x_pos += m_x_vel * iter->m_time; //Y m_y_vel += iter->m_y_accel * iter->m_time; m_y_pos += m_y_vel * iter->m_time; //Z m_z_vel += iter->m_z_accel * iter->m_time; m_z_pos += m_z_vel * iter->m_time; //YAW m_yaw += iter->m_yaw_rate * iter->m_time; //PITCH m_pitch += iter->m_pitch_rate * iter->m_time; //ROLL m_roll += iter->m_roll_rate * iter->m_time; } //clean up list m_recv_list.resize(0); //get actual data data = m_actual_IMU_data; pthread_mutex_unlock(&m_recv_list_lock); //output result (*outputs[m_xAccelID].buffer)[count] = ObjectRef(Float::alloc(data.m_x_accel)); (*outputs[m_yAccelID].buffer)[count] = ObjectRef(Float::alloc(data.m_y_accel)); (*outputs[m_zAccelID].buffer)[count] = ObjectRef(Float::alloc(data.m_z_accel)); (*outputs[m_yawRateID].buffer)[count] = ObjectRef(Float::alloc(data.m_yaw_rate)); (*outputs[m_pitchRateID].buffer)[count] = ObjectRef(Float::alloc(data.m_pitch_rate)); (*outputs[m_rollRateID].buffer)[count] = ObjectRef(Float::alloc(data.m_roll_rate)); (*outputs[m_xPosID].buffer)[count] = ObjectRef(Float::alloc(m_x_pos)); (*outputs[m_yPosID].buffer)[count] = ObjectRef(Float::alloc(m_y_pos)); (*outputs[m_zPosID].buffer)[count] = ObjectRef(Float::alloc(m_z_pos)); (*outputs[m_yawID].buffer)[count] = ObjectRef(Float::alloc(m_yaw)); (*outputs[m_pitchID].buffer)[count] = ObjectRef(Float::alloc(m_pitch)); (*outputs[m_rollID].buffer)[count] = ObjectRef(Float::alloc(m_roll)); } // end of calculate ~IMU400CC_200() { unsigned char message[IMU_PACKET_SIZE]; cerr << "IMU400CC_200 : destructor" << endl; cerr << "IMU400CC_200 : waiting for send & receive threads" << endl; pthread_mutex_unlock(&m_send_list_lock); // unlock all pthread_mutex_unlock(&m_recv_list_lock); m_killed = 1; pseudosem_post(&m_semaphore); pthread_join(m_sendThread,NULL); cerr << "IMU400CC_200 : Sending_thread killed (joined)" << endl; pthread_join(m_recvThread,NULL); cerr << "IMU400CC_200 : Receiving_thread killed (joined)" << endl; pseudosem_destroy(&m_semaphore); pthread_mutex_destroy(&m_send_list_lock); pthread_mutex_destroy(&m_recv_list_lock); cerr<<"STARTING POLLED MODE"<<endl; message[0] = 'P'; write(m_fd, message,1); sleep(1); End_session(m_fd); cerr << "IMU400CC_200 (done!)" << endl; } }; // end of IMU400CC_200 class void *imu400_send_thread (void *dev) { IMU400CC_200 *device = reinterpret_cast<IMU400CC_200*>(dev); cerr<<"IMU400CC_200 : Send thread starting."<<endl; if (device) { try { while(!device->m_killed) { pseudosem_wait(&device->m_semaphore); device->SendPacket(); } } catch (BaseException *e) { e->print(cerr); delete e; } } cerr<<"IMU400CC_200 : Send thread done."<<endl; return NULL; } void *imu400_recv_thread (void *dev) { IMU400CC_200 *device = reinterpret_cast<IMU400CC_200*>(dev); cerr<<"IMU400CC_200 : Receive thread starting."<<endl; if (device) { try { while (!device->m_killed) { device->ReceivePacket(); } } catch (BaseException *e) { e->print(cerr); delete e; } } cerr<<"IMU400CC_200 : Receive thread done."<<endl; return NULL; } }//namespace RobotFlow #endif |
|
From: Dominic L. <ma...@us...> - 2005-04-12 20:44:45
|
Update of /cvsroot/robotflow/RobotFlow/Vision/src In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv16521/src Modified Files: VisualFeatureDesc.cc Log Message: Fixed toVect function Index: VisualFeatureDesc.cc =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Vision/src/VisualFeatureDesc.cc,v retrieving revision 1.3 retrieving revision 1.4 diff -C2 -d -r1.3 -r1.4 *** VisualFeatureDesc.cc 12 Apr 2005 19:58:10 -0000 1.3 --- VisualFeatureDesc.cc 12 Apr 2005 20:44:16 -0000 1.4 *************** *** 157,169 **** } ! REGISTER_CONVERSION_TEMPLATE( ! VisualFeatureDesc<double>, ! Vector<VisualFeatureDesc<double> *>, ! VisualFeatDesc2VectorConversion); - static int dummy_template_vtable_init_for_VFDdouble_to_vector_VFDdouble = \ - vmethod()->registerFunct0(VisualFeatDesc2VectorConversion<VisualFeatureDesc<double>,Vector<VisualFeatureDesc<double> *> >,&typeid(VisualDoubleFeatureDesc),"toVect"); ! //REGISTER_VTABLE0(toVect, VisualFeatureDesc<double>, VisualFeatDesc2VectorConversion<VisualFeatureDesc<double>,Vector<VisualFeatureDesc<double> *> >, 6); ! } --- 157,171 ---- } ! static int dummy_init_for_visual_feature_to_vect = FD::vmethod()->registerFunct0( ! VisualFeatDesc2VectorConversion<VisualFeatureDesc<double>,Vector<VisualFeatureDesc<double>* > >, ! &typeid(VisualFeatureDesc<double>), ! "toVect"); ! ! //TODO working conversion! + //static int dummy_init_for_visual_feature_conversion = Conversion::addConvFunction + //<VisualFeatureDesc<double>, Vector<VisualFeatureDesc<double> *> > + //(VisualFeatDesc2VectorConversion<Vector<VisualFeatureDesc<double> *> >); ! }//namespace RobotFlow |
|
From: Dominic L. <ma...@us...> - 2005-04-12 20:09:20
|
Update of /cvsroot/robotflow/RobotFlow/Vision/include In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv28735/include Modified Files: ColorHistExtraction.h MeanShiftTracker.h VisualFeatureDesc.h VisualFeaturesExtraction.h VisualHistogramDesc.h VisualROI.h VisualTarget.h VisualTargetManager.h VisualTracker.h Log Message: fixed namespace problems Index: VisualFeaturesExtraction.h =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Vision/include/VisualFeaturesExtraction.h,v retrieving revision 1.2 retrieving revision 1.3 diff -C2 -d -r1.2 -r1.3 *** VisualFeaturesExtraction.h 12 Apr 2005 19:38:34 -0000 1.2 --- VisualFeaturesExtraction.h 12 Apr 2005 20:09:06 -0000 1.3 *************** *** 29,33 **** template <class FeatBaseType> ! class VisualFeaturesExtraction : public BufferedNode { public: --- 29,33 ---- template <class FeatBaseType> ! class VisualFeaturesExtraction : public FD::BufferedNode { public: *************** *** 37,41 **** } ! VisualFeaturesExtraction(string nodeName, ParameterSet params) : BufferedNode(nodeName, params) { --- 37,41 ---- } ! VisualFeaturesExtraction(std::string nodeName, FD::ParameterSet params) : BufferedNode(nodeName, params) { *************** *** 49,58 **** // Default routine to print a VisualFeaturesExtraction object to an output stream ! virtual void printOn(ostream &out) const = 0; // Default routine to read a VisualFeaturesExtraction object from an input stream ! virtual void readFrom(istream &in) = 0; ! virtual void calculate(int output_id, int count, Buffer &out) = 0; virtual void ExtractFeatures(IplImage *i_frame, VisualROI *i_roi) = 0; --- 49,58 ---- // Default routine to print a VisualFeaturesExtraction object to an output stream ! virtual void printOn(std::ostream &out) const = 0; // Default routine to read a VisualFeaturesExtraction object from an input stream ! virtual void readFrom(std::istream &in) = 0; ! virtual void calculate(int output_id, int count, FD::Buffer &out) = 0; virtual void ExtractFeatures(IplImage *i_frame, VisualROI *i_roi) = 0; Index: MeanShiftTracker.h =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Vision/include/MeanShiftTracker.h,v retrieving revision 1.2 retrieving revision 1.3 diff -C2 -d -r1.2 -r1.3 *** MeanShiftTracker.h 12 Apr 2005 19:38:34 -0000 1.2 --- MeanShiftTracker.h 12 Apr 2005 20:09:05 -0000 1.3 *************** *** 30,52 **** MeanShiftTracker(int i_maxNumMSIter, double i_minMSDistEpsilon); ! MeanShiftTracker(string nodeName, ParameterSet params); virtual ~MeanShiftTracker(); // Default routine to print a MeanShiftTracker object to an output stream ! void printOn(ostream &out) const { ! throw new GeneralException("Exception in MeanShiftTracker::printOn: method not yet implemented.",__FILE__,__LINE__); } // Default routine to read a MeanShiftTracker object from an input stream ! void readFrom(istream &in) { ! throw new GeneralException("Exception in MeanShiftTracker::printOn: method not yet implemented.",__FILE__,__LINE__); } ! virtual void request(int output_id, const ParameterSet &req); ! void calculate(int output_id, int count, Buffer &out); private: --- 30,52 ---- MeanShiftTracker(int i_maxNumMSIter, double i_minMSDistEpsilon); ! MeanShiftTracker(std::string nodeName, FD::ParameterSet params); virtual ~MeanShiftTracker(); // Default routine to print a MeanShiftTracker object to an output stream ! void printOn(std::ostream &out) const { ! throw new FD::GeneralException("Exception in MeanShiftTracker::printOn: method not yet implemented.",__FILE__,__LINE__); } // Default routine to read a MeanShiftTracker object from an input stream ! void readFrom(std::istream &in) { ! throw new FD::GeneralException("Exception in MeanShiftTracker::printOn: method not yet implemented.",__FILE__,__LINE__); } ! virtual void request(int output_id, const FD::ParameterSet &req); ! void calculate(int output_id, int count, FD::Buffer &out); private: Index: VisualTarget.h =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Vision/include/VisualTarget.h,v retrieving revision 1.2 retrieving revision 1.3 diff -C2 -d -r1.2 -r1.3 *** VisualTarget.h 12 Apr 2005 19:38:34 -0000 1.2 --- VisualTarget.h 12 Apr 2005 20:09:06 -0000 1.3 *************** *** 30,34 **** template <class FeatBaseType> ! class VisualTarget : public Object { public: --- 30,34 ---- template <class FeatBaseType> ! class VisualTarget : public FD::Object { public: *************** *** 42,46 **** VisualTarget(int i_id, VisualROI *i_roi, ! Vector<VisualFeatureDesc<FeatBaseType> *> *i_targetDesc) : m_valid(true), m_id(i_id), m_activeAge(0), m_passiveAge(0), m_curDescIdx(0), m_roi(NULL), m_targetDesc(NULL), m_cueWeights(NULL), --- 42,46 ---- VisualTarget(int i_id, VisualROI *i_roi, ! FD::Vector<VisualFeatureDesc<FeatBaseType> *> *i_targetDesc) : m_valid(true), m_id(i_id), m_activeAge(0), m_passiveAge(0), m_curDescIdx(0), m_roi(NULL), m_targetDesc(NULL), m_cueWeights(NULL), *************** *** 49,53 **** m_roi = new VisualROI(*i_roi); m_numDesc = i_targetDesc->size(); ! m_targetDesc = new Vector<VisualFeatureDesc<FeatBaseType> *>(m_numDesc); for (int i=0; i<m_numDesc; i++) { --- 49,53 ---- m_roi = new VisualROI(*i_roi); m_numDesc = i_targetDesc->size(); ! m_targetDesc = new FD::Vector<VisualFeatureDesc<FeatBaseType> *>(m_numDesc); for (int i=0; i<m_numDesc; i++) { *************** *** 95,100 **** } } ! catch (BaseException *e) { ! throw e->add(new GeneralException("Exception caught in VisualTarget::VisualTarget:",__FILE__,__LINE__)); } } --- 95,100 ---- } } ! catch (FD::BaseException *e) { ! throw e->add(new FD::GeneralException("Exception caught in VisualTarget::VisualTarget:",__FILE__,__LINE__)); } } *************** *** 151,180 **** return *this; } ! catch (BaseException *e) { ! throw e->add(new GeneralException("Exception caught in VisualTarget::operator=:",__FILE__,__LINE__)); } } // Default routine to print a VisualTarget object to an output stream ! void printOn(ostream &out) const { ! throw new GeneralException("Exception in VisualTarget::printOn: method not yet implemented.",__FILE__,__LINE__); } // Default routine to read a VisualTarget object from an input stream ! void readFrom(istream &in) { ! throw new GeneralException("Exception in VisualTarget::readFrom: method not yet implemented.",__FILE__,__LINE__); } ! void Adapt(Vector<VisualFeatureDesc<FeatBaseType> *> *i_desc, double i_rate) { if (m_numDesc != i_desc->size()) { ! throw new GeneralException("Exception in VisualTarget::Adapt: input descriptor vector size differs from current object's vector.",__FILE__,__LINE__); } for (int i=0; i<m_numDesc; i++) { if ((*m_targetDesc)[i]->GetType() != (*i_desc)[i]->GetType()) { ! throw new GeneralException("Exception in VisualTarget::Adapt: features descriptor must have the same type.",__FILE__,__LINE__); } --- 151,180 ---- return *this; } ! catch (FD::BaseException *e) { ! throw e->add(new FD::GeneralException("Exception caught in VisualTarget::operator=:",__FILE__,__LINE__)); } } // Default routine to print a VisualTarget object to an output stream ! void printOn(std::ostream &out) const { ! throw new FD::GeneralException("Exception in VisualTarget::printOn: method not yet implemented.",__FILE__,__LINE__); } // Default routine to read a VisualTarget object from an input stream ! void readFrom(std::istream &in) { ! throw new FD::GeneralException("Exception in VisualTarget::readFrom: method not yet implemented.",__FILE__,__LINE__); } ! void Adapt(FD::Vector<VisualFeatureDesc<FeatBaseType> *> *i_desc, double i_rate) { if (m_numDesc != i_desc->size()) { ! throw new FD::GeneralException("Exception in VisualTarget::Adapt: input descriptor vector size differs from current object's vector.",__FILE__,__LINE__); } for (int i=0; i<m_numDesc; i++) { if ((*m_targetDesc)[i]->GetType() != (*i_desc)[i]->GetType()) { ! throw new FD::GeneralException("Exception in VisualTarget::Adapt: features descriptor must have the same type.",__FILE__,__LINE__); } *************** *** 183,195 **** } ! void Adapt(Vector<VisualFeatureDesc<FeatBaseType> *> *i_desc, double *i_rate) { if (m_numDesc != i_desc->size()) { ! throw new GeneralException("Exception in VisualTarget::Adapt: input descriptor vector size differs from current object's vector.",__FILE__,__LINE__); } for (int i=0; i<m_numDesc; i++) { if ((*m_targetDesc)[i]->GetType() != (*i_desc)[i]->GetType()) { ! throw new GeneralException("Exception in VisualTarget::Adapt: features descriptor must have the same type.",__FILE__,__LINE__); } --- 183,195 ---- } ! void Adapt(FD::Vector<VisualFeatureDesc<FeatBaseType> *> *i_desc, double *i_rate) { if (m_numDesc != i_desc->size()) { ! throw new FD::GeneralException("Exception in VisualTarget::Adapt: input descriptor vector size differs from current object's vector.",__FILE__,__LINE__); } for (int i=0; i<m_numDesc; i++) { if ((*m_targetDesc)[i]->GetType() != (*i_desc)[i]->GetType()) { ! throw new FD::GeneralException("Exception in VisualTarget::Adapt: features descriptor must have the same type.",__FILE__,__LINE__); } *************** *** 198,205 **** } ! double Similarity(Vector<VisualFeatureDesc<FeatBaseType> *> *i_desc) { if (m_numDesc != i_desc->size()) { ! throw new GeneralException("Exception in VisualTarget::Similarity: input descriptor vector size differs from current object's vector.",__FILE__,__LINE__); } --- 198,205 ---- } ! double Similarity(FD::Vector<VisualFeatureDesc<FeatBaseType> *> *i_desc) { if (m_numDesc != i_desc->size()) { ! throw new FD::GeneralException("Exception in VisualTarget::Similarity: input descriptor vector size differs from current object's vector.",__FILE__,__LINE__); } *************** *** 208,212 **** for (int i=0; i<m_numDesc; i++) { if ((*m_targetDesc)[i]->GetType() != (*i_desc)[i]->GetType()) { ! throw new GeneralException("Exception in VisualTarget::Similarity: features descriptor must have the same type.",__FILE__,__LINE__); } --- 208,212 ---- for (int i=0; i<m_numDesc; i++) { if ((*m_targetDesc)[i]->GetType() != (*i_desc)[i]->GetType()) { ! throw new FD::GeneralException("Exception in VisualTarget::Similarity: features descriptor must have the same type.",__FILE__,__LINE__); } *************** *** 217,224 **** } ! double SimilarityWCueAdapt(Vector<VisualFeatureDesc<FeatBaseType> *> *i_desc, double i_rate) { if (m_numDesc != i_desc->size()) { ! throw new GeneralException("Exception in VisualTarget::SimilarityWCueAdapt: input descriptor vector size differs from current object's vector.",__FILE__,__LINE__); } --- 217,224 ---- } ! double SimilarityWCueAdapt(FD::Vector<VisualFeatureDesc<FeatBaseType> *> *i_desc, double i_rate) { if (m_numDesc != i_desc->size()) { ! throw new FD::GeneralException("Exception in VisualTarget::SimilarityWCueAdapt: input descriptor vector size differs from current object's vector.",__FILE__,__LINE__); } *************** *** 228,232 **** for (i=0; i<m_numDesc; i++) { if ((*m_targetDesc)[i]->GetType() != (*i_desc)[i]->GetType()) { ! throw new GeneralException("Exception in VisualTarget::SimilarityWCueAdapt: features descriptor must have the same type.",__FILE__,__LINE__); } --- 228,232 ---- for (i=0; i<m_numDesc; i++) { if ((*m_targetDesc)[i]->GetType() != (*i_desc)[i]->GetType()) { ! throw new FD::GeneralException("Exception in VisualTarget::SimilarityWCueAdapt: features descriptor must have the same type.",__FILE__,__LINE__); } *************** *** 253,257 **** sumCueSimInv = 1.0/sumCueSim; if (sumCueSimInv == 0.0) { ! throw new GeneralException("Exception in VisualTarget::SimilarityWCueAdapt: cannot have a target with all cue weights are zero.",__FILE__,__LINE__); } for (i=0; i<m_numDesc; i++) { --- 253,257 ---- sumCueSimInv = 1.0/sumCueSim; if (sumCueSimInv == 0.0) { ! throw new FD::GeneralException("Exception in VisualTarget::SimilarityWCueAdapt: cannot have a target with all cue weights are zero.",__FILE__,__LINE__); } for (i=0; i<m_numDesc; i++) { *************** *** 327,338 **** } ! Vector<VisualFeatureDesc<FeatBaseType> *> *GetDescriptorsVec() { return m_targetDesc; } ! const Vector<VisualFeatureDesc<FeatBaseType> *> *GetCstDescriptorsVec() const { ! return (const Vector<VisualFeatureDesc<FeatBaseType> *> *)m_targetDesc; } --- 327,338 ---- } ! FD::Vector<VisualFeatureDesc<FeatBaseType> *> *GetDescriptorsVec() { return m_targetDesc; } ! const FD::Vector<VisualFeatureDesc<FeatBaseType> *> *GetCstDescriptorsVec() const { ! return (const FD::Vector<VisualFeatureDesc<FeatBaseType> *> *)m_targetDesc; } *************** *** 340,344 **** { if (i_idx >= m_numDesc) { ! throw new GeneralException("Exception in VisualTarget::GetDescriptor: descriptor index is greater than the actual number of descriptors in current vector.",__FILE__,__LINE__); } --- 340,344 ---- { if (i_idx >= m_numDesc) { ! throw new FD::GeneralException("Exception in VisualTarget::GetDescriptor: descriptor index is greater than the actual number of descriptors in current vector.",__FILE__,__LINE__); } *************** *** 349,353 **** { if (i_idx >= m_numDesc) { ! throw new GeneralException("Exception in VisualTarget::GetCstDescriptor: descriptor index is greater than the actual number of descriptors in current vector.",__FILE__,__LINE__); } --- 349,353 ---- { if (i_idx >= m_numDesc) { ! throw new FD::GeneralException("Exception in VisualTarget::GetCstDescriptor: descriptor index is greater than the actual number of descriptors in current vector.",__FILE__,__LINE__); } *************** *** 368,372 **** { if (i_idx >= m_numDesc) { ! throw new GeneralException("Exception in VisualTarget::GetCueWeight: index is greater than the actual number of descriptors in current vector.",__FILE__,__LINE__); } --- 368,372 ---- { if (i_idx >= m_numDesc) { ! throw new FD::GeneralException("Exception in VisualTarget::GetCueWeight: index is greater than the actual number of descriptors in current vector.",__FILE__,__LINE__); } *************** *** 427,431 **** // Reset vector size m_numDesc = i_numDesc; ! m_targetDesc = new Vector<VisualFeatureDesc<FeatBaseType> *>(m_numDesc); m_cueWeights = new double[m_numDesc]; m_tmpCueProb = new double[m_numDesc]; --- 427,431 ---- // Reset vector size m_numDesc = i_numDesc; ! m_targetDesc = new FD::Vector<VisualFeatureDesc<FeatBaseType> *>(m_numDesc); m_cueWeights = new double[m_numDesc]; m_tmpCueProb = new double[m_numDesc]; *************** *** 436,440 **** { if (i_idx >= m_numDesc) { ! throw new GeneralException("Exception in VisualTarget::SetCurDescIdx: descriptor index is greater than the actual number of descriptors in current vector.",__FILE__,__LINE__); } --- 436,440 ---- { if (i_idx >= m_numDesc) { ! throw new FD::GeneralException("Exception in VisualTarget::SetCurDescIdx: descriptor index is greater than the actual number of descriptors in current vector.",__FILE__,__LINE__); } *************** *** 442,446 **** } ! void SetDescriptorsVec(Vector<VisualFeatureDesc<FeatBaseType> *> *i_descVec) { // Reset the number of descriptors --- 442,446 ---- } ! void SetDescriptorsVec(FD::Vector<VisualFeatureDesc<FeatBaseType> *> *i_descVec) { // Reset the number of descriptors *************** *** 455,459 **** { if (i_idx >= m_numDesc) { ! throw new GeneralException("Exception in VisualTarget::SetDescriptor: descriptor index is greater than the actual number of descriptors in current vector.",__FILE__,__LINE__); } --- 455,459 ---- { if (i_idx >= m_numDesc) { ! throw new FD::GeneralException("Exception in VisualTarget::SetDescriptor: descriptor index is greater than the actual number of descriptors in current vector.",__FILE__,__LINE__); } *************** *** 480,484 **** { if (i_idx >= m_numDesc) { ! throw new GeneralException("Exception in VisualTarget::SetCueWeight: index is greater than the actual number of descriptors in current vector.",__FILE__,__LINE__); } --- 480,484 ---- { if (i_idx >= m_numDesc) { ! throw new FD::GeneralException("Exception in VisualTarget::SetCueWeight: index is greater than the actual number of descriptors in current vector.",__FILE__,__LINE__); } *************** *** 494,498 **** int m_numDesc; int m_curDescIdx; ! Vector<VisualFeatureDesc<FeatBaseType> *> *m_targetDesc; double *m_cueWeights; double *m_tmpCueProb; --- 494,498 ---- int m_numDesc; int m_curDescIdx; ! FD::Vector<VisualFeatureDesc<FeatBaseType> *> *m_targetDesc; double *m_cueWeights; double *m_tmpCueProb; Index: VisualHistogramDesc.h =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Vision/include/VisualHistogramDesc.h,v retrieving revision 1.2 retrieving revision 1.3 diff -C2 -d -r1.2 -r1.3 *** VisualHistogramDesc.h 12 Apr 2005 19:38:34 -0000 1.2 --- VisualHistogramDesc.h 12 Apr 2005 20:09:06 -0000 1.3 *************** *** 65,69 **** VisualHistogramDesc(e_VISUALHIST_similarityType i_simType, bool i_normFlag, unsigned int i_numDimensions, ! const Vector<int> *i_numBins) : VisualFeatureDesc<BinType>(e_VISUALDESCRIPTOR_histogram), m_simType(i_simType), --- 65,69 ---- VisualHistogramDesc(e_VISUALHIST_similarityType i_simType, bool i_normFlag, unsigned int i_numDimensions, ! const FD::Vector<int> *i_numBins) : VisualFeatureDesc<BinType>(e_VISUALDESCRIPTOR_histogram), m_simType(i_simType), *************** *** 229,233 **** } else if (ch != '<') { ! throw new GeneralException ("VisualHistogramDesc::readFrom : Parse error: '<' expected",__FILE__,__LINE__); } --- 229,233 ---- } else if (ch != '<') { ! throw new FD::GeneralException ("VisualHistogramDesc::readFrom : Parse error: '<' expected",__FILE__,__LINE__); } *************** *** 241,245 **** if (m_numDimensions < 1) { ! throw new GeneralException ("VisualHistogramDesc::readFrom : invalid number of dimensions",__FILE__,__LINE__); } } --- 241,245 ---- if (m_numDimensions < 1) { ! throw new FD::GeneralException ("VisualHistogramDesc::readFrom : invalid number of dimensions",__FILE__,__LINE__); } } *************** *** 248,252 **** if (m_totalBins < 1) { ! throw new GeneralException ("VisualHistogramDesc::readFrom : invalid total number of bins",__FILE__,__LINE__); } } --- 248,252 ---- if (m_totalBins < 1) { ! throw new FD::GeneralException ("VisualHistogramDesc::readFrom : invalid total number of bins",__FILE__,__LINE__); } } *************** *** 296,309 **** } else { ! throw new GeneralException ("VisualHistogramDesc::readFrom : Unknown argument: " + tag,__FILE__,__LINE__); } if (!in) { ! throw new GeneralException ("VisualHistogramDesc::readFrom : Parse error trying to build " + tag,__FILE__,__LINE__); } in >> tag; if (tag != ">") { ! throw new GeneralException ("VisualHistogramDesc::readFrom : Parse error: '>' expected ",__FILE__,__LINE__); } } --- 296,309 ---- } else { ! throw new FD::GeneralException ("VisualHistogramDesc::readFrom : Unknown argument: " + tag,__FILE__,__LINE__); } if (!in) { ! throw new FD::GeneralException ("VisualHistogramDesc::readFrom : Parse error trying to build " + tag,__FILE__,__LINE__); } in >> tag; if (tag != ">") { ! throw new FD::GeneralException ("VisualHistogramDesc::readFrom : Parse error: '>' expected ",__FILE__,__LINE__); } } *************** *** 314,318 **** try { if (m_totalBins != i_size ) { ! throw new GeneralException ("VisualHistogramDesc::Similarity : number of histogram bins differs from current descriptor",__FILE__,__LINE__); } --- 314,318 ---- try { if (m_totalBins != i_size ) { ! throw new FD::GeneralException ("VisualHistogramDesc::Similarity : number of histogram bins differs from current descriptor",__FILE__,__LINE__); } *************** *** 320,325 **** return (this->*m_similarityFct)(i_candidate); } ! catch (BaseException *e) { ! throw e->add(new GeneralException("Exception caught in VisualHistogramDesc::Similarity:",__FILE__,__LINE__)); } } --- 320,325 ---- return (this->*m_similarityFct)(i_candidate); } ! catch (FD::BaseException *e) { ! throw e->add(new FD::GeneralException("Exception caught in VisualHistogramDesc::Similarity:",__FILE__,__LINE__)); } } *************** *** 329,337 **** try { if (m_totalBins != i_size ) { ! throw new GeneralException ("VisualHistogramDesc::Adapt : number of histogram bins differs from current descriptor",__FILE__,__LINE__); } if (i_rate < 0.0 || i_rate > 1.0) { ! throw new GeneralException ("VisualHistogramDesc::Adapt : adaptation rate must be in the interval [0.0,1.0]",__FILE__,__LINE__); } --- 329,337 ---- try { if (m_totalBins != i_size ) { ! throw new FD::GeneralException ("VisualHistogramDesc::Adapt : number of histogram bins differs from current descriptor",__FILE__,__LINE__); } if (i_rate < 0.0 || i_rate > 1.0) { ! throw new FD::GeneralException ("VisualHistogramDesc::Adapt : adaptation rate must be in the interval [0.0,1.0]",__FILE__,__LINE__); } *************** *** 349,354 **** (this->*m_adaptFct)(i_candidate, i_rate); } ! catch (BaseException *e) { ! throw e->add(new GeneralException("Exception caught in VisualHistogramDesc::Adapt:",__FILE__,__LINE__)); } } --- 349,354 ---- (this->*m_adaptFct)(i_candidate, i_rate); } ! catch (FD::BaseException *e) { ! throw e->add(new FD::GeneralException("Exception caught in VisualHistogramDesc::Adapt:",__FILE__,__LINE__)); } } *************** *** 358,362 **** { if (!m_bins) { ! throw new GeneralException ("VisualHistogramDesc::ComputeHistStd : histogram bins must be initialized.",__FILE__,__LINE__); } --- 358,362 ---- { if (!m_bins) { ! throw new FD::GeneralException ("VisualHistogramDesc::ComputeHistStd : histogram bins must be initialized.",__FILE__,__LINE__); } *************** *** 442,446 **** if (indx < 0 || indx >= m_totalBins) { ! throw new GeneralException ("VisualHistogramDesc::ComputeHistStd : invalid histogram bin index",__FILE__,__LINE__); } --- 442,446 ---- if (indx < 0 || indx >= m_totalBins) { ! throw new FD::GeneralException ("VisualHistogramDesc::ComputeHistStd : invalid histogram bin index",__FILE__,__LINE__); } *************** *** 468,472 **** { if (!m_bins) { ! throw new GeneralException ("VisualHistogramDesc::ComputeKernelWeightedHist : histogram bins must be initialized.",__FILE__,__LINE__); } --- 468,472 ---- { if (!m_bins) { ! throw new FD::GeneralException ("VisualHistogramDesc::ComputeKernelWeightedHist : histogram bins must be initialized.",__FILE__,__LINE__); } *************** *** 555,559 **** if (indx < 0 || indx >= m_totalBins) { ! throw new GeneralException ("VisualHistogramDesc::ComputeKernelWeightedHist : invalid histogram bin index",__FILE__,__LINE__); } --- 555,559 ---- if (indx < 0 || indx >= m_totalBins) { ! throw new FD::GeneralException ("VisualHistogramDesc::ComputeKernelWeightedHist : invalid histogram bin index",__FILE__,__LINE__); } *************** *** 589,596 **** void ComputeMSLocation(const FeatType* i_featMap, FeatType i_maxValPlusOne, BinType i_binIncScale, int i_width, int i_height, const VisualROI *i_roi, ! const BinType *i_refBins, double i_cueWeight, Vector<double> *o_msLoc) { if (!m_bins) { ! throw new GeneralException ("VisualHistogramDesc::ComputeMSLocation : histogram bins must be initialized.",__FILE__,__LINE__); } --- 589,596 ---- void ComputeMSLocation(const FeatType* i_featMap, FeatType i_maxValPlusOne, BinType i_binIncScale, int i_width, int i_height, const VisualROI *i_roi, ! const BinType *i_refBins, double i_cueWeight, FD::Vector<double> *o_msLoc) { if (!m_bins) { ! throw new FD::GeneralException ("VisualHistogramDesc::ComputeMSLocation : histogram bins must be initialized.",__FILE__,__LINE__); } *************** *** 664,668 **** if (indx < 0 || indx >= m_totalBins) { ! throw new GeneralException ("VisualHistogramDesc::ComputeMSLocation : invalid histogram bin index",__FILE__,__LINE__); } --- 664,668 ---- if (indx < 0 || indx >= m_totalBins) { ! throw new FD::GeneralException ("VisualHistogramDesc::ComputeMSLocation : invalid histogram bin index",__FILE__,__LINE__); } *************** *** 787,791 **** { if (m_totalBins != i_size ) { ! throw new GeneralException ("VisualHistogramDesc::SetFeatures : number of histogram bins differs from current descriptor",__FILE__,__LINE__); } --- 787,791 ---- { if (m_totalBins != i_size ) { ! throw new FD::GeneralException ("VisualHistogramDesc::SetFeatures : number of histogram bins differs from current descriptor",__FILE__,__LINE__); } *************** *** 824,829 **** return sqrt(1.0-BhattacharyyaCoeff(i_candidateBins)); } ! catch (BaseException *e) { ! throw e->add(new GeneralException("Exception caught in VisualHistogramDesc::BhattacharyyaDist:",__FILE__,__LINE__)); } } --- 824,829 ---- return sqrt(1.0-BhattacharyyaCoeff(i_candidateBins)); } ! catch (FD::BaseException *e) { ! throw e->add(new FD::GeneralException("Exception caught in VisualHistogramDesc::BhattacharyyaDist:",__FILE__,__LINE__)); } } Index: VisualROI.h =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Vision/include/VisualROI.h,v retrieving revision 1.2 retrieving revision 1.3 diff -C2 -d -r1.2 -r1.3 *** VisualROI.h 12 Apr 2005 19:38:34 -0000 1.2 --- VisualROI.h 12 Apr 2005 20:09:06 -0000 1.3 *************** *** 33,37 **** } e_VISUALROI_type; ! class VisualROI : public Object { public: --- 33,37 ---- } e_VISUALROI_type; ! class VisualROI : public FD::Object { public: *************** *** 48,55 **** // Default routine to print a VisualROI object to an output stream ! void printOn(ostream &out) const; // Default routine to read a VisualROI object from an input stream ! void readFrom(istream &in); void DrawROI(IplImage *io_frame, const unsigned char *i_color) const; --- 48,55 ---- // Default routine to print a VisualROI object to an output stream ! void printOn(std::ostream &out) const; // Default routine to read a VisualROI object from an input stream ! void readFrom(std::istream &in); void DrawROI(IplImage *io_frame, const unsigned char *i_color) const; Index: VisualFeatureDesc.h =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Vision/include/VisualFeatureDesc.h,v retrieving revision 1.2 retrieving revision 1.3 diff -C2 -d -r1.2 -r1.3 *** VisualFeatureDesc.h 12 Apr 2005 19:38:34 -0000 1.2 --- VisualFeatureDesc.h 12 Apr 2005 20:09:05 -0000 1.3 *************** *** 38,42 **** // template <class FeatBaseType> ! class VisualFeatureDesc : public Object { friend std::ostream &operator<<(std::ostream &o_out, const VisualFeatureDesc<FeatBaseType> &i_ref) --- 38,42 ---- // template <class FeatBaseType> ! class VisualFeatureDesc : public FD::Object { friend std::ostream &operator<<(std::ostream &o_out, const VisualFeatureDesc<FeatBaseType> &i_ref) *************** *** 47,52 **** return o_out; } ! catch (BaseException *e) { ! throw e->add(new GeneralException("Exception in VisualFeatureDesc::operator<<:",__FILE__,__LINE__)); } } --- 47,52 ---- return o_out; } ! catch (FD::BaseException *e) { ! throw e->add(new FD::GeneralException("Exception in VisualFeatureDesc::operator<<:",__FILE__,__LINE__)); } } *************** *** 59,64 **** return i_in; } ! catch (BaseException *e) { ! throw e->add(new GeneralException("Exception in VisualFeatureDesc::operator>>:",__FILE__,__LINE__)); } } --- 59,64 ---- return i_in; } ! catch (FD::BaseException *e) { ! throw e->add(new FD::GeneralException("Exception in VisualFeatureDesc::operator>>:",__FILE__,__LINE__)); } } *************** *** 106,110 **** virtual void printOn(std::ostream &out) const { ! throw new GeneralException("Exception in VisualFeatureDesc::printOn: cannot use base class routine.",__FILE__,__LINE__); } --- 106,110 ---- virtual void printOn(std::ostream &out) const { ! throw new FD::GeneralException("Exception in VisualFeatureDesc::printOn: cannot use base class routine.",__FILE__,__LINE__); } *************** *** 112,141 **** virtual void readFrom(std::istream &in) { ! throw new GeneralException("Exception in VisualFeatureDesc::readFrom: cannot use base class routine.",__FILE__,__LINE__); } virtual double Similarity(const FeatBaseType *i_candidate, unsigned int i_size) const { ! throw new GeneralException("Exception in VisualFeatureDesc::Similarity: cannot use base class routine.",__FILE__,__LINE__); } virtual void Adapt(const FeatBaseType *i_candidate, unsigned int i_size, double i_rate) { ! throw new GeneralException("Exception in VisualFeatureDesc::Adapt: cannot use base class routine.",__FILE__,__LINE__); } virtual unsigned int GetSize() const { ! throw new GeneralException("Exception in VisualFeatureDesc::GetSize: cannot use base class routine.",__FILE__,__LINE__); } virtual FeatBaseType *GetFeatures() { ! throw new GeneralException("Exception in VisualFeatureDesc::GetFeatures: cannot use base class routine.",__FILE__,__LINE__); } virtual const FeatBaseType *GetCstFeatures() const { ! throw new GeneralException("Exception in VisualFeatureDesc::GetCstFeatures: cannot use base class routine.",__FILE__,__LINE__); } --- 112,141 ---- virtual void readFrom(std::istream &in) { ! throw new FD::GeneralException("Exception in VisualFeatureDesc::readFrom: cannot use base class routine.",__FILE__,__LINE__); } virtual double Similarity(const FeatBaseType *i_candidate, unsigned int i_size) const { ! throw new FD::GeneralException("Exception in VisualFeatureDesc::Similarity: cannot use base class routine.",__FILE__,__LINE__); } virtual void Adapt(const FeatBaseType *i_candidate, unsigned int i_size, double i_rate) { ! throw new FD::GeneralException("Exception in VisualFeatureDesc::Adapt: cannot use base class routine.",__FILE__,__LINE__); } virtual unsigned int GetSize() const { ! throw new FD::GeneralException("Exception in VisualFeatureDesc::GetSize: cannot use base class routine.",__FILE__,__LINE__); } virtual FeatBaseType *GetFeatures() { ! throw new FD::GeneralException("Exception in VisualFeatureDesc::GetFeatures: cannot use base class routine.",__FILE__,__LINE__); } virtual const FeatBaseType *GetCstFeatures() const { ! throw new FD::GeneralException("Exception in VisualFeatureDesc::GetCstFeatures: cannot use base class routine.",__FILE__,__LINE__); } *************** *** 149,158 **** virtual void SetSize(unsigned int i_size) { ! throw new GeneralException("Exception in VisualFeatureDesc::SetSize: cannot use base class routine.",__FILE__,__LINE__); } virtual void SetFeatures(const FeatBaseType *i_ref, unsigned int i_size) { ! throw new GeneralException("Exception in VisualFeatureDesc::SetFeatures: cannot use base class routine.",__FILE__,__LINE__); } --- 149,158 ---- virtual void SetSize(unsigned int i_size) { ! throw new FD::GeneralException("Exception in VisualFeatureDesc::SetSize: cannot use base class routine.",__FILE__,__LINE__); } virtual void SetFeatures(const FeatBaseType *i_ref, unsigned int i_size) { ! throw new FD::GeneralException("Exception in VisualFeatureDesc::SetFeatures: cannot use base class routine.",__FILE__,__LINE__); } Index: VisualTargetManager.h =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Vision/include/VisualTargetManager.h,v retrieving revision 1.2 retrieving revision 1.3 diff -C2 -d -r1.2 -r1.3 *** VisualTargetManager.h 12 Apr 2005 19:38:34 -0000 1.2 --- VisualTargetManager.h 12 Apr 2005 20:09:06 -0000 1.3 *************** *** 29,57 **** namespace RobotFlow { ! class VisualTargetManager : public BufferedNode { ! friend class BufferedNode; public: VisualTargetManager(); ! VisualTargetManager(string nodeName, ParameterSet params); virtual ~VisualTargetManager(); // Default routine to print a VisualTargetManager object to an output stream ! void printOn(ostream &out) const { ! throw new GeneralException("Exception in VisualTargetManager::printOn: method not yet implemented.",__FILE__,__LINE__); } // Default routine to read a VisualTargetManager object from an input stream ! void readFrom(istream &in) { ! throw new GeneralException("Exception in VisualTargetManager::printOn: method not yet implemented.",__FILE__,__LINE__); } ! virtual void request(int output_id, const ParameterSet &req); ! void calculate(int output_id, int count, Buffer &out); private: --- 29,57 ---- namespace RobotFlow { ! class VisualTargetManager : public FD::BufferedNode { ! friend class FD::BufferedNode; public: VisualTargetManager(); ! VisualTargetManager(std::string nodeName, FD::ParameterSet params); virtual ~VisualTargetManager(); // Default routine to print a VisualTargetManager object to an output stream ! void printOn(std::ostream &out) const { ! throw new FD::GeneralException("Exception in VisualTargetManager::printOn: method not yet implemented.",__FILE__,__LINE__); } // Default routine to read a VisualTargetManager object from an input stream ! void readFrom(std::istream &in) { ! throw new FD::GeneralException("Exception in VisualTargetManager::printOn: method not yet implemented.",__FILE__,__LINE__); } ! virtual void request(int output_id, const FD::ParameterSet &req); ! void calculate(int output_id, int count, FD::Buffer &out); private: *************** *** 91,95 **** VisualTarget<double> *m_target; ! RCPtr<VisualTarget<double> > m_refTarget; //RCPtr<VisualROI> m_refROI; --- 91,95 ---- VisualTarget<double> *m_target; ! FD::RCPtr<VisualTarget<double> > m_refTarget; //RCPtr<VisualROI> m_refROI; Index: VisualTracker.h =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Vision/include/VisualTracker.h,v retrieving revision 1.2 retrieving revision 1.3 diff -C2 -d -r1.2 -r1.3 *** VisualTracker.h 12 Apr 2005 19:38:34 -0000 1.2 --- VisualTracker.h 12 Apr 2005 20:09:06 -0000 1.3 *************** *** 29,33 **** namespace RobotFlow { ! class VisualTracker : public BufferedNode { public: --- 29,33 ---- namespace RobotFlow { ! class VisualTracker : public FD::BufferedNode { public: *************** *** 37,41 **** } ! VisualTracker(string nodeName, ParameterSet params) : BufferedNode(nodeName, params) { --- 37,41 ---- } ! VisualTracker(std::string nodeName, FD::ParameterSet params) : BufferedNode(nodeName, params) { *************** *** 49,58 **** // Default routine to print a VisualTracker object to an output stream ! virtual void printOn(ostream &out) const = 0; // Default routine to read a VisualTracker object from an input stream ! virtual void readFrom(istream &in) = 0; ! virtual void calculate(int output_id, int count, Buffer &out) = 0; //virtual void TrackTarget() = 0; --- 49,58 ---- // Default routine to print a VisualTracker object to an output stream ! virtual void printOn(std::ostream &out) const = 0; // Default routine to read a VisualTracker object from an input stream ! virtual void readFrom(std::istream &in) = 0; ! virtual void calculate(int output_id, int count, FD::Buffer &out) = 0; //virtual void TrackTarget() = 0; Index: ColorHistExtraction.h =================================================================== RCS file: /cvsroot/robotflow/RobotFlow/Vision/include/ColorHistExtraction.h,v retrieving revision 1.2 retrieving revision 1.3 diff -C2 -d -r1.2 -r1.3 *** ColorHistExtraction.h 12 Apr 2005 19:38:34 -0000 1.2 --- ColorHistExtraction.h 12 Apr 2005 20:09:04 -0000 1.3 *************** *** 40,44 **** // ColorHistExtraction(int i_width, int i_height, ! int i_numChannels, const Vector<int> *i_numBins); // --- 40,44 ---- // ColorHistExtraction(int i_width, int i_height, ! int i_numChannels, const FD::Vector<int> *i_numBins); // *************** *** 50,59 **** // BufferedNode constructor // ! ColorHistExtraction(string nodeName, ParameterSet params); // // Constructor using input stream // ! ColorHistExtraction(istream &in) { readFrom(in); --- 50,59 ---- // BufferedNode constructor // ! ColorHistExtraction(std::string nodeName, FD::ParameterSet params); // // Constructor using input stream // ! ColorHistExtraction(std::istream &in) { readFrom(in); *************** *** 63,80 **** // Default routine to print a ColorHistExtraction object to an output stream ! void printOn(ostream &out) const { ! throw new GeneralException("Exception in ColorHistExtraction::printOn: method not yet implemented.",__FILE__,__LINE__); } // Default routine to read a ColorHistExtraction object from an input stream ! void readFrom(istream &in) { ! throw new GeneralException("Exception in ColorHistExtraction::readFrom: method not yet implemented.",__FILE__,__LINE__); } ! virtual void request(int output_id, const ParameterSet &req); ! void calculate(int output_id, int count, Buffer &out); void ExtractFeatures(VisualROI *i_roi); --- 63,80 ---- // Default routine to print a ColorHistExtraction object to an output stream ! void printOn(std::ostream &out) const { ! throw new FD::GeneralException("Exception in ColorHistExtraction::printOn: method not yet implemented.",__FILE__,__LINE__); } // Default routine to read a ColorHistExtraction object from an input stream ! void readFrom(std::istream &in) { ! throw new FD::GeneralException("Exception in ColorHistExtraction::readFrom: method not yet implemented.",__FILE__,__LINE__); } ! virtual void request(int output_id, const FD::ParameterSet &req); ! void calculate(int output_id, int count, FD::Buffer &out); void ExtractFeatures(VisualROI *i_roi); *************** *** 83,87 **** void EstimateMSLocation(const VisualTarget<double> *i_targetRef, ! int i_descIdx, Vector<double> *o_msLocVec); VisualFeatureDesc<double> *GetDescriptor() --- 83,87 ---- void EstimateMSLocation(const VisualTarget<double> *i_targetRef, ! int i_descIdx, FD::Vector<double> *o_msLocVec); VisualFeatureDesc<double> *GetDescriptor() *************** *** 96,100 **** private: ! void Initialize(const Vector<int> *i_numBins); private: --- 96,100 ---- private: ! void Initialize(const FD::Vector<int> *i_numBins); private: |
