<commit_before>
#include "algorithms/FEAL.hpp"
#include "util/Logging.hpp"
#include "algorithms/FEALLinearCA.hpp"

inline std::ostream& operator<<(std::ostream& o, const std::vector<char>& chars)
{
    for (auto c : chars)
    {
        o << std::hex << static_cast<unsigned int>(static_cast<unsigned char>(c));
    }
    return o;
}

int main()
{
    crypto::initLogging();

    CRYPTO_LOG_DEBUG(crypto::g_logger::get()) << "Program started";

    crypto::algorithms::FEAL feal(0xc531e0ae7f460f11);

    std::string myString;
    std::cout << "Enter string to encrypt: ";
    std::getline(std::cin, myString);

    while (myString != std::string("quit"))
    {
        crypto::algorithms::FEALLinearCA linearCa;

        std::cout << std::endl;

        if (myString.length() % feal.getBlockSize() != 0 || myString.length() == 0)
        {
            auto spaceToFill = feal.getBlockSize() - myString.length() % feal.getBlockSize();
            for (auto i = 0; i < spaceToFill; ++i)
            {
                myString += '\0';
            }
        }

        std::vector<char> myData(myString.c_str(), myString.c_str() + myString.length() + 1);

        auto encryptionPos = myData.begin();
        while (encryptionPos < myData.end() - feal.getBlockSize())
        {
            auto encryptedPos = encryptionPos;
            std::vector<char> plaintext{encryptionPos, encryptionPos + feal.getBlockSize()};

            encryptionPos = feal.encrypt(myData, encryptionPos);

            std::vector<char> ciphertext{encryptedPos, encryptedPos + feal.getBlockSize()};
            linearCa.addPlaintextCiphertextPair({plaintext, ciphertext});
        }

        std::cout << "Encrypted: " << myData << std::endl;

        auto decryptionPos = myData.begin();
        while (decryptionPos < myData.end() - feal.getBlockSize())
        {
            decryptionPos = feal.decrypt(myData, decryptionPos);
        }

        std::string myDecrypted(myData.begin(), myData.end());
        std::cout << "Decrypted: " << myDecrypted << std::endl;

        auto key = linearCa.breakCipher();

        std::cout << "Key: " << key << std::endl;

        std::cout << "Enter string to encrypt: ";
        std::getline(std::cin, myString);
    }

    return 0;
}
<commit_msg>Improve main program loop<commit_after>#include "algorithms/FEAL.hpp"
#include "util/Logging.hpp"
#include "algorithms/FEALLinearCA.hpp"

inline std::ostream& operator<<(std::ostream& o, const std::vector<char>& chars)
{
    for (auto c : chars)
    {
        o << std::setfill('0') << std::setw(2) << std::hex << static_cast<unsigned int>(static_cast<unsigned char>(c));
    }
    return o;
}

void string_to_vector(std::string str, std::vector<char>& array)
{
    auto length = str.length();
    // make sure the input string has an even digit numbers
    if (length % 2 == 1)
    {
        str = "0" + str;
        length++;
    }

    // allocate memory for the output array
    array.reserve(length / 2);

    std::stringstream sstr(str);
    for (int i = 0; i < length / 2; i++)
    {
        char ch1, ch2;
        sstr >> ch1 >> ch2;
        int dig1 = 0, dig2 = 0;
        if (isdigit(ch1)) dig1 = ch1 - '0';
        else if (ch1 >= 'A' && ch1 <= 'F') dig1 = ch1 - 'A' + 10;
        else if (ch1 >= 'a' && ch1 <= 'f') dig1 = ch1 - 'a' + 10;
        if (isdigit(ch2)) dig2 = ch2 - '0';
        else if (ch2 >= 'A' && ch2 <= 'F') dig2 = ch2 - 'A' + 10;
        else if (ch2 >= 'a' && ch2 <= 'f') dig2 = ch2 - 'a' + 10;
        array.push_back(dig1 * 16 + dig2);
    }
}

int main()
{
    crypto::initLogging();

    CRYPTO_LOG_DEBUG(crypto::g_logger::get()) << "Program started";

    std::string myString;
    std::cout << "enter 'breakCipher' to attempt to break a cipher or type anything else to encrypt something" << std::endl;
    std::getline(std::cin, myString);

    if (myString == "breakCipher")
    {
        std::cout << "Enter plaintext: ";
        std::getline(std::cin, myString);

        std::cout << "Enter ciphertext: ";
        std::string encrypted;
        std::getline(std::cin, encrypted);
        while (myString != "quit")
        {
            crypto::algorithms::FEAL feal{0x0};
            crypto::algorithms::FEALLinearCA linearCa;

            if (myString.length() % feal.getBlockSize() != 0 || myString.length() == 0)
            {
                auto spaceToFill = feal.getBlockSize() - myString.length() % feal.getBlockSize();
                for (auto i = 0; i < spaceToFill; ++i)
                {
                    myString += ' ';
                }
            }

            std::vector<char> myData(myString.c_str(), myString.c_str() + myString.length());
            std::vector<char> myEncrypted;

            string_to_vector(encrypted, myEncrypted);

            auto ptPos = myData.begin();
            auto encPos = myEncrypted.begin();
            while (ptPos <= myData.end() - feal.getBlockSize())
            {
                std::vector<char> plaintext{ptPos, ptPos + feal.getBlockSize()};

                std::vector<char> ciphertext{encPos, encPos + feal.getBlockSize()};
                linearCa.addPlaintextCiphertextPair({plaintext, ciphertext});

                ptPos += feal.getBlockSize();
                encPos += feal.getBlockSize();
            }

            try
            {
                std::cout << linearCa.breakCipher();
            }
            catch (std::runtime_error e)
            {
                std::cout << e.what() << std::endl;
            }

            std::cout << "Enter plaintext: ";
            std::getline(std::cin, myString);

            std::cout << "Enter ciphertext: ";
            std::getline(std::cin, encrypted);
        }
    }
    else
    {
        ////////
        std::cout << "Enter string to encrypt: ";
        std::getline(std::cin, myString);

        while (myString != std::string("quit"))
        {
            uint64_t myKey;
            {
                std::string input;

                std::cout << "Enter 8 byte hex key (i.e. 16 hex digits): ";
                std::getline(std::cin, input);
                while (input.length() != 16 || input.find_first_not_of("0123456789abcdefABCDEF") != std::string::npos)
                {
                    std::cout << "Not a hexadecimal key with 16 digits" << std::endl;
                    std::cout << "Enter 8 byte hex key (i.e. 16 hex digits): ";
                    std::getline(std::cin, input);
                }
                std::istringstream is(input);
                is >> std::hex >> myKey;
            }

            crypto::algorithms::FEAL feal(myKey);
            crypto::algorithms::FEALLinearCA linearCa;

            std::cout << std::endl;

            if (myString.length() % feal.getBlockSize() != 0 || myString.length() == 0)
            {
                auto spaceToFill = feal.getBlockSize() - myString.length() % feal.getBlockSize();
                for (auto i = 0; i < spaceToFill; ++i)
                {
                    myString += ' ';
                }
            }

            std::vector<char> myData(myString.c_str(), myString.c_str() + myString.length());

            auto encryptionPos = myData.begin();
            while (encryptionPos <= myData.end() - feal.getBlockSize())
            {
                auto encryptedPos = encryptionPos;
                std::vector<char> plaintext{encryptionPos, encryptionPos + feal.getBlockSize()};

                encryptionPos = feal.encrypt(myData, encryptionPos);

                std::vector<char> ciphertext{encryptedPos, encryptedPos + feal.getBlockSize()};
                linearCa.addPlaintextCiphertextPair({plaintext, ciphertext});
            }

            std::cout << "Encrypted: " << myData << std::endl;

            auto decryptionPos = myData.begin();
            while (decryptionPos <= myData.end() - feal.getBlockSize())
            {
                decryptionPos = feal.decrypt(myData, decryptionPos);
            }

            std::string myDecrypted(myData.begin(), myData.end());
            std::cout << "Decrypted: " << myDecrypted << std::endl;

            auto key = linearCa.breakCipher();

            std::cout << "Key: " << key << std::endl;

            std::cout << "Enter string to encrypt: ";
            std::getline(std::cin, myString);
        }
    }

    return 0;
}
<|endoftext|>


<commit_before>/* --------------------------------------------------------------------------
 *
 *    openscenegraph textLib / FTGL
 *
 * --------------------------------------------------------------------------
 *
 *    prog:    max rheiner;mrn@paus.ch
 *    date:    4/25/2001    (m/d/y)
 *
 * ----------------------------------------------------------------------------
 *
 * --------------------------------------------------------------------------
 */

#include <osgText/Font>

#include <osg/Notify>
#include <osgDB/FileUtils>

#include "FTFace.h"
#include "FTGLBitmapFont.h"
#include "FTGLPixmapFont.h"
#include "FTGLOutlineFont.h"
#include "FTGLPolygonFont.h"
#include "FTGLTextureFont.h"

using namespace osg;
using namespace osgText;

// define the default paths to look for fonts.
// note delimator is : for unix, ; for windows.
#if defined(__linux) || defined(__FreeBSD__) || defined (__sgi)
    static char* s_FontFilePath = ".:/usr/share/fonts/ttf:/usr/share/fonts/ttf/western:/usr/share/fonts/ttf/decoratives";
#elif defined(WIN32)
    static char* s_FontFilePath = ".;C:/windows/fonts";
#else
    static char* s_FontFilePath = ".:";
#endif

std::string findFontFile(const std::string& str)
{
    // try looking in OSGFILEPATH etc first for fonts.
    char* filename = osgDB::findFile(str.c_str());
    if (filename) return std::string(filename);

    // else fallback into the standard font file paths.
    if (s_FontFilePath)
    {
        filename = osgDB::findFileInPath(str.c_str(),s_FontFilePath);
        if (filename) return std::string(filename);
    }
    osg::notify(osg::WARN)<<"Unable to find font file '"<<str<<"'"<<endl;
    return std::string();
}

///////////////////////////////////////////////////////////////////////////////
// Font
Font::
Font()
{
    _init=false;
    _font=NULL;
    _created=false;

    _pointSize=14;
    _res=72;
}

bool Font::
init(const std::string& font)
{
    _font=NULL;
    _created=false;

    open(font);

    if(_font!=NULL)
        return true;
    else
        return false;
}

Font::
~Font()
{
    clear();
}

bool Font::
open(const std::string& font)
{
    clear();

        std::string filename = findFontFile(font);
        if (filename.empty()) return false;

    _font=createFontObj();
    if( _font!=NULL && _font->Open(filename.c_str()) )
    {
        _init=true;
        _fontName=font;
        return true;
    }
    else
        return false;
}

bool Font::
create(int pointSize,const unsigned int res)
{
    _pointSize=pointSize;
    _res=res;

    return create();
}

bool  Font::
create()
{
    if(_init)
    {
        if(_font->FaceSize(_pointSize,_res))
        {
            _created=true;
            return true;
        }
        else
            return false;
    }
    else
        return false;
}

