00001
00002
00003 #include <cmath>
00004 #include <iostream>
00005
00006 #include "MapBuilder.h"
00007 #include "PFShapeLocalization.h"
00008 #include "ShapeLocalizationParticle.h"
00009 #include "ShapeSpace.h"
00010 #include "ShapeFuns.h"
00011 #include "VRmixin.h"
00012
00013 using namespace std;
00014
00015 namespace DualCoding {
00016
00017 ParticleShapeEvaluator::ParticleShapeEvaluator(ShapeSpace &localShS, ShapeSpace &worldShS) :
00018 localLms(), worldLms(),
00019 maxDist(0),
00020 localMatches(), numMatches(0), localScores(),
00021 particleViewX(), particleViewY(), particleViewX2(), particleViewY2()
00022 {
00023 PfRoot::loadLms(localShS.allShapes(), false, localLms);
00024 PfRoot::loadLms(worldShS.allShapes(), true, worldLms);
00025 if ( localLms.size()==0 || worldLms.size()==0 ) {
00026 cout << "ParticleFilter::loadLms found " << localLms.size() << " local and "
00027 << worldLms.size() << " world landmarks: can't localize!" << endl;
00028 return;
00029 }
00030 unsigned int nlocal = localLms.size();
00031 localScores.resize(nlocal);
00032 localMatches.resize(nlocal);
00033 particleViewX.resize(nlocal);
00034 particleViewY.resize(nlocal);
00035 particleViewX2.resize(nlocal);
00036 particleViewY2.resize(nlocal);
00037 }
00038
00039 void ParticleShapeEvaluator::evaluate(LocalizationParticle& part) {
00040
00041 float const cosT = cos(-part.theta);
00042 float const sinT = sin(-part.theta);
00043 float const negSinT = -sinT;
00044
00045 for ( unsigned int j=0; j < localLms.size(); j++ ) {
00046 PfRoot &landmark = *(localLms[j]);
00047 particleViewX[j] = landmark.x * cosT + landmark.y * sinT + part.x;
00048 particleViewY[j] = landmark.x * negSinT + landmark.y * cosT + part.y;
00049 if ( landmark.type == lineDataType ) {
00050 const PfLine &line = static_cast<PfLine&>(landmark);
00051 particleViewX2[j] = line.x2 * cosT + line.y2 * sinT + part.x;
00052 particleViewY2[j] = line.x2 * negSinT + line.y2 * cosT + part.y;
00053 }
00054 }
00055
00056
00057 numMatches=0;
00058 for ( unsigned int j = 0; j<localLms.size(); j++ ) {
00059 float distsq = HUGE_VAL;
00060 localMatches[j] = -1;
00061 for ( unsigned int k=0; k<worldLms.size(); k++ ) {
00062 if ( localLms[j]->type == worldLms[k]->type &&
00063 localLms[j]->color == worldLms[k]->color ) {
00064 float const lx = particleViewX[j];
00065 float const ly = particleViewY[j];
00066 float const wx = worldLms[k]->x;
00067 float const wy = worldLms[k]->y;
00068 float tempDistsq;
00069 switch ( localLms[j]->type ) {
00070 case lineDataType: {
00071 PfLine &localLine = *static_cast<PfLine*>(localLms[j]);
00072 PfLine &worldLine = *static_cast<PfLine*>(worldLms[k]);
00073 float tempDistsq1, tempDistsq2;
00074
00075
00076
00077
00078
00079
00080 if ( (localLine.valid1 && worldLine.valid1) ||
00081 !( lx >= min(worldLine.x,worldLine.x2) &&
00082 lx <= max(worldLine.x,worldLine.x2) ||
00083 ly >= min(worldLine.y,worldLine.y2) &&
00084 ly <= max(worldLine.y,worldLine.y2) ) )
00085 tempDistsq1 = (lx-wx)*(lx-wx) + (ly-wy)*(ly-wy);
00086 else {
00087 float const tempDist1 = distanceFromLine(lx,ly,worldLine);
00088 tempDistsq1 = tempDist1 * tempDist1;
00089 }
00090 float const lx2 = particleViewX2[j];
00091 float const ly2 = particleViewY2[j];
00092 float const wx2 = worldLine.x2;
00093 float const wy2 = worldLine.y2;
00094 if ( (localLine.valid2 && worldLine.valid2) ||
00095 !( lx2 >= min(worldLine.x,worldLine.x2) &&
00096 lx2 <= max(worldLine.x,worldLine.x2) ||
00097 ly2 >= min(worldLine.y,worldLine.y2) &&
00098 ly2 <= max(worldLine.y,worldLine.y2) ) )
00099 tempDistsq2 = (lx2-wx2)*(lx2-wx2) + (ly2-wy2)*(ly2-wy2);
00100 else {
00101 float const tempDist2 = distanceFromLine(lx2,ly2,worldLine);
00102 tempDistsq2 = tempDist2 * tempDist2;
00103 }
00104 AngPi const localOrient = localLine.orientation + part.theta;
00105 AngPi const odiff = worldLine.orientation - localOrient;
00106 float const odist = 500 * sin(odiff);
00107 float const odistsq = odist * odist;
00108 tempDistsq = tempDistsq1 + tempDistsq2 + odistsq;
00109 }
00110 break;
00111 case ellipseDataType:
00112 case pointDataType:
00113 case blobDataType: {
00114 tempDistsq = (lx-wx)*(lx-wx) + (ly-wy)*(ly-wy);
00115 break;
00116 }
00117 default:
00118 std::cout << "ParticleFilter::computeMatchScore() can't match landmark type "
00119 << localLms[j]->type << std::endl;
00120 return;
00121 }
00122 if ( tempDistsq < distsq ) {
00123 distsq = tempDistsq;
00124 localMatches[j] = k;
00125 }
00126 }
00127 }
00128
00129 if ( localMatches[j] != -1 || !localLms[j]->mobile )
00130 localScores[numMatches++]=distsq;
00131 }
00132 }
00133
00134 float ParticleShapeEvaluator::distanceFromLine(coordinate_t x0, coordinate_t y0, PfLine &wline) {
00135 float const &x1 = wline.x;
00136 float const &y1 = wline.y;
00137 float const &x2 = wline.x2;
00138 float const &y2 = wline.y2;
00139 float const &length = wline.length;
00140 float const result = fabs((x2-x1)*(y1-y0) - (x1-x0)*(y2-y1)) / length;
00141 return result;
00142 }
00143
00144 void PFShapeLocalization::setAgent() const {
00145 const PFShapeLocalization::particle_type& best = particles[bestIndex];
00146 VRmixin::mapBuilder.setAgent(Point(best.x,best.y,0,allocentric), best.theta);
00147 }
00148
00149 void PFShapeLocalization::setPosition(float const x, float const y, AngTwoPi const orientation, float variance) {
00150 LocalizationParticle part(x, y, orientation);
00151 setPosition(part, variance);
00152 }
00153
00154 void PFShapeLocalization::setWorldBounds(const Shape<PolygonData> &bounds) {
00155 PFShapeDistributionPolicy<LocalizationParticle> *dist =
00156 dynamic_cast<PFShapeDistributionPolicy<LocalizationParticle> *>(&(getResamplingPolicy()->getDistributionPolicy()));
00157 if ( dist != NULL )
00158 dist->setWorldBounds(bounds);
00159 else
00160 cout << "Error: setWorldBounds found wrong type of DistributionPolicy" << endl;
00161 }
00162
00163 void PFShapeLocalization::displayParticles(float const howmany) const {
00164 ShapeSpace &wShS = sensorModel->getWorldShS();
00165 wShS.deleteShapes<LocalizationParticleData>();
00166 NEW_SHAPE(best, LocalizationParticleData, new LocalizationParticleData(wShS,particles[bestIndex]));
00167 best->setColor(VRmixin::mapBuilder.getAgent()->getColor());
00168 if ( howmany <= 0 ) return;
00169 unsigned int increment;
00170 if ( howmany <= 1.0 )
00171 increment = (unsigned int)ceil(1.0/howmany);
00172 else
00173 increment = (unsigned int)ceil(particles.size()/howmany);
00174 for (unsigned int i=0; i<particles.size(); i+=increment)
00175 if ( i != bestIndex ) {
00176 NEW_SHAPE(pt, LocalizationParticleData, new LocalizationParticleData(wShS,particles[i]));
00177 }
00178 }
00179
00180 }