<commit_before>#pragma ident "$Id$"

//============================================================================
//
//  This file is part of GPSTk, the GPS Toolkit.
//
//  The GPSTk is free software; you can redistribute it and/or modify
//  it under the terms of the GNU Lesser General Public License as published
//  by the Free Software Foundation; either version 2.1 of the License, or
//  any later version.
//
//  The GPSTk is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//  GNU Lesser General Public License for more details.
//
//  You should have received a copy of the GNU Lesser General Public
//  License along with GPSTk; if not, write to the Free Software Foundation,
//  Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
//
//  Copyright 2004, The University of Texas at Austin
//
//============================================================================

//============================================================================
//
//This software developed by Applied Research Laboratories at the University of
//Texas at Austin, under contract to an agency or agencies within the U.S.
//Department of Defense. The U.S. Government retains all rights to use,
//duplicate, distribute, disclose, or release this software.
//
//Pursuant to DoD Directive 523024
//
// DISTRIBUTION STATEMENT A: This software has been approved for public
//                           release, distribution is unlimited.
//
//=============================================================================

/**
 * @file SP3Data.cpp
 * Encapsulate SP3 file data, including I/O
 */

#include "SP3Stream.hpp"
#include "SP3Header.hpp"
#include "SP3Data.hpp"
#include "StringUtils.hpp"
#include "CivilTime.hpp"
#include "GPSWeekSecond.hpp"

using namespace gpstk::StringUtils;
using namespace std;

namespace Rinex3
{
   void SP3Data::reallyPutRecord(FFStream& ffs) const
      throw(std::exception, FFStreamError, StringException)
   {
      SP3Stream& strm = dynamic_cast<SP3Stream&>(ffs);

      string line;
      cout << flag << endl;
      if(flag == '*') {// output Epoch Header Record
         line = "* ";
         line += ((CivilTime)time).printf(" %4Y %2m %2d %2H %2M");
         line += " " + rightJustify(((CivilTime)time).printf("%.8f"),11);
      }
      else {           // output Position and Clock OR Velocity and Clock Rate Record
         line = flag;
         if(version == 'c')
            line += SP3SatID(sat).toString();
         else
            line += rightJustify(asString(sat.id),3);
         line += rightJustify(asString(x[0],6),14);
         line += rightJustify(asString(x[1],6),14);
         line += rightJustify(asString(x[2],6),14);
         line += rightJustify(asString(clk,6),14);
         if(version == 'c') {
            line += rightJustify(asString(sig[0]),3);
            line += rightJustify(asString(sig[1]),3);
            line += rightJustify(asString(sig[2]),3);
            line += rightJustify(asString(sig[3]),4);
            if(flag == 'P') {
               line += string(" ");
               line += (clockEventFlag ? string("E") : string(" "));
               line += (clockPredFlag ? string("P") : string(" "));
               line += string("  ");
               line += (orbitManeuverFlag ? string("M") : string(" "));
               line += (orbitPredFlag ? string("P") : string(" "));
            }
            //else {
            //   line += string("       ");
            //}
         }

         // if version is 'c' and correlation flag is set,
         // output P|V Correlation Record
         if(version == 'c' && correlationFlag) {
            // first output the P|V record
            strm << line << endl;

            // now output the correlation record
            if(flag == 'P') line = "EP ";
            else line = "EV ";
            line += rightJustify(asString(sdev[0]),5);
            line += rightJustify(asString(sdev[1]),5);
            line += rightJustify(asString(sdev[2]),5);
            line += rightJustify(asString(sdev[3]),8);
            for(int i=0; i<6; i++)
               line += rightJustify(asString(correlation[i]),9);
         }
      }

      // write the line
      strm << line << endl;
   }

   void SP3Data::dump(ostream& s) const
   {
      s << flag << " " << sat
         << " " << ((CivilTime)time).printf("%Y/%02m/%02d %2H:%02M:%06.3f = ") << ((GPSWeekSecond)time).printf("%F/%10.3g");
      if(flag != '*') {
         s << fixed << setprecision(6)
           << " X=" << setw(14) << x[0]
           << " Y=" << setw(14) << x[1]
           << " Z=" << setw(14) << x[2]
           << " C=" << setw(14) << clk;

         if(version == 'c') {
            s << " sX=" << setw(2) << sig[0]
              << " sY=" << setw(2) << sig[1]
              << " sZ=" << setw(2) << sig[2]
              << " sC=" << setw(3) << sig[3];
            if(flag == 'P')
              s << " " << (clockEventFlag ? "clockEvent" : "-")
                << " " << (clockPredFlag ? "clockPrediction" : "-")
                << " " << (orbitManeuverFlag ? "orbitManeuver" : "-")
                << " " << (orbitPredFlag ? "orbitPrediction" : "-");
            if(correlationFlag)
               s << endl
                 << 'E' << flag
                 << " cXX=" << setw(4) << sdev[0]
                 << " cYY=" << setw(4) << sdev[1]
                 << " cZZ=" << setw(4) << sdev[2]
                 << " cCC=" << setw(7) << sdev[3]
                 << " cXY=" << setw(8) << correlation[0]
                 << " cXZ=" << setw(8) << correlation[1]
                 << " cXC=" << setw(8) << correlation[2]
                 << " cYZ=" << setw(8) << correlation[3]
                 << " cYC=" << setw(8) << correlation[4]
                 << " cZC=" << setw(8) << correlation[5];
         }
      }
      s << endl;
   };

   void SP3Data::reallyGetRecord(FFStream& ffs)
      throw(std::exception, FFStreamError, StringException)
   {
      SP3Stream& strm = dynamic_cast<SP3Stream&>(ffs);

      correlationFlag = false;
      int status = 0;                                          // initial status = 0
      while(1) {
         // set the time in the record
         time = strm.currentEpoch;

         // ---------------------------------------------------------
         // process the buffer containing the last line read
         if(strm.buffer.size() < 3) {
            // nothing in buffer - do nothing here, get another line
            ;
         }

         else if(strm.buffer.substr(0,3) == string("EOF")) {      // 'EOF' record
            // if a data record has been processed during this call, then
            // return and let the next call process this EOF.
            //if(status == 1) throw - found EOF right after an epoch record
            if(status > 1) break;

            // this next read had better fail - if it does, an exception will
            // be thrown, and the FFStreamError created next won't get thrown
            strm.formattedGetLine(strm.buffer, true);

            FFStreamError err("EOF text found but file didn't end");
            GPSTK_THROW(err);
         }

         else if(strm.buffer[0] == '*') {                         // Epoch record
            // if another record has been process during this call, quit now
            if(status > 0) break;
            status = 1;                                           // epoch status = 1

            // throw if the line is short
            if(strm.buffer.size() <= 30) {
               FFStreamError err("Invalid line length "+asString(strm.buffer.size()));
               GPSTK_THROW(err);
            }

            // parse the epoch line

            flag = strm.buffer[0];
		cout << "WE GOT HERE" << flag << endl;
            int year = asInt(strm.buffer.substr(3,4));
            int month = asInt(strm.buffer.substr(8,2));
            int dom = asInt(strm.buffer.substr(11,2));
            int hour = asInt(strm.buffer.substr(14,2));
            int minute = asInt(strm.buffer.substr(17,2));
            double second = asInt(strm.buffer.substr(20,10));
            CivilTime t;
            try {
               t = CivilTime(year, month, dom, hour, minute, second);
            }
            catch (gpstk::Exception& e) {
               FFStreamError fe("Invalid time in:" + strm.buffer);
               GPSTK_THROW(fe);
            }
            time = strm.currentEpoch = t;
         }

         else if(strm.buffer[0] == 'P' || strm.buffer[0] == 'V') {// P|V record
            // if nothing, or epoch record, was processed during this call,
            // process this P|V, otherwise (P|V or EP|V were processed), quit now
            if(status > 0) break;
            status = 2;                                           // P|V status = 2

            flag = strm.buffer[0];     // P or V

            // 6/20/07 IGS SP3c files sometimes have short lines, apparently implying
            // zero sigma and no flags...add the following to be tolerant of this.
            if (version == 'a' && (strm.buffer.substr(1,1)=="G" ||
                                   strm.buffer.substr(1,1)=="R" ||
                                   strm.buffer.substr(1,1)=="L" ||
                                   strm.buffer.substr(1,1)=="E"))
            {
               version='c';
            }

            if (version == 'c' && strm.buffer.size() < 73 && strm.buffer.size() > 59)
               leftJustify(strm.buffer,73);

            // throw if the line is short
            if ((version == 'a' && strm.buffer.size() < 60) ||
               (version == 'c' && strm.buffer.size() < 73) ) {
               FFStreamError err("Invalid line length "+asString(strm.buffer.size()));
               GPSTK_THROW(err);
            }

            // parse the line
            if(version == 'a')
               sat = SatID(asInt(strm.buffer.substr(1, 3)), SP3SatID::systemGPS);
            else
               sat = SP3SatID(strm.buffer.substr(1,3));

            x[0] = asDouble(strm.buffer.substr(4,14));
            x[1] = asDouble(strm.buffer.substr(18,14));
            x[2] = asDouble(strm.buffer.substr(32,14));
            clk = asDouble(strm.buffer.substr(46,14));

            if(version == 'c') {
               // get sigmas from P|V record
               sig[0] = asInt(strm.buffer.substr(61,2));
               sig[1] = asInt(strm.buffer.substr(64,2));
               sig[2] = asInt(strm.buffer.substr(67,2));
               sig[3] = asInt(strm.buffer.substr(70,3));

               // get flags
               if(flag == 'P') {
                  clockEventFlag = clockPredFlag
                     = orbitManeuverFlag = orbitPredFlag = false;
                  if(strm.buffer.size() >= 75 && strm.buffer[74] == 'E')
                     clockEventFlag = true;
                  if(strm.buffer.size() >= 76 && strm.buffer[75] == 'P')
                     clockPredFlag = true;
                  if(strm.buffer.size() >= 79 && strm.buffer[78] == 'M')
                     orbitManeuverFlag = true;
                  if(strm.buffer.size() >= 80 && strm.buffer[79] == 'P')
                     orbitPredFlag = true;
               }
            }

         }
         else if(strm.buffer[0] == 'E' &&                      // EP|EV record
               (strm.buffer[1] == 'P' || strm.buffer[1] == 'V')) {

            // always process an EP|V immediately, since it must follow P|V
            status = 3;                                        // EP|V status = 3

            // throw if correlation record did not follow corresponding P|V record
            if(strm.buffer[1] != flag) {
               Exception e("SP3c correlation record mismatched with previous P|V");
               GPSTK_THROW(e);
            }

            // throw if line is short
            if(strm.buffer.size()<80) {
               FFStreamError err("Invalid SP3c correlation line length "
                  + asString(strm.buffer.size()));
               GPSTK_THROW(err);
            }

            sdev[0] = abs(asInt(strm.buffer.substr(4,4)));
            sdev[1] = abs(asInt(strm.buffer.substr(9,4)));
            sdev[2] = abs(asInt(strm.buffer.substr(14,4)));
            sdev[3] = abs(asInt(strm.buffer.substr(19,7)));
            correlation[0] = asInt(strm.buffer.substr(27,8));
            correlation[1] = asInt(strm.buffer.substr(36,8));
            correlation[2] = asInt(strm.buffer.substr(45,8));
            correlation[3] = asInt(strm.buffer.substr(54,8));
            correlation[4] = asInt(strm.buffer.substr(63,8));
            correlation[5] = asInt(strm.buffer.substr(72,8));

            // tell the caller that correlation data is now present
            correlationFlag = true;
         }

         else {                              // Unknown record
            FFStreamError err("Unknown line label " + strm.buffer.substr(0,2));
            GPSTK_THROW(err);
         }

         // ---------------------------------------------------------
         // read next line into the buffer
         strm.formattedGetLine(strm.buffer);

         // ---------------------------------------------------------
         // quit if EP|EV was processed
         if(status == 3) break;
         // go back if buffer was empty    (0)
         // go back if epoch was processed (1)
         // go back if P|V was processed   (2)
      }

   }   // end reallyGetRecord()

} // namespace
<commit_msg>Checking in a checkpoint revision of SP3Data while I figureout the last  bug<commit_after>#pragma ident "$Id$"