void Font::
output(const char* text)
{
    if(_created)
        _font->render(text);
    else
        create(_pointSize);
}

void  Font::
clear()
{
    _init=false;

    if(_font)
    {
        delete _font;
        _font=NULL;
    }

    _fontName="";
}

float Font::
getWidth(const char* text)  const
{
    if(_init && _created)
        return _font->Advance(text);
    else
        return -1;
}

int Font::
getHeight()  const
{
    if(_init && _created)
        return _pointSize;
    else
        return -1;
}

int Font::
getDescender() const
{
    if(_init && _created)
        return _font->Descender();
    else
        return -1;
}

int Font::
getAscender() const
{
    if(_init && _created)
        return _font->Ascender();
    else
        return -1;
}

// Font
///////////////////////////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////////////////////////
// BitmapFont

BitmapFont::
BitmapFont(const std::string&    font,
           int                    point_size):
RasterFont()
{
    if(init(font))
    {
    }
    _pointSize=point_size;
}

FTFont*  BitmapFont::
createFontObj(void)
{
    return (FTFont*)(new FTGLBitmapFont);
}

// BitmapFont
///////////////////////////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////////////////////////
// PixmapFont

PixmapFont::
PixmapFont(const std::string&    font,
            int                    point_size):
RasterFont(font)
{
    if(init(font))
    {
    }
    _pointSize=point_size;
}

FTFont*  PixmapFont::
createFontObj(void)
{
    return (FTFont*)(new FTGLPixmapFont);
}

// PixmapFont
///////////////////////////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////////////////////////
// PixmapFont

TextureFont::
TextureFont(const std::string&    font,
            int                    point_size):
RasterFont(font)
{
    if(init(font))
    {
    }
    _pointSize=point_size;
}

FTFont*  TextureFont::
createFontObj(void)
{
    return (FTFont*)(new FTGLTextureFont);
}

// PixmapFont
///////////////////////////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////////////////////////
// _FTGLOutlineFont

OutlineFont::
OutlineFont(const std::string&    font,
            int                    point_size,
            double                precision):
VectorFont(font)
{
    if(init(font))
    {
    }
    _pointSize=point_size;
    _precision=precision;
}

FTFont*  OutlineFont::
createFontObj(void)
{
    return (FTFont*)(new FTGLOutlineFont);
}

// _FTGLOutlineFont
///////////////////////////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////////////////////////
// PolygonFont

PolygonFont::
PolygonFont(const std::string&    font,
            int                    point_size,
            double                precision):
VectorFont(font)
{
    if(init(font))
    {
    }
    _pointSize=point_size;
    _precision=precision;
}

FTFont*  PolygonFont::
createFontObj(void)
{
    return (FTFont*)(new FTGLPolygonFont);
}

// PolygonFont
///////////////////////////////////////////////////////////////////////////////
<commit_msg>Made the Warning font file not found message consistent with other file not found messages.<commit_after>/* --------------------------------------------------------------------------
 *
 *    openscenegraph textLib / FTGL
 *
 * --------------------------------------------------------------------------
 *
 *    prog:    max rheiner;mrn@paus.ch
 *    date:    4/25/2001    (m/d/y)
 *
 * ----------------------------------------------------------------------------
 *
 * --------------------------------------------------------------------------
 */

#include <osgText/Font>

#include <osg/Notify>
#include <osgDB/FileUtils>

#include "FTFace.h"
#include "FTGLBitmapFont.h"
#include "FTGLPixmapFont.h"
#include "FTGLOutlineFont.h"
#include "FTGLPolygonFont.h"
#include "FTGLTextureFont.h"

using namespace osg;
using namespace osgText;

// define the default paths to look for fonts.
// note delimator is : for unix, ; for windows.
#if defined(__linux) || defined(__FreeBSD__) || defined (__sgi)
    static char* s_FontFilePath = ".:/usr/share/fonts/ttf:/usr/share/fonts/ttf/western:/usr/share/fonts/ttf/decoratives";
#elif defined(WIN32)
    static char* s_FontFilePath = ".;C:/windows/fonts";
#else
    static char* s_FontFilePath = ".:";
#endif

std::string findFontFile(const std::string& str)
{
    // try looking in OSGFILEPATH etc first for fonts.
    char* filename = osgDB::findFile(str.c_str());
    if (filename) return std::string(filename);

    // else fallback into the standard font file paths.
    if (s_FontFilePath)
    {
        filename = osgDB::findFileInPath(str.c_str(),s_FontFilePath);
        if (filename) return std::string(filename);
    }
    osg::notify(osg::WARN)<<"Warning: font file \""<<str<<"\" not found."<<endl;
    return std::string();
}

///////////////////////////////////////////////////////////////////////////////
// Font
Font::
Font()
{
    _init=false;
    _font=NULL;
    _created=false;

    _pointSize=14;
    _res=72;
}

bool Font::
init(const std::string& font)
{
    _font=NULL;
    _created=false;

    open(font);

    if(_font!=NULL)
        return true;
    else
        return false;
}

Font::
~Font()
{
    clear();
}

bool Font::
open(const std::string& font)
{
    clear();

        std::string filename = findFontFile(font);
        if (filename.empty()) return false;

    _font=createFontObj();
    if( _font!=NULL && _font->Open(filename.c_str()) )
    {
        _init=true;
        _fontName=font;
        return true;
    }
    else
        return false;
}

bool Font::
create(int pointSize,const unsigned int res)
{
    _pointSize=pointSize;
    _res=res;

    return create();
}

bool  Font::
create()
{
    if(_init)
    {
        if(_font->FaceSize(_pointSize,_res))
        {
            _created=true;
            return true;
        }
        else
            return false;
    }
    else
        return false;
}

void Font::
output(const char* text)
{
    if(_created)
        _font->render(text);
    else
        create(_pointSize);
}

void  Font::
clear()
{
    _init=false;

    if(_font)
    {
        delete _font;
        _font=NULL;
    }

    _fontName="";
}

float Font::
getWidth(const char* text)  const
{
    if(_init && _created)
        return _font->Advance(text);
    else
        return -1;
}

int Font::
getHeight()  const
{
    if(_init && _created)
        return _pointSize;
    else
        return -1;
}

int Font::
getDescender() const
{
    if(_init && _created)
        return _font->Descender();
    else
        return -1;
}

int Font::
getAscender() const
{
    if(_init && _created)
        return _font->Ascender();
    else
        return -1;
}

// Font
///////////////////////////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////////////////////////
// BitmapFont

BitmapFont::
BitmapFont(const std::string&    font,
           int                    point_size):
RasterFont()
{
    if(init(font))
    {
    }
    _pointSize=point_size;
}

FTFont*  BitmapFont::
createFontObj(void)
{
    return (FTFont*)(new FTGLBitmapFont);
}

// BitmapFont
///////////////////////////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////////////////////////
// PixmapFont

PixmapFont::
PixmapFont(const std::string&    font,
            int                    point_size):
RasterFont(font)
{
    if(init(font))
    {
    }
    _pointSize=point_size;
}

FTFont*  PixmapFont::
createFontObj(void)
{
    return (FTFont*)(new FTGLPixmapFont);
}

// PixmapFont
///////////////////////////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////////////////////////
// PixmapFont

TextureFont::
TextureFont(const std::string&    font,
            int                    point_size):
RasterFont(font)
{
    if(init(font))
    {
    }
    _pointSize=point_size;
}

FTFont*  TextureFont::
createFontObj(void)
{
    return (FTFont*)(new FTGLTextureFont);
}

// PixmapFont
///////////////////////////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////////////////////////
// _FTGLOutlineFont

OutlineFont::
OutlineFont(const std::string&    font,
            int                    point_size,
            double                precision):
VectorFont(font)
{
    if(init(font))
    {
    }
    _pointSize=point_size;
    _precision=precision;
}

FTFont*  OutlineFont::
createFontObj(void)
{
    return (FTFont*)(new FTGLOutlineFont);
}

// _FTGLOutlineFont
///////////////////////////////////////////////////////////////////////////////

///////////////////////////////////////////////////////////////////////////////
// PolygonFont

PolygonFont::
PolygonFont(const std::string&    font,
            int                    point_size,
            double                precision):
VectorFont(font)
{
    if(init(font))
    {
    }
    _pointSize=point_size;
    _precision=precision;
}

FTFont*  PolygonFont::
createFontObj(void)
{
    return (FTFont*)(new FTGLPolygonFont);
}

// PolygonFont
///////////////////////////////////////////////////////////////////////////////
<|endoftext|>


<commit_before>#include "ardrone/ardrone.h"
#include <iostream>
#include <vector>

