$search
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2008, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Willow Garage nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00035 /* Author: Wim Meeussen */ 00036 /* modified for SRS by Alex Noyvirt*/ 00037 00038 #include "srs_people_tracking_filter/mcpdf_vector.h" 00039 #include "srs_people_tracking_filter/state_pos_vel.h" 00040 #include <assert.h> 00041 #include <vector> 00042 #include <std_msgs/Float64.h> 00043 #include "srs_people_tracking_filter/rgb.h" 00044 00045 00046 using namespace MatrixWrapper; 00047 using namespace BFL; 00048 using namespace tf; 00049 00050 static const unsigned int NUM_CONDARG = 1; 00051 00052 00053 MCPdfVector::MCPdfVector (unsigned int num_samples) 00054 : MCPdf<Vector3> ( num_samples, NUM_CONDARG ) 00055 {} 00056 00057 MCPdfVector::~MCPdfVector(){} 00058 00059 00060 WeightedSample<Vector3> 00061 MCPdfVector::SampleGet(unsigned int particle) const 00062 { 00063 assert ((int)particle >= 0 && particle < _listOfSamples.size()); 00064 return _listOfSamples[particle]; 00065 } 00066 00067 00068 Vector3 MCPdfVector::ExpectedValueGet() const 00069 { 00070 Vector3 pos(0,0,0); 00071 double current_weight; 00072 std::vector<WeightedSample<Vector3> >::const_iterator it_los; 00073 for ( it_los = _listOfSamples.begin() ; it_los != _listOfSamples.end() ; it_los++ ){ 00074 current_weight = it_los->WeightGet(); 00075 pos += (it_los->ValueGet() * current_weight); 00076 } 00077 00078 return Vector3(pos); 00079 } 00080 00081 00083 void MCPdfVector::getParticleCloud(const Vector3& step, double threshold, sensor_msgs::PointCloud& cloud) const 00084 { 00085 unsigned int num_samples = _listOfSamples.size(); 00086 assert(num_samples > 0); 00087 Vector3 m = _listOfSamples[0].ValueGet(); 00088 Vector3 M = _listOfSamples[0].ValueGet(); 00089 00090 // calculate min and max 00091 for (unsigned int s=0; s<num_samples; s++){ 00092 Vector3 v = _listOfSamples[s].ValueGet(); 00093 for (unsigned int i=0; i<3; i++){ 00094 if (v[i] < m[i]) m[i] = v[i]; 00095 if (v[i] > M[i]) M[i] = v[i]; 00096 } 00097 } 00098 00099 // get point cloud from histogram 00100 Matrix hist = getHistogram(m, M, step); 00101 unsigned int row = hist.rows(); 00102 unsigned int col = hist.columns(); 00103 unsigned int total = 0; 00104 unsigned int t = 0; 00105 for (unsigned int r=1; r<= row; r++) 00106 for (unsigned int c=1; c<= col; c++) 00107 if (hist(r,c) > threshold) total++; 00108 cout << "size total " << total << endl; 00109 00110 vector<geometry_msgs::Point32> points(total); 00111 vector<float> weights(total); 00112 sensor_msgs::ChannelFloat32 channel; 00113 for (unsigned int r=1; r<= row; r++) 00114 for (unsigned int c=1; c<= col; c++) 00115 if (hist(r,c) > threshold){ 00116 for (unsigned int i=0; i<3; i++) 00117 points[t].x = m[0] + (step[0] * r); 00118 points[t].y = m[1] + (step[1] * c); 00119 points[t].z = m[2]; 00120 weights[t] = rgb[999-(int)trunc(max(0.0,min(999.0,hist(r,c)*2*total*total)))]; 00121 t++; 00122 } 00123 cout << "points size " << points.size() << endl; 00124 cloud.header.frame_id = "base_link"; 00125 cloud.points = points; 00126 channel.name = "rgb"; 00127 channel.values = weights; 00128 cloud.channels.push_back(channel); 00129 } 00130 00131 00133 MatrixWrapper::Matrix MCPdfVector::getHistogram(const Vector3& m, const Vector3& M, const Vector3& step) const 00134 { 00135 unsigned int num_samples = _listOfSamples.size(); 00136 unsigned int rows = round((M[0]-m[0])/step[0]); 00137 unsigned int cols = round((M[1]-m[1])/step[1]); 00138 Matrix hist(rows, cols); 00139 hist = 0; 00140 00141 // calculate histogram 00142 for (unsigned int i=0; i<num_samples; i++){ 00143 Vector3 rel(_listOfSamples[i].ValueGet() - m); 00144 unsigned int r = round(rel[0] / step[0]); 00145 unsigned int c = round(rel[1] / step[1]); 00146 if (r >= 1 && c >= 1 && r <= rows && c <= cols) 00147 hist(r,c) += _listOfSamples[i].WeightGet(); 00148 } 00149 00150 return hist; 00151 } 00152 00153 00154 00155 unsigned int 00156 MCPdfVector::numParticlesGet() const 00157 { 00158 return _listOfSamples.size(); 00159 } 00160 00161