//============================================================================
//
//  This file is part of GPSTk, the GPS Toolkit.
//
//  The GPSTk is free software; you can redistribute it and/or modify
//  it under the terms of the GNU Lesser General Public License as published
//  by the Free Software Foundation; either version 2.1 of the License, or
//  any later version.
//
//  The GPSTk is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//  GNU Lesser General Public License for more details.
//
//  You should have received a copy of the GNU Lesser General Public
//  License along with GPSTk; if not, write to the Free Software Foundation,
//  Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
//
//  Copyright 2004, The University of Texas at Austin
//
//============================================================================

//============================================================================
//
//This software developed by Applied Research Laboratories at the University of
//Texas at Austin, under contract to an agency or agencies within the U.S.
//Department of Defense. The U.S. Government retains all rights to use,
//duplicate, distribute, disclose, or release this software.
//
//Pursuant to DoD Directive 523024
//
// DISTRIBUTION STATEMENT A: This software has been approved for public
//                           release, distribution is unlimited.
//
//=============================================================================

/**
 * @file SP3Data.cpp
 * Encapsulate SP3 file data, including I/O
 */

#include "SP3Stream.hpp"
#include "SP3Header.hpp"
#include "SP3Data.hpp"
#include "StringUtils.hpp"
#include "CivilTime.hpp"
#include "GPSWeekSecond.hpp"

using namespace gpstk::StringUtils;
using namespace std;

namespace Rinex3
{
   void SP3Data::reallyPutRecord(FFStream& ffs) const
      throw(std::exception, FFStreamError, StringException)
   {
      SP3Stream& strm = dynamic_cast<SP3Stream&>(ffs);

      string line;
      if(flag == '*') {// output Epoch Header Record
         line = "* ";
         line += ((CivilTime)time).printf(" %4Y %2m %2d %2H %2M");
         line += " " + rightJustify(((CivilTime)time).printf("%.8f"),11);
      }
      else {           // output Position and Clock OR Velocity and Clock Rate Record
         line = flag;
         if(version == 'c')
            line += SP3SatID(sat).toString();
         else
            line += rightJustify(asString(sat.id),3);
         line += rightJustify(asString(x[0],6),14);
         line += rightJustify(asString(x[1],6),14);
         line += rightJustify(asString(x[2],6),14);
         line += rightJustify(asString(clk,6),14);
         if(version == 'c') {
            line += rightJustify(asString(sig[0]),3);
            line += rightJustify(asString(sig[1]),3);
            line += rightJustify(asString(sig[2]),3);
            line += rightJustify(asString(sig[3]),4);
            if(flag == 'P') {
               line += string(" ");
               line += (clockEventFlag ? string("E") : string(" "));
               line += (clockPredFlag ? string("P") : string(" "));
               line += string("  ");
               line += (orbitManeuverFlag ? string("M") : string(" "));
               line += (orbitPredFlag ? string("P") : string(" "));
            }
            //else {
            //   line += string("       ");
            //}
         }

         // if version is 'c' and correlation flag is set,
         // output P|V Correlation Record
         if(version == 'c' && correlationFlag) {
            // first output the P|V record
            strm << line << endl;

            // now output the correlation record
            if(flag == 'P') line = "EP ";
            else line = "EV ";
            line += rightJustify(asString(sdev[0]),5);
            line += rightJustify(asString(sdev[1]),5);
            line += rightJustify(asString(sdev[2]),5);
            line += rightJustify(asString(sdev[3]),8);
            for(int i=0; i<6; i++)
               line += rightJustify(asString(correlation[i]),9);
         }
      }

      // write the line
      strm << line << endl;
   }

   void SP3Data::dump(ostream& s) const
   {
      s << flag << " " << sat
         << " " << ((CivilTime)time).printf("%Y/%02m/%02d %2H:%02M:%06.3f = ") << ((GPSWeekSecond)time).printf("%F/%10.3g");
      if(flag != '*') {
         s << fixed << setprecision(6)
           << " X=" << setw(14) << x[0]
           << " Y=" << setw(14) << x[1]
           << " Z=" << setw(14) << x[2]
           << " C=" << setw(14) << clk;

         if(version == 'c') {
            s << " sX=" << setw(2) << sig[0]
              << " sY=" << setw(2) << sig[1]
              << " sZ=" << setw(2) << sig[2]
              << " sC=" << setw(3) << sig[3];
            if(flag == 'P')
              s << " " << (clockEventFlag ? "clockEvent" : "-")
                << " " << (clockPredFlag ? "clockPrediction" : "-")
                << " " << (orbitManeuverFlag ? "orbitManeuver" : "-")
                << " " << (orbitPredFlag ? "orbitPrediction" : "-");
            if(correlationFlag)
               s << endl
                 << 'E' << flag
                 << " cXX=" << setw(4) << sdev[0]
                 << " cYY=" << setw(4) << sdev[1]
                 << " cZZ=" << setw(4) << sdev[2]
                 << " cCC=" << setw(7) << sdev[3]
                 << " cXY=" << setw(8) << correlation[0]
                 << " cXZ=" << setw(8) << correlation[1]
                 << " cXC=" << setw(8) << correlation[2]
                 << " cYZ=" << setw(8) << correlation[3]
                 << " cYC=" << setw(8) << correlation[4]
                 << " cZC=" << setw(8) << correlation[5];
         }
      }
      s << endl;
   };

   void SP3Data::reallyGetRecord(FFStream& ffs)
      throw(std::exception, FFStreamError, StringException)
   {
      SP3Stream& strm = dynamic_cast<SP3Stream&>(ffs);

      correlationFlag = false;
      int status = 0;                                          // initial status = 0
      while(1) {
         // set the time in the record
         time = strm.currentEpoch;

         // ---------------------------------------------------------
         // process the buffer containing the last line read
         if(strm.buffer.size() < 3) {
            // nothing in buffer - do nothing here, get another line
            ;
         }

         else if(strm.buffer.substr(0,3) == string("EOF")) {      // 'EOF' record
            // if a data record has been processed during this call, then
            // return and let the next call process this EOF.
            //if(status == 1) throw - found EOF right after an epoch record
            if(status > 1) break;

            // this next read had better fail - if it does, an exception will
            // be thrown, and the FFStreamError created next won't get thrown
            strm.formattedGetLine(strm.buffer, true);

            FFStreamError err("EOF text found but file didn't end");
            GPSTK_THROW(err);
         }

         else if(strm.buffer[0] == '*') {                         // Epoch record
            // if another record has been process during this call, quit now
            if(status > 0) break;
            status = 1;                                           // epoch status = 1

            // throw if the line is short
            if(strm.buffer.size() <= 30) {
               FFStreamError err("Invalid line length "+asString(strm.buffer.size()));
               GPSTK_THROW(err);
            }

            // parse the epoch line

            flag = strm.buffer[0];
            int year = asInt(strm.buffer.substr(3,4));
            int month = asInt(strm.buffer.substr(8,2));
            int dom = asInt(strm.buffer.substr(11,2));
            int hour = asInt(strm.buffer.substr(14,2));
            int minute = asInt(strm.buffer.substr(17,2));
            double second = asInt(strm.buffer.substr(20,10));
            CivilTime t;
            try {
               t = CivilTime(year, month, dom, hour, minute, second);
            }
            catch (gpstk::Exception& e) {
               FFStreamError fe("Invalid time in:" + strm.buffer);
               GPSTK_THROW(fe);
            }
            time = strm.currentEpoch = t;
         }

         else if(strm.buffer[0] == 'P' || strm.buffer[0] == 'V') {// P|V record
            // if nothing, or epoch record, was processed during this call,
            // process this P|V, otherwise (P|V or EP|V were processed), quit now
            if(status > 0) break;
            status = 2;                                           // P|V status = 2

            flag = strm.buffer[0];     // P or V

            // 6/20/07 IGS SP3c files sometimes have short lines, apparently implying
            // zero sigma and no flags...add the following to be tolerant of this.
            if (version == 'a' && (strm.buffer.substr(1,1)=="G" ||
                                   strm.buffer.substr(1,1)=="R" ||
                                   strm.buffer.substr(1,1)=="L" ||
                                   strm.buffer.substr(1,1)=="E"))
            {
               version='c';
            }

            if (version == 'c' && strm.buffer.size() < 73 && strm.buffer.size() > 59)
               leftJustify(strm.buffer,73);

            // throw if the line is short
            if ((version == 'a' && strm.buffer.size() < 60) ||
               (version == 'c' && strm.buffer.size() < 73) ) {
               FFStreamError err("Invalid line length "+asString(strm.buffer.size()));
               GPSTK_THROW(err);
            }

            // parse the line
            if(version == 'a')
               sat = SatID(asInt(strm.buffer.substr(1, 3)), SP3SatID::systemGPS);
            else
               sat = SP3SatID(strm.buffer.substr(1,3));

            x[0] = asDouble(strm.buffer.substr(4,14));
            x[1] = asDouble(strm.buffer.substr(18,14));
            x[2] = asDouble(strm.buffer.substr(32,14));
            clk = asDouble(strm.buffer.substr(46,14));

            if(version == 'c') {
               // get sigmas from P|V record
               sig[0] = asInt(strm.buffer.substr(61,2));
               sig[1] = asInt(strm.buffer.substr(64,2));
               sig[2] = asInt(strm.buffer.substr(67,2));
               sig[3] = asInt(strm.buffer.substr(70,3));

               // get flags
               if(flag == 'P') {
                  clockEventFlag = clockPredFlag
                     = orbitManeuverFlag = orbitPredFlag = false;
                  if(strm.buffer.size() >= 75 && strm.buffer[74] == 'E')
                     clockEventFlag = true;
                  if(strm.buffer.size() >= 76 && strm.buffer[75] == 'P')
                     clockPredFlag = true;
                  if(strm.buffer.size() >= 79 && strm.buffer[78] == 'M')
                     orbitManeuverFlag = true;
                  if(strm.buffer.size() >= 80 && strm.buffer[79] == 'P')
                     orbitPredFlag = true;
               }
            }

         }
         else if(strm.buffer[0] == 'E' &&                      // EP|EV record
               (strm.buffer[1] == 'P' || strm.buffer[1] == 'V')) {

            // always process an EP|V immediately, since it must follow P|V
            status = 3;                                        // EP|V status = 3

            // throw if correlation record did not follow corresponding P|V record
            if(strm.buffer[1] != flag) {
               Exception e("SP3c correlation record mismatched with previous P|V");
               GPSTK_THROW(e);
            }

            // throw if line is short
            if(strm.buffer.size()<80) {
               FFStreamError err("Invalid SP3c correlation line length "
                  + asString(strm.buffer.size()));
               GPSTK_THROW(err);
            }

            sdev[0] = abs(asInt(strm.buffer.substr(4,4)));
            sdev[1] = abs(asInt(strm.buffer.substr(9,4)));
            sdev[2] = abs(asInt(strm.buffer.substr(14,4)));
            sdev[3] = abs(asInt(strm.buffer.substr(19,7)));
            correlation[0] = asInt(strm.buffer.substr(27,8));
            correlation[1] = asInt(strm.buffer.substr(36,8));
            correlation[2] = asInt(strm.buffer.substr(45,8));
            correlation[3] = asInt(strm.buffer.substr(54,8));
            correlation[4] = asInt(strm.buffer.substr(63,8));
            correlation[5] = asInt(strm.buffer.substr(72,8));

            // tell the caller that correlation data is now present
            correlationFlag = true;
         }

         else {                              // Unknown record
            FFStreamError err("Unknown line label " + strm.buffer.substr(0,2));
            GPSTK_THROW(err);
         }

         // ---------------------------------------------------------
         // read next line into the buffer
         strm.formattedGetLine(strm.buffer);

         // ---------------------------------------------------------
         // quit if EP|EV was processed
         if(status == 3) break;
         // go back if buffer was empty    (0)
         // go back if epoch was processed (1)
         // go back if P|V was processed   (2)
      }

   }   // end reallyGetRecord()

} // namespace
<|endoftext|>