using namespace std;
// --------------------------------------------------------------------------
// main(Number of arguments, Argument values)
// Description  : This is the entry point of the program.
// Return value : SUCCESS:0  ERROR:-1
// --------------------------------------------------------------------------
int main(int argc, char *argv[])
{
  //AR.Drone class
  ARDrone ardrone;

  //Display variables
  char vxDisplay[80]; //print buffer for x (forward/reverse) velocity
  char vyDisplay[80]; //print buffer for y (sideways) velocity
  char vzDisplay[80]; //print buffer for z (up/down) velocity
  char vrDisplay[80]; //print buffer for r rotational velocity

  char speedDisplay[80]; //print buffer for speed
  char headingDisplay[80]; //print buffer for heading
  char hsvSampleDisplay[80]; //print buffer for learning HSV values

  cv::Scalar green = CV_RGB(0,255,0); //putText color value

  //Drone control
  bool autonomous = false;
  bool learnMode = false;

  float speed = 0.0;
  double vx = 0.0;
  double vy = 0.0; double vz = 0.0;
  double vr = 0.0;

  //Image recognition values
  int learnedHue;
  int learnedSaturation;
  int learnedValue;
  int minH = 0, maxH = 255;
  int minS = 0, maxS = 255;
  int minV = 0, maxV = 255;

  //Sampling time [s]
  const double dt = 1.0;

  printf("Connecting to the drone\n");
  printf("If there is no version number response in the next 10 seconds, please restart the drone and code.\n");
  printf("To disconnect, press the ESC key\n");
  fflush(stdout);

  // Initialize
  if (!ardrone.open()) {
    printf("Failed to initialize.\n");
    return -1;
  }

  // XML save data
  std::string filename("thresholds.xml");
  cv::FileStorage fs(filename, cv::FileStorage::READ);

  // If there is a save file then read it
  if (fs.isOpened()) {
    maxH = fs["H_MAX"];
    minH = fs["H_MIN"];
    maxS = fs["S_MAX"];
    minS = fs["S_MIN"];
    maxV = fs["V_MAX"];
    minV = fs["V_MIN"];
    fs.release();
  }

  // Create a window
  cv::namedWindow("binalized");
  cv::createTrackbar("H max", "binalized", &maxH, 255);
  cv::createTrackbar("H min", "binalized", &minH, 255);
  cv::createTrackbar("S max", "binalized", &maxS, 255);
  cv::createTrackbar("S min", "binalized", &minS, 255);
  cv::createTrackbar("V max", "binalized", &maxV, 255);
  cv::createTrackbar("V min", "binalized", &minV, 255);
  cv::resizeWindow("binalized", 0, 0);

  // Kalman filter
  cv::KalmanFilter kalman(4, 2, 0);

  // Transition matrix (x, y, vx, vy)
  cv::Mat1f A(4, 4);
  A << 1.0, 0.0,  dt, 0.0,
       0.0, 1.0, 0.0,  dt,
       0.0, 0.0, 1.0, 0.0,
       0.0, 0.0, 0.0, 1.0;
  kalman.transitionMatrix = A;

  // Measurement matrix (x, y)
  cv::Mat1f H(2, 4);
  H << 1, 0, 0, 0,
       0, 1, 0, 0;
  kalman.measurementMatrix = H;

  // Process noise covairance (x, y, vx, vy)
  cv::Mat1f Q(4, 4);
  Q << 1e-5,  0.0,  0.0,  0.0,
        0.0, 1e-5,  0.0,  0.0,
        0.0,  0.0, 1e-5,  0.0,
        0.0,  0.0,  0.0, 1e-5;
  kalman.processNoiseCov = Q;

  // Measurement noise covariance (x, y)
  cv::Mat1f R(2, 2);
  R << 1e-1,  0.0,
        0.0, 1e-1;
  kalman.measurementNoiseCov = R;

  // Main loop
  while (1) {
    // Key input
    int key = cv::waitKey(33);
    printf("%c\n", (char)key);

    //press the escape key to exit
    if (key == 0x1b) {
      break;
    }

    //switch between learning and non-learning mode
    if (key == 'l') {
      learnMode = !learnMode;
      if (learnMode) {
        printf("Learning mode is enabled\n");
        printf("The color at the crosshairs is being learned\n");
        printf("Press l again to turn off learning mode\n");
      }
      else {
        printf("Learning mode is disabled\n");
        printf("The color last at the crosshairs will be targeted\n");
        printf("Press l again to learn a different color\n");
      }
    }

    //switch between autonomous and manual flying mode
    if (key == 'm') {
      autonomous = !autonomous;
      if (autonomous) {
        printf("Autonomous flying mode is enabled\n");
	printf("Press m again to switch to manual flying mode\n");
      }
      else {
        printf("Manual flying mode is enabled\n");
        printf("Press m again to switch to automatic flying mode\n");
      }
    }

    // Get an image
    cv::Mat image = ardrone.getImage();

    // HSV image
    cv::Mat hsv;
    cv::cvtColor(image, hsv, cv::COLOR_BGR2HSV_FULL);

    // Binalize
    cv::Mat binalized;
    cv::Scalar lower(minH, minS, minV);
    cv::Scalar upper(maxH, maxS, maxV);
    cv::inRange(hsv, lower, upper, binalized);

    // Show result
    cv::imshow("binalized", binalized);

    // De-noising
    cv::Mat kernel = getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3));
    cv::morphologyEx(binalized, binalized, cv::MORPH_CLOSE, kernel);

    // Detect contours
    vector<vector<cv::Point> > contours;
    cv::findContours(binalized.clone(), contours, cv::RETR_CCOMP, cv::CHAIN_APPROX_SIMPLE);

    // Find largest contour
    int contour_index = -1;
    double max_area = 0.0;
    for (size_t i = 0; i < contours.size(); i++) {
      double area = fabs(cv::contourArea(contours[i]));
      if (area > max_area) {
        contour_index = i;
        max_area = area;
      }
    }

    // Object detected
    if (contour_index >= 0) {
      // Moments
      cv::Moments moments = cv::moments(contours[contour_index], true);
      double marker_y = (int)(moments.m01 / moments.m00);
      double marker_x = (int)(moments.m10 / moments.m00);

      // Measurements
      cv::Mat measurement = (cv::Mat1f(2, 1) << marker_x, marker_y);

      // Correction
      cv::Mat estimated = kalman.correct(measurement);

      // Show result
      cv::Rect rect = cv::boundingRect(contours[contour_index]);
      cv::rectangle(image, rect, cv::Scalar(0, 255, 0));
    }

    // Prediction
    cv::Mat1f prediction = kalman.predict();
    int radius = 1e+3 * kalman.errorCovPre.at<float>(0, 0);

    // Calculate object heading fraction
    float heading = -((image.cols/2) - prediction(0, 0))/(image.cols/2);

    // Sample the object color
    if(learnMode) {
      // Show targeting crosshairs
      cv::line(image, cvPoint(image.cols/2, 0), cvPoint(image.cols/2, image.rows/2 - 2), green); //top vertical crosshair
      cv::line(image, cvPoint(image.cols/2, image.rows/2 + 2), cvPoint(image.cols/2, image.rows), green); //bottom vertical crosshair
      cv::line(image, cvPoint(0, image.rows/2), cvPoint(image.cols/2 - 2, image.rows/2), green); //left horizontal crosshair
      cv::line(image, cvPoint(image.cols/2 + 2, image.rows/2), cvPoint(image.cols, image.rows/2), green); //right horizontal crosshair

      cv::Vec3b hsvSample = hsv.at<cv::Vec3b>(cvPoint(image.cols/2, image.rows/2));

      learnedHue = hsvSample[0];
      learnedSaturation = hsvSample[1];
      learnedValue = hsvSample[2];

      //Auto set Hue, Saturation, and Value tracking bars
      cv::setTrackbarPos("H max", "binalized", learnedHue+20);
      cv::setTrackbarPos("H min", "binalized", learnedHue-20);

      cv::setTrackbarPos("S max", "binalized", learnedSaturation+20);
      cv::setTrackbarPos("S min", "binalized", learnedSaturation-20);

      cv::setTrackbarPos("V max", "binalized", learnedValue+30);
      cv::setTrackbarPos("V min", "binalized", learnedValue-30);
    }

    // Show predicted position
    cv::circle(image, cv::Point(prediction(0, 0), prediction(0, 1)), radius, green, 2);

    //TODO:Switch control between manual and automatic mode

    //Speed
    if ((key >= '0') && (key <= '9'))
    {
      speed = (key-'0')*0.1;
    }

    // Auto-follow
    if (autonomous) {
      //TODO: set speed to 0.0 when close enough to object
      vx = speed;
      vr = -heading;
    }
    //Manual flying mode
    else {
      if (key == 0x260000) { vx =  1.0 * speed; } //up arrow key
      if (key == 0x280000) { vx = -1.0; } //down arrow key
      if (key == 0x250000) { vr =  1.0; } //left arrow key
      if (key == 0x270000) { vr = -1.0; } //right arrow key
      if (key == 'q') { vz =  1.0; } //q key
      if (key == 'a') { vz = -1.0; } //a key
    }

    ardrone.move3D(vx, vy, vz, vr);

    // Take off / Landing to specific height
    if (key == ' ') {
      if (ardrone.onGround()) {
        ardrone.takeoff();
      }
      else {
        ardrone.landing();
      }
    }

    //Display the camera feed
    cv::imshow("camera", image);

    //Write control information over the image
    sprintf(speedDisplay, "speed = %3.2f", speed);
    putText(image, speedDisplay, cvPoint(30,20), cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, green, 1, CV_AA);

    //If running autonomously
    if (autonomous) {
      sprintf(headingDisplay, "heading = %+3.2f", heading);
      sprintf(hsvSampleDisplay, "hsvSample = %3d, %3d, %3d", learnedHue, learnedSaturation, learnedValue);

      putText(image, headingDisplay, cvPoint(30,40), cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, green, 1, CV_AA);
      putText(image, hsvSampleDisplay, cvPoint(30,60), cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, green, 1, CV_AA);
    }
    //If running in manual mode
    else {
      sprintf(vxDisplay, "vx = %3.2f", vx);
      sprintf(vyDisplay, "vy = %3.2f", vy);
      sprintf(vzDisplay, "vz = %3.2f", vz);
      sprintf(vrDisplay, "vr = %3.2f", vr);

      putText(image, vxDisplay, cvPoint(30,40), cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, green, 1, CV_AA);
      putText(image, vyDisplay, cvPoint(30,60), cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, green, 1, CV_AA);
      putText(image, vzDisplay, cvPoint(30,80), cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, green, 1, CV_AA);
      putText(image, vrDisplay, cvPoint(30,100), cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, green, 1, CV_AA);
    }
  }

  //Save thresholds
  fs.open(filename, cv::FileStorage::WRITE);
  if (fs.isOpened()) {
    cv::write(fs, "H_MAX", maxH);
    cv::write(fs, "H_MIN", minH);
    cv::write(fs, "S_MAX", maxS);
    cv::write(fs, "S_MIN", minS);
    cv::write(fs, "V_MAX", maxV);
    cv::write(fs, "V_MIN", minV);
    fs.release();
  }

  //Close connection to drone
  ardrone.close();

  return 0;
}
<commit_msg>Update control scheme for three modes.<commit_after>/*
  Original code from tekkies/CVdrone (get actual address from github)

  Modified by Elliot Greenlee, Caleb Mennen, Jacob Pollack
  COSC 402 Senior Design

  Min Kao Drone Tour
  See readme at (get actual address from github)
*/

#include "ardrone/ardrone.h"
#include <iostream>
#include <vector>

using namespace std;

