ShapeSensorModel.cc

Go to the documentation of this file.

#include "ShapeSensorModel.h"

#include "Motion/Kinematics.h"  // for kine variable
#include "Shared/Config.h"  // for config variable
#include "DualCoding/VRmixin.h"
#include "ShapeLandmarks.h"

#include <cmath>
#include <iostream>

using namespace DualCoding;

float const LocalShapeEvaluator::maxDist = 1e10;
float const LocalShapeEvaluator::stdevSq = 60*60;

LocalShapeEvaluator::LocalShapeEvaluator(ShapeSpace &localShS, ShapeSpace &worldShS) : 
  localLms(), worldLms() {
  PfRoot::loadLms(localShS.allShapes(), false, localLms);
  PfRoot::loadLms(worldShS.allShapes(), true, worldLms);
  std::cout << "LocalShapeEvaluator: " << localShS.allShapes().size() << " local shapes,   "
      << worldShS.allShapes().size() << " world shapes " << std::endl;
  if ( localLms.size()==0 || worldLms.size()==0 ) {
    std::cout << "ParticleFilter::loadLms found " << localLms.size() << " local and "
        << worldLms.size() << " world landmarks: can't localize!" << std::endl;
  }
}

void LocalShapeEvaluator::evaluate(LocalizationParticle& p) {
  unsigned int const nLocals = localLms.size();
  float particleViewX[nLocals],  particleViewY[nLocals], particleViewX2[nLocals],  particleViewY2[nLocals];
  int localMatches[nLocals];
  float localScores[nLocals];
  evaluateWorkhorse(p, nLocals, particleViewX, particleViewY, particleViewX2, particleViewY2,
        localMatches, localScores);
  //  for (unsigned int i=0; i<nLocals; i++)
  //    std::cout <<"  lm " << i << ":   " << localMatches[i] <<  " score " << localScores[i] << std::endl;
  //  std::cout << "    oldweight " << p.weight;
  updateWeight(p, localMatches, localScores);
  //  std::cout << "    newweight " << p.weight << std::endl;
}