<commit_before>/* Numenta Platform for Intelligent Computing (NuPIC)
 * Copyright (C) 2016, Numenta, Inc.  Unless you have an agreement
 * with Numenta, Inc., for a separate license for this software code, the
 * following terms and conditions apply:
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Affero Public License version 3 as
 * published by the Free Software Foundation.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 * See the GNU Affero Public License for more details.
 *
 * You should have received a copy of the GNU Affero Public License
 * along with this program.  If not, see http://www.gnu.org/licenses.
 *
 * http://numenta.org/licenses/
 * ---------------------------------------------------------------------
 */

#ifndef NUPIC_UTIL_SLIDING_WINDOW_HPP
#define NUPIC_UTIL_SLIDING_WINDOW_HPP

#include <vector>

#include <nupic/types/Types.hpp>
#include <nupic/utils/Log.hpp>

namespace nupic {
  namespace util {

    template<class T>
    class SlidingWindow {
      public:
        SlidingWindow(UInt maxCapacity);
        SlidingWindow(UInt maxCapacity, std::vector<T> initialData);
        const UInt maxCapacity;
        const T NEUTRAL_VALUE = 0.0;
        UInt size() const;
        /** append new value to the end of the buffer and handle the "overflows"-may pop the first element if full.
         :return addition(+) neutral value (that is 0) when size()< maxCapacity; when full - return the value of the dropped element
        */
        T append(T newValue);
        /**
        :return unordered content (data ) of this sl. window; call linearize() if you need them oredered from oldest->newest
        */
        const std::vector<T>& getData() const;

        bool operator==(const SlidingWindow& r2) const;
        bool operator!=(const SlidingWindow& r2) const;

    // HELPER:
     static std::vector<T> getLastNValues(std::vector<T> biggerData, nupic::UInt n) {
      NTA_CHECK(n >=0 && n <= biggerData.size());
    std::vector<T> v;
    v.reserve(n);
    v.insert(begin(v), end(biggerData) - n, end(biggerData));
    NTA_ASSERT(v.size() == n);
    return v;
    };

      private:
        std::vector<T> buffer_;
        UInt idxNext_;
        bool firstRun_ = true; //fill be false once "first run" (=size reaches maxCapacity for the first time) is completed
};
}} //end ns

/// IMPLEMENTATION
#include <cmath>

using namespace std;
using namespace nupic;
using namespace nupic::util;

template<class T>
SlidingWindow<T>::SlidingWindow(UInt maxCapacity) :
  maxCapacity(maxCapacity) {
  NTA_CHECK(maxCapacity > 0);
  buffer_.reserve(maxCapacity);
  idxNext_ = 0;
}

template<class T>
SlidingWindow<T>::SlidingWindow(UInt maxCapacity, vector<T> initialData)  :
  SlidingWindow(maxCapacity) {
  NTA_CHECK(initialData.size() <= maxCapacity);
  buffer_.insert(begin(buffer_), begin(initialData), end(initialData));
  idxNext_ = initialData.size();
}

template<class T>
UInt SlidingWindow<T>::size() const {
  NTA_ASSERT(buffer_.size() <= maxCapacity);
  return buffer_.size();
}

template<class T>
T SlidingWindow<T>::append(T newValue) {
  Real old = NEUTRAL_VALUE;
  if(firstRun_ && size() == maxCapacity) {
    firstRun_ = false;
  }

  if(firstRun_) {
    buffer_.emplace_back(newValue);
  } else {
    old = buffer_[idxNext_];  //FIXME this IF is here only because size() wouldn't work w/o it or similar hack
    buffer_[idxNext_] = newValue;
  }
  idxNext_ = ++idxNext_ % maxCapacity;
  return old;
}

template<class T>
const vector<T>& SlidingWindow<T>::getData() const {
  return buffer_;
}

template<class T>
bool SlidingWindow<T>::operator==(const SlidingWindow& r2) const //FIXME review the ==, on my machine it randomly passes/fails the test!
{
  bool sameData = this->getData()== r2.getData();
  return ((this->size() == r2.size()) && (this->maxCapacity == r2.maxCapacity) && sameData);
}

template<class T>
bool SlidingWindow<T>::operator!=(const SlidingWindow& r2) const
{
  return !operator==(r2);
}

#endif //header
<commit_msg>SlidingWindow: add linearize()<commit_after>/* Numenta Platform for Intelligent Computing (NuPIC)
 * Copyright (C) 2016, Numenta, Inc.  Unless you have an agreement
 * with Numenta, Inc., for a separate license for this software code, the
 * following terms and conditions apply:
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Affero Public License version 3 as
 * published by the Free Software Foundation.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 * See the GNU Affero Public License for more details.
 *
 * You should have received a copy of the GNU Affero Public License
 * along with this program.  If not, see http://www.gnu.org/licenses.
 *
 * http://numenta.org/licenses/
 * ---------------------------------------------------------------------
 */

#ifndef NUPIC_UTIL_SLIDING_WINDOW_HPP
#define NUPIC_UTIL_SLIDING_WINDOW_HPP

#include <vector>

#include <nupic/types/Types.hpp>
#include <nupic/utils/Log.hpp>

namespace nupic {
  namespace util {

    template<class T>
    class SlidingWindow {
      public:
        SlidingWindow(UInt maxCapacity);
        SlidingWindow(UInt maxCapacity, std::vector<T> initialData);
        const UInt maxCapacity;
        const T NEUTRAL_VALUE = 0.0;
        UInt size() const;
        /** append new value to the end of the buffer and handle the "overflows"-may pop the first element if full.
         :return addition(+) neutral value (that is 0) when size()< maxCapacity; when full - return the value of the dropped element
        */
        T append(T newValue);
        /**
        * :return unordered content (data ) of this sl. window; call linearize() if you need them oredered from oldest->newest
        * This direct access method is fast.
        */
        const std::vector<T>& getData() const;

        /** linearize method for the internal buffer; this is slower than the pure getData() but ensures that the data are ordered (oldest at the
         * beginning, newest at the end of the vector
         * This handles case of |5,6;1,2,3,4| => |1,2,3,4,5,6|
         * :return new linearized vector
        */
        std::vector<T> linearize() const;

        bool operator==(const SlidingWindow& r2) const;
        bool operator!=(const SlidingWindow& r2) const;

    // HELPER:
     static std::vector<T> getLastNValues(std::vector<T> biggerData, nupic::UInt n) {
      NTA_CHECK(n >=0 && n <= biggerData.size());
    std::vector<T> v;
    v.reserve(n);
    v.insert(begin(v), end(biggerData) - n, end(biggerData));
    NTA_ASSERT(v.size() == n);
    return v;
    };

      private:
        std::vector<T> buffer_;
        UInt idxNext_;
        bool firstRun_ = true; //fill be false once "first run" (=size reaches maxCapacity for the first time) is completed
};
}} //end ns

/// IMPLEMENTATION
#include <cmath>

using namespace std;
using namespace nupic;
using namespace nupic::util;

template<class T>
SlidingWindow<T>::SlidingWindow(UInt maxCapacity) :
  maxCapacity(maxCapacity) {
  NTA_CHECK(maxCapacity > 0);
  buffer_.reserve(maxCapacity);
  idxNext_ = 0;
}

template<class T>
SlidingWindow<T>::SlidingWindow(UInt maxCapacity, vector<T> initialData)  :
  SlidingWindow(maxCapacity) {
  NTA_CHECK(initialData.size() <= maxCapacity);
  buffer_.insert(begin(buffer_), begin(initialData), end(initialData));
  idxNext_ = initialData.size();
}

template<class T>
UInt SlidingWindow<T>::size() const {
  NTA_ASSERT(buffer_.size() <= maxCapacity);
  return buffer_.size();
}

template<class T>
T SlidingWindow<T>::append(T newValue) {
  Real old = NEUTRAL_VALUE;
  if(firstRun_ && size() == maxCapacity) {
    firstRun_ = false;
  }

  if(firstRun_) {
    buffer_.emplace_back(newValue);
  } else {
    old = buffer_[idxNext_];  //FIXME this IF is here only because size() wouldn't work w/o it or similar hack
    buffer_[idxNext_] = newValue;
  }
  idxNext_ = ++idxNext_ % maxCapacity;
  return old;
}

template<class T>
const vector<T>& SlidingWindow<T>::getData() const {
  return buffer_;
}

template<class T>
vector<T> SlidingWindow<T>::linearize() const {
  vector<T> lin;
  lin.reserve(buffer_.size());

  if(size() ==0) return lin; //empty buffer
  lin.insert(begin(lin), begin(buffer_) + idxNext_, end(buffer_)); //insert the "older" part at the beginning
  if(idxNext_ != 0) { //need to append second (unlinear) part
    lin.insert(end(lin), begin(buffer_), begin(buffer_) + idxNext_); //append the "newer" part to the end of the constructed vect
  }
  return lin;
}

template<class T>
bool SlidingWindow<T>::operator==(const SlidingWindow& r2) const //FIXME review the ==, on my machine it randomly passes/fails the test!
{
  bool sameData = this->getData()== r2.getData();
  return ((this->size() == r2.size()) && (this->maxCapacity == r2.maxCapacity) && sameData);
}

template<class T>
bool SlidingWindow<T>::operator!=(const SlidingWindow& r2) const
{
  return !operator==(r2);
}

#endif //header
<|endoftext|>


<commit_before>#ifndef _SWITCHBACK_HPP_
#define _SWITCHBACK_HPP_

#include <vector>

#include <boost/array.hpp>
#include <boost/none.hpp>
#include <boost/optional.hpp>
#include <boost/pool/pool_alloc.hpp>
#include <boost/unordered_map.hpp>

#include "search/BucketPriorityQueue.hpp"
#include "util/PointerOps.hpp"

template <
  class Domain,
  class Node
  >
class Switchback
{
private:
  typedef typename Node::Cost Cost;
  typedef typename Node::State State;
  typedef BucketPriorityQueue<Node> Open;
  typedef boost::optional<typename Open::ItemPointer> MaybeItemPointer;

  typedef boost::unordered_map<
    Node *,
    MaybeItemPointer,
    PointerHash<Node>,
    PointerEq<Node>
    , boost::fast_pool_allocator< std::pair<Node * const, MaybeItemPointer> >
    > Closed;

  typedef typename Closed::iterator ClosedIterator;
  typedef typename Closed::const_iterator ClosedConstIterator;

private:
  const Node *goal;
  bool searched;

  const Domain &domain;

  const static unsigned hierarchy_height = Domain::num_abstraction_levels + 1;

  boost::array<unsigned, hierarchy_height> num_expanded;
  boost::array<unsigned, hierarchy_height> num_generated;

  boost::array<Open, hierarchy_height> open;
  boost::array<Closed, hierarchy_height> closed;

private:
  Switchback(const Switchback<Domain, Node> &);
  Switchback<Domain, Node> & operator =(const Switchback<Domain, Node> &);

public:
  Switchback(const Domain &domain)
    : goal(NULL)
    , searched(false)
    , domain(domain)
  {
    num_expanded.assign(0);
    num_generated.assign(0);
  }

  ~Switchback()
  {
  }

  void search()
  {
    if (searched || goal != NULL)
      return;
    searched = true;

    init_open_and_closed();
    goal = resume_search(0, domain.get_goal_state());
  }

  const Node * get_goal() const
  {
    return goal;
  }

  const Domain & get_domain() const
  {
    return domain;
  }