int main(int argc, char *argv[])
{
  //AR.Drone class
  ARDrone ardrone;

  //Display variables
  char vxDisplay[80]; //print buffer for x (forward/reverse) velocity
  char vyDisplay[80]; //print buffer for y (sideways) velocity
  char vzDisplay[80]; //print buffer for z (up/down) velocity
  char vrDisplay[80]; //print buffer for r rotational velocity

  char speedDisplay[80]; //print buffer for speed
  char headingDisplay[80]; //print buffer for heading
  char hsvSampleDisplay[80]; //print buffer for learning HSV values

  cv::Scalar green = CV_RGB(0,255,0); //putText color value

  //Drone control
  bool autonomous = false;
  bool learnMode = false;

  float speed = 0.0;
  double vx = 0.0;
  double vy = 0.0;
  double vz = 0.0;
  double vr = 0.0;

  //Object Following Image recognition values
  int learnedHue;
  int learnedSaturation;
  int learnedValue;
  int minH = 0, maxH = 255;
  int minS = 0, maxS = 255;
  int minV = 0, maxV = 255;

  //Sampling time [s]
  const double dt = 1.0;

  enum FlyingMode {
    Manual,
    ObjectFollowing,
    LineFollowing
  };

  FlyingMode flyingMode = Manual;

  //Initializing Message
  printf("Connecting to the drone\n");
  printf("If there is no version number response in the next 10 seconds, please restart the drone and code.\n");
  printf("To disconnect, press the ESC key\n");
  fflush(stdout);

  // Initialize
  if (!ardrone.open()) {
    printf("Failed to initialize.\n");
    return -1;
  }

  // XML save data for object following color thresholds
  std::string filename("thresholds.xml");
  cv::FileStorage fs(filename, cv::FileStorage::READ);

  // If there is a save file then read it
  if (fs.isOpened()) {
    maxH = fs["H_MAX"];
    minH = fs["H_MIN"];
    maxS = fs["S_MAX"];
    minS = fs["S_MIN"];
    maxV = fs["V_MAX"];
    minV = fs["V_MIN"];
    fs.release();
  }

  // Create a window
  cv::namedWindow("binalized");
  cv::createTrackbar("Hue max", "binalized", &maxH, 255);
  cv::createTrackbar("Hue min", "binalized", &minH, 255);
  cv::createTrackbar("Saturation max", "binalized", &maxS, 255);
  cv::createTrackbar("Saturation min", "binalized", &minS, 255);
  cv::createTrackbar("Value max", "binalized", &maxV, 255);
  cv::createTrackbar("Value min", "binalized", &minV, 255);
  cv::resizeWindow("binalized", 0, 0);

  // Kalman filter
  cv::KalmanFilter kalman(4, 2, 0);

  // Transition matrix (x, y, vx, vy)
  cv::Mat1f A(4, 4);
  A << 1.0, 0.0,  dt, 0.0,
       0.0, 1.0, 0.0,  dt,
       0.0, 0.0, 1.0, 0.0,
       0.0, 0.0, 0.0, 1.0;
  kalman.transitionMatrix = A;

  // Measurement matrix (x, y)
  cv::Mat1f H(2, 4);
  H << 1, 0, 0, 0,
       0, 1, 0, 0;
  kalman.measurementMatrix = H;

  // Process noise covairance (x, y, vx, vy)
  cv::Mat1f Q(4, 4);
  Q << 1e-5,  0.0,  0.0,  0.0,
        0.0, 1e-5,  0.0,  0.0,
        0.0,  0.0, 1e-5,  0.0,
        0.0,  0.0,  0.0, 1e-5;
  kalman.processNoiseCov = Q;

  // Measurement noise covariance (x, y)
  cv::Mat1f R(2, 2);
  R << 1e-1,  0.0,
        0.0, 1e-1;
  kalman.measurementNoiseCov = R;

  // Main loop
  while (1) {
    // Key input
    int key = cv::waitKey(33);
    printf("%c\n", (char)key);

    //press the escape key to exit
    if (key == 0x1b) {
      break;
    }

    //switch between learning and non-learning mode
    if (key == 'l') {
      learnMode = !learnMode;
      if (learnMode) {
        printf("Learning mode is enabled\n");
        printf("The color at the crosshairs is being learned\n");
        printf("Press l again to turn off learning mode\n");
      }
      else {
        printf("Learning mode is disabled\n");
        printf("The color last at the crosshairs will be targeted\n");
        printf("Press l again to learn a different color\n");
      }
    }

    //switch between flying modes
    if (key == 'b') {
      flyingMode = Manual;
      printf("Manual flying mode is enabled\n");
      printf("Press n for object following and m for line following\n");
    }
    else if (key == 'n') {
      flyingMode = ObjectFollowing;
      printf("Object Following flying mode is enabled\n");
      printf("Press b for manual and m for line following\n");
    }
    else if (key == 'm') {
      flyingMode = LineFollowing;
      printf("Line Following flying mode is enabled\n");
      printf("Press b for manual and n for object following\n");
    }

    // Get an image
    cv::Mat image = ardrone.getImage();

    // HSV image
    cv::Mat hsv;
    cv::cvtColor(image, hsv, cv::COLOR_BGR2HSV_FULL);

    // Binalize
    cv::Mat binalized;
    cv::Scalar lower(minH, minS, minV);
    cv::Scalar upper(maxH, maxS, maxV);
    cv::inRange(hsv, lower, upper, binalized);

    // Show result
    cv::imshow("binalized", binalized);

    // De-noising
    cv::Mat kernel = getStructuringElement(cv::MORPH_RECT, cv::Size(3, 3));
    cv::morphologyEx(binalized, binalized, cv::MORPH_CLOSE, kernel);

    // Detect contours
    vector<vector<cv::Point> > contours;
    cv::findContours(binalized.clone(), contours, cv::RETR_CCOMP, cv::CHAIN_APPROX_SIMPLE);

    // Find largest contour
    int contour_index = -1;
    double max_area = 0.0;
    for (size_t i = 0; i < contours.size(); i++) {
      double area = fabs(cv::contourArea(contours[i]));
      if (area > max_area) {
        contour_index = i;
        max_area = area;
      }
    }

    // Object detected
    if (contour_index >= 0) {
      // Moments
      cv::Moments moments = cv::moments(contours[contour_index], true);
      double marker_y = (int)(moments.m01 / moments.m00);
      double marker_x = (int)(moments.m10 / moments.m00);

      // Measurements
      cv::Mat measurement = (cv::Mat1f(2, 1) << marker_x, marker_y);

      // Correction
      cv::Mat estimated = kalman.correct(measurement);

      // Show result
      cv::Rect rect = cv::boundingRect(contours[contour_index]);
      cv::rectangle(image, rect, cv::Scalar(0, 255, 0));
    }

    // Prediction
    cv::Mat1f prediction = kalman.predict();
    int radius = 1e+3 * kalman.errorCovPre.at<float>(0, 0);

    // Calculate object heading fraction
    float heading = -((image.cols/2) - prediction(0, 0))/(image.cols/2);

    // Sample the object color
    if(learnMode) {
      // Show targeting crosshairs
      cv::line(image, cvPoint(image.cols/2, 0), cvPoint(image.cols/2, image.rows/2 - 2), green); //top vertical crosshair
      cv::line(image, cvPoint(image.cols/2, image.rows/2 + 2), cvPoint(image.cols/2, image.rows), green); //bottom vertical crosshair
      cv::line(image, cvPoint(0, image.rows/2), cvPoint(image.cols/2 - 2, image.rows/2), green); //left horizontal crosshair
      cv::line(image, cvPoint(image.cols/2 + 2, image.rows/2), cvPoint(image.cols, image.rows/2), green); //right horizontal crosshair

      cv::Vec3b hsvSample = hsv.at<cv::Vec3b>(cvPoint(image.cols/2, image.rows/2));

      learnedHue = hsvSample[0];
      learnedSaturation = hsvSample[1];
      learnedValue = hsvSample[2];

      //Auto set Hue, Saturation, and Value tracking bars
      cv::setTrackbarPos("H max", "binalized", learnedHue+20);
      cv::setTrackbarPos("H min", "binalized", learnedHue-20);

      cv::setTrackbarPos("S max", "binalized", learnedSaturation+20);
      cv::setTrackbarPos("S min", "binalized", learnedSaturation-20);

      cv::setTrackbarPos("V max", "binalized", learnedValue+30);
      cv::setTrackbarPos("V min", "binalized", learnedValue-30);
    }

    // Show predicted position
    cv::circle(image, cv::Point(prediction(0, 0), prediction(0, 1)), radius, green, 2);

    //TODO:Switch control between control modes
    switch (flyingMode) {
      case Manual:
        printf("Manual Mode\n");
      case ObjectFollowing:
        printf("Object Following Mode\n");
      case LineFollowing:
        printf("Line Following Mode\n");
    }

    //Speed
    if ((key >= '0') && (key <= '9'))
    {
      speed = (key-'0')*0.1;
    }

    // Auto-follow
    if (autonomous) {
      //TODO: set speed to 0.0 when close enough to object
      vx = speed;
      vr = -heading;
    }
    //Manual flying mode
    else {
      if (key == 0x260000) { vx =  1.0 * speed; } //up arrow key
      if (key == 0x280000) { vx = -1.0; } //down arrow key
      if (key == 0x250000) { vr =  1.0; } //left arrow key
      if (key == 0x270000) { vr = -1.0; } //right arrow key
      if (key == 'q') { vz =  1.0; } //q key
      if (key == 'a') { vz = -1.0; } //a key
    }

    ardrone.move3D(vx, vy, vz, vr);

    // Take off / Landing to specific height
    if (key == ' ') {
      if (ardrone.onGround()) {
        ardrone.takeoff();
      }
      else {
        ardrone.landing();
      }
    }

    //Display the camera feed
    cv::imshow("camera", image);

    //Write control information over the image
    sprintf(speedDisplay, "speed = %3.2f", speed);
    putText(image, speedDisplay, cvPoint(30,20), cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, green, 1, CV_AA);

    //If running autonomously
    if (autonomous) {
      sprintf(headingDisplay, "heading = %+3.2f", heading);
      sprintf(hsvSampleDisplay, "hsvSample = %3d, %3d, %3d", learnedHue, learnedSaturation, learnedValue);

      putText(image, headingDisplay, cvPoint(30,40), cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, green, 1, CV_AA);
      putText(image, hsvSampleDisplay, cvPoint(30,60), cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, green, 1, CV_AA);
    }
    //If running in manual mode
    else {
      sprintf(vxDisplay, "vx = %3.2f", vx);
      sprintf(vyDisplay, "vy = %3.2f", vy);
      sprintf(vzDisplay, "vz = %3.2f", vz);
      sprintf(vrDisplay, "vr = %3.2f", vr);

      putText(image, vxDisplay, cvPoint(30,40), cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, green, 1, CV_AA);
      putText(image, vyDisplay, cvPoint(30,60), cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, green, 1, CV_AA);
      putText(image, vzDisplay, cvPoint(30,80), cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, green, 1, CV_AA);
      putText(image, vrDisplay, cvPoint(30,100), cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, green, 1, CV_AA);
    }
  }

  //Save thresholds
  fs.open(filename, cv::FileStorage::WRITE);
  if (fs.isOpened()) {
    cv::write(fs, "H_MAX", maxH);
    cv::write(fs, "H_MIN", minH);
    cv::write(fs, "S_MAX", maxS);
    cv::write(fs, "S_MIN", minS);
    cv::write(fs, "V_MAX", maxV);
    cv::write(fs, "V_MIN", minV);
    fs.release();
  }

  //Close connection to drone
  ardrone.close();

  return 0;
}
<|endoftext|>


<commit_before>/*
 * File:   main.cpp
 * Author: vlad
 *
 * Created on December 12, 2013, 10:38 PM
 */

#include "qpp.h"
#include "qudit.h"
//#include "matlab.h" // support for MATLAB

// TODO: use .data() raw pointer instead of looping
// TODO: optimize syspermute: ?Eigen::Map?
// TODO: IMPORTANT Rewrite partial trace without syspermute
// TODO: further parallelization
// TODO: testing

using namespace std;
using namespace qpp;
using namespace qpp::types;