void LocalShapeEvaluator::evaluateWorkhorse
(LocalizationParticle& p, const unsigned int nLocals,
 float particleViewX[], float particleViewY[], float particleViewX2[], float particleViewY2[],
 int localMatches[], float localScores[]) {
  // determine position of local space landmark in world given the current particle
  float const cosT = std::cos(-p.theta);
  float const sinT = std::sin(-p.theta);
  float const negSinT = -sinT;
  for ( unsigned int indexL=0; indexL < nLocals; indexL++ ) {
    PfRoot &landmark = *(localLms[indexL]);
    particleViewX[indexL] = landmark.x * cosT + landmark.y * sinT + p.x; 
    particleViewY[indexL] = landmark.x * negSinT + landmark.y * cosT + p.y;
    if ( landmark.type == lineDataType ) {
      const PfLine &line = static_cast<PfLine&>(landmark);
      particleViewX2[indexL] = line.x2 * cosT + line.y2 * sinT + p.x;
      particleViewY2[indexL] = line.x2 * negSinT + line.y2 * cosT + p.y;
    }
  }
  // Now compute match scores for the particle by finding matches between local landmarks and world landmarks.
  for ( unsigned int indexL = 0; indexL < nLocals; indexL++ ) {
    float distsq = maxDist; // distance > this is treated as a non-match; value should be < 1e10 to avoid underflows when not using log weights
    localMatches[indexL] = -1;  // assume no match unless we find something
    for ( unsigned int indexW=0; indexW<worldLms.size(); indexW++ ) {
      if ( localLms[indexL]->type == worldLms[indexW]->type &&
     localLms[indexL]->color == worldLms[indexW]->color ) {
  float const lx = particleViewX[indexL];
  float const ly = particleViewY[indexL];
  float const wx = worldLms[indexW]->x;
  float const wy = worldLms[indexW]->y;
  float tempDistsq;

  // dispatch on landmark type because lines and markers require special match handling
  switch ( localLms[indexL]->type ) {
  case lineDataType: {
    PfLine &localLine = *static_cast<PfLine*>(localLms[indexL]);
    PfLine &worldLine = *static_cast<PfLine*>(worldLms[indexW]);
    float tempDistsq1, tempDistsq2;
    // If endpoints are valid, compare distance between endpoints.
    // If not valid, measure perpendicular distance from the local endpoint
    // to the world line segment, if the projection of the endpoint onto the
    // segment occurs within the segment, not beyond it.  Instead of calculating
    // the projection we use a heuristic test: either the x or y endpoint value must
    // lie within the range of the line segment.
    if ( (localLine.valid1 && worldLine.valid1) ||
         !( (lx >= std::min(worldLine.x,worldLine.x2) && lx <= std::max(worldLine.x,worldLine.x2)) ||
      (ly >= std::min(worldLine.y,worldLine.y2) && ly <= std::max(worldLine.y,worldLine.y2)) ) )
      tempDistsq1 = (lx-wx)*(lx-wx) + (ly-wy)*(ly-wy);
    else {
      float const tempDist1 = distanceFromLine(lx,ly,worldLine);
      tempDistsq1 = tempDist1 * tempDist1;
    }
    float const lx2 = particleViewX2[indexL];
    float const ly2 = particleViewY2[indexL];
    float const wx2 = worldLine.x2;
    float const wy2 = worldLine.y2;
    if ( (localLine.valid2 && worldLine.valid2) ||
         !( (lx2 >= std::min(worldLine.x,worldLine.x2) && lx2 <= std::max(worldLine.x,worldLine.x2)) ||
      (ly2 >= std::min(worldLine.y,worldLine.y2) && ly2 <= std::max(worldLine.y,worldLine.y2)) ) )
      tempDistsq2 = (lx2-wx2)*(lx2-wx2) + (ly2-wy2)*(ly2-wy2);
    else {
      float const tempDist2 = distanceFromLine(lx2,ly2,worldLine);
      tempDistsq2 = tempDist2 * tempDist2;
    }
    AngPi const localOrient = localLine.orientation + p.theta;
    AngPi const odiff = worldLine.orientation - localOrient;
    float const odist = 500 * std::sin(odiff);
    float const odistsq = odist * odist;
    tempDistsq = tempDistsq1 + tempDistsq2 + odistsq; // plus orientation match term?
  }
    break;
  case ellipseDataType:
  case blobDataType: {
    tempDistsq = (lx-wx)*(lx-wx) + (ly-wy)*(ly-wy);
    break;
  }
  case markerDataType: {
    PfMarker &localMarker = *static_cast<PfMarker*>(localLms[indexL]);
    PfMarker &worldMarker = *static_cast<PfMarker*>(worldLms[indexW]);

    // check for marker "equality"
    if (localMarker.data->isMatchingMarker(worldMarker.data))
      tempDistsq = (lx-wx)*(lx-wx) + (ly-wy)*(ly-wy);
    else
      continue;
    break;
  }
  case aprilTagDataType: {
    PfAprilTag &localTag = *static_cast<PfAprilTag*>(localLms[indexL]);
    PfAprilTag &worldTag = *static_cast<PfAprilTag*>(worldLms[indexW]);

    // check for tag "equality"
    if ( localTag.data->getTagID() == worldTag.data->getTagID() )
      tempDistsq = (lx-wx)*(lx-wx) + (ly-wy)*(ly-wy);
    else
      continue;
    break;
  }
  case pointDataType:
    // don't try to match points; they're just placeholders, not landmarks
    tempDistsq = maxDist;
    break;
  default:
    std::cout << "ParticleFilter::computeMatchScore() can't match landmark type "
        << localLms[indexL]->type << std::endl;
    return;
  }

  // if this world landmark is a closer match, accept it
  if ( tempDistsq < distsq ) {
    distsq = tempDistsq;
    localMatches[indexL] = indexW;
  }
      }
    }

    if ( localMatches[indexL] != -1 || ! localLms[indexL]->mobile )
      localScores[indexL] = distsq;
  }
}