  unsigned get_num_generated() const
  {
    unsigned sum = 0;
    for (unsigned i = 0; i < hierarchy_height; i += 1)
      sum += num_generated[i];

    return sum;
  }

  unsigned get_num_generated(const unsigned level) const
  {
    return num_generated[level];
  }

  unsigned get_num_expanded() const
  {
    unsigned sum = 0;
    for (unsigned i = 0; i < hierarchy_height; i += 1)
      sum += num_expanded[i];

    return sum;
  }

  unsigned get_num_expanded(const unsigned level) const
  {
    return num_expanded[level];
  }

private:
  Cost heuristic(const unsigned level, const State &goal_state)
  {
    assert(is_valid_level(level));

    if (level == Domain::num_abstraction_levels)
      return 0;

    // Need to create a dummy goal node to look up in the hash table.
    // This smells of bad design!
    Node goal_node(goal_state, 0, 0);
    ClosedIterator closed_it = closed[level].find(&goal_node);
    if (closed_it != closed[level].end())
      return closed_it->first->get_g();

    const State abstracted_goal_state = domain.abstract(level + 1, goal_state);

    Node *result = resume_search(level + 1, abstracted_goal_state);
    if (result == NULL) {
      std::cerr << "whoops, infinite heuristic estimate!" << std::endl;
      assert(false);  // for the domains I am running on, there should
                      // never be an infinite heuristic estimate.
    }

    return result->get_g();
  }

  Node * resume_search(const unsigned level, const State &goal_state)
  {
    assert(is_valid_level(level));

    std::cerr << "conducting hierarchical search at level " << level
              << " for the following state:" << std::endl
              << goal_state << std::endl;

    dump_open_sizes(std::cerr);
    dump_closed_sizes(std::cerr);

    // Dummy goal node, for hash table lookup.
    Node goal_node(goal_state, 0, 0);
    ClosedIterator closed_it = closed[level].find(&goal_node);

    if (closed_it != closed[level].end())
      return closed_it->first;

    std::vector<Node *> children;

    // A*-ish code goes here
    while (!open[level].empty()) {
      Node *n = open[level].top();
      open[level].pop();
      assert(closed[level].find(n) != closed[level].end());
      closed[level][n] = boost::none;
      assert(all_closed_item_ptrs_valid(level));

      if (level % 2 == 0)
        domain.compute_successors(*n, children);
      else
        domain.compute_predecessors(*n, children);
      num_expanded[level] += 1;
      num_generated[level] += children.size();

      for (unsigned child_idx = 0; child_idx < children.size(); child_idx += 1) {
        Node *child = children[child_idx];
        assert(open[level].size() <= closed[level].size());
        assert(all_closed_item_ptrs_valid(level));
        ClosedIterator closed_it = closed[level].find(child);
        if (closed_it != closed[level].end()) {
          MaybeItemPointer &open_ptr = closed_it->second;

          if (open_ptr) {
            Node *old_child = open[level].lookup(*open_ptr);

            if ( child->get_f() < old_child->get_f() ) {
              // A worse copy of succ is in the open list.  Replace it!
              assert(old_child->get_state() == child->get_state());
              assert(*old_child == *child);

              domain.free_node(old_child);    // get rid of the old copy
              open[level].erase(*open_ptr);
              closed[level].erase(child);

              open_ptr = open[level].push(child);    // insert the new copy
              closed[level][child] = open_ptr;

              assert(all_closed_item_ptrs_valid(level));
            }
          }
          else {
            // child is not in the open list, but is closed.  Drop it!
            domain.free_node(child);
          }
        }
        else {
          // child has not been generated before.
          MaybeItemPointer open_ptr = open[level].push(child);
          closed[level][child] = open_ptr;
          assert(closed[level].find(open[level].lookup(*open_ptr)) != closed[level].end());
          assert(all_closed_item_ptrs_valid(level));
        }
      } /* end for */

      if (n->get_state() == goal_state)
        return n;
    } /* end while */

    return NULL;
  }

  void init_open_and_closed()
  {
    for (int level = Domain::num_abstraction_levels; level >= 0; level -= 1) {
      std::cerr << "initializing level " << level << std::endl;
      State state = level % 2 == 0
                      ? domain.get_start_state()
                      : domain.get_goal_state();

      State abstract_state = domain.abstract(level, state);
      Node *start = domain.create_node(abstract_state,
                                       0,
                                       heuristic(level, abstract_state),
                                       NULL
                                       );
      MaybeItemPointer open_ptr = open[level].push(start);
      closed[level][start] = open_ptr;
    }
  }

  bool all_closed_item_ptrs_valid(const unsigned level) const
  {
#ifdef CHECK_ALL_CLOSED_ITEM_PTRS_VALID
    std::cerr << "checking if item pointers are valid" << std::endl
              << "  " << closed.size() << " pointers to check" << std::endl;
    for (ClosedConstIterator closed_it = closed[level].begin();
         closed_it != closed[level].end();
         ++closed_it) {
      if ( closed_it->second && !open[level].valid_item_pointer(*closed_it->second) )
        return false;
    }
    return true;
#else
    return true;
#endif
  }

  static bool is_valid_level(const unsigned level)
  {
    return level <= Domain::num_abstraction_levels;
  }

  void dump_open_sizes(std::ostream &o) const
  {
    o << "open sizes: " << std::endl;
    for (unsigned level = 0; level < hierarchy_height; level += 1)
      o << "  " << level << ": " << open[level].size() << std::endl;
  }

  void dump_closed_sizes(std::ostream &o) const
  {
    o << "closed sizes: " << std::endl;
    for (unsigned level = 0; level < hierarchy_height; level += 1)
      o << "  " << level << ": " << closed[level].size() << std::endl;
  }
};

#endif /* !_SWITCHBACK_HPP_ */
<commit_msg>more debugging output<commit_after>#ifndef _SWITCHBACK_HPP_
#define _SWITCHBACK_HPP_

#include <vector>

#include <boost/array.hpp>
#include <boost/none.hpp>
#include <boost/optional.hpp>
#include <boost/pool/pool_alloc.hpp>
#include <boost/unordered_map.hpp>

#include "search/BucketPriorityQueue.hpp"
#include "util/PointerOps.hpp"

template <
  class Domain,
  class Node
  >
class Switchback
{
private:
  typedef typename Node::Cost Cost;
  typedef typename Node::State State;
  typedef BucketPriorityQueue<Node> Open;
  typedef boost::optional<typename Open::ItemPointer> MaybeItemPointer;

  typedef boost::unordered_map<
    Node *,
    MaybeItemPointer,
    PointerHash<Node>,
    PointerEq<Node>
    , boost::fast_pool_allocator< std::pair<Node * const, MaybeItemPointer> >
    > Closed;

  typedef typename Closed::iterator ClosedIterator;
  typedef typename Closed::const_iterator ClosedConstIterator;

private:
  const Node *goal;
  bool searched;

  const Domain &domain;

  const static unsigned hierarchy_height = Domain::num_abstraction_levels + 1;

  boost::array<unsigned, hierarchy_height> num_expanded;
  boost::array<unsigned, hierarchy_height> num_generated;

  boost::array<Open, hierarchy_height> open;
  boost::array<Closed, hierarchy_height> closed;

private:
  Switchback(const Switchback<Domain, Node> &);
  Switchback<Domain, Node> & operator =(const Switchback<Domain, Node> &);

public:
  Switchback(const Domain &domain)
    : goal(NULL)
    , searched(false)
    , domain(domain)
  {
    num_expanded.assign(0);
    num_generated.assign(0);
  }

  ~Switchback()
  {
  }

  void search()
  {
    if (searched || goal != NULL)
      return;
    searched = true;

    init_open_and_closed();
    goal = resume_search(0, domain.get_goal_state());
  }

  const Node * get_goal() const
  {
    return goal;
  }

  const Domain & get_domain() const
  {
    return domain;
  }

  unsigned get_num_generated() const
  {
    unsigned sum = 0;
    for (unsigned i = 0; i < hierarchy_height; i += 1)
      sum += num_generated[i];

    return sum;
  }

  unsigned get_num_generated(const unsigned level) const
  {
    return num_generated[level];
  }

  unsigned get_num_expanded() const
  {
    unsigned sum = 0;
    for (unsigned i = 0; i < hierarchy_height; i += 1)
      sum += num_expanded[i];

    return sum;
  }

  unsigned get_num_expanded(const unsigned level) const
  {
    return num_expanded[level];
  }

private:
  Cost heuristic(const unsigned level, const State &goal_state)
  {
    assert(is_valid_level(level));

    if (level == Domain::num_abstraction_levels)
      return 0;

    // Need to create a dummy goal node to look up in the hash table.
    // This smells of bad design!
    Node goal_node(goal_state, 0, 0);
    ClosedIterator closed_it = closed[level].find(&goal_node);
    if (closed_it != closed[level].end())
      return closed_it->first->get_g();

    const State abstracted_goal_state = domain.abstract(level + 1, goal_state);

    Node *result = resume_search(level + 1, abstracted_goal_state);
    if (result == NULL) {
      std::cerr << "whoops, infinite heuristic estimate!" << std::endl;
      assert(false);  // for the domains I am running on, there should
                      // never be an infinite heuristic estimate.
    }

    return result->get_g();
  }

  Node * resume_search(const unsigned level, const State &goal_state)
  {
    assert(is_valid_level(level));

    std::cerr << "conducting hierarchical search at level " << level
              << " for the following state:" << std::endl
              << goal_state << std::endl;

    dump_open_sizes(std::cerr);
    dump_closed_sizes(std::cerr);

    // Dummy goal node, for hash table lookup.
    Node goal_node(goal_state, 0, 0);
    ClosedIterator closed_it = closed[level].find(&goal_node);

    if (closed_it != closed[level].end())
      return closed_it->first;

    std::vector<Node *> children;

    // A*-ish code goes here
    while (!open[level].empty()) {
      if (get_num_expanded() % 500000 == 0) {
        std::cerr << get_num_expanded() << " total nodes expanded" << std::endl
                  << get_num_generated() << " total nodes generated" << std::endl;
        dump_open_sizes(std::cerr);
        dump_closed_sizes(std::cerr);
      }

      Node *n = open[level].top();
      open[level].pop();
      assert(closed[level].find(n) != closed[level].end());
      closed[level][n] = boost::none;
      assert(all_closed_item_ptrs_valid(level));

      if (level % 2 == 0)
        domain.compute_successors(*n, children);
      else
        domain.compute_predecessors(*n, children);
      num_expanded[level] += 1;
      num_generated[level] += children.size();

      for (unsigned child_idx = 0; child_idx < children.size(); child_idx += 1) {
        Node *child = children[child_idx];
        assert(open[level].size() <= closed[level].size());
        assert(all_closed_item_ptrs_valid(level));
        ClosedIterator closed_it = closed[level].find(child);
        if (closed_it != closed[level].end()) {
          MaybeItemPointer &open_ptr = closed_it->second;

          if (open_ptr) {
            Node *old_child = open[level].lookup(*open_ptr);

            if ( child->get_f() < old_child->get_f() ) {
              // A worse copy of succ is in the open list.  Replace it!
              assert(old_child->get_state() == child->get_state());
              assert(*old_child == *child);

              domain.free_node(old_child);    // get rid of the old copy
              open[level].erase(*open_ptr);
              closed[level].erase(child);

              open_ptr = open[level].push(child);    // insert the new copy
              closed[level][child] = open_ptr;

              assert(all_closed_item_ptrs_valid(level));
            }
          }
          else {
            // child is not in the open list, but is closed.  Drop it!
            domain.free_node(child);
          }
        }
        else {
          // child has not been generated before.
          MaybeItemPointer open_ptr = open[level].push(child);
          closed[level][child] = open_ptr;
          assert(closed[level].find(open[level].lookup(*open_ptr)) != closed[level].end());
          assert(all_closed_item_ptrs_valid(level));
        }
      } /* end for */

      if (n->get_state() == goal_state)
        return n;
    } /* end while */

    return NULL;
  }