int main()
{
	cout << "Starting qpp..." << endl;

	// output format
	// cout << std::scientific;
	cout << std::fixed; // use fixed format for nice formatting
	cout << std::setprecision(4); // only for fixed or scientific modes

	ket zd0(3);
	zd0<<1,0,0;
	Qudit q(proj(zd0));
	cmat U = randU(3);

	// von Neumann projective measurement
	cout << q.measure(gt.Zd(3), true) << endl << endl;

	cout << q.measure() << endl;
	cout << q.measure(true) << endl;
	cout << endl;
	cout << q.measure(gt.Xd(3)) << endl;
	cout << q.measure(gt.Xd(3)) << endl;
	cout << q.measure(gt.Xd(3)) << endl;
	cout << q.measure(gt.Xd(3),true) << endl;
	cout << q.measure(gt.Xd(3)) << endl;
	cout << q.measure(gt.Xd(3)) << endl;

	ket a(3);

	size_t D = 4;
	cmat b = cmat::Identity(D, D);

	/*
	 // TESTING

	 // Bell state generator
	 cout << endl << "Bell state generator: " << endl;
	 cmat circuit;
	 circuit = gt.CTRL(gt.X, { 0 }, { 1 }, 2) * expandout(gt.H, 0, { 2, 2 });
	 cmat z0(2, 1);
	 z0 << 1, 0;
	 cmat z1(2, 1);
	 z1 << 0, 1;
	 cmat input = kron(z0, z0);
	 cmat output = circuit * input;
	 cout << "Circuit matrix representation: " << endl;
	 displn(circuit);
	 cout << endl << "Output (|Bell_0> state) of the circuit on |00>: " << endl;
	 displn(output);

	 // 3-qubit repetion code
	 cout << endl << "3-qubit repetition code: " << endl;
	 cmat rep;
	 rep = gt.CTRL(gt.X, { 0 }, { 2 }, 3) * gt.CTRL(gt.X, { 0 }, { 1 }, 3);
	 input = kronlist<cmat>( { z1, z0, z0 });
	 output = rep * input;
	 cout << "Circuit acting on |000> produces |111>. Check: " << endl;
	 displn(output);

	 // Gram-Schmidt
	 cout << endl << "Gram-Schmidt on matrix:" << endl;
	 cmat A(3, 3);
	 A << 1, 1, 0, 0, 2, 0, 0, 0, 0;
	 displn(A);
	 cmat Ags = grams(A);
	 cout << endl << "Result:" << endl;
	 displn(Ags);
	 cout << endl << "Projector is:" << endl;
	 displn(Ags * adjoint(Ags));

	 // spectral decomposition test
	 cout << endl << "Spectral decomposition tests." << endl;
	 size_t D = 4;
	 cmat rH = randH(D);
	 cmat evalsH = hevals(rH);
	 cmat evectsH = hevects(rH);
	 cmat spec = cmat::Zero(D, D);
	 for (size_t i = 0; i < D; i++)
	 spec += evalsH(i) * proj(evectsH.col(i));
	 cout << "Original matrix: " << endl;
	 displn(rH);
	 cout << endl << "Reconstructed from spectral decomposition: " << endl;
	 displn(spec);
	 cout << "Difference in norm: " << norm(spec - rH) << endl;

	 // channel tests
	 cout << endl << "Channel tests." << endl;
	 size_t nk = 10, d = 2; // nk Kraus on d-dimensional system
	 cout << "Generating a random channel with " << nk
	 << " Kraus operators on a " << d << " dimensional space..." << endl;
	 std::vector<cmat> Ks = randKraus(nk, d);

	 cmat rho_in = randrho(d); // input state
	 cmat rho_out = channel(rho_in, Ks); // output state

	 cout << "Computing its Choi matrix..." << endl;
	 cmat choim = choi(Ks);
	 cout << "Choi matrix:" << endl;
	 displn(choim);
	 cout << endl << "The eigenvalues of the Choi matrix are: " << endl;
	 displn(transpose(hevals(choim)));
	 cout << endl << "Their sum is: " << sum(hevals(choim)).real() << endl;
	 std::vector<cmat> Kperps = choi2kraus(choim);
	 cout << endl << "The Kraus rank of the channel is: " << Kperps.size()
	 << endl;
	 cmat rho_out1 = channel(rho_in, Kperps);
	 cout << endl << "Difference in norm on output states: "
	 << norm(rho_out1 - rho_out) << endl;
	 cout << endl << "Superoperator matrix:" << endl;
	 cmat smat = super(Ks);
	 displn(smat);
	 cout << endl << "The eigenvalues of the superoperator matrix are: " << endl;
	 cmat evalsupop = evals(smat);
	 displn(transpose(evalsupop));
	 cout << endl << "Their absolute values are: " << endl;
	 for (size_t i = 0; i < (size_t) evalsupop.size(); i++)
	 cout << std::abs((cplx) evalsupop(i)) << " ";
	 cout << endl << endl << "Diference in norm for superoperator action: ";
	 cmat rho_out2 = transpose(
	 reshape(smat * reshape(transpose(rho_in), d * d, 1), d, d));
	 cout << norm(rho_out - rho_out2) << endl;

	 // statistics tests
	 cout << endl << "Statistics tests." << endl;
	 std::vector<cplx> ampl = { 1. + ct::ii, 1. - ct::ii };
	 cmat va(1, 4);
	 va << 0.1, 1, 1. + ct::ii, 1. + 2. * ct::ii;
	 stat::DiscreteDistributionFromComplex dc(va);
	 cout << "The probabilities are: ";
	 displnSTL(dc.probabilities(), ", ", "{", "}");

	 // other tests
	 cout << endl << "Timing tests..." << endl;
	 size_t n = 12; // number of qubits
	 size_t N = std::pow(2, n);
	 vector<size_t> dims; // local dimensions
	 for (size_t i = 0; i < n; i++)
	 dims.push_back(2);
	 cout << "n = " << n << " qubits, matrix size " << N << " x " << N << "."
	 << endl;

	 // TIMING
	 Timer t, total; // start the timer, automatic tic() in the constructor

	 // matrix initialization
	 cout << endl << "Matrix initialization timing." << endl;
	 cmat randcmat = cmat::Random(N, N);
	 t.toc(); // read the time
	 cout << "Took " << t << " seconds." << endl;

	 // lazy matrix product
	 cout << endl << "Lazy matrix product timing." << endl;
	 t.tic();
	 auto lazyprod = randcmat * randcmat; // lazyprod has type GenMatProduct
	 t.toc(); // read the time
	 cout << "Took " << t << " seconds." << endl;

	 // matrix product
	 cout << endl << "Matrix product timing." << endl;
	 t.tic(); // reset the chronometer
	 cmat prodmat = randcmat * randcmat; // explicit cmat now
	 t.toc(); // read the time
	 cout << "Took " << t << " seconds." << endl;

	 // ptrace2 timing
	 cout << endl << "ptrace2 timing." << endl;
	 t.tic(); // reset the chronometer
	 // trace away half of the qubits
	 ptrace2(randcmat, { (size_t) std::sqrt(N), (size_t) std::sqrt(N) });
	 t.toc(); // read the time
	 cout << "Took " << t << " seconds." << endl;

	 // ptrace SLOW SLOW SLOW (because of syspermute, do it without)
	 cout << endl << "ptrace timing." << endl;
	 vector<size_t> subsys_ptrace = { 0 }; // trace away the first qubit
	 cout << "Subsytem(s): ";
	 displnSTL(subsys_ptrace, ", ");
	 t.tic();
	 ptrace(randcmat, subsys_ptrace, dims);
	 t.toc();
	 cout << "Took " << t << " seconds." << endl;

	 // ptranspose
	 cout << endl << "ptranspose timing." << endl;
	 vector<size_t> subsys_ptranspose; // partially transpose all subsystems
	 for (size_t i = 0; i < n; i++)
	 subsys_ptranspose.push_back(i);
	 cout << "Subsytem(s): ";
	 displnSTL(subsys_ptranspose, ", ");
	 t.tic();
	 ptranspose(randcmat, subsys_ptranspose, dims);
	 t.toc();
	 cout << "Took " << t << " seconds." << endl;

	 // syspermute SLOW SLOW SLOW
	 cout << endl << "syspermute timing." << endl;
	 vector<size_t> perm; // left-shift all subsystems by 1
	 for (size_t i = 0; i < n; i++)
	 perm.push_back((i + 1) % n);
	 cout << "Subsytem(s): ";
	 displnSTL(perm, ", ");
	 t.tic();
	 syspermute(randcmat, perm, dims);
	 t.toc();
	 cout << "Took " << t << " seconds." << endl;

	 // END TIMING
	 total.toc(); // read the total running time
	 cout << endl << "Total time: " << total.seconds() << " seconds.";
	 */

	cout << endl << "Exiting qpp..." << endl;
}
<commit_msg>commit<commit_after>/*
 * File:   main.cpp
 * Author: vlad
 *
 * Created on December 12, 2013, 10:38 PM
 */

#include "qpp.h"
#include "qudit.h"

#include <functional>
//#include "matlab.h" // support for MATLAB

// TODO: use .data() raw pointer instead of looping
// TODO: optimize syspermute: ?Eigen::Map?
// TODO: IMPORTANT Rewrite partial trace without syspermute
// TODO: further parallelization
// TODO: testing

using namespace std;
using namespace qpp;
using namespace qpp::types;

