flirtlib: ShapeContext.cpp Source File

00001 #include "ShapeContext.h"
00002 
00003 ShapeContextGenerator::ShapeContextGenerator(double minRho, double maxRho, unsigned int binRho, unsigned int binPhi)
00004 {
00005     setEdges(minRho, maxRho, binRho, binPhi);
00006 }
00007 
00008 ShapeContextGenerator::ShapeContextGenerator(const std::vector<double>& rhoEdges, const std::vector<double>& phiEdges):
00009     m_rhoEdges(rhoEdges),
00010     m_phiEdges(phiEdges)
00011 {
00012 
00013 }
00014 
00015 void ShapeContextGenerator::setEdges(double minRho, double maxRho, unsigned int binRho, unsigned int binPhi){
00016     m_rhoEdges.resize(binRho+1);
00017     m_phiEdges.resize(binPhi+1);
00018     double minPhi = -M_PI, maxPhi = M_PI;
00019     for(unsigned int i = 0; i <= binRho; i++){
00020                 m_rhoEdges[i] = minRho + double(i)*(maxRho - minRho)/double(binRho);
00021     }
00022     for(unsigned int i = 0; i <= binPhi; i++){
00023                 m_phiEdges[i] = minPhi + double(i)*(maxPhi - minPhi)/double(binPhi);
00024     }
00025 }
00026 
00027 
00028 Descriptor* ShapeContextGenerator::describe(const InterestPoint& point, const LaserReading& reading){
00029     return describe(point.getPosition(), reading);
00030 }
00031         
00032 Descriptor* ShapeContextGenerator::describe(const OrientedPoint2D& point, const LaserReading& reading){
00033     unsigned int accumulator = 0;
00034     ShapeContext * shape = new ShapeContext();
00035     shape->getHistogram().resize(m_phiEdges.size() - 1, std::vector<double>(m_rhoEdges.size() - 1, 0.));
00036     for(unsigned int i = 0; i < reading.getWorldCartesian().size(); i++){
00037                 Point2D difference = reading.getWorldCartesian()[i] - point;
00038                 double distance = hypot(difference.x, difference.y);
00039                 if ((distance >= m_rhoEdges[0] && distance < m_rhoEdges[m_rhoEdges.size() - 1])){
00040                         for(unsigned int rho = 0; rho < m_rhoEdges.size() - 1; rho++){
00041                                 if((distance < m_rhoEdges[rho + 1] && distance >= m_rhoEdges[rho])){
00042                                         double angle = atan2(difference.y, difference.x);
00043                                         angle = normAngle(angle - point.theta, -M_PI);
00044                                         for(unsigned int phi = 0; phi < m_phiEdges.size() - 1; phi++){
00045                                                 if(angle < m_phiEdges[phi + 1] && angle >= m_phiEdges[phi]){
00046                                                         shape->getHistogram()[phi][rho] += 1.;
00047                                                         accumulator += 1;
00048                                                 }
00049                                         }
00050                                 }
00051                         }
00052                 }
00053     }
00054     int size = shape->getHistogram().size() * shape->getHistogram().front().size();
00055     for(unsigned int i = 0; i < shape->getHistogram().size(); i++){
00056         for(unsigned int j = 0; j < shape->getHistogram()[i].size(); j++){
00057             shape->getHistogram()[i][j] = accumulator ? shape->getHistogram()[i][j]/double(accumulator) : 1./double(size);
00058         }
00059     }
00060     shape->setDistanceFunction(m_distanceFunction);
00061     return shape;
00062 }
00063         
00064 Descriptor* ShapeContext::clone() const{
00065     return new ShapeContext(*this);
00066 }
00067         
00068 double ShapeContext::distance(const Descriptor* descriptor) const {
00069     const ShapeContext *shapeContext = dynamic_cast<const ShapeContext *>(descriptor);
00070     if(!m_distanceFunction || !shapeContext){
00071         return 10e16;
00072     }
00073     return m_distanceFunction->distance(this->getHistogram(), shapeContext->getHistogram());
00074 }