  void init_open_and_closed()
  {
    for (int level = Domain::num_abstraction_levels; level >= 0; level -= 1) {
      std::cerr << "initializing level " << level << std::endl;
      State state = level % 2 == 0
                      ? domain.get_start_state()
                      : domain.get_goal_state();

      State abstract_state = domain.abstract(level, state);
      Node *start = domain.create_node(abstract_state,
                                       0,
                                       heuristic(level, abstract_state),
                                       NULL
                                       );
      MaybeItemPointer open_ptr = open[level].push(start);
      closed[level][start] = open_ptr;
    }
  }

  bool all_closed_item_ptrs_valid(const unsigned level) const
  {
#ifdef CHECK_ALL_CLOSED_ITEM_PTRS_VALID
    std::cerr << "checking if item pointers are valid" << std::endl
              << "  " << closed.size() << " pointers to check" << std::endl;
    for (ClosedConstIterator closed_it = closed[level].begin();
         closed_it != closed[level].end();
         ++closed_it) {
      if ( closed_it->second && !open[level].valid_item_pointer(*closed_it->second) )
        return false;
    }
    return true;
#else
    return true;
#endif
  }

  static bool is_valid_level(const unsigned level)
  {
    return level <= Domain::num_abstraction_levels;
  }

  void dump_open_sizes(std::ostream &o) const
  {
    o << "open sizes: " << std::endl;
    for (unsigned level = 0; level < hierarchy_height; level += 1)
      o << "  " << level << ": " << open[level].size() << std::endl;
  }

  void dump_closed_sizes(std::ostream &o) const
  {
    o << "closed sizes: " << std::endl;
    for (unsigned level = 0; level < hierarchy_height; level += 1)
      o << "  " << level << ": " << closed[level].size() << std::endl;
  }
};

#endif /* !_SWITCHBACK_HPP_ */
<|endoftext|>


<commit_before>#ifndef _SWITCHBACK_HPP_
#define _SWITCHBACK_HPP_

#include <vector>

#include <boost/array.hpp>
#include <boost/none.hpp>
#include <boost/optional.hpp>
#include <boost/pool/pool_alloc.hpp>
#include <boost/unordered_map.hpp>
#include <boost/utility.hpp>

#include "search/BucketPriorityQueue.hpp"
#include "util/PointerOps.hpp"

template <
  class Domain,
  class Node
  >
class Switchback : boost::noncopyable
{
private:
  typedef typename Node::Cost Cost;
  typedef typename Node::State State;
  typedef BucketPriorityQueue<Node> Open;
  typedef boost::optional<typename Open::ItemPointer> MaybeItemPointer;

  typedef boost::unordered_map<
    Node *,
    MaybeItemPointer,
    PointerHash<Node>,
    PointerEq<Node>
    , boost::fast_pool_allocator< std::pair<Node * const, MaybeItemPointer> >
    > Closed;

  typedef typename Closed::iterator ClosedIterator;
  typedef typename Closed::const_iterator ClosedConstIterator;

private:
  const Node *goal;
  bool searched;

  const Domain &domain;

  const static unsigned hierarchy_height = Domain::num_abstraction_levels + 1;

  boost::array<unsigned, hierarchy_height> num_expanded;
  boost::array<unsigned, hierarchy_height> num_generated;

  boost::array<Open, hierarchy_height> open;
  boost::array<Closed, hierarchy_height> closed;

public:
  Switchback(const Domain &domain)
    : goal(NULL)
    , searched(false)
    , domain(domain)
  {
    num_expanded.assign(0);
    num_generated.assign(0);
  }

  ~Switchback()
  {
  }

  void search()
  {
    if (searched)
      return;
    searched = true;

    init_open_and_closed();
    goal = resume_search(0, domain.get_goal_state());
  }

  const Node * get_goal() const
  {
    std::cerr << "final open/closed sizes:" << std::endl;
    dump_open_sizes(std::cerr);
    dump_closed_sizes(std::cerr);

    return goal;
  }

  const Domain & get_domain() const
  {
    return domain;
  }

  unsigned get_num_generated() const
  {
    unsigned sum = 0;
    for (unsigned i = 0; i < hierarchy_height; i += 1)
      sum += num_generated[i];

    return sum;
  }

  unsigned get_num_generated(const unsigned level) const
  {
    return num_generated[level];
  }

  unsigned get_num_expanded() const
  {
    unsigned sum = 0;
    for (unsigned i = 0; i < hierarchy_height; i += 1)
      sum += num_expanded[i];

    return sum;
  }

  unsigned get_num_expanded(const unsigned level) const
  {
    return num_expanded[level];
  }

private:
  Cost heuristic(const unsigned level, const State &goal_state)
  {
    assert(is_valid_level(level));

    if (level == Domain::num_abstraction_levels)
      return 0;

    // Need to create a dummy goal node to look up in the hash table.
    // This smells of bad design!
    const unsigned next_level = level + 1;
    const State abstract_goal_state = domain.abstract(next_level, goal_state);
    Node abstract_goal_node(abstract_goal_state, 0, 0);

    ClosedIterator closed_it = closed[next_level].find(&abstract_goal_node);
    if (closed_it != closed[next_level].end() && !closed_it->second) {
      return closed_it->first->get_g();
    }

    Node *result = resume_search(next_level, abstract_goal_state);
    if (result == NULL) {
      std::cerr << "whoops, infinite heuristic estimate!" << std::endl;
      assert(false);  // for the domains I am running on, there should
                      // never be an infinite heuristic estimate.
    }
    assert(closed[next_level].find(&abstract_goal_node) != closed[next_level].end());
    assert(!closed[next_level].find(&abstract_goal_node)->second);

    return result->get_g();
  }

  Node * resume_search(const unsigned level, const State &goal_state)
  {
    assert(is_valid_level(level));

    // Dummy goal node, for hash table lookup.
    Node goal_node(goal_state, 0, 0);
    ClosedIterator closed_it = closed[level].find(&goal_node);

    if (closed_it != closed[level].end() &&!closed_it->second)
      return closed_it->first;

    std::vector<Node *> children;

    // A*-ish code ahead
    while (!open[level].empty()) {
      if (get_num_expanded() % 500000 == 0) {
        std::cerr << get_num_expanded() << " total nodes expanded" << std::endl
                  << get_num_generated() << " total nodes generated" << std::endl;
        dump_open_sizes(std::cerr);
        dump_closed_sizes(std::cerr);
      }

      Node *n = open[level].top();
      assert(closed[level].find(n) != closed[level].end());
      assert(closed[level].find(n)->second);
      assert(open[level].valid_item_pointer(*closed[level].find(n)->second));
      open[level].pop();

      closed[level][n] = boost::none;
      assert(all_closed_item_ptrs_valid(level));

      if (level % 2 == 0)
        domain.compute_successors(*n, children);
      else
        domain.compute_predecessors(*n, children);
      num_expanded[level] += 1;
      num_generated[level] += children.size();

      for (unsigned child_idx = 0; child_idx < children.size(); child_idx += 1) {
        process_child(level, children[child_idx]);
      }

      if (n->get_state() == goal_state)
        return n;
    } /* end while */

    return NULL;
  }

  void process_child(const unsigned level, Node *child)
  {
    assert(open[level].invariants_satisfied());
    assert(open[level].size() <= closed[level].size());
    assert(all_closed_item_ptrs_valid(level));

    child->set_h(heuristic(level, child->get_state()));

    ClosedIterator closed_it = closed[level].find(child);
    if (closed_it == closed[level].end()) {
      // The child has not been generated before.
      closed[level][child] = open[level].push(child);
    }
    else if (closed_it->second && child->get_f() < closed_it->first->get_f()) {
      // A worse version of the child is in the open list.
      open[level].erase(*closed_it->second);  // knock out the old
                                              // one from the open
                                              // list

      domain.free_node(closed_it->first);     // free the old,
                                              // worse copy
      closed[level].erase(closed_it);

      closed[level][child] = open[level].push(child);  // insert better version of child
    }
    else {
      // The child has either already been expanded, or is worse
      // than the version in the open list.
      domain.free_node(child);
    }

    assert(all_closed_item_ptrs_valid(level));
    assert(open[level].invariants_satisfied());
  }

  void init_open_and_closed()
  {
    for (unsigned level = 0; level <= Domain::num_abstraction_levels; level += 1) {
      std::cerr << "initializing level " << level << std::endl;

      num_generated[level] += 1;
      State start = level % 2 == 0
                      ? domain.get_start_state()
                      : domain.get_goal_state();
      Node *start_node = domain.create_node(domain.abstract(level, start),
                                            0,
                                            0,
                                            NULL
                                            );
      closed[level][start_node] = open[level].push(start_node);
    }

    std::cerr << "###### INITIALIZATION COMPLETE ######" << std::endl;
  }

  bool all_closed_item_ptrs_valid(const unsigned level) const
  {
#ifdef CHECK_ALL_CLOSED_ITEM_PTRS_VALID
    std::cerr << "checking if item pointers are valid" << std::endl
              << "  " << closed.size() << " pointers to check" << std::endl;
    for (ClosedConstIterator closed_it = closed[level].begin();
         closed_it != closed[level].end();
         ++closed_it) {
      if ( closed_it->second && !open[level].valid_item_pointer(*closed_it->second) )
        return false;
    }
    return true;
#else
    return true;
#endif
  }

  static bool is_valid_level(const unsigned level)
  {
    return level <= Domain::num_abstraction_levels;
  }

  void dump_open_sizes(std::ostream &o) const
  {
    o << "open sizes: " << std::endl;
    for (unsigned level = 0; level < hierarchy_height; level += 1)
      o << "  " << level << ": " << open[level].size() << std::endl;
  }

  void dump_closed_sizes(std::ostream &o) const
  {
    o << "closed sizes: " << std::endl;
    for (unsigned level = 0; level < hierarchy_height; level += 1)
      o << "  " << level << ": " << closed[level].size() << std::endl;
  }
};

#endif /* !_SWITCHBACK_HPP_ */
<commit_msg>Constructor cleanup.<commit_after>#ifndef _SWITCHBACK_HPP_
#define _SWITCHBACK_HPP_

#include <vector>

#include <boost/array.hpp>
#include <boost/none.hpp>
#include <boost/optional.hpp>
#include <boost/pool/pool_alloc.hpp>
#include <boost/unordered_map.hpp>
#include <boost/utility.hpp>

#include "search/BucketPriorityQueue.hpp"
#include "util/PointerOps.hpp"

template <
  class Domain,
  class Node
  >
class Switchback : boost::noncopyable
{
private:
  typedef typename Node::Cost Cost;
  typedef typename Node::State State;
  typedef BucketPriorityQueue<Node> Open;
  typedef boost::optional<typename Open::ItemPointer> MaybeItemPointer;

  typedef boost::unordered_map<
    Node *,
    MaybeItemPointer,
    PointerHash<Node>,
    PointerEq<Node>
    , boost::fast_pool_allocator< std::pair<Node * const, MaybeItemPointer> >
    > Closed;

  typedef typename Closed::iterator ClosedIterator;
  typedef typename Closed::const_iterator ClosedConstIterator;

private:
  const static unsigned hierarchy_height = Domain::num_abstraction_levels + 1;

  const Node *goal;
  bool searched;

  const Domain &domain;

  boost::array<unsigned, hierarchy_height> num_expanded;
  boost::array<unsigned, hierarchy_height> num_generated;

  boost::array<Open, hierarchy_height> open;
  boost::array<Closed, hierarchy_height> closed;

public:
  Switchback(const Domain &domain)
    : goal(NULL)
    , searched(false)
    , domain(domain)
    , num_expanded()
    , num_generated()
    , open()
    , closed()
  {
    num_expanded.assign(0);
    num_generated.assign(0);
  }

  ~Switchback()
  {
  }