int main()
{
	cout << "Starting qpp..." << endl;

	// output format
	// cout << std::scientific;
	cout << std::fixed; // use fixed format for nice formatting
	cout << std::setprecision(4); // only for fixed or scientific modes

	ket zd0(3);
	zd0 << 1, 0, 0;
	Qudit q(proj(zd0));
	cmat U = randU(3);

	// von Neumann projective measurement
	cout << q.measure(gt.Zd(3), true) << endl << endl;

	cout << q.measure() << endl;
	cout << q.measure(true) << endl;
	cout << endl;
	cout << q.measure(gt.Xd(3)) << endl;
	cout << q.measure(gt.Xd(3)) << endl;
	cout << q.measure(gt.Xd(3)) << endl;
	cout << q.measure(gt.Xd(3), true) << endl;
	cout << q.measure(gt.Xd(3)) << endl;
	cout << q.measure(gt.Xd(3)) << endl;

	ket a(3);

	size_t D = 4;
	cmat b = cmat::Identity(D, D);

	/*
	 // TESTING

	 // Bell state generator
	 cout << endl << "Bell state generator: " << endl;
	 cmat circuit;
	 circuit = gt.CTRL(gt.X, { 0 }, { 1 }, 2) * expandout(gt.H, 0, { 2, 2 });
	 cmat z0(2, 1);
	 z0 << 1, 0;
	 cmat z1(2, 1);
	 z1 << 0, 1;
	 cmat input = kron(z0, z0);
	 cmat output = circuit * input;
	 cout << "Circuit matrix representation: " << endl;
	 displn(circuit);
	 cout << endl << "Output (|Bell_0> state) of the circuit on |00>: " << endl;
	 displn(output);

	 // 3-qubit repetion code
	 cout << endl << "3-qubit repetition code: " << endl;
	 cmat rep;
	 rep = gt.CTRL(gt.X, { 0 }, { 2 }, 3) * gt.CTRL(gt.X, { 0 }, { 1 }, 3);
	 input = kronlist<cmat>( { z1, z0, z0 });
	 output = rep * input;
	 cout << "Circuit acting on |000> produces |111>. Check: " << endl;
	 displn(output);

	 // Gram-Schmidt
	 cout << endl << "Gram-Schmidt on matrix:" << endl;
	 cmat A(3, 3);
	 A << 1, 1, 0, 0, 2, 0, 0, 0, 0;
	 displn(A);
	 cmat Ags = grams(A);
	 cout << endl << "Result:" << endl;
	 displn(Ags);
	 cout << endl << "Projector is:" << endl;
	 displn(Ags * adjoint(Ags));

	 // spectral decomposition test
	 cout << endl << "Spectral decomposition tests." << endl;
	 size_t D = 4;
	 cmat rH = randH(D);
	 cmat evalsH = hevals(rH);
	 cmat evectsH = hevects(rH);
	 cmat spec = cmat::Zero(D, D);
	 for (size_t i = 0; i < D; i++)
	 spec += evalsH(i) * proj(evectsH.col(i));
	 cout << "Original matrix: " << endl;
	 displn(rH);
	 cout << endl << "Reconstructed from spectral decomposition: " << endl;
	 displn(spec);
	 cout << "Difference in norm: " << norm(spec - rH) << endl;

	 // channel tests
	 cout << endl << "Channel tests." << endl;
	 size_t nk = 10, d = 2; // nk Kraus on d-dimensional system
	 cout << "Generating a random channel with " << nk
	 << " Kraus operators on a " << d << " dimensional space..." << endl;
	 std::vector<cmat> Ks = randKraus(nk, d);

	 cmat rho_in = randrho(d); // input state
	 cmat rho_out = channel(rho_in, Ks); // output state

	 cout << "Computing its Choi matrix..." << endl;
	 cmat choim = choi(Ks);
	 cout << "Choi matrix:" << endl;
	 displn(choim);
	 cout << endl << "The eigenvalues of the Choi matrix are: " << endl;
	 displn(transpose(hevals(choim)));
	 cout << endl << "Their sum is: " << sum(hevals(choim)).real() << endl;
	 std::vector<cmat> Kperps = choi2kraus(choim);
	 cout << endl << "The Kraus rank of the channel is: " << Kperps.size()
	 << endl;
	 cmat rho_out1 = channel(rho_in, Kperps);
	 cout << endl << "Difference in norm on output states: "
	 << norm(rho_out1 - rho_out) << endl;
	 cout << endl << "Superoperator matrix:" << endl;
	 cmat smat = super(Ks);
	 displn(smat);
	 cout << endl << "The eigenvalues of the superoperator matrix are: " << endl;
	 cmat evalsupop = evals(smat);
	 displn(transpose(evalsupop));
	 cout << endl << "Their absolute values are: " << endl;
	 for (size_t i = 0; i < (size_t) evalsupop.size(); i++)
	 cout << std::abs((cplx) evalsupop(i)) << " ";
	 cout << endl << endl << "Diference in norm for superoperator action: ";
	 cmat rho_out2 = transpose(
	 reshape(smat * reshape(transpose(rho_in), d * d, 1), d, d));
	 cout << norm(rho_out - rho_out2) << endl;

	 // statistics tests
	 cout << endl << "Statistics tests." << endl;
	 std::vector<cplx> ampl = { 1. + ct::ii, 1. - ct::ii };
	 cmat va(1, 4);
	 va << 0.1, 1, 1. + ct::ii, 1. + 2. * ct::ii;
	 stat::DiscreteDistributionFromComplex dc(va);
	 cout << "The probabilities are: ";
	 displnSTL(dc.probabilities(), ", ", "{", "}");

	 // other tests
	 cout << endl << "Timing tests..." << endl;
	 size_t n = 12; // number of qubits
	 size_t N = std::pow(2, n);
	 vector<size_t> dims; // local dimensions
	 for (size_t i = 0; i < n; i++)
	 dims.push_back(2);
	 cout << "n = " << n << " qubits, matrix size " << N << " x " << N << "."
	 << endl;

	 // TIMING
	 Timer t, total; // start the timer, automatic tic() in the constructor

	 // matrix initialization
	 cout << endl << "Matrix initialization timing." << endl;
	 cmat randcmat = cmat::Random(N, N);
	 t.toc(); // read the time
	 cout << "Took " << t << " seconds." << endl;

	 // lazy matrix product
	 cout << endl << "Lazy matrix product timing." << endl;
	 t.tic();
	 auto lazyprod = randcmat * randcmat; // lazyprod has type GenMatProduct
	 t.toc(); // read the time
	 cout << "Took " << t << " seconds." << endl;

	 // matrix product
	 cout << endl << "Matrix product timing." << endl;
	 t.tic(); // reset the chronometer
	 cmat prodmat = randcmat * randcmat; // explicit cmat now
	 t.toc(); // read the time
	 cout << "Took " << t << " seconds." << endl;

	 // ptrace2 timing
	 cout << endl << "ptrace2 timing." << endl;
	 t.tic(); // reset the chronometer
	 // trace away half of the qubits
	 ptrace2(randcmat, { (size_t) std::sqrt(N), (size_t) std::sqrt(N) });
	 t.toc(); // read the time
	 cout << "Took " << t << " seconds." << endl;

	 // ptrace SLOW SLOW SLOW (because of syspermute, do it without)
	 cout << endl << "ptrace timing." << endl;
	 vector<size_t> subsys_ptrace = { 0 }; // trace away the first qubit
	 cout << "Subsytem(s): ";
	 displnSTL(subsys_ptrace, ", ");
	 t.tic();
	 ptrace(randcmat, subsys_ptrace, dims);
	 t.toc();
	 cout << "Took " << t << " seconds." << endl;

	 // ptranspose
	 cout << endl << "ptranspose timing." << endl;
	 vector<size_t> subsys_ptranspose; // partially transpose all subsystems
	 for (size_t i = 0; i < n; i++)
	 subsys_ptranspose.push_back(i);
	 cout << "Subsytem(s): ";
	 displnSTL(subsys_ptranspose, ", ");
	 t.tic();
	 ptranspose(randcmat, subsys_ptranspose, dims);
	 t.toc();
	 cout << "Took " << t << " seconds." << endl;

	 // syspermute SLOW SLOW SLOW
	 cout << endl << "syspermute timing." << endl;
	 vector<size_t> perm; // left-shift all subsystems by 1
	 for (size_t i = 0; i < n; i++)
	 perm.push_back((i + 1) % n);
	 cout << "Subsytem(s): ";
	 displnSTL(perm, ", ");
	 t.tic();
	 syspermute(randcmat, perm, dims);
	 t.toc();
	 cout << "Took " << t << " seconds." << endl;

	 // END TIMING
	 total.toc(); // read the total running time
	 cout << endl << "Total time: " << total.seconds() << " seconds.";
	 */

	cout << endl << "Exiting qpp..." << endl;
}
<|endoftext|>


<commit_before>
//
// ortho view generation tool
// ulrich.krispel@fraunhofer.at
//

#include <vector>
#include <string>
#include <algorithm>
#include <cmath>
#include <iomanip>
#include <limits>
#include <chrono>

#include <cstdlib>

#include <boost/program_options.hpp>

#include "rapidjson/document.h"
#include "rapidjson/filereadstream.h"   // FileReadStream
#include "rapidjson/encodedstream.h"    // AutoUTFInputStream

#include "types.h"
#include "projection.h"
#include "meanshift.h"
#include "extraction.h"

namespace po = boost::program_options;

// configuration variables
double WINDOW_SIZE_NORMAL = 0.3;
double WINDOW_SIZE_DISTANCE = 0.1;

typedef struct json_token JSONToken;
typedef struct json_token *JSONTokens;

using namespace OrthoGen;

