NaughtData.cc

Go to the documentation of this file.

#include <iostream>
#include <vector>
#include <list>
#include <math.h>

#include "SketchSpace.h"
#include "Sketch.h"
#include "Region.h"
#include "visops.h"

#include "ShapeSpace.h"
#include "ShapeRoot.h"

#include <DualCoding/NaughtData.h>
#include <DualCoding/ShapeNaught.h>

#include "Crew/MapBuilder.h"
#include "VRmixin.h"


using namespace std;

namespace DualCoding {

DATASTUFF_CC(NaughtData);

NaughtData::NaughtData(ShapeSpace& _space, const Point& _centroid, float _height, float _radius) :
  BaseData(_space, getStaticType()), centroid(_centroid), height(_height), radius(_radius) {}

bool NaughtData::isMatchFor(const ShapeRoot& other) const {
  if (!(isSameTypeAs(other) && isSameColorAs(other)))
    return false;
  
  const Shape<NaughtData>& otherCyl = ShapeRootTypeConst(other,NaughtData);
  
  float diff = (centroid - otherCyl->centroid).xyzNorm();
  //if (diff < 2*radius && fabs(radius - otherCyl->radius) < radius/2)
  if (diff < 3*radius && fabs(radius - otherCyl->radius) < radius*0.8) // *** quick fix for Tapia
    return true;
  else
    return false;
}

bool NaughtData::updateParams(const ShapeRoot& other, bool) {
  const Shape<NaughtData>& other_naught = ShapeRootTypeConst(other,NaughtData);
  if (other_naught->confidence <= 0)
    return false;
  const int other_conf = other_naught->confidence;
  centroid = (centroid*confidence + other_naught->getCentroid()*other_conf) / (confidence+other_conf);
  radius = (radius*confidence + other_naught->getRadius()*other_conf) / (confidence+other_conf);
  height = (height*confidence + other_naught->getHeight()*other_conf) / (confidence+other_conf);
  return true;
}

void NaughtData::printParams() const {
  cout << "Type = " << getTypeName() << "  ID=" << getId() << "  ParentID=" << getParentId() << endl;
  printf("  color = %d %d %d\n",getColor().red,getColor().green,getColor().blue);
  cout << "  centroid = " << centroid << endl;
  cout << "  radius = " << radius << endl;
  cout << "  height = " << height << endl;
}

void NaughtData::applyTransform(const fmat::Transform& Tmat, const ReferenceFrameType_t newref) {
  centroid.applyTransform(Tmat, newref);
}

void NaughtData::projectToGround(const fmat::Transform& camToBase, const PlaneEquation& groundplane) {
  PlaneEquation raisedGroundPlane(groundplane);
  raisedGroundPlane.setDisplacement(groundplane.getDisplacement() + height);
  centroid.projectToGround(camToBase, raisedGroundPlane);
}

// Extract Naught similar to the ellipse by finding a region, checking to see if its of the 
// appropriate shape to be a Naught and then creating said naught
std::vector<Shape<NaughtData> > NaughtData::extractNaughts(const Sketch<bool> &sketch, 
                                                           const fmat::Column<3>& dimensions) {
  NEW_SKETCH_N(labels,uint,visops::oldlabelcc(sketch,visops::EightWayConnect));
  const int REGION_THRESH = 25;  // ignore regions smaller than this
  list<Region> regionlist = Region::extractRegions(labels,REGION_THRESH);
  std::vector<Shape<NaughtData> > naughts;

  // A naught is accepted if the number of pixels in the outer 2/3 is
  // 5 times greater than the number in the inner 1/3, and if the
  // two sums along each semi-axis are relatively equal to each other.
  for ( Region reg : regionlist ) {
    Point center = reg.findCentroid();
    float semimaj = reg.findSemiMajorAxisLength();
    float semimin = reg.findSemiMinorAxisLength();
    float orient = reg.findPrincipalAxisOrientation();
    float tcos = cos(orient), tsin = sin(orient);
    float majthird = semimaj / 3, minthird = semimin / 3;
    int counts[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
    for ( float r = 0; r < semimaj+1; r++ ) {
      float x1 = center.coordX() + r * tcos;
      float y1 = center.coordY() + r * tsin;
      float x2 = center.coordX() - r * tcos;
      float y2 = center.coordY() - r * tsin;
      if ( x1 >= 0 && x1 < sketch.width && y1 >= 0 && y1 < sketch.height )
        counts[0][int(r/majthird)] += sketch(x1,y1);
      if ( x2 >= 0 && x2 < sketch.width && y2 >= 0 && y2 < sketch.height )
        counts[1][int(r/majthird)] += sketch(x2,y2);
    }
    for ( float r = 0; r < semimin+1; r++ ) {
      float x3 = center.coordX() + r * tsin;
      float y3 = center.coordY() + r * tcos;
      float x4 = center.coordX() - r * tsin;
      float y4 = center.coordY() - r * tcos;
      if ( x3 >= 0 && x3 < sketch.width && y3 >= 0 && y3 < sketch.height )
        counts[2][int(r/minthird)] += sketch(x3,y3);
      if ( x4 >= 0 && x4 < sketch.width && y4 >= 0 && y4 < sketch.height )
        counts[3][int(r/minthird)] += sketch(x4,y4);
    }
    // cout << " centroid=" << reg.findCentroid() << " area=" << reg.findArea() 
    //     << " semimaj=" << semimaj << " semimin=" << semimin << " counts ";
    bool good = true;
    int sums[4];
    for ( int i = 0; i<4; i++ ) {
      sums[i] = counts[i][1] + counts[i][2];
      if ( ! (sums[i] > 5*counts[i][0] && sums[i] >= 2) )
        good = false;
      sums[i] += 5; // so ratios computed below aren't too sensitive to small counts
      // for ( int j=0; j<3; j++ )
      //  cout << " " << counts[i][j];
      // cout << " / ";
    }
    // now check the ratios of sums to make sure the shape is reasonably symmetric
    if ( float(min(sums[0],sums[1])) / max(sums[0],sums[1]) < 0.8 ||
         float(min(sums[2],sums[3])) / max(sums[2],sums[3]) < 0.8 )
      good = false;
    if ( good ) {   // all tests passed
      Shape<NaughtData> temp_naught(reg, dimensions[2]); 
      temp_naught->setParentId(sketch->getViewableId());
      temp_naught->setColor(sketch->getColor());
      naughts.push_back(Shape<NaughtData>(temp_naught));
    }
  }
  return naughts;
}

Sketch<bool>* NaughtData::render() const {
  SketchSpace &SkS = space->getDualSpace();
  fmat::Column<3> ctr(centroid.getCoords());
  SkS.applyTmat(ctr);
  const float &cx = ctr[0];
  const float &cy = ctr[1];
  fmat::Column<3> rad = fmat::pack(radius,0,0);
  SkS.applyTmat(rad);
  const float &scaledRadius = fabs(rad[0]);
  Sketch<bool>* result = new Sketch<bool>(SkS, "render("+getName()+")");
  (*result)->inheritFrom(*this);
  *result = 0;
  for ( float dx=-scaledRadius; dx<=scaledRadius; dx+=0.2 ) {
    float dy = sqrt(scaledRadius*scaledRadius - dx*dx);
      int const px = round(cx + dx);
      int const py1 = round(cy - dy);
      int const py2 = round(cy + dy);
      if ( px >= 0 && px < result->width) {
        if ( py1 >= 0 && py1 < result->height )
          (*result)(px,py1) = true;
        if ( py2 >= 0 && py2 < result->height )
          (*result)(px,py2) = true;
      }
  }
  return result;
}

} // namespace