  void search()
  {
    if (searched)
      return;
    searched = true;

    init_open_and_closed();
    goal = resume_search(0, domain.get_goal_state());
  }

  const Node * get_goal() const
  {
    std::cerr << "final open/closed sizes:" << std::endl;
    dump_open_sizes(std::cerr);
    dump_closed_sizes(std::cerr);

    return goal;
  }

  const Domain & get_domain() const
  {
    return domain;
  }

  unsigned get_num_generated() const
  {
    unsigned sum = 0;
    for (unsigned i = 0; i < hierarchy_height; i += 1)
      sum += num_generated[i];

    return sum;
  }

  unsigned get_num_generated(const unsigned level) const
  {
    return num_generated[level];
  }

  unsigned get_num_expanded() const
  {
    unsigned sum = 0;
    for (unsigned i = 0; i < hierarchy_height; i += 1)
      sum += num_expanded[i];

    return sum;
  }

  unsigned get_num_expanded(const unsigned level) const
  {
    return num_expanded[level];
  }

private:
  Cost heuristic(const unsigned level, const State &goal_state)
  {
    assert(is_valid_level(level));

    if (level == Domain::num_abstraction_levels)
      return 0;

    // Need to create a dummy goal node to look up in the hash table.
    // This smells of bad design!
    const unsigned next_level = level + 1;
    const State abstract_goal_state = domain.abstract(next_level, goal_state);
    Node abstract_goal_node(abstract_goal_state, 0, 0);

    ClosedIterator closed_it = closed[next_level].find(&abstract_goal_node);
    if (closed_it != closed[next_level].end() && !closed_it->second) {
      return closed_it->first->get_g();
    }

    Node *result = resume_search(next_level, abstract_goal_state);
    if (result == NULL) {
      std::cerr << "whoops, infinite heuristic estimate!" << std::endl;
      assert(false);  // for the domains I am running on, there should
                      // never be an infinite heuristic estimate.
    }
    assert(closed[next_level].find(&abstract_goal_node) != closed[next_level].end());
    assert(!closed[next_level].find(&abstract_goal_node)->second);

    return result->get_g();
  }

  Node * resume_search(const unsigned level, const State &goal_state)
  {
    assert(is_valid_level(level));

    // Dummy goal node, for hash table lookup.
    Node goal_node(goal_state, 0, 0);
    ClosedIterator closed_it = closed[level].find(&goal_node);

    if (closed_it != closed[level].end() &&!closed_it->second)
      return closed_it->first;

    std::vector<Node *> children;

    // A*-ish code ahead
    while (!open[level].empty()) {
      if (get_num_expanded() % 500000 == 0) {
        std::cerr << get_num_expanded() << " total nodes expanded" << std::endl
                  << get_num_generated() << " total nodes generated" << std::endl;
        dump_open_sizes(std::cerr);
        dump_closed_sizes(std::cerr);
      }

      Node *n = open[level].top();
      assert(closed[level].find(n) != closed[level].end());
      assert(closed[level].find(n)->second);
      assert(open[level].valid_item_pointer(*closed[level].find(n)->second));
      open[level].pop();

      closed[level][n] = boost::none;
      assert(all_closed_item_ptrs_valid(level));

      if (level % 2 == 0)
        domain.compute_successors(*n, children);
      else
        domain.compute_predecessors(*n, children);
      num_expanded[level] += 1;
      num_generated[level] += children.size();

      for (unsigned child_idx = 0; child_idx < children.size(); child_idx += 1) {
        process_child(level, children[child_idx]);
      }

      if (n->get_state() == goal_state)
        return n;
    } /* end while */

    return NULL;
  }

  void process_child(const unsigned level, Node *child)
  {
    assert(open[level].invariants_satisfied());
    assert(open[level].size() <= closed[level].size());
    assert(all_closed_item_ptrs_valid(level));

    child->set_h(heuristic(level, child->get_state()));

    ClosedIterator closed_it = closed[level].find(child);
    if (closed_it == closed[level].end()) {
      // The child has not been generated before.
      closed[level][child] = open[level].push(child);
    }
    else if (closed_it->second && child->get_f() < closed_it->first->get_f()) {
      // A worse version of the child is in the open list.
      open[level].erase(*closed_it->second);  // knock out the old
                                              // one from the open
                                              // list

      domain.free_node(closed_it->first);     // free the old,
                                              // worse copy
      closed[level].erase(closed_it);

      closed[level][child] = open[level].push(child);  // insert better version of child
    }
    else {
      // The child has either already been expanded, or is worse
      // than the version in the open list.
      domain.free_node(child);
    }

    assert(all_closed_item_ptrs_valid(level));
    assert(open[level].invariants_satisfied());
  }

  void init_open_and_closed()
  {
    for (unsigned level = 0; level <= Domain::num_abstraction_levels; level += 1) {
      std::cerr << "initializing level " << level << std::endl;

      num_generated[level] += 1;
      State start = level % 2 == 0
                      ? domain.get_start_state()
                      : domain.get_goal_state();
      Node *start_node = domain.create_node(domain.abstract(level, start),
                                            0,
                                            0,
                                            NULL
                                            );
      closed[level][start_node] = open[level].push(start_node);
    }

    std::cerr << "###### INITIALIZATION COMPLETE ######" << std::endl;
  }

  bool all_closed_item_ptrs_valid(const unsigned level) const
  {
#ifdef CHECK_ALL_CLOSED_ITEM_PTRS_VALID
    std::cerr << "checking if item pointers are valid" << std::endl
              << "  " << closed.size() << " pointers to check" << std::endl;
    for (ClosedConstIterator closed_it = closed[level].begin();
         closed_it != closed[level].end();
         ++closed_it) {
      if ( closed_it->second && !open[level].valid_item_pointer(*closed_it->second) )
        return false;
    }
    return true;
#else
    return true;
#endif
  }

  static bool is_valid_level(const unsigned level)
  {
    return level <= Domain::num_abstraction_levels;
  }

  void dump_open_sizes(std::ostream &o) const
  {
    o << "open sizes: " << std::endl;
    for (unsigned level = 0; level < hierarchy_height; level += 1)
      o << "  " << level << ": " << open[level].size() << std::endl;
  }

  void dump_closed_sizes(std::ostream &o) const
  {
    o << "closed sizes: " << std::endl;
    for (unsigned level = 0; level < hierarchy_height; level += 1)
      o << "  " << level << ": " << closed[level].size() << std::endl;
  }
};

#endif /* !_SWITCHBACK_HPP_ */
<|endoftext|>


<commit_before>/*
 * Copyright (c) 2015 New Designs Unlimited, LLC
 * Opensource Automated License Plate Recognition [http://www.openalpr.com]
 *
 * This file is part of OpenAlpr.
 *
 * OpenAlpr is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Affero General Public License
 * version 3 as published by the Free Software Foundation
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Affero General Public License for more details.
 *
 * You should have received a copy of the GNU Affero General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
*/

#include "platelines.h"

using namespace cv;
using namespace std;

const float MIN_CONFIDENCE = 0.3;

namespace alpr
{

  PlateLines::PlateLines(PipelineData* pipelineData)
  {
    this->pipelineData = pipelineData;

    this->debug = pipelineData->config->debugPlateLines;

    if (debug)
      cout << "PlateLines constructor" << endl;
  }

  PlateLines::~PlateLines()
  {
  }

  void PlateLines::processImage(Mat inputImage, vector<TextLine> textLines, float sensitivity)
  {
    if (this->debug)
      cout << "PlateLines findLines" << endl;

    timespec startTime;
    getTimeMonotonic(&startTime);

    // Ignore input images that are pure white or pure black
    Scalar avgPixelIntensity = mean(inputImage);
    if (avgPixelIntensity[0] == 255)
      return;
    else if (avgPixelIntensity[0] == 0)
      return;

    // Do a bilateral filter to clean the noise but keep edges sharp
    Mat smoothed(inputImage.size(), inputImage.type());
    adaptiveBilateralFilter(inputImage, smoothed, Size(3,3), 45, 45);

    int morph_elem  = 2;
    int morph_size = 2;
    Mat element = getStructuringElement( morph_elem, Size( 2*morph_size + 1, 2*morph_size+1 ), Point( morph_size, morph_size ) );

    Mat edges(inputImage.size(), inputImage.type());
    Canny(smoothed, edges, 66, 133);

    // Create a mask that is dilated based on the detected characters

    Mat mask = Mat::zeros(inputImage.size(), CV_8U);

    for (unsigned int i = 0; i < textLines.size(); i++)
    {
      vector<vector<Point> > polygons;
      polygons.push_back(textLines[i].textArea);
      fillPoly(mask, polygons, Scalar(255,255,255));
    }

    dilate(mask, mask, getStructuringElement( 1, Size( 1 + 1, 2*1+1 ), Point( 1, 1 ) ));
    bitwise_not(mask, mask);

    // AND canny edges with the character mask
    bitwise_and(edges, mask, edges);

    vector<PlateLine> hlines = this->getLines(edges, sensitivity, false);
    vector<PlateLine> vlines = this->getLines(edges, sensitivity, true);
    for (unsigned int i = 0; i < hlines.size(); i++)
      this->horizontalLines.push_back(hlines[i]);
    for (unsigned int i = 0; i < vlines.size(); i++)
      this->verticalLines.push_back(vlines[i]);

    // if debug is enabled, draw the image
    if (this->debug)
    {
      Mat debugImgHoriz(edges.size(), edges.type());
      Mat debugImgVert(edges.size(), edges.type());
      edges.copyTo(debugImgHoriz);
      edges.copyTo(debugImgVert);
      cvtColor(debugImgHoriz,debugImgHoriz,CV_GRAY2BGR);
      cvtColor(debugImgVert,debugImgVert,CV_GRAY2BGR);

      for( size_t i = 0; i < this->horizontalLines.size(); i++ )
      {
        line( debugImgHoriz, this->horizontalLines[i].line.p1, this->horizontalLines[i].line.p2, Scalar(0,0,255), 1, CV_AA);
      }

      for( size_t i = 0; i < this->verticalLines.size(); i++ )
      {
        line( debugImgVert, this->verticalLines[i].line.p1, this->verticalLines[i].line.p2, Scalar(0,0,255), 1, CV_AA);
      }

      vector<Mat> images;
      images.push_back(debugImgHoriz);
      images.push_back(debugImgVert);

      Mat dashboard = drawImageDashboard(images, debugImgVert.type(), 1);
      displayImage(pipelineData->config, "Hough Lines", dashboard);
    }

    if (pipelineData->config->debugTiming)
    {
      timespec endTime;
      getTimeMonotonic(&endTime);
      cout << "Plate Lines Time: " << diffclock(startTime, endTime) << "ms." << endl;
    }

  }