int main(int ac, char *av[]) {

  // main application state
  std::vector<SphericalPanoramaImageProjection> scans;
  std::vector<Quad3Dd> walls;

  double resolution = 0.001;                // default: 1 mm/pixel
  double walljson_scalefactor = 0.001;      // wall json is in mm
  bool exportOBJ = false;
  //bool exportSphere = false;
  std::cout << "OrthoGen orthographic image generator for DuraArk" << std::endl;
  std::cout << "developed by Fraunhofer Austria Research GmbH" << std::endl;
  std::string output = "ortho";
  std::string panopath = ".\\";
  std::string aligncmd = "panoalign.exe";

  try {
    po::options_description desc("commandline options");
    desc.add_options()
        ("help", "show this help message")
        ("walljson", po::value<std::string>(), "input wall json [.json]")
        ("e57metadata", po::value<std::string>(), "e57 metadata json [.json]")
        ("panopath", po::value<std::string>(), "path to pano images")
        ("align", po::value<std::string>(), "align executable")
        ("output", po::value< std::string >(), "output filename [.jpg] will be appended")
        ("exgeom", "export textured geometry as .obj")
        ;

    po::variables_map vm;
    po::store(po::parse_command_line(ac, av, desc,
                                     po::command_line_style::unix_style ^
                                     po::command_line_style::allow_short),
              vm);
    po::notify(vm);

    if (vm.count("help")) //|| (!vm.count("walljson")) || (!vm.count("e57metadata")))
    {
      std::cout << desc << "\n";
      return 0;
    }

    if (vm.count("panopath"))
    {
        panopath = vm["panopath"].as<std::string>();
        std::string last = panopath.substr(panopath.length() - 1, 1);
        if (!(last.compare("\\") == 0 || last.compare("/") == 0))
        {
            panopath.append("/");
        }
    }

    if (vm.count("align")) { aligncmd = vm["align"].as<std::string>(); }
    if (vm.count("output")) { aligncmd = vm["output"].as<std::string>(); }
    exportOBJ = vm.count("exgeom") != 0;

    // parse jsons
    // ================================================= E57 Metadata
    // - parse scans (pose, name, panorama)

    {
        std::string e57mfn = vm["e57metadata"].as<std::string>();
        FILE* fp = fopen(e57mfn.c_str(), "rb"); // non-Windows use "r"
        char readBuffer[256];
        rapidjson::FileReadStream bis(fp, readBuffer, sizeof(readBuffer));
        rapidjson::AutoUTFInputStream<unsigned, rapidjson::FileReadStream> eis(bis);  // wraps bis into eis
        rapidjson::Document e57;         // Document is GenericDocument<UTF8<> >
        if (e57.ParseStream<0, rapidjson::AutoUTF<unsigned> >(eis).HasParseError())
        {
            std::cout << "[ERROR] parsing input JSON file "
                << e57mfn << std::endl;
            return -1;
        }
        fclose(fp);

        const rapidjson::Value& e57m = e57["e57m"];
        const rapidjson::Value& arr = e57["e57m"]["e57scan"];

        if (!e57m.IsObject() || !arr.IsArray()) {
            std::cout << "[ERROR] parsing input JSON file "
                << e57mfn << std::endl;
            return -1;
        }

      {
          for (rapidjson::SizeType i = 0, ie = arr.Size(); i < ie; ++i)
          {
              const rapidjson::Value& item = arr[i];
              SphericalPanoramaImageProjection new_scan;

              new_scan.basename = item["name"].GetString();
              std::cout << "found scan " << new_scan.basename << std::endl;
              // translation
              {
                  double x = item["pose"]["translation"]["x"].GetDouble();
                  double y = item["pose"]["translation"]["y"].GetDouble();
                  double z = item["pose"]["translation"]["z"].GetDouble();
                  new_scan.setPosition(Vec3d(x, y, z));
              }
              // orientation
            {
                double w = item["pose"]["rotation"]["w"].GetDouble();
                double x = item["pose"]["rotation"]["x"].GetDouble();
                double y = item["pose"]["rotation"]["y"].GetDouble();
                double z = item["pose"]["rotation"]["z"].GetDouble();
                Quaterniond quaternion(w, x, y, z);
                std::cout << " applying quaternion [" << quaternion.x() << ","
                    << quaternion.y() << "," << quaternion.z() << "," << quaternion.w()
                    << "]" << std::endl;                new_scan.applyRotation(quaternion);
            }
            std::cout << "pose: ";
            new_scan.printPose();

            // bounds
            {
                new_scan.elevationRange[0] = item["sphericalbounds"]["elevation_minimum"].GetDouble();
                new_scan.elevationRange[1] = item["sphericalbounds"]["elevation_maximum"].GetDouble();
                new_scan.azimuthRange[0] = item["sphericalbounds"]["azimuth_minimum"].GetDouble();
                new_scan.azimuthRange[1] = item["sphericalbounds"]["azimuth_maximum"].GetDouble();
                std::cout << "bounds: elevation [" << new_scan.elevationRange[0] << "<>" << new_scan.elevationRange[1]
                          << "] azimuth [" << new_scan.azimuthRange[0] << "<>" << new_scan.azimuthRange[1] << "]" << std::endl;
            }

            // load panoramic image (align if not found)
            {
                std::ostringstream ss;
                ss << panopath << new_scan.basename << "_aligned.jpg";
                Image img;
                if (loadJPEG(ss.str().c_str(), img)) {
                    new_scan.setPanoramicImage(img);
                }
                else {
                    std::cout << "aligned image not found, aligning..." << std::endl;
                    std::ostringstream cmd;
                    cmd.precision(std::numeric_limits<double>::max_digits10);
                    cmd << aligncmd << " ";
                    cmd << "--imsrc " << panopath << new_scan.basename << "_Faro.jpg ";
                    cmd << "--imdst " << panopath << new_scan.basename << "_Manual.jpg ";
                    cmd << "--outname " << panopath << new_scan.basename << "_aligned.jpg ";
                    cmd << "--selrange " << new_scan.elevationRange[0] << " " << new_scan.elevationRange[1] << " ";
                    std::system(cmd.str().c_str());
                }
            }

            scans.push_back(new_scan);
            std::cout << "-------------------------------------" << std::endl;
          }
      }
    }

    // ---------------------------------------------- WALL JSON
    {
        std::string wallfn = vm["walljson"].as<std::string>();
        FILE* fp = fopen(wallfn.c_str(), "rb"); // non-Windows use "r"
        char readBuffer[256];
        rapidjson::FileReadStream bis(fp, readBuffer, sizeof(readBuffer));
        rapidjson::AutoUTFInputStream<unsigned, rapidjson::FileReadStream> eis(bis);  // wraps bis into eis
        rapidjson::Document walljson;         // Document is GenericDocument<UTF8<> >
        if (walljson.ParseStream<0, rapidjson::AutoUTF<unsigned> >(eis).HasParseError())
        {
            std::cout << "[ERROR] parsing input JSON file " << wallfn << std::endl;
        }
        fclose(fp);

        const rapidjson::Value& wallarr = walljson["Walls"];
        if (wallarr.IsArray())
        {
            for (rapidjson::SizeType i = 0, ie = wallarr.Size(); i < ie; ++i)
            {
                const rapidjson::Value& wall = wallarr[i];
                std::string label = wall["label"].GetString();
                if (label.compare("WALL") != 0) {
                    std::cout << "[error] parsing wall " << wall["attributes"]["id"].GetString() << std::endl;
                }
                else {
                    const rapidjson::Value& att = wall["attributes"];

                    Vec3d O(att["origin"][0].GetDouble(),
                            att["origin"][1].GetDouble(),
                            att["origin"][2].GetDouble());
                    O *= walljson_scalefactor;
                    Vec3d W(att["x"][0].GetDouble(),
                            att["x"][1].GetDouble(),
                            att["x"][2].GetDouble());
                    W *= att["width"].GetDouble();
                    W *= walljson_scalefactor;
                    Vec3d H(att["y"][0].GetDouble(),
                            att["y"][1].GetDouble(),
                            att["y"][2].GetDouble());
                    H *= att["height"].GetDouble();
                    H *= walljson_scalefactor;
                    Quad3Dd wallgeometry(O, O + H, O + W + H, O + W);
                    wallgeometry.id = wall["attributes"]["id"].GetString();
                    walls.push_back(wallgeometry);
                }
            }
        }
        else {
            std::cout << "[ERROR] parsing input JSON file " << wallfn << std::endl;
        }

    }

    std::cout << "# # # # # # # # # # # # #" << std::endl;
    std::cout << "parsed " << scans.size() << " scans and " << walls.size() << " walls." << std::endl;

    {
        // export walls
        myIFS quadgeometry; // ifs with orthophoto quads

        quadgeometry.useTextureCoordinates = true;
        quadgeometry.texcoordinates.push_back(Vec2d(0, 1));
        quadgeometry.texcoordinates.push_back(Vec2d(0, 0));
        quadgeometry.texcoordinates.push_back(Vec2d(1, 0));
        quadgeometry.texcoordinates.push_back(Vec2d(1, 1));

        for (auto const &wall : walls) {

            // perform orthophoto projection

            //Image orthophoto = q.performProjection(projection, resolution);

            Vec3d W = wall.W();
            Vec3d H = wall.H();
            double width = W.norm();
            double height = H.norm();
            Vec3d xdir = W; xdir.normalize();
            Vec3d ydir = H; ydir.normalize();

            Image ortho;
            ortho.initialize((int)(width / resolution) + 1, (int)(height / resolution) + 1, 24);

#pragma omp parallel for
            for (int y = 0; y < ortho.height(); ++y)
            {
                const Vec3d vecy = H * (y / (double)ortho.height());
                for (int x = 0; x < ortho.width(); ++x)
                {
                    // calculate position in world coordinates
                    const Vec3d vecx = W * (x / (double)ortho.width());
                    const Vec3d position = wall.V[0] + vecx + vecy;

                    // project color value: choose nearest scan
                    double neardist = DBL_MAX;
                    const SphericalPanoramaImageProjection *S = 0;
                    for (auto const &scan : scans)
                    {
                        double dist = (position - scan.pose.O).norm();
                        if (dist < neardist) {
                            neardist = dist;
                            S = &scan;
                        }
                    }
                    if (S)
                    {
                        RGB color = S->getColorProjection(position);
                        // write color value
                        ortho(x, y, 0) = color[0];
                        ortho(x, y, 1) = color[1];
                        ortho(x, y, 2) = color[2];
                    }
                }
            }

            {
                std::ostringstream oss;

                oss << output << "_" << wall.id << ".jpg";
                saveJPEG(oss.str().c_str(), ortho);

                std::cout << oss.str() << " : " << ortho.width() << "x"
                    << ortho.height() << " n: " << wall.pose.Z << std::endl;
            }

            std::ostringstream matname, texname;
            matname << output << "_" << wall.id;
            texname << output << "_" << wall.id << ".jpg";

            if (exportOBJ) {
                // create quad geometry, quads always use the same texcoordinates
                myIFS::IFSINDICES face;
                myIFS::IFSINDICES facetc;
                face.push_back(quadgeometry.vertex2index(wall.V[0]));
                facetc.push_back(0);
                face.push_back(quadgeometry.vertex2index(wall.V[1]));
                facetc.push_back(1);
                face.push_back(quadgeometry.vertex2index(wall.V[2]));
                facetc.push_back(2);
                face.push_back(quadgeometry.vertex2index(wall.V[3]));
                facetc.push_back(3);
                quadgeometry.faces.push_back(face);
                quadgeometry.facetexc.push_back(facetc);
                quadgeometry.materials.push_back(
                    IFS::Material(matname.str(), texname.str()));
                quadgeometry.facematerial.push_back(quadgeometry.materials.size() -
                    1);
                assert(quadgeometry.faces.size() == quadgeometry.facetexc.size()
                    && quadgeometry.faces.size() == quadgeometry.facematerial.size());
            }

        }
        if (exportOBJ)
        {
            std::ostringstream objname;
            objname << output << "_geometry.obj";
            IFS::exportOBJ(quadgeometry, objname.str(), "# OrthoGen textured quads\n");
        }
    }

  } catch (std::exception &e) {
    std::cerr << "error: " << e.what() << "\n";
    return -1;
  }

  std::cout << "--help for options." << std::endl;
}<commit_msg>print wall info<commit_after>
//
// ortho view generation tool
// ulrich.krispel@fraunhofer.at
//

#include <vector>
#include <string>
#include <algorithm>
#include <cmath>
#include <iomanip>
#include <limits>
#include <chrono>

#include <cstdlib>

#include <boost/program_options.hpp>

#include "rapidjson/document.h"
#include "rapidjson/filereadstream.h"   // FileReadStream
#include "rapidjson/encodedstream.h"    // AutoUTFInputStream

#include "types.h"
#include "projection.h"
#include "meanshift.h"
#include "extraction.h"

namespace po = boost::program_options;

// configuration variables
double WINDOW_SIZE_NORMAL = 0.3;
double WINDOW_SIZE_DISTANCE = 0.1;

typedef struct json_token JSONToken;
typedef struct json_token *JSONTokens;

using namespace OrthoGen;

template <typename V>
void prvec(const V& v, const std::string msg="")
{
    std::cout << msg << "[" << v[0] << "," << v[1] << "," << v[2] << "]";
}

