$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 00037 #include "srs_people_tracking_filter/mcpdf_pos_vel.h" 00038 #include <assert.h> 00039 #include <vector> 00040 #include <std_msgs/Float64.h> 00041 #include "srs_people_tracking_filter/rgb.h" 00042 00043 00044 using namespace MatrixWrapper; 00045 using namespace BFL; 00046 using namespace tf; 00047 00048 static const unsigned int NUM_CONDARG = 1; 00049 00050 00051 MCPdfPosVel::MCPdfPosVel (unsigned int num_samples) 00052 : MCPdf<StatePosVel> ( num_samples, NUM_CONDARG ) 00053 {} 00054 00055 MCPdfPosVel::~MCPdfPosVel(){} 00056 00057 00058 WeightedSample<StatePosVel> 00059 MCPdfPosVel::SampleGet(unsigned int particle) const 00060 { 00061 assert ((int)particle >= 0 && particle < _listOfSamples.size()); 00062 return _listOfSamples[particle]; 00063 } 00064 00065 00066 StatePosVel MCPdfPosVel::ExpectedValueGet() const 00067 { 00068 tf::Vector3 pos(0,0,0); tf::Vector3 vel(0,0,0); 00069 double current_weight; 00070 std::vector<WeightedSample<StatePosVel> >::const_iterator it_los; 00071 for ( it_los = _listOfSamples.begin() ; it_los != _listOfSamples.end() ; it_los++ ){ 00072 current_weight = it_los->WeightGet(); 00073 pos += (it_los->ValueGet().pos_ * current_weight); 00074 vel += (it_los->ValueGet().vel_ * current_weight); 00075 } 00076 return StatePosVel(pos, vel); 00077 } 00078 00079 00081 void MCPdfPosVel::getParticleCloud(const tf::Vector3& step, double threshold, sensor_msgs::PointCloud& cloud) const 00082 { 00083 unsigned int num_samples = _listOfSamples.size(); 00084 assert(num_samples > 0); 00085 Vector3 m = _listOfSamples[0].ValueGet().pos_; 00086 Vector3 M = _listOfSamples[0].ValueGet().pos_; 00087 00088 // calculate min and max 00089 for (unsigned int s=0; s<num_samples; s++){ 00090 Vector3 v = _listOfSamples[s].ValueGet().pos_; 00091 for (unsigned int i=0; i<3; i++){ 00092 if (v[i] < m[i]) m[i] = v[i]; 00093 if (v[i] > M[i]) M[i] = v[i]; 00094 } 00095 } 00096 00097 // get point cloud from histogram 00098 Matrix hist = getHistogramPos(m, M, step); 00099 unsigned int row = hist.rows(); 00100 unsigned int col = hist.columns(); 00101 unsigned int total = 0; 00102 unsigned int t = 0; 00103 for (unsigned int r=1; r<= row; r++) 00104 for (unsigned int c=1; c<= col; c++) 00105 if (hist(r,c) > threshold) total++; 00106 00107 vector<geometry_msgs::Point32> points(total); 00108 vector<float> weights(total); 00109 sensor_msgs::ChannelFloat32 channel; 00110 for (unsigned int r=1; r<= row; r++) 00111 for (unsigned int c=1; c<= col; c++) 00112 if (hist(r,c) > threshold){ 00113 for (unsigned int i=0; i<3; i++) 00114 points[t].x = m[0] + (step[0] * r); 00115 points[t].y = m[1] + (step[1] * c); 00116 points[t].z = m[2]; 00117 weights[t] = rgb[999-(int)trunc(max(0.0,min(999.0,hist(r,c)*2*total*total)))]; 00118 t++; 00119 } 00120 cloud.header.frame_id = "odom_combined"; 00121 cloud.points = points; 00122 channel.name = "rgb"; 00123 channel.values = weights; 00124 cloud.channels.push_back(channel); 00125 } 00126 00127 00129 MatrixWrapper::Matrix MCPdfPosVel::getHistogramPos(const Vector3& m, const Vector3& M, const Vector3& step) const 00130 { 00131 return getHistogram(m, M, step, true); 00132 } 00133 00134 00136 MatrixWrapper::Matrix MCPdfPosVel::getHistogramVel(const Vector3& m, const Vector3& M, const Vector3& step) const 00137 { 00138 return getHistogram(m, M, step, false); 00139 } 00140 00141 00143 MatrixWrapper::Matrix MCPdfPosVel::getHistogram(const Vector3& m, const Vector3& M, const Vector3& step, bool pos_hist) const 00144 { 00145 unsigned int num_samples = _listOfSamples.size(); 00146 unsigned int rows = round((M[0]-m[0])/step[0]); 00147 unsigned int cols = round((M[1]-m[1])/step[1]); 00148 Matrix hist(rows, cols); 00149 hist = 0; 00150 00151 // calculate histogram 00152 for (unsigned int i=0; i<num_samples; i++){ 00153 Vector3 rel; 00154 if (pos_hist) 00155 rel = _listOfSamples[i].ValueGet().pos_ - m; 00156 else 00157 rel = _listOfSamples[i].ValueGet().vel_ - m; 00158 00159 unsigned int r = round(rel[0] / step[0]); 00160 unsigned int c = round(rel[1] / step[1]); 00161 if (r >= 1 && c >= 1 && r <= rows && c <= cols) 00162 hist(r,c) += _listOfSamples[i].WeightGet(); 00163 } 00164 00165 return hist; 00166 } 00167 00168 00169 00170 unsigned int 00171 MCPdfPosVel::numParticlesGet() const 00172 { 00173 return _listOfSamples.size(); 00174 } 00175 00176