  vector<PlateLine> PlateLines::getLines(Mat edges, float sensitivityMultiplier, bool vertical)
  {
    if (this->debug)
      cout << "PlateLines::getLines" << endl;

    static int HORIZONTAL_SENSITIVITY = pipelineData->config->plateLinesSensitivityHorizontal;
    static int VERTICAL_SENSITIVITY = pipelineData->config->plateLinesSensitivityVertical;

    vector<Vec2f> allLines;
    vector<PlateLine> filteredLines;

    int sensitivity;
    if (vertical)
      sensitivity = VERTICAL_SENSITIVITY * (1.0 / sensitivityMultiplier);
    else
      sensitivity = HORIZONTAL_SENSITIVITY * (1.0 / sensitivityMultiplier);

    HoughLines( edges, allLines, 1, CV_PI/180, sensitivity, 0, 0 );

    for( size_t i = 0; i < allLines.size(); i++ )
    {
      float rho = allLines[i][0], theta = allLines[i][1];
      Point pt1, pt2;
      double a = cos(theta), b = sin(theta);
      double x0 = a*rho, y0 = b*rho;

      double angle = theta * (180 / CV_PI);
      pt1.x = cvRound(x0 + 1000*(-b));
      pt1.y = cvRound(y0 + 1000*(a));
      pt2.x = cvRound(x0 - 1000*(-b));
      pt2.y = cvRound(y0 - 1000*(a));

      if (vertical)
      {
        if (angle < 20 || angle > 340 || (angle > 160 && angle < 210))
        {
          // good vertical

          LineSegment line;
          if (pt1.y <= pt2.y)
            line = LineSegment(pt2.x, pt2.y, pt1.x, pt1.y);
          else
            line = LineSegment(pt1.x, pt1.y, pt2.x, pt2.y);

          // Get rid of the -1000, 1000 stuff.  Terminate at the edges of the image
          // Helps with debugging/rounding issues later
          LineSegment top(0, 0, edges.cols, 0);
          LineSegment bottom(0, edges.rows, edges.cols, edges.rows);
          Point p1 = line.intersection(bottom);
          Point p2 = line.intersection(top);

          PlateLine plateLine;
          plateLine.line = LineSegment(p1.x, p1.y, p2.x, p2.y);
          plateLine.confidence = (1.0 - MIN_CONFIDENCE) * ((float) (allLines.size() - i)) / ((float)allLines.size()) + MIN_CONFIDENCE;
          filteredLines.push_back(plateLine);
        }
      }
      else
      {
        if ( (angle > 70 && angle < 110) || (angle > 250 && angle < 290))
        {
          // good horizontal

          LineSegment line;
          if (pt1.x <= pt2.x)
            line = LineSegment(pt1.x, pt1.y, pt2.x, pt2.y);
          else
            line =LineSegment(pt2.x, pt2.y, pt1.x, pt1.y);

          // Get rid of the -1000, 1000 stuff.  Terminate at the edges of the image
          // Helps with debugging/ rounding issues later
          int newY1 = line.getPointAt(0);
          int newY2 = line.getPointAt(edges.cols);

          PlateLine plateLine;
          plateLine.line = LineSegment(0, newY1, edges.cols, newY2);
          plateLine.confidence = (1.0 - MIN_CONFIDENCE) * ((float) (allLines.size() - i)) / ((float)allLines.size()) + MIN_CONFIDENCE;
          filteredLines.push_back(plateLine);
        }
      }
    }

    return filteredLines;
  }

  Mat PlateLines::customGrayscaleConversion(Mat src)
  {
    Mat img_hsv;
    cvtColor(src,img_hsv,CV_BGR2HSV);

    Mat grayscale = Mat(img_hsv.size(), CV_8U );
    Mat hue(img_hsv.size(), CV_8U );

    for (int row = 0; row < img_hsv.rows; row++)
    {
      for (int col = 0; col < img_hsv.cols; col++)
      {
        int h = (int) img_hsv.at<Vec3b>(row, col)[0];
        //int s = (int) img_hsv.at<Vec3b>(row, col)[1];
        int v = (int) img_hsv.at<Vec3b>(row, col)[2];

        int pixval = pow(v, 1.05);

        if (pixval > 255)
          pixval = 255;
        grayscale.at<uchar>(row, col) = pixval;

        hue.at<uchar>(row, col) = h * (255.0 / 180.0);
      }
    }

    //displayImage(config, "Hue", hue);
    return grayscale;
  }

}<commit_msg>Ignore crops that are pure white or pure black with a little leniency<commit_after>/*
 * Copyright (c) 2015 New Designs Unlimited, LLC
 * Opensource Automated License Plate Recognition [http://www.openalpr.com]
 *
 * This file is part of OpenAlpr.
 *
 * OpenAlpr is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Affero General Public License
 * version 3 as published by the Free Software Foundation
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Affero General Public License for more details.
 *
 * You should have received a copy of the GNU Affero General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
*/

#include "platelines.h"

using namespace cv;
using namespace std;

const float MIN_CONFIDENCE = 0.3;

namespace alpr
{

  PlateLines::PlateLines(PipelineData* pipelineData)
  {
    this->pipelineData = pipelineData;

    this->debug = pipelineData->config->debugPlateLines;

    if (debug)
      cout << "PlateLines constructor" << endl;
  }

  PlateLines::~PlateLines()
  {
  }

  void PlateLines::processImage(Mat inputImage, vector<TextLine> textLines, float sensitivity)
  {
    if (this->debug)
      cout << "PlateLines findLines" << endl;

    timespec startTime;
    getTimeMonotonic(&startTime);

    // Ignore input images that are pure white or pure black
    Scalar avgPixelIntensity = mean(inputImage);
    if (avgPixelIntensity[0] >= 252)
      return;
    else if (avgPixelIntensity[0] <= 3)
      return;

    // Do a bilateral filter to clean the noise but keep edges sharp
    Mat smoothed(inputImage.size(), inputImage.type());
    adaptiveBilateralFilter(inputImage, smoothed, Size(3,3), 45, 45);

    int morph_elem  = 2;
    int morph_size = 2;
    Mat element = getStructuringElement( morph_elem, Size( 2*morph_size + 1, 2*morph_size+1 ), Point( morph_size, morph_size ) );

    Mat edges(inputImage.size(), inputImage.type());
    Canny(smoothed, edges, 66, 133);

    // Create a mask that is dilated based on the detected characters

    Mat mask = Mat::zeros(inputImage.size(), CV_8U);

    for (unsigned int i = 0; i < textLines.size(); i++)
    {
      vector<vector<Point> > polygons;
      polygons.push_back(textLines[i].textArea);
      fillPoly(mask, polygons, Scalar(255,255,255));
    }

    dilate(mask, mask, getStructuringElement( 1, Size( 1 + 1, 2*1+1 ), Point( 1, 1 ) ));
    bitwise_not(mask, mask);

    // AND canny edges with the character mask
    bitwise_and(edges, mask, edges);

    vector<PlateLine> hlines = this->getLines(edges, sensitivity, false);
    vector<PlateLine> vlines = this->getLines(edges, sensitivity, true);
    for (unsigned int i = 0; i < hlines.size(); i++)
      this->horizontalLines.push_back(hlines[i]);
    for (unsigned int i = 0; i < vlines.size(); i++)
      this->verticalLines.push_back(vlines[i]);

    // if debug is enabled, draw the image
    if (this->debug)
    {
      Mat debugImgHoriz(edges.size(), edges.type());
      Mat debugImgVert(edges.size(), edges.type());
      edges.copyTo(debugImgHoriz);
      edges.copyTo(debugImgVert);
      cvtColor(debugImgHoriz,debugImgHoriz,CV_GRAY2BGR);
      cvtColor(debugImgVert,debugImgVert,CV_GRAY2BGR);

      for( size_t i = 0; i < this->horizontalLines.size(); i++ )
      {
        line( debugImgHoriz, this->horizontalLines[i].line.p1, this->horizontalLines[i].line.p2, Scalar(0,0,255), 1, CV_AA);
      }

      for( size_t i = 0; i < this->verticalLines.size(); i++ )
      {
        line( debugImgVert, this->verticalLines[i].line.p1, this->verticalLines[i].line.p2, Scalar(0,0,255), 1, CV_AA);
      }

      vector<Mat> images;
      images.push_back(debugImgHoriz);
      images.push_back(debugImgVert);

      Mat dashboard = drawImageDashboard(images, debugImgVert.type(), 1);
      displayImage(pipelineData->config, "Hough Lines", dashboard);
    }

    if (pipelineData->config->debugTiming)
    {
      timespec endTime;
      getTimeMonotonic(&endTime);
      cout << "Plate Lines Time: " << diffclock(startTime, endTime) << "ms." << endl;
    }

  }

  vector<PlateLine> PlateLines::getLines(Mat edges, float sensitivityMultiplier, bool vertical)
  {
    if (this->debug)
      cout << "PlateLines::getLines" << endl;

    static int HORIZONTAL_SENSITIVITY = pipelineData->config->plateLinesSensitivityHorizontal;
    static int VERTICAL_SENSITIVITY = pipelineData->config->plateLinesSensitivityVertical;

    vector<Vec2f> allLines;
    vector<PlateLine> filteredLines;

    int sensitivity;
    if (vertical)
      sensitivity = VERTICAL_SENSITIVITY * (1.0 / sensitivityMultiplier);
    else
      sensitivity = HORIZONTAL_SENSITIVITY * (1.0 / sensitivityMultiplier);

    HoughLines( edges, allLines, 1, CV_PI/180, sensitivity, 0, 0 );

    for( size_t i = 0; i < allLines.size(); i++ )
    {
      float rho = allLines[i][0], theta = allLines[i][1];
      Point pt1, pt2;
      double a = cos(theta), b = sin(theta);
      double x0 = a*rho, y0 = b*rho;

      double angle = theta * (180 / CV_PI);
      pt1.x = cvRound(x0 + 1000*(-b));
      pt1.y = cvRound(y0 + 1000*(a));
      pt2.x = cvRound(x0 - 1000*(-b));
      pt2.y = cvRound(y0 - 1000*(a));

      if (vertical)
      {
        if (angle < 20 || angle > 340 || (angle > 160 && angle < 210))
        {
          // good vertical

          LineSegment line;
          if (pt1.y <= pt2.y)
            line = LineSegment(pt2.x, pt2.y, pt1.x, pt1.y);
          else
            line = LineSegment(pt1.x, pt1.y, pt2.x, pt2.y);

          // Get rid of the -1000, 1000 stuff.  Terminate at the edges of the image
          // Helps with debugging/rounding issues later
          LineSegment top(0, 0, edges.cols, 0);
          LineSegment bottom(0, edges.rows, edges.cols, edges.rows);
          Point p1 = line.intersection(bottom);
          Point p2 = line.intersection(top);

          PlateLine plateLine;
          plateLine.line = LineSegment(p1.x, p1.y, p2.x, p2.y);
          plateLine.confidence = (1.0 - MIN_CONFIDENCE) * ((float) (allLines.size() - i)) / ((float)allLines.size()) + MIN_CONFIDENCE;
          filteredLines.push_back(plateLine);
        }
      }
      else
      {
        if ( (angle > 70 && angle < 110) || (angle > 250 && angle < 290))
        {
          // good horizontal

          LineSegment line;
          if (pt1.x <= pt2.x)
            line = LineSegment(pt1.x, pt1.y, pt2.x, pt2.y);
          else
            line =LineSegment(pt2.x, pt2.y, pt1.x, pt1.y);

          // Get rid of the -1000, 1000 stuff.  Terminate at the edges of the image
          // Helps with debugging/ rounding issues later
          int newY1 = line.getPointAt(0);
          int newY2 = line.getPointAt(edges.cols);

          PlateLine plateLine;
          plateLine.line = LineSegment(0, newY1, edges.cols, newY2);
          plateLine.confidence = (1.0 - MIN_CONFIDENCE) * ((float) (allLines.size() - i)) / ((float)allLines.size()) + MIN_CONFIDENCE;
          filteredLines.push_back(plateLine);
        }
      }
    }

    return filteredLines;
  }

  Mat PlateLines::customGrayscaleConversion(Mat src)
  {
    Mat img_hsv;
    cvtColor(src,img_hsv,CV_BGR2HSV);

    Mat grayscale = Mat(img_hsv.size(), CV_8U );
    Mat hue(img_hsv.size(), CV_8U );

    for (int row = 0; row < img_hsv.rows; row++)
    {
      for (int col = 0; col < img_hsv.cols; col++)
      {
        int h = (int) img_hsv.at<Vec3b>(row, col)[0];
        //int s = (int) img_hsv.at<Vec3b>(row, col)[1];
        int v = (int) img_hsv.at<Vec3b>(row, col)[2];

        int pixval = pow(v, 1.05);

        if (pixval > 255)
          pixval = 255;
        grayscale.at<uchar>(row, col) = pixval;

        hue.at<uchar>(row, col) = h * (255.0 / 180.0);
      }
    }

    //displayImage(config, "Hue", hue);
    return grayscale;
  }

}<|endoftext|>