int main(int ac, char *av[]) {

  // main application state
  std::vector<SphericalPanoramaImageProjection> scans;
  std::vector<Quad3Dd> walls;

  double resolution = 0.001;                // default: 1 mm/pixel
  double walljson_scalefactor = 0.001;      // wall json is in mm
  bool exportOBJ = false;
  //bool exportSphere = false;
  std::cout << "OrthoGen orthographic image generator for DuraArk" << std::endl;
  std::cout << "developed by Fraunhofer Austria Research GmbH" << std::endl;
  std::string output = "ortho";
  std::string panopath = ".\\";
  std::string aligncmd = "panoalign.exe";

  try {
    po::options_description desc("commandline options");
    desc.add_options()
        ("help", "show this help message")
        ("walljson", po::value<std::string>(), "input wall json [.json]")
        ("e57metadata", po::value<std::string>(), "e57 metadata json [.json]")
        ("panopath", po::value<std::string>(), "path to pano images")
        ("align", po::value<std::string>(), "align executable")
        ("output", po::value< std::string >(), "output filename [.jpg] will be appended")
        ("exgeom", "export textured geometry as .obj")
        ;

    po::variables_map vm;
    po::store(po::parse_command_line(ac, av, desc,
                                     po::command_line_style::unix_style ^
                                     po::command_line_style::allow_short),
              vm);
    po::notify(vm);

    if (vm.count("help")) //|| (!vm.count("walljson")) || (!vm.count("e57metadata")))
    {
      std::cout << desc << "\n";
      return 0;
    }

    if (vm.count("panopath"))
    {
        panopath = vm["panopath"].as<std::string>();
        std::string last = panopath.substr(panopath.length() - 1, 1);
        if (!(last.compare("\\") == 0 || last.compare("/") == 0))
        {
            panopath.append("/");
        }
    }

    if (vm.count("align")) { aligncmd = vm["align"].as<std::string>(); }
    if (vm.count("output")) { aligncmd = vm["output"].as<std::string>(); }
    exportOBJ = vm.count("exgeom") != 0;

    // parse jsons
    // ================================================= E57 Metadata
    // - parse scans (pose, name, panorama)

    {
        std::string e57mfn = vm["e57metadata"].as<std::string>();
        FILE* fp = fopen(e57mfn.c_str(), "rb"); // non-Windows use "r"
        char readBuffer[256];
        rapidjson::FileReadStream bis(fp, readBuffer, sizeof(readBuffer));
        rapidjson::AutoUTFInputStream<unsigned, rapidjson::FileReadStream> eis(bis);  // wraps bis into eis
        rapidjson::Document e57;         // Document is GenericDocument<UTF8<> >
        if (e57.ParseStream<0, rapidjson::AutoUTF<unsigned> >(eis).HasParseError())
        {
            std::cout << "[ERROR] parsing input JSON file "
                << e57mfn << std::endl;
            return -1;
        }
        fclose(fp);

        const rapidjson::Value& e57m = e57["e57m"];
        const rapidjson::Value& arr = e57["e57m"]["e57scan"];

        if (!e57m.IsObject() || !arr.IsArray()) {
            std::cout << "[ERROR] parsing input JSON file "
                << e57mfn << std::endl;
            return -1;
        }

      {
          for (rapidjson::SizeType i = 0, ie = arr.Size(); i < ie; ++i)
          {
              const rapidjson::Value& item = arr[i];
              SphericalPanoramaImageProjection new_scan;

              new_scan.basename = item["name"].GetString();
              std::cout << "found scan " << new_scan.basename << std::endl;
              // translation
              {
                  double x = item["pose"]["translation"]["x"].GetDouble();
                  double y = item["pose"]["translation"]["y"].GetDouble();
                  double z = item["pose"]["translation"]["z"].GetDouble();
                  new_scan.setPosition(Vec3d(x, y, z));
              }
              // orientation
            {
                double w = item["pose"]["rotation"]["w"].GetDouble();
                double x = item["pose"]["rotation"]["x"].GetDouble();
                double y = item["pose"]["rotation"]["y"].GetDouble();
                double z = item["pose"]["rotation"]["z"].GetDouble();
                Quaterniond quaternion(w, x, y, z);
                std::cout << " applying quaternion [" << quaternion.x() << ","
                    << quaternion.y() << "," << quaternion.z() << "," << quaternion.w()
                    << "]" << std::endl;                new_scan.applyRotation(quaternion);
            }
            std::cout << "pose: ";
            new_scan.printPose();

            // bounds
            {
                new_scan.elevationRange[0] = item["sphericalbounds"]["elevation_minimum"].GetDouble();
                new_scan.elevationRange[1] = item["sphericalbounds"]["elevation_maximum"].GetDouble();
                new_scan.azimuthRange[0] = item["sphericalbounds"]["azimuth_minimum"].GetDouble();
                new_scan.azimuthRange[1] = item["sphericalbounds"]["azimuth_maximum"].GetDouble();
                std::cout << "bounds: elevation [" << new_scan.elevationRange[0] << "<>" << new_scan.elevationRange[1]
                          << "] azimuth [" << new_scan.azimuthRange[0] << "<>" << new_scan.azimuthRange[1] << "]" << std::endl;
            }

            // load panoramic image (align if not found)
            {
                std::ostringstream ss;
                ss << panopath << new_scan.basename << "_aligned.jpg";
                Image img;
                if (loadJPEG(ss.str().c_str(), img)) {
                    new_scan.setPanoramicImage(img);
                }
                else {
                    std::cout << "aligned image not found, aligning..." << std::endl;
                    std::ostringstream cmd;
                    cmd.precision(std::numeric_limits<double>::max_digits10);
                    cmd << aligncmd << " ";
                    cmd << "--imsrc " << panopath << new_scan.basename << "_Faro.jpg ";
                    cmd << "--imdst " << panopath << new_scan.basename << "_Manual.jpg ";
                    cmd << "--outname " << panopath << new_scan.basename << "_aligned.jpg ";
                    cmd << "--selrange " << new_scan.elevationRange[0] << " " << new_scan.elevationRange[1] << " ";
                    std::system(cmd.str().c_str());
                }
            }

            scans.push_back(new_scan);
            std::cout << "-------------------------------------" << std::endl;
          }
      }
    }

    // ---------------------------------------------- WALL JSON
    {
        std::string wallfn = vm["walljson"].as<std::string>();
        FILE* fp = fopen(wallfn.c_str(), "rb"); // non-Windows use "r"
        char readBuffer[256];
        rapidjson::FileReadStream bis(fp, readBuffer, sizeof(readBuffer));
        rapidjson::AutoUTFInputStream<unsigned, rapidjson::FileReadStream> eis(bis);  // wraps bis into eis
        rapidjson::Document walljson;         // Document is GenericDocument<UTF8<> >
        if (walljson.ParseStream<0, rapidjson::AutoUTF<unsigned> >(eis).HasParseError())
        {
            std::cout << "[ERROR] parsing input JSON file " << wallfn << std::endl;
        }
        fclose(fp);

        const rapidjson::Value& wallarr = walljson["Walls"];
        if (wallarr.IsArray())
        {
            for (rapidjson::SizeType i = 0, ie = wallarr.Size(); i < ie; ++i)
            {
                const rapidjson::Value& wall = wallarr[i];
                std::string label = wall["label"].GetString();
                if (label.compare("WALL") != 0) {
                    std::cout << "[error] parsing wall " << wall["attributes"]["id"].GetString() << std::endl;
                }
                else {
                    const rapidjson::Value& att = wall["attributes"];

                    Vec3d O(att["origin"][0].GetDouble(),
                            att["origin"][1].GetDouble(),
                            att["origin"][2].GetDouble());
                    O *= walljson_scalefactor;
                    Vec3d W(att["x"][0].GetDouble(),
                            att["x"][1].GetDouble(),
                            att["x"][2].GetDouble());
                    W *= att["width"].GetDouble();
                    W *= walljson_scalefactor;
                    Vec3d H(att["y"][0].GetDouble(),
                            att["y"][1].GetDouble(),
                            att["y"][2].GetDouble());
                    H *= att["height"].GetDouble();
                    H *= walljson_scalefactor;
                    Quad3Dd wallgeometry(O, O + H, O + W + H, O + W);
                    wallgeometry.id = wall["attributes"]["id"].GetString();
                    walls.push_back(wallgeometry);
                }
            }
        }
        else {
            std::cout << "[ERROR] parsing input JSON file " << wallfn << std::endl;
        }

    }

    std::cout << "# # # # # # # # # # # # #" << std::endl;
    std::cout << "parsed " << scans.size() << " scans and " << walls.size() << " walls." << std::endl;

    {
        // export walls
        myIFS quadgeometry; // ifs with orthophoto quads

        quadgeometry.useTextureCoordinates = true;
        quadgeometry.texcoordinates.push_back(Vec2d(0, 1));
        quadgeometry.texcoordinates.push_back(Vec2d(0, 0));
        quadgeometry.texcoordinates.push_back(Vec2d(1, 0));
        quadgeometry.texcoordinates.push_back(Vec2d(1, 1));

        for (auto const &wall : walls) {
            std::ostringstream outfile;
            outfile << output << "_" << wall.id;
            std::cout << "* Processing " << outfile.str() << std::endl;;
            outfile << ".jpg";

            // perform orthophoto projection
            Vec3d W = wall.W();
            Vec3d H = wall.H();
            double width = W.norm();
            double height = H.norm();
            Vec3d xdir = W; xdir.normalize();
            Vec3d ydir = H; ydir.normalize();

            Image ortho;
            ortho.initialize((int)(width / resolution) + 1, (int)(height / resolution) + 1, 24);

            prvec(wall.V[0], " V0:");
            prvec(wall.V[1], " V1:");
            std::cout << std::endl;
            prvec(wall.V[2], " V2:");
            prvec(wall.V[3], " V3:");
            std::cout << std::endl;
            std::cout << ortho.width() << "x" << ortho.height() << " n: " << wall.pose.Z << std::endl;

#pragma omp parallel for
            for (int y = 0; y < ortho.height(); ++y)
            {
                const Vec3d vecy = H * (y / (double)ortho.height());
                for (int x = 0; x < ortho.width(); ++x)
                {
                    // calculate position in world coordinates
                    const Vec3d vecx = W * (x / (double)ortho.width());
                    const Vec3d position = wall.V[0] + vecx + vecy;

                    // project color value: choose nearest scan
                    double neardist = DBL_MAX;
                    const SphericalPanoramaImageProjection *S = 0;
                    for (auto const &scan : scans)
                    {
                        double dist = (position - scan.pose.O).norm();
                        if (dist < neardist) {
                            neardist = dist;
                            S = &scan;
                        }
                    }
                    if (S)
                    {
                        RGB color = S->getColorProjection(position);
                        // write color value
                        ortho(x, y, 0) = color[0];
                        ortho(x, y, 1) = color[1];
                        ortho(x, y, 2) = color[2];
                    }
                }
            }

            {
                saveJPEG(outfile.str().c_str(), ortho);

            }

            std::ostringstream matname, texname;
            matname << output << "_" << wall.id;
            texname << output << "_" << wall.id << ".jpg";

            if (exportOBJ) {
                // create quad geometry, quads always use the same texcoordinates
                myIFS::IFSINDICES face;
                myIFS::IFSINDICES facetc;
                face.push_back(quadgeometry.vertex2index(wall.V[0]));
                facetc.push_back(0);
                face.push_back(quadgeometry.vertex2index(wall.V[1]));
                facetc.push_back(1);
                face.push_back(quadgeometry.vertex2index(wall.V[2]));
                facetc.push_back(2);
                face.push_back(quadgeometry.vertex2index(wall.V[3]));
                facetc.push_back(3);
                quadgeometry.faces.push_back(face);
                quadgeometry.facetexc.push_back(facetc);
                quadgeometry.materials.push_back(
                    IFS::Material(matname.str(), texname.str()));
                quadgeometry.facematerial.push_back(quadgeometry.materials.size() -
                    1);
                assert(quadgeometry.faces.size() == quadgeometry.facetexc.size()
                    && quadgeometry.faces.size() == quadgeometry.facematerial.size());
            }

        }
        if (exportOBJ)
        {
            std::ostringstream objname;
            objname << output << "_geometry.obj";
            IFS::exportOBJ(quadgeometry, objname.str(), "# OrthoGen textured quads\n");
        }
    }

  } catch (std::exception &e) {
    std::cerr << "error: " << e.what() << "\n";
    return -1;
  }

  std::cout << "--help for options." << std::endl;
}<|endoftext|>