void LocalShapeEvaluator::updateWeight(LocalizationParticle &p, 
               int const localMatches[], float const localScores[]) {
  for (unsigned int i=0; i < localLms.size(); i++)
    if ( localMatches[i] != -1 )
      p.weight += -localScores[i]/stdevSq;
}

float LocalShapeEvaluator::distanceFromLine(coordinate_t x0, coordinate_t y0, PfLine &wline) {
  float const &x1 = wline.x;
  float const &y1 = wline.y;
  float const &x2 = wline.x2;
  float const &y2 = wline.y2;
  float const &length = wline.length;
  float const result = std::fabs((x2-x1)*(y1-y0) - (x1-x0)*(y2-y1)) / length;
  return result;
}

bool projectShapeToCamera(const LocalizationParticle &particle, const ShapeRoot *shp, float& px, float& py)
{
  //    cout << "PROJECT: marker to cam: part " << particle << " marker " << m << endl;

  // get centroid
  BaseData const &bd = shp->getData();
  Point worldPt = bd.getCentroid();

  // transform to local frame
  // lx = (wx - x) * cos(theta) + (wy - y) * sin(theta)
  // ...
  Point localPt;
  worldPt.coords[0] -= particle.x;
  worldPt.coords[1] -= particle.y;

  localPt.coords[0] = worldPt.coords[0] * std::cos(particle.theta) + worldPt.coords[1] * std::sin(particle.theta);
  localPt.coords[1] = worldPt.coords[0] * -std::sin(particle.theta) + worldPt.coords[1] * std::cos(particle.theta);
  localPt.coords[2] = worldPt.coords[2];
  //    cout << "PROJECT: transformed to local: " << localPt << endl;

  // check for shape behind the camera (bad projection)
  if (localPt.coords[0] < 0)
    return false;

  // transform from local to camera coordinates
  Point cameraPt(localPt);
#ifdef TGT_HAS_CAMERA
  cameraPt.applyTransform(kine->baseToLink(CameraFrameOffset));
#endif

  // compute pixels
  float camXnorm, camYnorm;
  config->vision.computePixel(cameraPt.coordX(), cameraPt.coordY(), cameraPt.coordZ(), camXnorm, camYnorm);

  // transform from normalized [-1,+1] coordinates to actual pixel coordinates
  px = (camXnorm + 1)/2*VRmixin::camSkS.getWidth();
  py = (camYnorm + 1)/2*VRmixin::camSkS.getHeight();
  return true;
}

void CameraShapeEvaluator::computeLikelihood(LocalizationParticle &particle)
{
  //    cout << "SENSOR MODEL: Computing likelihood for particle " << particle << endl;

  // look through all markers in sensor reading (image)
  SHAPEROOTVEC_ITERATE(cShS, cs)
    switch (cs->getType()) {
    case markerDataType : {
      BaseData const &bd = cs.getData();
      //cout << "SENSOR MODEL: Searching for match for " << cm << endl;
      fmat::Column<2> original(bd.getCentroid().coords);

      float bestMatch = std::log(pRandom);

      // look for best match in world space
      SHAPEROOTVEC_ITERATE(wShS, wm)

  if (wm->isSameTypeAs(cs)) {
    //cout << "SENSOR MODEL: Found matching markers: " << cm << " and " << wm << endl;
    // try and project marker
    fmat::Column<2> projected;
    DualCoding::ShapeRoot *targetLandMark(&wm);
    if (projectShapeToCamera(particle, targetLandMark, projected[0], projected[1])) {
      float prob = std::log((1- pRandom) * (normpdf(projected[0] - original[0], xvar) *
               normpdf(projected[1] - original[1], yvar)) + pRandom) * alpha;
      if (prob > bestMatch) {
        bestMatch = prob;
      }
    }
  }
      END_ITERATE;
      // don't need to normalize here because every particle will have same
      // number of markers in image
      particle.weight += bestMatch;
    }
      break;
    default:
      break;
    }
  END_ITERATE;
}