<commit_before>// MSLParser.cpp : Defines the entry point for the console application.
//

#include "stdafx.h"
#define BOOST_RESULT_OF_USE_DECLTYPE
#define BOOST_SPIRIT_USE_PHOENIX_V3

#include <string>
#include <vector>
#include <map>
#include <memory>
#include <boost/config/warning_disable.hpp>
#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix_core.hpp>
#include <boost/spirit/include/phoenix_operator.hpp>
#include <boost/make_shared.hpp>
#include <boost/fusion/include/std_pair.hpp>

/*
MSL Parser

Wbudowane typy:
- string (cudzys?owia, bez cudzys?owiu)
- float (1. 1 100%)
- boolean (true, false)
- null

- tablica [10 20 30]
- mapa {1:1 2:2 3:3}

- named value   name() value

-komentarze

[name() aaa
*/

namespace msl
{
	class Value
	{
	public:
		using pointer = std::shared_ptr<Value>;
		using ArrayType = std::vector<pointer>;
		using MapType = std::map<pointer, pointer>;

		enum class Type { String, Float, Boolean, Null, Array, Map };

		virtual float asFloat() { return 0.0f; }
		virtual const std::string& asString() { static std::string n = "Null"; return n; };
		virtual const ArrayType& asArray() { throw std::exception(); }
		virtual const MapType& asMap() { throw std::exception(); }

	protected:
		Type _type = Type::Null;
	};

	template<typename T, Value::Type type>
	class TemplatedValue : public Value
	{
	public:
		using base_type = TemplatedValue<T, type>;

		TemplatedValue() : { _type = type; }
		TemplatedValue(const T& t) : _value(t) { _type = type; }
		TemplatedValue(T&& t) : _value(std::move(t)) { _type = type; }

	protected:
		T _value;
	};

	class StringValue : public TemplatedValue<std::string, Value::Type::String>
	{
	public:
		using base_type::TemplatedValue;
		const std::string& asString() override { return _value; }
	};

	class FloatValue : public TemplatedValue<float, Value::Type::Float>
	{
	public:
		using base_type::TemplatedValue;
		float asFloat() override { return _value; }
	};

	class ArrayValue : public TemplatedValue<Value::ArrayType, Value::Type::Array>
	{
	public:
		using base_type::TemplatedValue;
		const ArrayType& asArray() override { return _value; }
	};

	class MapValue : public TemplatedValue<Value::MapType, Value::Type::Map>
	{
	public:
		using base_type::TemplatedValue;
		const MapType& asMap() override { return _value; }
	};
};

namespace client
{

	using namespace boost::spirit;

	template <typename Iterator>
	struct msl_grammar : qi::grammar<Iterator, msl::Value::pointer(), ascii::space_type>
	{
		using standard_rule = qi::rule<Iterator, msl::Value::pointer(), ascii::space_type>;

		msl_grammar() : msl_grammar::base_type(msl)
		{
			using namespace msl;
			using qi::lit;
			using qi::rule;
			using qi::float_;
			using qi::_1;
			using qi::phrase_parse;
			using ascii::space;
			using ascii::char_;
			using ascii::string;
			using namespace qi::labels;
			using namespace boost::fusion;

			//float
			{
				auto ffunc = [](auto& f, auto &c)
				{
					at_c<0>(c.attributes) = std::make_shared<FloatValue>(f);
				};

				rule_float = float_[ffunc];
			}

			//string
			{
				auto func = [](auto&& f, auto &c)
				{
					at_c<0>(c.attributes) = std::make_shared<StringValue>(f);
				};

				quoted_string %= lexeme['"' >> *(char_ - '"') >> '"'];

				rule_string = quoted_string[func];
			}

			//array
			{
				rule_varray %= '[' >> *rule_value >> ']';

				auto func = [](auto&& f, auto &c)
				{
					at_c<0>(c.attributes) = std::make_shared<ArrayValue>(f);
				};
				rule_array = rule_varray[func];
			}

			//map
			{
				rule_vmap %= qi::lit("{") >> *(rule_value >> qi::lit(":") >> rule_value) >> qi::lit("}");

				auto func = [](auto&& f, auto &c)
				{
					at_c<0>(c.attributes) = std::make_shared<MapValue>(f);
				};
				rule_map = rule_vmap[func];
			}

			rule_value = rule_float | rule_array | rule_map | rule_string;
			msl = rule_value;
		}

		standard_rule msl;

		standard_rule rule_value;
		standard_rule rule_float;
		standard_rule rule_string;
		standard_rule rule_array;
		standard_rule rule_map;

		qi::rule<Iterator, msl::Value::MapType(), ascii::space_type> rule_vmap;
		qi::rule<Iterator, msl::Value::ArrayType(), ascii::space_type> rule_varray;
		qi::rule<Iterator, std::string()> quoted_string;
	};

	template <typename Iterator>
	msl::Value::pointer parse_msl(Iterator first, Iterator last)
	{
		msl_grammar<Iterator> msl; // Our grammar
		msl::Value::pointer ast; // Our tree

		using boost::spirit::ascii::space;
		bool r = phrase_parse(first, last, msl, space, ast);

		return ast;
	}

}

int main()
{
	R"foo(

	{
		HP: 100
		Damage: 20

	}

	)foo";

#if 1
	{
		std::string str = "34.1";
		auto p = client::parse_msl(str.begin(), str.end());

		//std::string strArr = "[34.1 {1:1 \"Mateusz\":\"Borycki\"} \"Test\"]";

		std::string strArr = R"foo(

		[
			34.1
			{
				1:1
				"Mateusz":"Borycki"
			}
			"Test"
		]

		)foo";

		auto arr = client::parse_msl(strArr.begin(), strArr.end());
	}

#else
	{
		std::string str = "34.1";
		auto p = client::parse_msl(str.begin(), str.end());
	}

	{
		std::string str = "aaa sadas";
		auto p = client::parse_complex(str.begin(), str.end());
	}

	{
		std::string str = "\"aaa sa\"das";
		auto p = client::parse_complex(str.begin(), str.end());
	}
#endif

    return 0;
}

<commit_msg>- less code<commit_after>// MSLParser.cpp : Defines the entry point for the console application.
//

#include "stdafx.h"
#define BOOST_RESULT_OF_USE_DECLTYPE
#define BOOST_SPIRIT_USE_PHOENIX_V3

#include <string>
#include <vector>
#include <map>
#include <memory>
#include <boost/config/warning_disable.hpp>
#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix_core.hpp>
#include <boost/spirit/include/phoenix_operator.hpp>
#include <boost/make_shared.hpp>
#include <boost/fusion/include/std_pair.hpp>

/*
MSL Parser

Wbudowane typy:
- string (cudzys?owia, bez cudzys?owiu)
- float (1. 1 100%)
- boolean (true, false)
- null

- tablica [10 20 30]
- mapa {1:1 2:2 3:3}

- named value   name() value

-komentarze

[name() aaa
*/

namespace msl
{
	class Value
	{
	public:
		using pointer = std::shared_ptr<Value>;
		using ArrayType = std::vector<pointer>;
		using MapType = std::map<pointer, pointer>;

		enum class Type { String, Float, Boolean, Null, Array, Map };

		virtual float asFloat() { return 0.0f; }
		virtual const std::string& asString() { static std::string n = "Null"; return n; };
		virtual const ArrayType& asArray() { throw std::exception(); }
		virtual const MapType& asMap() { throw std::exception(); }

	protected:
		Type _type = Type::Null;
	};

	template<typename T, Value::Type type>
	class TemplatedValue : public Value
	{
	public:
		using base_type = TemplatedValue<T, type>;

		TemplatedValue() : { _type = type; }
		TemplatedValue(const T& t) : _value(t) { _type = type; }
		TemplatedValue(T&& t) : _value(std::move(t)) { _type = type; }

	protected:
		T _value;
	};

	class StringValue : public TemplatedValue<std::string, Value::Type::String>
	{
	public:
		using base_type::TemplatedValue;
		const std::string& asString() override { return _value; }
	};

	class FloatValue : public TemplatedValue<float, Value::Type::Float>
	{
	public:
		using base_type::TemplatedValue;
		float asFloat() override { return _value; }
	};

	class ArrayValue : public TemplatedValue<Value::ArrayType, Value::Type::Array>
	{
	public:
		using base_type::TemplatedValue;
		const ArrayType& asArray() override { return _value; }
	};

	class MapValue : public TemplatedValue<Value::MapType, Value::Type::Map>
	{
	public:
		using base_type::TemplatedValue;
		const MapType& asMap() override { return _value; }
	};
};

namespace client
{

	using namespace boost::spirit;

	template <typename Iterator>
	struct msl_grammar : qi::grammar<Iterator, msl::Value::pointer(), ascii::space_type>
	{
		using standard_rule = qi::rule<Iterator, msl::Value::pointer(), ascii::space_type>;

		template<typename Type>
		auto createAttrSynthesizer()
		{

			return [](auto&& f, auto &c)
			{
				using namespace boost::fusion;
				at_c<0>(c.attributes) = std::make_shared<Type>(f);
			};
		}

		msl_grammar() : msl_grammar::base_type(msl)
		{
			using namespace msl;
			using qi::lit;
			using qi::rule;
			using qi::float_;
			using qi::_1;
			using qi::phrase_parse;
			using ascii::space;
			using ascii::char_;
			using ascii::string;
			using namespace qi::labels;

			//float
			{
				rule_float = float_[createAttrSynthesizer<FloatValue>()];
			}

			//string
			{
				quoted_string %= lexeme['"' >> *(char_ - '"') >> '"'];
				rule_string = quoted_string[createAttrSynthesizer<StringValue>()];
			}

			//array
			{
				rule_varray %= '[' >> *rule_value >> ']';
				rule_array = rule_varray[createAttrSynthesizer<ArrayValue>()];
			}

			//map
			{
				rule_vmap %= qi::lit("{") >> *(rule_value >> qi::lit(":") >> rule_value) >> qi::lit("}");
				rule_map = rule_vmap[createAttrSynthesizer<MapValue>()];
			}

			rule_value = rule_float | rule_array | rule_map | rule_string;
			msl = rule_value;
		}

		standard_rule msl;

		standard_rule rule_value;
		standard_rule rule_float;
		standard_rule rule_string;
		standard_rule rule_array;
		standard_rule rule_map;

		qi::rule<Iterator, msl::Value::MapType(), ascii::space_type> rule_vmap;
		qi::rule<Iterator, msl::Value::ArrayType(), ascii::space_type> rule_varray;
		qi::rule<Iterator, std::string()> quoted_string;
	};

	template <typename Iterator>
	msl::Value::pointer parse_msl(Iterator first, Iterator last)
	{
		msl_grammar<Iterator> msl; // Our grammar
		msl::Value::pointer ast; // Our tree

		using boost::spirit::ascii::space;
		bool r = phrase_parse(first, last, msl, space, ast);

		return ast;
	}

}

int main()
{
	R"foo(

	{
		HP: 100
		Damage: 20

	}

	)foo";

#if 1
	{
		std::string str = "34.1";
		auto p = client::parse_msl(str.begin(), str.end());

		//std::string strArr = "[34.1 {1:1 \"Mateusz\":\"Borycki\"} \"Test\"]";

		std::string strArr = R"foo(

		[
			34.1
			{
				1:1
				"Mateusz":"Borycki"
			}
			"Test"
		]

		)foo";

		auto arr = client::parse_msl(strArr.begin(), strArr.end());
	}

#else
	{
		std::string str = "34.1";
		auto p = client::parse_msl(str.begin(), str.end());
	}

	{
		std::string str = "aaa sadas";
		auto p = client::parse_complex(str.begin(), str.end());
	}

	{
		std::string str = "\"aaa sa\"das";
		auto p = client::parse_complex(str.begin(), str.end());
	}
#endif

    return 0;
}

<|endoftext|>