<commit_before>/**********************************************************************
* File:        TessTools.cpp
* Description: Tesseract tools
* Author:      Zdenko Podobny
* Created:     2012-03-27
*
* (C) Copyright 2012-2013, Zdenko Podobny
**
** Licensed under the Apache License, Version 2.0 (the "License");
** you may not use this file except in compliance with the License.
** You may obtain a copy of the License at
**
**    http://www.apache.org/licenses/LICENSE-2.0
**
** Unless required by applicable law or agreed to in writing, software
** distributed under the License is distributed on an "AS IS" BASIS,
** WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
** See the License for the specific language governing permissions and
** limitations under the License.
*
**********************************************************************/

#include <locale.h>
#include "TessTools.h"
#include "Settings.h"

#ifdef TESSERACT_VERSION  // 3.03 API
#include <tesseract/renderer.h>
#endif  // TESSERACT_VERSION
#include <tesseract/strngs.h>

#include <QApplication>
#include <QWidget>
#include <QTextStream>
#include <QSettings>
#include <QStringList>
#include <QMessageBox>
#include <QDir>
#include <QDebug>
#include <QFile>

#if QT_VERSION >= QT_VERSION_CHECK(5, 0, 0)
#include <QGuiApplication>
#endif  // QT5

const char *TessTools::kTrainedDataSuffix = "traineddata";

// TODO(zdenop): Improve code here...

TessTools::TessTools() {
}

TessTools::~TessTools() {
}

/*
 * QString to const char
 */
const char *TessTools::qString2Char(QString string) {
    QByteArray byteArray = string.toUtf8();
    const char * constChar = byteArray.data();
    return constChar;
}

/*!
 * Create tesseract box data from QImage
 */
QString TessTools::makeBoxes(const QImage& qImage, const int page) {
  PIX   *pixs;

  if ((pixs = qImage2PIX(qImage)) == NULL) {
    msg("Unsupported image type");
    return "";
  }

  // http://code.google.com/p/tesseract-ocr/issues/detail?id=228
  setlocale(LC_NUMERIC, "C");
  // QString to  const char *
  #if QT_VERSION < QT_VERSION_CHECK(5, 0, 0)
  QByteArray byteArray = getLang().toAscii();
  #else
  QByteArray byteArray = getLang().toLocal8Bit();
  #endif
  const char * apiLang = byteArray.constData();

  // workaroung if datapath/TESSDATA_PREFIX is set...
  #ifdef _WIN32
  QString envQString = "TESSDATA_PREFIX=" + getDataPath() ;
  QByteArray byteArrayWin = envQString.toUtf8();
  const char * env = byteArrayWin.data();
  putenv(env);
  #else
  QByteArray byteArray1 = getDataPath().toUtf8();
  const char * datapath = byteArray1.data();
  setenv("TESSDATA_PREFIX", datapath, 1);
  #endif

  tesseract::TessBaseAPI *api = new tesseract::TessBaseAPI();
  if (api->Init(NULL, apiLang)) {
    msg("Could not initialize tesseract.\n");
    return "";
  }

  STRING text_out;
  QApplication::setOverrideCursor(Qt::WaitCursor);

  int timeout_millisec = 0;
  const char* filename = NULL;
  const char* retry_config = NULL;
#ifdef TESSERACT_VERSION
  const char* outputbase = NULL;
  tesseract::TessResultRenderer* renderer = NULL;
  renderer = new tesseract::TessBoxTextRenderer(outputbase);
  if (!api->ProcessPage(pixs, page, filename, retry_config,
                        timeout_millisec, renderer)) {
#else
  api->SetVariable("tessedit_create_boxfile", "1");
  if (!api->ProcessPage(pixs, page, filename, retry_config, timeout_millisec,
                        &text_out)) {
#endif  // TESSERACT_VERSION
    msg("Error during processing.\n");
  }
  QApplication::restoreOverrideCursor();

  pixDestroy(&pixs);
  api->End();
  delete api;
  return QString::fromUtf8(text_out.string());
}

/*!
 * Convert QT QImage to PIX
 * input: QImage
 * result: PIX
 */
PIX* TessTools::qImage2PIX(const QImage& qImage) {
  PIX * pixs;

  QImage myImage = qImage.rgbSwapped();
  int width = myImage.width();
  int height = myImage.height();
  int depth = myImage.depth();
  int wpl = myImage.bytesPerLine() / 4;

  pixs = pixCreate(width, height, depth);
  pixSetWpl(pixs, wpl);
  pixSetColormap(pixs, NULL);
  l_uint32 *datas = pixs->data;

  for (int y = 0; y < height; y++) {
    l_uint32 *lines = datas + y * wpl;
    QByteArray a((const char*)myImage.scanLine(y), myImage.bytesPerLine());
    for (int j = 0; j < a.size(); j++) {
      *((l_uint8 *)lines + j) = a[j];
    }
  }

  const qreal toDPM = 1.0 / 0.0254;
  int resolutionX = myImage.dotsPerMeterX() / toDPM;
  int resolutionY = myImage.dotsPerMeterY() / toDPM;

  if (resolutionX < 300) resolutionX = 300;
  if (resolutionY < 300) resolutionY = 300;
  pixSetResolution(pixs, resolutionX, resolutionY);

  return pixEndianByteSwapNew(pixs);
}

/*!
 * Convert Leptonica PIX to QImage
 * input: PIX
 * result: QImage
 */
QImage TessTools::PIX2qImage(PIX *pixImage) {
  int width = pixGetWidth(pixImage);
  int height = pixGetHeight(pixImage);
  int depth = pixGetDepth(pixImage);
  int bytesPerLine = pixGetWpl(pixImage) * 4;
  l_uint32 * datas = pixGetData(pixEndianByteSwapNew(pixImage));

  QImage::Format format;
  if (depth == 1)
    format = QImage::Format_Mono;
  else if (depth == 8)
    format = QImage::Format_Indexed8;
  else
    format = QImage::Format_RGB32;

  QImage result((uchar*)datas, width, height, bytesPerLine, format);

  // Set resolution
  l_int32 	xres, yres;
  pixGetResolution(pixImage, &xres, &yres);
  const qreal toDPM = 1.0 / 0.0254;
  result.setDotsPerMeterX(xres * toDPM);
  result.setDotsPerMeterY(yres * toDPM);

  // Handle palette
  QVector<QRgb> _bwCT;
  _bwCT.append(qRgb(255,255,255));
  _bwCT.append(qRgb(0,0,0));

  QVector<QRgb> _grayscaleCT(256);
  for (int i = 0; i < 256; i++)  {
    _grayscaleCT[i] = qRgb(i, i, i);
  }
  switch (depth) {
    case 1:
      result.setColorTable(_bwCT);
      break;
    case 8:
      result.setColorTable(_grayscaleCT);
      break;
    default:
      result.setColorTable(_grayscaleCT);
  }

  if (result.isNull()) {
    static QImage none(0,0,QImage::Format_Invalid);
    qDebug("Invalid format!!!\n");
    return none;
  }

  return result.rgbSwapped();
}

QImage TessTools::GetThresholded(const QImage& qImage) {
    // TODO(zdenop): Check this for memory leak
    PIX * pixs = qImage2PIX(qImage);

    // Set tessdata as Enviromental Variable to avoid problems
    QByteArray byteArray1 = getDataPath().toUtf8();
    #ifdef _WIN32
    QString envQString = "TESSDATA_PREFIX=" + byteArray1 ;
    const char * env = qString2Char(envQString);
    putenv(env);
    #else
    const char * datapath = byteArray1.data();
    setenv("TESSDATA_PREFIX", datapath, 1);
    #endif

    // TODO(zdenop): Why apiLang = qString2Char(getLang()) do not work???
    #if QT_VERSION < QT_VERSION_CHECK(5, 0, 0)
    QByteArray byteArray = getLang().toAscii();
    #else
    QByteArray byteArray = getLang().toLocal8Bit();
    #endif
    const char * apiLang = byteArray.constData();

    setlocale(LC_NUMERIC, "C");

    tesseract::TessBaseAPI *api = new tesseract::TessBaseAPI();
    if (api->Init(NULL, apiLang)) {
        msg("Could not initialize tesseract.\n");
        return QImage();
    }
    api->SetImage(pixs);
    PIX * pixq = api->GetThresholdedImage();
    QImage tresholdedImage = PIX2qImage(pixq);
    api->End();
    delete api;
    pixDestroy(&pixs);
    pixDestroy(&pixq);

    return tresholdedImage;
}

QString TessTools::getDataPath() {
    QSettings settings(QSettings::IniFormat, QSettings::UserScope,
                       SETTING_ORGANIZATION, SETTING_APPLICATION);
    QString dataPath;
    if (settings.contains("Tesseract/DataPath")) {
      dataPath = settings.value("Tesseract/DataPath").toString();
    }
    return dataPath;
}

QString TessTools::getLang() {
    QSettings settings(QSettings::IniFormat, QSettings::UserScope,
                       SETTING_ORGANIZATION, SETTING_APPLICATION);
    QString lang;
    if (settings.contains("Tesseract/Lang")) {
        lang = settings.value("Tesseract/Lang").toString();
    }

    if (lang.isNull()) {
        msg("You need to configure tesseract in Settings!");
        return "";
    }
   return lang;
}

/*!
 * Get QList<QString> with list of available languages
 */
QList<QString> TessTools::getLanguages(QString datapath) {
    QList<QString> languages;
    QDir dir(datapath);

    if (!dir.exists()) {
      QMessageBox msgBox;
      msgBox.setText(QObject::tr("Cannot find the tessdata directory '%1'!\n").arg(datapath) +
                  QObject::tr("Please check your configuration or tesseract instalation"));
      msgBox.exec();
      return languages;
      }

    QString filter = "*.";
    filter += kTrainedDataSuffix;
    QStringList filters;
    filters << filter.trimmed();
    dir.setNameFilters(filters);

    QFileInfoList list = dir.entryInfoList();

    for (int i = 0; i < list.size(); ++i) {
      QFileInfo fileInfo = list.at(i);
      languages.append(QString("%1").arg(fileInfo.baseName()));
    }
    qSort(languages);

    return languages;
}

void TessTools::msg(QString messageText) {
    QMessageBox msgBox;
    msgBox.setText(messageText);
    msgBox.exec();
}

<commit_msg>fix generating boxes with tesseract 3.04.00/3.05.00dev - fixes #53<commit_after>/**********************************************************************
* File:        TessTools.cpp
* Description: Tesseract tools
* Author:      Zdenko Podobny
* Created:     2012-03-27
*
* (C) Copyright 2012-2013, Zdenko Podobny
**
** Licensed under the Apache License, Version 2.0 (the "License");
** you may not use this file except in compliance with the License.
** You may obtain a copy of the License at
**
**    http://www.apache.org/licenses/LICENSE-2.0
**
** Unless required by applicable law or agreed to in writing, software
** distributed under the License is distributed on an "AS IS" BASIS,
** WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
** See the License for the specific language governing permissions and
** limitations under the License.
*
**********************************************************************/

#include <locale.h>
#include "TessTools.h"
#include "Settings.h"

#ifdef TESSERACT_VERSION  // 3.03 API
#include <tesseract/renderer.h>
#endif  // TESSERACT_VERSION
#include <tesseract/strngs.h>

#include <QApplication>
#include <QWidget>
#include <QTextStream>
#include <QSettings>
#include <QStringList>
#include <QMessageBox>
#include <QDir>
#include <QDebug>
#include <QFile>

#if QT_VERSION >= QT_VERSION_CHECK(5, 0, 0)
#include <QGuiApplication>
#endif  // QT5

const char *TessTools::kTrainedDataSuffix = "traineddata";

// TODO(zdenop): Improve code here...

TessTools::TessTools() {
}

TessTools::~TessTools() {
}

/*
 * QString to const char
 */
const char *TessTools::qString2Char(QString string) {
    QByteArray byteArray = string.toUtf8();
    const char * constChar = byteArray.data();
    return constChar;
}

/*!
 * Create tesseract box data from QImage
 */
QString TessTools::makeBoxes(const QImage& qImage, const int page) {
  PIX   *pixs;
  char  *outText;

  if ((pixs = qImage2PIX(qImage)) == NULL) {
    msg("Unsupported image type");
    return "";
  }

  // http://code.google.com/p/tesseract-ocr/issues/detail?id=228
  setlocale(LC_NUMERIC, "C");
  // QString to  const char *
  #if QT_VERSION < QT_VERSION_CHECK(5, 0, 0)
  QByteArray byteArray = getLang().toAscii();
  #else
  QByteArray byteArray = getLang().toLocal8Bit();
  #endif
  const char * apiLang = byteArray.constData();

  // workaroung if datapath/TESSDATA_PREFIX is set...
  #ifdef _WIN32
  QString envQString = "TESSDATA_PREFIX=" + getDataPath() ;
  QByteArray byteArrayWin = envQString.toUtf8();
  const char * env = byteArrayWin.data();
  putenv(env);
  #else
  QByteArray byteArray1 = getDataPath().toUtf8();
  const char * datapath = byteArray1.data();
  setenv("TESSDATA_PREFIX", datapath, 1);
  #endif

  tesseract::TessBaseAPI *api = new tesseract::TessBaseAPI();
  if (api->Init(NULL, apiLang)) {
    msg("Could not initialize tesseract.\n");
    return "";
  }

  QApplication::setOverrideCursor(Qt::WaitCursor);

#ifdef TESSERACT_VERSION
  api->SetImage(pixs);
  outText = api->GetBoxText(page);
#else
  api->SetVariable("tessedit_create_boxfile", "1");
  int timeout_millisec = 0;
  const char* filename = NULL;
  const char* retry_config = NULL;
  STRING text_out;
  if (!api->ProcessPage(pixs, page, filename, retry_config,
                        timeout_millisec, &text_out)) {
    msg("Error during processing.\n");
  }
  outText = text_out.string();
#endif  // TESSERACT_VERSION
  QApplication::restoreOverrideCursor();

  pixDestroy(&pixs);
  api->End();
  delete api;
  return QString::fromUtf8(outText);
}

/*!
 * Convert QT QImage to PIX
 * input: QImage
 * result: PIX
 */
PIX* TessTools::qImage2PIX(const QImage& qImage) {
  PIX * pixs;

  QImage myImage = qImage.rgbSwapped();
  int width = myImage.width();
  int height = myImage.height();
  int depth = myImage.depth();
  int wpl = myImage.bytesPerLine() / 4;

  pixs = pixCreate(width, height, depth);
  pixSetWpl(pixs, wpl);
  pixSetColormap(pixs, NULL);
  l_uint32 *datas = pixs->data;

  for (int y = 0; y < height; y++) {
    l_uint32 *lines = datas + y * wpl;
    QByteArray a((const char*)myImage.scanLine(y), myImage.bytesPerLine());
    for (int j = 0; j < a.size(); j++) {
      *((l_uint8 *)lines + j) = a[j];
    }
  }

  const qreal toDPM = 1.0 / 0.0254;
  int resolutionX = myImage.dotsPerMeterX() / toDPM;
  int resolutionY = myImage.dotsPerMeterY() / toDPM;

  if (resolutionX < 300) resolutionX = 300;
  if (resolutionY < 300) resolutionY = 300;
  pixSetResolution(pixs, resolutionX, resolutionY);

  return pixEndianByteSwapNew(pixs);
}

/*!
 * Convert Leptonica PIX to QImage
 * input: PIX
 * result: QImage
 */
QImage TessTools::PIX2qImage(PIX *pixImage) {
  int width = pixGetWidth(pixImage);
  int height = pixGetHeight(pixImage);
  int depth = pixGetDepth(pixImage);
  int bytesPerLine = pixGetWpl(pixImage) * 4;
  l_uint32 * datas = pixGetData(pixEndianByteSwapNew(pixImage));

  QImage::Format format;
  if (depth == 1)
    format = QImage::Format_Mono;
  else if (depth == 8)
    format = QImage::Format_Indexed8;
  else
    format = QImage::Format_RGB32;

  QImage result((uchar*)datas, width, height, bytesPerLine, format);

  // Set resolution
  l_int32 	xres, yres;
  pixGetResolution(pixImage, &xres, &yres);
  const qreal toDPM = 1.0 / 0.0254;
  result.setDotsPerMeterX(xres * toDPM);
  result.setDotsPerMeterY(yres * toDPM);

  // Handle palette
  QVector<QRgb> _bwCT;
  _bwCT.append(qRgb(255,255,255));
  _bwCT.append(qRgb(0,0,0));

  QVector<QRgb> _grayscaleCT(256);
  for (int i = 0; i < 256; i++)  {
    _grayscaleCT[i] = qRgb(i, i, i);
  }
  switch (depth) {
    case 1:
      result.setColorTable(_bwCT);
      break;
    case 8:
      result.setColorTable(_grayscaleCT);
      break;
    default:
      result.setColorTable(_grayscaleCT);
  }

  if (result.isNull()) {
    static QImage none(0,0,QImage::Format_Invalid);
    qDebug("Invalid format!!!\n");
    return none;
  }

  return result.rgbSwapped();
}

QImage TessTools::GetThresholded(const QImage& qImage) {
    // TODO(zdenop): Check this for memory leak
    PIX * pixs = qImage2PIX(qImage);

    // Set tessdata as Enviromental Variable to avoid problems
    QByteArray byteArray1 = getDataPath().toUtf8();
    #ifdef _WIN32
    QString envQString = "TESSDATA_PREFIX=" + byteArray1 ;
    const char * env = qString2Char(envQString);
    putenv(env);
    #else
    const char * datapath = byteArray1.data();
    setenv("TESSDATA_PREFIX", datapath, 1);
    #endif

    // TODO(zdenop): Why apiLang = qString2Char(getLang()) do not work???
    #if QT_VERSION < QT_VERSION_CHECK(5, 0, 0)
    QByteArray byteArray = getLang().toAscii();
    #else
    QByteArray byteArray = getLang().toLocal8Bit();
    #endif
    const char * apiLang = byteArray.constData();

    setlocale(LC_NUMERIC, "C");

    tesseract::TessBaseAPI *api = new tesseract::TessBaseAPI();
    if (api->Init(NULL, apiLang)) {
        msg("Could not initialize tesseract.\n");
        return QImage();
    }
    api->SetImage(pixs);
    PIX * pixq = api->GetThresholdedImage();
    QImage tresholdedImage = PIX2qImage(pixq);
    api->End();
    delete api;
    pixDestroy(&pixs);
    pixDestroy(&pixq);

    return tresholdedImage;
}

QString TessTools::getDataPath() {
    QSettings settings(QSettings::IniFormat, QSettings::UserScope,
                       SETTING_ORGANIZATION, SETTING_APPLICATION);
    QString dataPath;
    if (settings.contains("Tesseract/DataPath")) {
      dataPath = settings.value("Tesseract/DataPath").toString();
    }
    return dataPath;
}

QString TessTools::getLang() {
    QSettings settings(QSettings::IniFormat, QSettings::UserScope,
                       SETTING_ORGANIZATION, SETTING_APPLICATION);
    QString lang;
    if (settings.contains("Tesseract/Lang")) {
        lang = settings.value("Tesseract/Lang").toString();
    }

    if (lang.isNull()) {
        msg("You need to configure tesseract in Settings!");
        return "";
    }
   return lang;
}

/*!
 * Get QList<QString> with list of available languages
 */
QList<QString> TessTools::getLanguages(QString datapath) {
    QList<QString> languages;
    QDir dir(datapath);

    if (!dir.exists()) {
      QMessageBox msgBox;
      msgBox.setText(QObject::tr("Cannot find the tessdata directory '%1'!\n").arg(datapath) +
                  QObject::tr("Please check your configuration or tesseract instalation"));
      msgBox.exec();
      return languages;
      }

    QString filter = "*.";
    filter += kTrainedDataSuffix;
    QStringList filters;
    filters << filter.trimmed();
    dir.setNameFilters(filters);

    QFileInfoList list = dir.entryInfoList();

    for (int i = 0; i < list.size(); ++i) {
      QFileInfo fileInfo = list.at(i);
      languages.append(QString("%1").arg(fileInfo.baseName()));
    }
    qSort(languages);

    return languages;
}

void TessTools::msg(QString messageText) {
    QMessageBox msgBox;
    msgBox.setText(messageText);
    msgBox.exec();
}

<|endoftext|>


<commit_before>/*
 *  StringDiff.cpp
 *  xydiff
 *
 *  Created by Frankie Dintino on 2/3/09.
 *  Copyright 2009 __MyCompanyName__. All rights reserved.
 *
 */

#include "include/XyStrDiff.hpp"

#include "include/XyLatinStr.hpp"
#include "xercesc/util/XMLString.hpp"
#include "xercesc/util/XMLUniDefs.hpp"

#include <stdlib.h>
#include <string.h>
#include <sstream>

/*
 * XyStrDiff functions (character-by-character string diffs)
 */

XERCES_CPP_NAMESPACE_USE

#define STRDIFF_NOOP 0
#define STRDIFF_SUB 1
#define STRDIFF_INS 2
#define STRDIFF_DEL 3

XyStrDiff::XyStrDiff(const char* strX, const char *strY, int sizeXStr, int sizeYStr)
{

	sizex = sizeXStr;
	sizey = sizeYStr;

	if ((strX==NULL)||(sizex==0)) return;
	if (sizex<0) sizex = strlen(strX);
	x = new char[sizex+1];
	memcpy(x, strX, sizex*sizeof(char));
	x[sizex]='\0';

	if ((strY==NULL)||(sizey==0)) return;
	if (sizey<0) sizey = strlen(strY);
	y = new char[sizey+1];
	memcpy(y, strY, sizey*sizeof(char));
	y[sizey]='\0';

	n = sizex;
	m = sizey;

	int malloclen = (sizeof(int))*(sizex+1)*(sizey+1);
	// c = LCS Length matrix

	c = (int*) malloc(malloclen);
	// d = Levenshtein Distance matrix
	d = (int*) malloc(malloclen);
	t = (int*) malloc(malloclen);

	currop = -1;
}

/*
 * Destructor
 */

XyStrDiff::~XyStrDiff(void)
{
	free(t);
	free(c);
	free(d);
	delete [] x;
	delete [] y;

}

int XyStrDiff::LevenshteinDistance()
{
	// Step 1
	int k, i, j, cost, distance;
	n = strlen(x);
	m = strlen(y);
	if (n != 0 && m != 0) {
		m++;
		n++;
		// Step 2
		for(k = 0; k < n; k++) {
			c[k] = 0;
			d[k] = k;
		}
		for(k = 0; k < m; k++) {
			c[k*n] = 0;
			d[k*n] = k;
		}

		// Step 3 and 4
		for(i = 1; i < n; i++) {
			for(j = 1; j < m; j++) {
				// Step 5
				if (x[i-1] == y[j-1]) {

					cost = 0;
					c[j*n+i] = c[(j-1)*n + i-1] + 1;
				} else {
					cost = 1;
					c[j*n+i] = max(c[(j-1)*n + i], c[j*n + i-1]);
				}
				// Step 6
				int del = d[j*n+i-1] + 1;
				int ins = d[(j-1)*n+i] + 1;
				int sub = d[(j-1)*n+i-1] + cost;
				if (sub <= del && sub <= ins) {
					d[j*n+i] = sub;
					t[j*n+i] = STRDIFF_SUB;
				} else if (del <= ins) {
					d[j*n+i] = del;
					t[j*n+i] = STRDIFF_DEL;
				} else {
					d[j*n+i] = ins;
					t[j*n+i] = STRDIFF_INS;
				}
			}
		}
		distance = d[n*m-1];
		this->getPath();
		std::cout << s1 << std::endl;
		std::cout << s2 << std::endl;
		this->flushBuffers();
		std::cout << "outstr: " << outstr << std::endl;
		return distance;
	} else  {
		return -1; //a negative return value means that one or both strings are empty.
	}
}

void XyStrDiff::getPath(int i, int j)
{
	if (i == -1) i = sizex;
	if (j == -1) j = sizey;

	if (i > 0 && j > 0 && (x[i-1] == y[j-1])) {
		this->getPath(i-1, j-1);
		s1.append(" ");
		s2 += x[i-1];
		this->alterText(i, j, STRDIFF_NOOP, x[i-1]);
	} else {
		if (j > 0 && (i == 0 || c[(j-1)*n+i] >= c[j*n+i-1])) {
			this->getPath(i, j-1);
			s1.append("+");
			s2 += y[j-1];
			this->alterText(i, j, STRDIFF_INS, y[j-1]);
		} else if (i > 0 && (j == 0 || c[(j-1)*n+i] < c[j*n+i-1])) {
			this->getPath(i-1, j);
			s1.append("-");
			s2 += x[i-1];
			this->alterText(i, j, STRDIFF_DEL, x[i-1]);
		}
	}
}

void XyStrDiff::alterText(int i, int j, int optype, char chr)
{
	if (wordbuf.empty()) {
		wordbuf = chr;
	}
	xpos = i;
	ypos = j;
	if (currop == -1) {
		currop = optype;

	}
	if (currop == STRDIFF_SUB) {
		if (optype == STRDIFF_DEL) {
			delbuf += chr;
		} else if (optype == STRDIFF_INS) {
			insbuf += chr;
		} else {
			this->flushBuffers();
			currop = optype;
			noopbuf += chr;
		}
	}
	else if (optype == STRDIFF_DEL) {
		currop = optype;
		delbuf += chr;
	}
	else if (optype == STRDIFF_INS) {
		currop = (currop == STRDIFF_DEL) ? STRDIFF_SUB : STRDIFF_INS;
		insbuf += chr;
	}
	else if (optype == STRDIFF_NOOP) {
		noopbuf += chr;
		this->flushBuffers();
		currop = optype;
	}
}

void XyStrDiff::flushBuffers()
{
	if (currop == STRDIFF_NOOP) {
		outstr.append(noopbuf);
		noopbuf = "";
	} else if (currop == STRDIFF_SUB) {
		int start = xpos - insbuf.length();
		outstr.append("<r st=\"" + itoa(start) + "\" len=\"" + itoa(delbuf.length()) + " repl=\"" + insbuf + "\"\"/>");
		delbuf = "";
		insbuf = "";
	} else if (currop == STRDIFF_INS) {
		outstr.append(""+insbuf+"");
		insbuf = "";
	} else if (currop == STRDIFF_DEL) {
		outstr.append("<d>"+delbuf+"</d>");
		delbuf = "";
	}
}

std::string itoa (int n)
{
	char * s = new char[17];
	std::string u;

	if (n < 0) {         //turns n positive
		n = (-1 * n);
		u = "-";         //adds '-' on result string
	}

	int i = 0;  // s counter

	do {
		s[i++] = n % 10 + '0'; //conversion of each digit of n to char
		n -= n % 10;           //update n value
	} while ((n /= 10) > 0);

	for (int j = i-1; j >= 0; j--) {
		u += s[j];    //building our string number
	}

	delete[] s;       //free-up the memory!
	return u;
}<commit_msg>Worked on XyStrDiff algorithm<commit_after>/*
 *  StringDiff.cpp
 *  xydiff
 *
 *  Created by Frankie Dintino on 2/3/09.
 *  Copyright 2009 __MyCompanyName__. All rights reserved.
 *
 */

#include "include/XyStrDiff.hpp"

#include "include/XyLatinStr.hpp"
#include "xercesc/util/XMLString.hpp"
#include "xercesc/util/XMLUniDefs.hpp"

#include <stdlib.h>
#include <string.h>
#include <sstream>

/*
 * XyStrDiff functions (character-by-character string diffs)
 */

XERCES_CPP_NAMESPACE_USE

#define STRDIFF_NOOP 0
#define STRDIFF_SUB 1
#define STRDIFF_INS 2
#define STRDIFF_DEL 3

XyStrDiff::XyStrDiff(const char* strX, const char *strY, int sizeXStr, int sizeYStr)
{

	sizex = sizeXStr;
	sizey = sizeYStr;

	if ((strX==NULL)||(sizex==0)) return;
	if (sizex<0) sizex = strlen(strX);
	x = new char[sizex+1];
	memcpy(x, strX, sizex*sizeof(char));
	x[sizex]='\0';

	if ((strY==NULL)||(sizey==0)) return;
	if (sizey<0) sizey = strlen(strY);
	y = new char[sizey+1];
	memcpy(y, strY, sizey*sizeof(char));
	y[sizey]='\0';

	n = sizex;
	m = sizey;

	int malloclen = (sizeof(int))*(sizex+1)*(sizey+1);
	// c = LCS Length matrix

	c = (int*) malloc(malloclen);
	// d = Levenshtein Distance matrix
	d = (int*) malloc(malloclen);
	t = (int*) malloc(malloclen);

	currop = -1;
}

/*
 * Destructor
 */

XyStrDiff::~XyStrDiff(void)
{
	free(t);
	free(c);
	free(d);
	delete [] x;
	delete [] y;

}

int XyStrDiff::LevenshteinDistance()
{
	// Step 1
	int k, i, j, cost, distance;
	n = strlen(x);
	m = strlen(y);
	if (n != 0 && m != 0) {
		m++;
		n++;
		// Step 2
		for(k = 0; k < n; k++) {
			c[k] = 0;
			d[k] = k;
		}
		for(k = 0; k < m; k++) {
			c[k*n] = 0;
			d[k*n] = k;
		}

		// Step 3 and 4
		for(i = 1; i < n; i++) {
			for(j = 1; j < m; j++) {
				// Step 5
				if (x[i-1] == y[j-1]) {

					cost = 0;
					c[j*n+i] = c[(j-1)*n + i-1] + 1;
				} else {
					cost = 1;
					c[j*n+i] = max(c[(j-1)*n + i], c[j*n + i-1]);
				}
				// Step 6
				int del = d[j*n+i-1] + 1;
				int ins = d[(j-1)*n+i] + 1;
				int sub = d[(j-1)*n+i-1] + cost;
				if (sub <= del && sub <= ins) {
					d[j*n+i] = sub;
					t[j*n+i] = STRDIFF_SUB;
				} else if (del <= ins) {
					d[j*n+i] = del;
					t[j*n+i] = STRDIFF_DEL;
				} else {
					d[j*n+i] = ins;
					t[j*n+i] = STRDIFF_INS;
				}
			}
		}
		distance = d[n*m-1];
		this->getPath();
		std::cout << s1 << std::endl;
		std::cout << s2 << std::endl;
		this->flushBuffers();
		std::cout << "outstr: " << outstr << std::endl;
		return distance;
	} else  {
		return -1; //a negative return value means that one or both strings are empty.
	}
}

void XyStrDiff::getPath(int i, int j)
{
	if (i == -1) i = sizex;
	if (j == -1) j = sizey;

	if (i > 0 && j > 0 && (x[i-1] == y[j-1])) {
		this->getPath(i-1, j-1);
		s1.append(" ");
		s2 += x[i-1];
		this->alterText(i, j, STRDIFF_NOOP, x[i-1]);
	} else {
		if (j > 0 && (i == 0 || c[(j-1)*n+i] >= c[j*n+i-1])) {
			this->getPath(i, j-1);
			s1.append("+");
			s2 += y[j-1];
			this->alterText(i, j, STRDIFF_INS, y[j-1]);
		} else if (i > 0 && (j == 0 || c[(j-1)*n+i] < c[j*n+i-1])) {
			this->getPath(i-1, j);
			s1.append("-");
			s2 += x[i-1];
			this->alterText(i, j, STRDIFF_DEL, x[i-1]);
		}
	}
}

void XyStrDiff::alterText(int i, int j, int optype, char chr)
{
	if (wordbuf.empty()) {
		wordbuf = chr;
	}
	xpos = i;
	ypos = j;
	if (currop == -1) {
		currop = optype;

	}
	if (currop == STRDIFF_SUB) {
		if (optype == STRDIFF_DEL) {
			delbuf += chr;
		} else if (optype == STRDIFF_INS) {
			insbuf += chr;
		} else {
			this->flushBuffers();
			currop = optype;
			noopbuf += chr;
		}
	}
	else if (optype == STRDIFF_DEL) {
		currop = optype;
		delbuf += chr;
	}
	else if (optype == STRDIFF_INS) {
		currop = (currop == STRDIFF_DEL) ? STRDIFF_SUB : STRDIFF_INS;
		insbuf += chr;
	}
	else if (optype == STRDIFF_NOOP) {
		noopbuf += chr;
		this->flushBuffers();
		currop = optype;
	}
}

void XyStrDiff::flushBuffers()
{
	int startpos, len;
	if (currop == STRDIFF_NOOP) {
		//outstr.append(noopbuf);
		noopbuf = "";
	} else if (currop == STRDIFF_SUB) {
		startpos = xpos - delbuf.length()-1;
		len = delbuf.length();
		outstr.append("<r st=\"" + itoa(startpos) + "\" len=\"" + itoa(len) + "\">" + insbuf + "</r>\n");
		delbuf = "";
		insbuf = "";
	} else if (currop == STRDIFF_INS) {
		startpos = xpos - 1;
		outstr.append(""+insbuf+"\n");
		insbuf = "";
	} else if (currop == STRDIFF_DEL) {
		startpos = xpos - delbuf.length() - 1;
		int len = delbuf.length();
		outstr.append("<d st=\"" + itoa(startpos) + "\" len=\"" + itoa(len) + "\">"+delbuf+"</d>\n");
		delbuf = "";
	}
}

std::string itoa (int n)
{
	char * s = new char[17];
	std::string u;

	if (n < 0) {         //turns n positive
		n = (-1 * n);
		u = "-";         //adds '-' on result string
	}

	int i = 0;  // s counter

	do {
		s[i++] = n % 10 + '0'; //conversion of each digit of n to char
		n -= n % 10;           //update n value
	} while ((n /= 10) > 0);

	for (int j = i-1; j >= 0; j--) {
		u += s[j];    //building our string number
	}

	delete[] s;       //free-up the memory!
	return u;
}<|endoftext|>


<commit_before>/*************************************************************************
 *
 *  OpenOffice.org - a multi-platform office productivity suite
 *
 *  $RCSfile: simplecontinuousactivitybase.cxx,v $
 *
 *  $Revision: 1.9 $
 *
 *  last change: $Author: kz $ $Date: 2006-12-13 15:27:30 $
 *
 *  The Contents of this file are made available subject to
 *  the terms of GNU Lesser General Public License Version 2.1.
 *
 *
 *    GNU Lesser General Public License Version 2.1
 *    =============================================
 *    Copyright 2005 by Sun Microsystems, Inc.
 *    901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *    This library is free software; you can redistribute it and/or
 *    modify it under the terms of the GNU Lesser General Public
 *    License version 2.1, as published by the Free Software Foundation.
 *
 *    This library 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 this library; if not, write to the Free Software
 *    Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 *    MA  02111-1307  USA
 *
 ************************************************************************/

// MARKER(update_precomp.py): autogen include statement, do not remove
#include "precompiled_slideshow.hxx"

// must be first
#include <canvas/debug.hxx>
#include <canvas/verbosetrace.hxx>

#include <simplecontinuousactivitybase.hxx>

namespace slideshow
{
    namespace internal
    {
        SimpleContinuousActivityBase::SimpleContinuousActivityBase(
            const ActivityParameters& rParms ) :
            ActivityBase( rParms ),
            maTimer( rParms.mrActivitiesQueue.getTimer() ),
            mnMinSimpleDuration( rParms.mnMinDuration ),
            mnMinNumberOfFrames( rParms.mnMinNumberOfFrames ),
            mnCurrPerformCalls( 0 )
        {
        }

        void SimpleContinuousActivityBase::startAnimation()
        {
            // init timer. We measure animation time only when we're
            // actually started.
            maTimer.reset();
        }

        double SimpleContinuousActivityBase::calcTimeLag() const
        {
            ActivityBase::calcTimeLag();
            if (! isActive())
                return 0.0;

            // retrieve locally elapsed time
            const double nCurrElapsedTime( maTimer.getElapsedTime() );

            // log time
            VERBOSE_TRACE( "SimpleContinuousActivityBase::calcTimeLag(): "
                           "next step is based on time: %f", nCurrElapsedTime );

            // go to great length to ensure a proper animation
            // run. Since we don't know how often we will be called
            // here, try to spread the animator calls uniquely over
            // the [0,1] parameter range. Be aware of the fact that
            // perform will be called at least mnMinNumberOfTurns
            // times.

            // fraction of time elapsed
            const double nFractionElapsedTime(
                nCurrElapsedTime / mnMinSimpleDuration );

            // fraction of minimum calls performed
            const double nFractionRequiredCalls(
                (double)mnCurrPerformCalls / mnMinNumberOfFrames );

            // okay, so now, the decision is easy:
            //
            // If the fraction of time elapsed is smaller than the
            // number of calls required to be performed, then we calc
            // the position on the animation range according to
            // elapsed time. That is, we're so to say ahead of time.
            //
            // In contrary, if the fraction of time elapsed is larger,
            // then we're lagging, and we thus calc the position on
            // the animation time line according to the fraction of
            // calls performed. Thus, the animation is forced to slow
            // down, and take the required minimal number of steps,
            // sufficiently equally distributed across the animation
            // time line.
            if( nFractionElapsedTime < nFractionRequiredCalls )
            {
                VERBOSE_TRACE( "SimpleContinuousActivityBase::calcTimeLag(): "
                               "t=%f is based on time", nFractionElapsedTime );
                return 0.0;
            }
            else
            {
                VERBOSE_TRACE( "SimpleContinuousActivityBase::perform(): "
                               "t=%f is based on number of calls",
                               nFractionRequiredCalls );

                // lag global time, so all other animations lag, too:
                return ((nFractionElapsedTime - nFractionRequiredCalls)
                        * mnMinSimpleDuration);
            }
        }

        bool SimpleContinuousActivityBase::perform()
        {
            // call base class, for start() calls and end handling
            if( !ActivityBase::perform() )
                return false; // done, we're ended

            // get relative animation position
            // ===============================

            const double nCurrElapsedTime( maTimer.getElapsedTime() );
            double nT( nCurrElapsedTime / mnMinSimpleDuration );

            // one of the stop criteria reached?
            // =================================

            // will be set to true below, if one of the termination criteria
            // matched.
            bool bActivityEnding( false );

            if( isRepeatCountValid() )
            {
                // Finite duration
                // ===============

                // When we've autoreverse on, the repeat count
                // doubles
                const double nRepeatCount( getRepeatCount() );
                const double nEffectiveRepeat( isAutoReverse() ?
                                               2.0*nRepeatCount :
                                               nRepeatCount );

                // time (or frame count) elapsed?
                if( nEffectiveRepeat <= nT )
                {
                    // okee. done for now. Will not exit right here,
                    // to give animation the chance to render the last
                    // frame below
                    bActivityEnding = true;

                    // clamp animation to max permissible value
                    nT = nEffectiveRepeat;
                }
            }

            // need to do auto-reverse?
            // ========================

            double nRepeats;
            double nRelativeSimpleTime;

            // TODO(Q3): Refactor this mess
            if( isAutoReverse() )
            {
                // divert active duration into repeat and
                // fractional part.
                const double nFractionalActiveDuration( modf(nT, &nRepeats) );

                // for auto-reverse, map ranges [1,2), [3,4), ...
                // to ranges [0,1), [1,2), etc.
                if( ((int)nRepeats) % 2 )
                {
                    // we're in an odd range, reverse sweep
                    nRelativeSimpleTime = 1.0 - nFractionalActiveDuration;
                }
                else
                {
                    // we're in an even range, pass on as is
                    nRelativeSimpleTime = nFractionalActiveDuration;
                }

                // effective repeat count for autoreverse is half of
                // the input time's value (each run of an autoreverse
                // cycle is half of a repeat)
                nRepeats /= 2;
            }
            else
            {
                // determine repeat
                // ================

                // calc simple time and number of repeats from nT
                // Now, that's easy, since the fractional part of
                // nT gives the relative simple time, and the
                // integer part the number of full repeats:
                nRelativeSimpleTime = modf(nT, &nRepeats);

                // clamp repeats to max permissible value (maRepeats.getValue() - 1.0)
                if( isRepeatCountValid() &&
                    nRepeats >= getRepeatCount() )
                {
                    // Note that this code here only gets
                    // triggered if maRepeats.getValue() is an
                    // _integer_. Otherwise, nRepeats will never
                    // reach nor exceed
                    // maRepeats.getValue(). Thus, the code below
                    // does not need to handle cases of fractional
                    // repeats, and can always assume that a full
                    // animation run has ended (with
                    // nRelativeSimpleTime=1.0 for
                    // non-autoreversed activities).

                    // with modf, nRelativeSimpleTime will never
                    // become 1.0, since nRepeats is incremented and
                    // nRelativeSimpleTime set to 0.0 then.
                    //
                    // For the animation to reach its final value,
                    // nRepeats must although become
                    // maRepeats.getValue()-1.0, and
                    // nRelativeSimpleTime=1.0.
                    nRelativeSimpleTime = 1.0;
                    nRepeats -= 1.0;
                }
            }

            // actually perform something
            // ==========================

            simplePerform( nRelativeSimpleTime,
                           // nRepeats is already integer-valued
                           static_cast<sal_uInt32>( nRepeats ) );

            // delayed endActivity() call from end condition check
            // below. Issued after the simplePerform() call above, to
            // give animations the chance to correctly reach the
            // animation end value, without spurious bail-outs because
            // of isActive() returning false.
            if( bActivityEnding )
                endActivity();

            // one more frame successfully performed
            ++mnCurrPerformCalls;

            return isActive();
        }
    }
}
<commit_msg>INTEGRATION: CWS presfixes12 (1.9.12); FILE MERGED 2007/02/02 08:47:51 thb 1.9.12.1: #i37778# Moved all old-style casts to sal::static_int_cast<commit_after>/*************************************************************************
 *
 *  OpenOffice.org - a multi-platform office productivity suite
 *
 *  $RCSfile: simplecontinuousactivitybase.cxx,v $
 *
 *  $Revision: 1.10 $
 *
 *  last change: $Author: obo $ $Date: 2007-07-17 14:45:59 $
 *
 *  The Contents of this file are made available subject to
 *  the terms of GNU Lesser General Public License Version 2.1.
 *
 *
 *    GNU Lesser General Public License Version 2.1
 *    =============================================
 *    Copyright 2005 by Sun Microsystems, Inc.
 *    901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *    This library is free software; you can redistribute it and/or
 *    modify it under the terms of the GNU Lesser General Public
 *    License version 2.1, as published by the Free Software Foundation.
 *
 *    This library 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 this library; if not, write to the Free Software
 *    Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 *    MA  02111-1307  USA
 *
 ************************************************************************/

// MARKER(update_precomp.py): autogen include statement, do not remove
#include "precompiled_slideshow.hxx"

// must be first
#include <canvas/debug.hxx>
#include <canvas/verbosetrace.hxx>

#include <simplecontinuousactivitybase.hxx>

namespace slideshow
{
    namespace internal
    {
        SimpleContinuousActivityBase::SimpleContinuousActivityBase(
            const ActivityParameters& rParms ) :
            ActivityBase( rParms ),
            maTimer( rParms.mrActivitiesQueue.getTimer() ),
            mnMinSimpleDuration( rParms.mnMinDuration ),
            mnMinNumberOfFrames( rParms.mnMinNumberOfFrames ),
            mnCurrPerformCalls( 0 )
        {
        }

        void SimpleContinuousActivityBase::startAnimation()
        {
            // init timer. We measure animation time only when we're
            // actually started.
            maTimer.reset();
        }

        double SimpleContinuousActivityBase::calcTimeLag() const
        {
            ActivityBase::calcTimeLag();
            if (! isActive())
                return 0.0;

            // retrieve locally elapsed time
            const double nCurrElapsedTime( maTimer.getElapsedTime() );

            // log time
            VERBOSE_TRACE( "SimpleContinuousActivityBase::calcTimeLag(): "
                           "next step is based on time: %f", nCurrElapsedTime );

            // go to great length to ensure a proper animation
            // run. Since we don't know how often we will be called
            // here, try to spread the animator calls uniquely over
            // the [0,1] parameter range. Be aware of the fact that
            // perform will be called at least mnMinNumberOfTurns
            // times.

            // fraction of time elapsed
            const double nFractionElapsedTime(
                nCurrElapsedTime / mnMinSimpleDuration );

            // fraction of minimum calls performed
            const double nFractionRequiredCalls(
                double(mnCurrPerformCalls) / mnMinNumberOfFrames );

            // okay, so now, the decision is easy:
            //
            // If the fraction of time elapsed is smaller than the
            // number of calls required to be performed, then we calc
            // the position on the animation range according to
            // elapsed time. That is, we're so to say ahead of time.
            //
            // In contrary, if the fraction of time elapsed is larger,
            // then we're lagging, and we thus calc the position on
            // the animation time line according to the fraction of
            // calls performed. Thus, the animation is forced to slow
            // down, and take the required minimal number of steps,
            // sufficiently equally distributed across the animation
            // time line.
            if( nFractionElapsedTime < nFractionRequiredCalls )
            {
                VERBOSE_TRACE( "SimpleContinuousActivityBase::calcTimeLag(): "
                               "t=%f is based on time", nFractionElapsedTime );
                return 0.0;
            }
            else
            {
                VERBOSE_TRACE( "SimpleContinuousActivityBase::perform(): "
                               "t=%f is based on number of calls",
                               nFractionRequiredCalls );

                // lag global time, so all other animations lag, too:
                return ((nFractionElapsedTime - nFractionRequiredCalls)
                        * mnMinSimpleDuration);
            }
        }

        bool SimpleContinuousActivityBase::perform()
        {
            // call base class, for start() calls and end handling
            if( !ActivityBase::perform() )
                return false; // done, we're ended

            // get relative animation position
            // ===============================

            const double nCurrElapsedTime( maTimer.getElapsedTime() );
            double nT( nCurrElapsedTime / mnMinSimpleDuration );

            // one of the stop criteria reached?
            // =================================

            // will be set to true below, if one of the termination criteria
            // matched.
            bool bActivityEnding( false );

            if( isRepeatCountValid() )
            {
                // Finite duration
                // ===============

                // When we've autoreverse on, the repeat count
                // doubles
                const double nRepeatCount( getRepeatCount() );
                const double nEffectiveRepeat( isAutoReverse() ?
                                               2.0*nRepeatCount :
                                               nRepeatCount );

                // time (or frame count) elapsed?
                if( nEffectiveRepeat <= nT )
                {
                    // okee. done for now. Will not exit right here,
                    // to give animation the chance to render the last
                    // frame below
                    bActivityEnding = true;

                    // clamp animation to max permissible value
                    nT = nEffectiveRepeat;
                }
            }

            // need to do auto-reverse?
            // ========================

            double nRepeats;
            double nRelativeSimpleTime;

            // TODO(Q3): Refactor this mess
            if( isAutoReverse() )
            {
                // divert active duration into repeat and
                // fractional part.
                const double nFractionalActiveDuration( modf(nT, &nRepeats) );

                // for auto-reverse, map ranges [1,2), [3,4), ...
                // to ranges [0,1), [1,2), etc.
                if( ((int)nRepeats) % 2 )
                {
                    // we're in an odd range, reverse sweep
                    nRelativeSimpleTime = 1.0 - nFractionalActiveDuration;
                }
                else
                {
                    // we're in an even range, pass on as is
                    nRelativeSimpleTime = nFractionalActiveDuration;
                }

                // effective repeat count for autoreverse is half of
                // the input time's value (each run of an autoreverse
                // cycle is half of a repeat)
                nRepeats /= 2;
            }
            else
            {
                // determine repeat
                // ================

                // calc simple time and number of repeats from nT
                // Now, that's easy, since the fractional part of
                // nT gives the relative simple time, and the
                // integer part the number of full repeats:
                nRelativeSimpleTime = modf(nT, &nRepeats);

                // clamp repeats to max permissible value (maRepeats.getValue() - 1.0)
                if( isRepeatCountValid() &&
                    nRepeats >= getRepeatCount() )
                {
                    // Note that this code here only gets
                    // triggered if maRepeats.getValue() is an
                    // _integer_. Otherwise, nRepeats will never
                    // reach nor exceed
                    // maRepeats.getValue(). Thus, the code below
                    // does not need to handle cases of fractional
                    // repeats, and can always assume that a full
                    // animation run has ended (with
                    // nRelativeSimpleTime=1.0 for
                    // non-autoreversed activities).

                    // with modf, nRelativeSimpleTime will never
                    // become 1.0, since nRepeats is incremented and
                    // nRelativeSimpleTime set to 0.0 then.
                    //
                    // For the animation to reach its final value,
                    // nRepeats must although become
                    // maRepeats.getValue()-1.0, and
                    // nRelativeSimpleTime=1.0.
                    nRelativeSimpleTime = 1.0;
                    nRepeats -= 1.0;
                }
            }

            // actually perform something
            // ==========================

            simplePerform( nRelativeSimpleTime,
                           // nRepeats is already integer-valued
                           static_cast<sal_uInt32>( nRepeats ) );

            // delayed endActivity() call from end condition check
            // below. Issued after the simplePerform() call above, to
            // give animations the chance to correctly reach the
            // animation end value, without spurious bail-outs because
            // of isActive() returning false.
            if( bActivityEnding )
                endActivity();

            // one more frame successfully performed
            ++mnCurrPerformCalls;

            return isActive();
        }
    }
}
<|endoftext|>


<commit_before>/*!
 * @file alignment.cpp
 *
 * @brief Alignment class source file
 */

#include <limits>
#include <algorithm>

#include "node.hpp"
#include "edge.hpp"
#include "graph.hpp"
#include "alignment.hpp"

AlignmentParams::AlignmentParams(int16_t m, int16_t mm, int16_t gap_opn,
    int16_t gap_ext, AlignmentType t) :
        match(m), mismatch(mm), insertion_open(gap_opn), insertion_extend(gap_ext),
        deletion_open(gap_opn), deletion_extend(gap_ext), type(t) {
}

AlignmentParams::AlignmentParams(int16_t m, int16_t mm, int16_t ins_opn,
    int16_t ins_ext, int16_t del_opn, int16_t del_ext, AlignmentType t) :
        match(m), mismatch(mm), insertion_open(ins_opn), insertion_extend(ins_ext),
        deletion_open(del_opn), deletion_extend(del_ext), type(t) {
}

AlignmentParams::~AlignmentParams() {
}

Alignment::MatrixElement::MatrixElement(int32_t s, int32_t p_i, int32_t p_j,
    int16_t del, int16_t ins) :
        score(s), prev_i(p_i), prev_j(p_j), deletion_cost(del), insertion_cost(ins) {
}

Alignment::MatrixElement::~MatrixElement() {
}

Alignment::MatrixMove::MatrixMove(int32_t s, int32_t k, int32_t l, int32_t t) :
        score(s), i(k), j(l), type(t) {
}

Alignment::MatrixMove::~MatrixMove() {
}

std::unique_ptr<Alignment> createAlignment(const std::string& sequence,
    GraphSharedPtr graph, AlignmentParams params) {

    return std::unique_ptr<Alignment>(new Alignment(sequence, graph,
        std::move(params)));
}

Alignment::Alignment(const std::string& sequence, GraphSharedPtr graph,
    AlignmentParams params) :
        sequence_(sequence), graph_(graph), params_(std::move(params)) {

    assert(sequence_.size() != 0);

    matrix_width_ = sequence_.size() + 1;
    matrix_height_ = graph_->nodes().size() + 1;

    H_.resize(matrix_width_ * matrix_height_, 0);
    F_.resize(matrix_width_ * matrix_height_, 0);
    E_.resize(matrix_width_ * matrix_height_, 0);

    int32_t big_negative_value = std::numeric_limits<int32_t>::min() + 1000;
    for (uint32_t j = 1; j < matrix_width_; ++j) {
        F_[j] = big_negative_value;
    }
    for (uint32_t i = 1; i < matrix_height_; ++i) {
        E_[i * matrix_width_] = big_negative_value;
    }

    is_aligned_ = false;
    max_i_ = -1;
    max_j_ = -1;
    //max_score_ = params.type == AlignmentType::kNW ? big_negative_value : 0;
    max_score_ = 0;

    graph_->topological_sort();
    const auto& sorted_nodes_ids = graph_->sorted_nodes_ids();

    node_id_to_graph_id_.resize(sorted_nodes_ids.size());
    for (uint32_t i = 0; i < sorted_nodes_ids.size(); ++i) {
        node_id_to_graph_id_[sorted_nodes_ids[i]] = i;
    }

    is_backtracked_ = false;

    if (params_.type == AlignmentType::kNW) {

        for (uint32_t j = 1; j < matrix_width_; ++j) {
            H_[j] = params_.insertion_open + (j - 1) * params_.insertion_extend;
            E_[j] = H_[j];
        }

        for (uint32_t node_id: sorted_nodes_ids) {
            const auto& node = graph_->node(node_id);
            uint32_t i = node_id_to_graph_id_[node_id] + 1;

            if (node->in_edges().size() == 0) {
                H_[i * matrix_width_] = params_.deletion_open;
            } else {
                int32_t max_score = big_negative_value;
                for (const auto& edge: node->in_edges()) {
                    uint32_t pred_i = node_id_to_graph_id_[edge->begin_node_id()] + 1;
                    max_score = std::max(max_score, H_[pred_i * matrix_width_]);
                }
                H_[i * matrix_width_] = max_score + params_.deletion_extend;
            }
            F_[i * matrix_width_] = H_[i * matrix_width_];
        }
    }

    // print_matrix();
}

Alignment::~Alignment() {
}

void Alignment::align_sequence_to_graph() {

    if (is_aligned_ == true) {
        return;
    }

    graph_->topological_sort();
    const auto& sorted_nodes_ids = graph_->sorted_nodes_ids();

    for (uint32_t node_id: sorted_nodes_ids) {
        const auto& node = graph_->node(node_id);
        char graph_letter = node->letter();
        uint32_t i = node_id_to_graph_id_[node_id] + 1;

        int32_t* H_row = &H_[i * matrix_width_];
        int32_t* F_row = &F_[i * matrix_width_];

        uint32_t pred_i = node->in_edges().empty() ? 0 :
            node_id_to_graph_id_[node->in_edges().front()->begin_node_id()] + 1;

        int32_t* H_pred_row = &H_[pred_i * matrix_width_];
        int32_t* F_pred_row = &F_[pred_i * matrix_width_];

        for (uint32_t j = 1; j < matrix_width_; ++j) {
            int32_t match_cost = graph_letter == sequence_[j - 1] ? params_.match : params_.mismatch;
            // update F
            F_row[j] = std::max(H_pred_row[j] + params_.insertion_open, F_pred_row[j] + params_.insertion_extend);
            // update H
            H_row[j] = std::max(H_pred_row[j - 1] + match_cost, F_row[j]);
        }

        // check other predeccessors
        for (uint32_t p = 1; p < node->in_edges().size(); ++p) {
            pred_i = node_id_to_graph_id_[node->in_edges()[p]->begin_node_id()] + 1;

            H_pred_row = &H_[pred_i * matrix_width_];
            F_pred_row = &F_[pred_i * matrix_width_];

            for (uint32_t j = 1; j < matrix_width_; ++j) {
                int32_t match_cost = graph_letter == sequence_[j - 1] ? params_.match : params_.mismatch;
                // update F
                F_row[j] = std::max(F_row[j], std::max(H_pred_row[j] + params_.insertion_open, F_pred_row[j] + params_.insertion_extend));
                // update H
                H_row[j] = std::max(H_row[j], std::max(H_pred_row[j - 1] + match_cost, F_row[j]));
            }
        }

        int32_t* E_row = &E_[i * matrix_width_];

        for (uint32_t j = 1; j < matrix_width_; ++j) {
            // update E
            E_row[j] = std::max(H_row[j - 1] + params_.deletion_open, E_row[j - 1] + params_.deletion_extend);
            // update H
            H_row[j] = std::max(H_row[j], E_row[j]);

            if (params_.type == AlignmentType::kSW) {
                H_row[j] = std::max(H_row[j], 0);
                update_max_score(H_row, i, j);
            } else if (params_.type == AlignmentType::kNW) {
                if (j == matrix_width_ - 1 && node->out_edges().size() == 0) {
                    update_max_score(H_row, i, j);
                }
            } else if (params_.type == AlignmentType::kOV) {
                if (j == matrix_width_ - 1 || node->out_edges().size() == 0) {
                    update_max_score(H_row, i, j);
                }
            }
        }
    }

    is_aligned_ = true;

    // print_matrix();
}

int32_t Alignment::alignment_score() const {
    assert(is_aligned_ == true && "No alignment done!");
    return max_score_;
}

void Alignment::backtrack() {

    if (is_backtracked_ == true) {
        return;
    }
    assert(is_aligned_  == true && "No alignment done!");

    if (max_i_ == -1 && max_j_ == -1) { // no alignment found
        is_backtracked_ = true;
        return;
    }

    uint32_t i = max_i_;
    uint32_t j = max_j_;
    fprintf(stderr, "Score, i, j = %d, %d, %d\n", max_score_, i, j);
    //is_backtracked_ = true;
    //return;

    auto sw_condition = [&]() { return (H_[i * matrix_width_ + j] == 0) ? false : true; };
    auto nw_condition = [&]() { return (i == 0 && j == 0) ? false : true; };
    auto ov_condition = [&]() { return (i == 0 || j == 0) ? false : true; };

    const auto& graph_id_to_node_id = graph_->sorted_nodes_ids();

    uint32_t prev_i = 0, prev_j = 0;

    while ((params_.type == AlignmentType::kSW && sw_condition()) ||
        (params_.type == AlignmentType::kNW && nw_condition()) ||
        (params_.type == AlignmentType::kOV && ov_condition())) {

        // bloody backtrack
        const auto& node = graph_->node(graph_id_to_node_id[i - 1]);

        /*if (H_[i * matrix_width_ + j] == E_[i * matrix_width_ + j]) {
            prev_i = i;
            prev_j = j - 1;
        } else if (H_[i * matrix_width_ + j] == F_[i * matrix_width_ + j]) {
            prev_i = i - 1;
            prev_j = j;
        } else {
            assert(H_[i * matrix_width_ + j] == H_[(i - 1) * matrix_width_ + (j - 1)] + match_cost);
            prev_i = i - 1;
            prev_j = j - 1;
        }*/

        auto H_ij = H_[i * matrix_width_ + j];

        if (H_ij == E_[i * matrix_width_ + j]) {
            prev_i = i;
            prev_j = j - 1;
        } else if (node->in_edges().size() < 2) {
            uint32_t pred_i = node->in_edges().empty() ? 0 :
                node_id_to_graph_id_[node->in_edges()[0]->begin_node_id()] + 1;

            prev_i = pred_i;
            if (H_ij != F_[i * matrix_width_ + j]) {
                prev_j = j - 1;
            }
        } else {
            if (H_ij != F_[i * matrix_width_ + j]) {
                int32_t match_cost = sequence_[j - 1] == node->letter() ?
                    params_.match : params_.mismatch;

                for (const auto& edge: node->in_edges()) {
                    uint32_t pred_i = node_id_to_graph_id_[edge->begin_node_id()] + 1;
                    if (H_ij == H_[pred_i * matrix_width_ + (j - 1)] + match_cost) {
                        prev_i = pred_i;
                        prev_j = j - 1;
                        break;
                    }
                }
            } else {
                for (const auto& edge: node->in_edges()) {
                    uint32_t pred_i = node_id_to_graph_id_[edge->begin_node_id()] + 1;
                    if ((F_[i * matrix_width_ + j] == F_[pred_i * matrix_width_ + j] + params_.insertion_extend) ||
                        (H_ij == H_[pred_i * matrix_width_ + j] + params_.insertion_open)){
                        prev_i = pred_i;
                        prev_j = j;
                        break;
                    }
                }
            }
        }

        // fprintf(stderr, "%d %d\n", prev_i, prev_j);

        alignment_node_ids_.emplace_back(i == prev_i ? -1 : graph_id_to_node_id[i - 1]);
        alignment_seq_ids_.emplace_back(j == prev_j ? -1 : j - 1);

        i = prev_i;
        j = prev_j;
    }

    std::reverse(alignment_node_ids_.begin(), alignment_node_ids_.end());
    std::reverse(alignment_seq_ids_.begin(), alignment_seq_ids_.end());

    is_backtracked_ = true;
}

inline void Alignment::update_max_score(int32_t* H_row, uint32_t i, uint32_t j) {
    if (max_score_ < H_row[j]) {
        max_score_ = H_row[j];
        max_i_ = i;
        max_j_ = j;
    }
}

void Alignment::print_matrix() {
    for (uint32_t i = 0; i < matrix_height_; ++i) {
        for (uint32_t j = 0; j < matrix_width_; ++j) {
            printf("(%3d %3d %3d) ", H_[i * matrix_width_ + j],
                E_[i * matrix_width_ + j], F_[i * matrix_width_ + j]);
        }
        printf("\n");
    }
    printf("\n");
}
<commit_msg>Fixed bug in backtrack<commit_after>/*!
 * @file alignment.cpp
 *
 * @brief Alignment class source file
 */

#include <limits>
#include <algorithm>

#include "node.hpp"
#include "edge.hpp"
#include "graph.hpp"
#include "alignment.hpp"

AlignmentParams::AlignmentParams(int16_t m, int16_t mm, int16_t gap_opn,
    int16_t gap_ext, AlignmentType t) :
        match(m), mismatch(mm), insertion_open(gap_opn), insertion_extend(gap_ext),
        deletion_open(gap_opn), deletion_extend(gap_ext), type(t) {
}

AlignmentParams::AlignmentParams(int16_t m, int16_t mm, int16_t ins_opn,
    int16_t ins_ext, int16_t del_opn, int16_t del_ext, AlignmentType t) :
        match(m), mismatch(mm), insertion_open(ins_opn), insertion_extend(ins_ext),
        deletion_open(del_opn), deletion_extend(del_ext), type(t) {
}

AlignmentParams::~AlignmentParams() {
}

Alignment::MatrixElement::MatrixElement(int32_t s, int32_t p_i, int32_t p_j,
    int16_t del, int16_t ins) :
        score(s), prev_i(p_i), prev_j(p_j), deletion_cost(del), insertion_cost(ins) {
}

Alignment::MatrixElement::~MatrixElement() {
}

Alignment::MatrixMove::MatrixMove(int32_t s, int32_t k, int32_t l, int32_t t) :
        score(s), i(k), j(l), type(t) {
}

Alignment::MatrixMove::~MatrixMove() {
}

std::unique_ptr<Alignment> createAlignment(const std::string& sequence,
    GraphSharedPtr graph, AlignmentParams params) {

    return std::unique_ptr<Alignment>(new Alignment(sequence, graph,
        std::move(params)));
}

Alignment::Alignment(const std::string& sequence, GraphSharedPtr graph,
    AlignmentParams params) :
        sequence_(sequence), graph_(graph), params_(std::move(params)) {

    assert(sequence_.size() != 0);

    matrix_width_ = sequence_.size() + 1;
    matrix_height_ = graph_->nodes().size() + 1;

    H_.resize(matrix_width_ * matrix_height_, 0);
    F_.resize(matrix_width_ * matrix_height_, 0);
    E_.resize(matrix_width_ * matrix_height_, 0);

    int32_t big_negative_value = std::numeric_limits<int32_t>::min() + 1000;
    for (uint32_t j = 1; j < matrix_width_; ++j) {
        F_[j] = big_negative_value;
    }
    for (uint32_t i = 1; i < matrix_height_; ++i) {
        E_[i * matrix_width_] = big_negative_value;
    }

    is_aligned_ = false;
    max_i_ = -1;
    max_j_ = -1;
    //max_score_ = params.type == AlignmentType::kNW ? big_negative_value : 0;
    max_score_ = 0;

    graph_->topological_sort();
    const auto& sorted_nodes_ids = graph_->sorted_nodes_ids();

    node_id_to_graph_id_.resize(sorted_nodes_ids.size());
    for (uint32_t i = 0; i < sorted_nodes_ids.size(); ++i) {
        node_id_to_graph_id_[sorted_nodes_ids[i]] = i;
    }

    is_backtracked_ = false;

    if (params_.type == AlignmentType::kNW) {

        for (uint32_t j = 1; j < matrix_width_; ++j) {
            H_[j] = params_.insertion_open + (j - 1) * params_.insertion_extend;
            E_[j] = H_[j];
        }

        for (uint32_t node_id: sorted_nodes_ids) {
            const auto& node = graph_->node(node_id);
            uint32_t i = node_id_to_graph_id_[node_id] + 1;

            if (node->in_edges().size() == 0) {
                H_[i * matrix_width_] = params_.deletion_open;
            } else {
                int32_t max_score = big_negative_value;
                for (const auto& edge: node->in_edges()) {
                    uint32_t pred_i = node_id_to_graph_id_[edge->begin_node_id()] + 1;
                    max_score = std::max(max_score, H_[pred_i * matrix_width_]);
                }
                H_[i * matrix_width_] = max_score + params_.deletion_extend;
            }
            F_[i * matrix_width_] = H_[i * matrix_width_];
        }
    }

    // print_matrix();
}

Alignment::~Alignment() {
}

void Alignment::align_sequence_to_graph() {

    if (is_aligned_ == true) {
        return;
    }

    graph_->topological_sort();
    const auto& sorted_nodes_ids = graph_->sorted_nodes_ids();

    for (uint32_t node_id: sorted_nodes_ids) {
        const auto& node = graph_->node(node_id);
        char graph_letter = node->letter();
        uint32_t i = node_id_to_graph_id_[node_id] + 1;

        int32_t* H_row = &H_[i * matrix_width_];
        int32_t* F_row = &F_[i * matrix_width_];

        uint32_t pred_i = node->in_edges().empty() ? 0 :
            node_id_to_graph_id_[node->in_edges().front()->begin_node_id()] + 1;

        int32_t* H_pred_row = &H_[pred_i * matrix_width_];
        int32_t* F_pred_row = &F_[pred_i * matrix_width_];

        for (uint32_t j = 1; j < matrix_width_; ++j) {
            int32_t match_cost = graph_letter == sequence_[j - 1] ? params_.match : params_.mismatch;
            // update F
            F_row[j] = std::max(H_pred_row[j] + params_.insertion_open, F_pred_row[j] + params_.insertion_extend);
            // update H
            H_row[j] = std::max(H_pred_row[j - 1] + match_cost, F_row[j]);
        }

        // check other predeccessors
        for (uint32_t p = 1; p < node->in_edges().size(); ++p) {
            pred_i = node_id_to_graph_id_[node->in_edges()[p]->begin_node_id()] + 1;

            H_pred_row = &H_[pred_i * matrix_width_];
            F_pred_row = &F_[pred_i * matrix_width_];

            for (uint32_t j = 1; j < matrix_width_; ++j) {
                int32_t match_cost = graph_letter == sequence_[j - 1] ? params_.match : params_.mismatch;
                // update F
                F_row[j] = std::max(F_row[j], std::max(H_pred_row[j] + params_.insertion_open, F_pred_row[j] + params_.insertion_extend));
                // update H
                H_row[j] = std::max(H_row[j], std::max(H_pred_row[j - 1] + match_cost, F_row[j]));
            }
        }

        int32_t* E_row = &E_[i * matrix_width_];

        for (uint32_t j = 1; j < matrix_width_; ++j) {
            // update E
            E_row[j] = std::max(H_row[j - 1] + params_.deletion_open, E_row[j - 1] + params_.deletion_extend);
            // update H
            H_row[j] = std::max(H_row[j], E_row[j]);

            if (params_.type == AlignmentType::kSW) {
                H_row[j] = std::max(H_row[j], 0);
                update_max_score(H_row, i, j);
            } else if (params_.type == AlignmentType::kNW) {
                if (j == matrix_width_ - 1 && node->out_edges().size() == 0) {
                    update_max_score(H_row, i, j);
                }
            } else if (params_.type == AlignmentType::kOV) {
                if (j == matrix_width_ - 1 || node->out_edges().size() == 0) {
                    update_max_score(H_row, i, j);
                }
            }
        }
    }

    is_aligned_ = true;

    // print_matrix();
}

int32_t Alignment::alignment_score() const {
    assert(is_aligned_ == true && "No alignment done!");
    return max_score_;
}

void Alignment::backtrack() {

    if (is_backtracked_ == true) {
        return;
    }
    assert(is_aligned_  == true && "No alignment done!");

    if (max_i_ == -1 && max_j_ == -1) { // no alignment found
        is_backtracked_ = true;
        return;
    }

    uint32_t i = max_i_;
    uint32_t j = max_j_;
    // fprintf(stderr, "Score, i, j = %d, %d, %d\n", max_score_, i, j);

    auto sw_condition = [&]() { return (H_[i * matrix_width_ + j] == 0) ? false : true; };
    auto nw_condition = [&]() { return (i == 0 && j == 0) ? false : true; };
    auto ov_condition = [&]() { return (i == 0 || j == 0) ? false : true; };

    const auto& graph_id_to_node_id = graph_->sorted_nodes_ids();

    uint32_t prev_i = 0, prev_j = 0;

    while ((params_.type == AlignmentType::kSW && sw_condition()) ||
        (params_.type == AlignmentType::kNW && nw_condition()) ||
        (params_.type == AlignmentType::kOV && ov_condition())) {

        // bloody backtrack
        const auto& node = graph_->node(graph_id_to_node_id[i - 1]);

        auto H_ij = H_[i * matrix_width_ + j];

        if (H_ij == E_[i * matrix_width_ + j]) {
            prev_i = i;
            prev_j = j - 1;
        } else if (node->in_edges().size() < 2) {
            uint32_t pred_i = node->in_edges().empty() ? 0 :
                node_id_to_graph_id_[node->in_edges()[0]->begin_node_id()] + 1;

            prev_i = pred_i;
            if (H_ij != F_[i * matrix_width_ + j]) {
                prev_j = j - 1;
            } else {
                prev_j = j;
            }
        } else {
            if (H_ij != F_[i * matrix_width_ + j]) {
                int32_t match_cost = sequence_[j - 1] == node->letter() ?
                    params_.match : params_.mismatch;

                for (const auto& edge: node->in_edges()) {
                    uint32_t pred_i = node_id_to_graph_id_[edge->begin_node_id()] + 1;
                    if (H_ij == H_[pred_i * matrix_width_ + (j - 1)] + match_cost) {
                        prev_i = pred_i;
                        prev_j = j - 1;
                        break;
                    }
                }
            } else {
                for (const auto& edge: node->in_edges()) {
                    uint32_t pred_i = node_id_to_graph_id_[edge->begin_node_id()] + 1;
                    if ((F_[i * matrix_width_ + j] == F_[pred_i * matrix_width_ + j] + params_.insertion_extend) ||
                        (H_ij == H_[pred_i * matrix_width_ + j] + params_.insertion_open)){
                        prev_i = pred_i;
                        prev_j = j;
                        break;
                    }
                }
            }
        }

        alignment_node_ids_.emplace_back(i == prev_i ? -1 : graph_id_to_node_id[i - 1]);
        alignment_seq_ids_.emplace_back(j == prev_j ? -1 : j - 1);

        i = prev_i;
        j = prev_j;
    }

    std::reverse(alignment_node_ids_.begin(), alignment_node_ids_.end());
    std::reverse(alignment_seq_ids_.begin(), alignment_seq_ids_.end());

    is_backtracked_ = true;
}

inline void Alignment::update_max_score(int32_t* H_row, uint32_t i, uint32_t j) {
    if (max_score_ < H_row[j]) {
        max_score_ = H_row[j];
        max_i_ = i;
        max_j_ = j;
    }
}

void Alignment::print_matrix() {
    for (uint32_t i = 0; i < matrix_height_; ++i) {
        for (uint32_t j = 0; j < matrix_width_; ++j) {
            printf("(%3d %3d %3d) ", H_[i * matrix_width_ + j],
                E_[i * matrix_width_ + j], F_[i * matrix_width_ + j]);
        }
        printf("\n");
    }
    printf("\n");
}
<|endoftext|>


<commit_before>/***********************************************************************

Copyright (c) 2014, Carnegie Mellon University
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are
met:

  Redistributions of source code must retain the above copyright
  notice, this list of conditions and the following disclaimer.

  Redistributions in binary form must reproduce the above copyright
  notice, this list of conditions and the following disclaimer in the
  documentation and/or other materials provided with the distribution.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*************************************************************************/
#include <ros/ros.h>
#include <ros/forwards.h>
#include <ros/single_subscriber_publisher.h>
#include <sensor_msgs/Image.h>
#include <image_transport/image_transport.h>
#include <visualization_msgs/Marker.h>
#include <opencv/cv.h>
#include <opencv/highgui.h>
#include <cv_bridge/cv_bridge.h>

#include <src/TagDetector.h>
#include <src/TagDetection.h>
#include <src/TagFamily.h>

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <visualization_msgs/MarkerArray.h>
#include "yaml-cpp/yaml.h"
#include <sstream>
#include <fstream>

#include <boost/unordered_set.hpp>
#include <boost/unordered_map.hpp>
#include <boost/make_shared.hpp>

#include "apriltags.h"
#include <apriltags/AprilTagDetections.h>

using namespace std;

// Functions

double GetTagSize(int tag_id)
{
    boost::unordered_map<size_t, double>::iterator tag_sizes_it =
        tag_sizes_.find(tag_id);
    if(tag_sizes_it != tag_sizes_.end()) {
        return tag_sizes_it->second;
    } else {
        return default_tag_size_;
    }
}

Eigen::Matrix4d GetDetectionTransform(TagDetection detection)
{
    // Check if fx,fy or cx,cy are not set
    if ((camera_info_.K[0] == 0.0) || (camera_info_.K[4] == 0.0) || (camera_info_.K[2] == 0.0) || (camera_info_.K[5] == 0.0))
    {
        ROS_WARN("Warning: Camera intrinsic matrix K is not set, can't recover 3D pose");
    }

    double tag_size = GetTagSize(detection.id);

    std::vector<cv::Point3f> object_pts;
    std::vector<cv::Point2f> image_pts;
    double tag_radius = tag_size/2.;

    object_pts.push_back(cv::Point3f(-tag_radius, -tag_radius, 0));
    object_pts.push_back(cv::Point3f( tag_radius, -tag_radius, 0));
    object_pts.push_back(cv::Point3f( tag_radius,  tag_radius, 0));
    object_pts.push_back(cv::Point3f(-tag_radius,  tag_radius, 0));

    image_pts.push_back(detection.p[0]);
    image_pts.push_back(detection.p[1]);
    image_pts.push_back(detection.p[2]);
    image_pts.push_back(detection.p[3]);

    cv::Matx33f intrinsics(camera_info_.K[0], 0, camera_info_.K[2],
                           0, camera_info_.K[4], camera_info_.K[5],
                           0, 0, 1);
    cv::Vec4f distortion_coeff(camera_info_.D[0], camera_info_.D[1], camera_info_.D[2], camera_info_.D[3]);

    cv::Mat rvec, tvec;
    cv::solvePnP(object_pts, image_pts, intrinsics, distortion_coeff, rvec, tvec);

    cv::Matx33d r;
    cv::Rodrigues(rvec, r);
    Eigen::Matrix3d rot;
    rot << r(0,0), r(0,1), r(0,2),
           r(1,0), r(1,1), r(1,2),
           r(2,0), r(2,1), r(2,2);

    Eigen::Matrix4d T;
    T.topLeftCorner(3,3) = rot;
    T.col(3).head(3) <<
            tvec.at<double>(0), tvec.at<double>(1), tvec.at<double>(2);
    T.row(3) << 0,0,0,1;

    return T;
}

// Callback for camera info
void InfoCallback(const sensor_msgs::CameraInfoConstPtr& camera_info)
{
    camera_info_ = (*camera_info);
    has_camera_info_ = true;
}

// Callback for image data
void ImageCallback(const sensor_msgs::ImageConstPtr& msg)
{
    if(!has_camera_info_){
        ROS_WARN("No Camera Info Received Yet");
        return;
    }

    // Get the image
    cv_bridge::CvImagePtr subscribed_ptr;
    try
    {
        subscribed_ptr = cv_bridge::toCvCopy(msg, "mono8");
    }
    catch(cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }

    cv::Mat subscribed_gray = subscribed_ptr->image;
    cv::Mat tmp;
    cv::Point2d opticalCenter;

    if ((camera_info_.K[2] > 1.0) && (camera_info_.K[5] > 1.0))
    {
        // cx,cy from intrinsic matric look reasonable, so we'll use that
        opticalCenter = cv::Point2d(camera_info_.K[5], camera_info_.K[2]);
    }
    else
    {
        opticalCenter = cv::Point2d(0.5*subscribed_gray.rows, 0.5*subscribed_gray.cols);
    }

    TagDetectionArray detections;
    detector_->process(subscribed_gray, opticalCenter, detections);
    visualization_msgs::MarkerArray marker_transforms;
    apriltags::AprilTagDetections apriltag_detections;
    apriltag_detections.header.frame_id = msg->header.frame_id;
    apriltag_detections.header.stamp = msg->header.stamp;

    if(viewer_)
    {
        subscribed_gray = family_->superimposeDetections(subscribed_gray,
                                                         detections);
    }
    for(unsigned int i = 0; i < detections.size(); ++i)
    {
        // skip bad detections
        if(!detections[i].good)
        {
            continue;
        }

        Eigen::Matrix4d pose = GetDetectionTransform(detections[i]);

        // Get this info from earlier code, don't extract it again
        Eigen::Matrix3d R = pose.block<3,3>(0,0);
        Eigen::Quaternion<double> q(R);

        double tag_size = GetTagSize(detections[i].id);
        cout << tag_size << " " << detections[i].id << endl;

        visualization_msgs::Marker marker_transform;
        marker_transform.header.frame_id = msg->header.frame_id;
        marker_transform.header.stamp = msg->header.stamp;

        // Only publish marker for 0.5 seconds after it
        // was last seen
        marker_transform.lifetime = ros::Duration(0.5);

        stringstream convert;
        convert << "tag" << detections[i].id;
        marker_transform.ns = convert.str().c_str();
        marker_transform.id = detections[i].id;
        if(display_type_ == "ARROW"){
            marker_transform.type = visualization_msgs::Marker::ARROW;
            marker_transform.scale.x = tag_size; // arrow length
            marker_transform.scale.y = tag_size/10.0; // diameter
            marker_transform.scale.z = tag_size/10.0; // diameter
        }
        else if(display_type_ == "CUBE"){
            marker_transform.type = visualization_msgs::Marker::CUBE;
            marker_transform.scale.x = tag_size;
            marker_transform.scale.y = tag_size;
            marker_transform.scale.z = marker_thickness_;
        }
        marker_transform.action = visualization_msgs::Marker::ADD;
        marker_transform.pose.position.x = pose(0,3);
        marker_transform.pose.position.y = pose(1,3);
        marker_transform.pose.position.z = pose(2,3);
        marker_transform.pose.orientation.x = q.x();
        marker_transform.pose.orientation.y = q.y();
        marker_transform.pose.orientation.z = q.z();
        marker_transform.pose.orientation.w = q.w();

        marker_transform.color.r = 1.0;
        marker_transform.color.g = 0.0;
        marker_transform.color.b = 1.0;
        marker_transform.color.a = 1.0;
        marker_transforms.markers.push_back(marker_transform);

        // Fill in AprilTag detection.
        apriltags::AprilTagDetection apriltag_det;
        apriltag_det.header = marker_transform.header;
        apriltag_det.id = marker_transform.id;
        apriltag_det.tag_size = tag_size;
        apriltag_det.pose = marker_transform.pose;
        const TagDetection &det = detections[i];
        for(uint pt_i = 0; pt_i < 4; ++pt_i)
        {
            geometry_msgs::Point32 img_pt;
            img_pt.x = det.p[pt_i].x;
            img_pt.y = det.p[pt_i].y;
            img_pt.z = 1;
            apriltag_det.corners2d[pt_i] = img_pt;
        }
        apriltag_detections.detections.push_back(apriltag_det);
    }
    marker_publisher_.publish(marker_transforms);
    apriltag_publisher_.publish(apriltag_detections);

    if(publish_detections_image_)
    {
        subscribed_ptr->image = family_->superimposeDetections(subscribed_ptr->image, detections);
        image_publisher_.publish(subscribed_ptr->toImageMsg());
    }

    if(viewer_)
    {
        cv::imshow("AprilTags", subscribed_gray);
    }
}

void ConnectCallback(const ros::SingleSubscriberPublisher& info)
{
    // Check for subscribers.
    uint32_t subscribers = marker_publisher_.getNumSubscribers()
                           + apriltag_publisher_.getNumSubscribers();
    ROS_DEBUG("Subscription detected! (%d subscribers)", subscribers);

    if(subscribers && !running_)
    {
        ROS_DEBUG("New Subscribers, Connecting to Input Image Topic.");
        ros::TransportHints ros_transport_hints(ros::TransportHints().tcpNoDelay());
        image_transport::TransportHints image_transport_hint(image_transport::TransportHints(
                                "raw", ros_transport_hints, (*node_),
                                "image_transport"));

        image_subscriber = (*image_).subscribe(
                DEFAULT_IMAGE_TOPIC, 1, &ImageCallback,
                image_transport_hint);
        info_subscriber = (*node_).subscribe(
                DEFAULT_CAMERA_INFO_TOPIC, 10, &InfoCallback);
        running_ = true;
    }
}

void DisconnectHandler()
{
}

void DisconnectCallback(const ros::SingleSubscriberPublisher& info)
{
    // Check for subscribers.
    uint32_t subscribers = marker_publisher_.getNumSubscribers()
                           + apriltag_publisher_.getNumSubscribers();
    ROS_DEBUG("Unsubscription detected! (%d subscribers)", subscribers);

    if(!subscribers && running_)
    {
        ROS_DEBUG("No Subscribers, Disconnecting from Input Image Topic.");
        image_subscriber.shutdown();
        info_subscriber.shutdown();
        running_ = false;
    }
}

void GetParameterValues()
{
    // Load node-wide configuration values.
    node_->param("viewer", viewer_, false);
    node_->param("publish_detections_image", publish_detections_image_, false);
    node_->param("tag_family", tag_family_name_, DEFAULT_TAG_FAMILY);
    node_->param("default_tag_size", default_tag_size_, DEFAULT_TAG_SIZE);
    node_->param("display_type", display_type_, DEFAULT_DISPLAY_TYPE);
    node_->param("marker_thickness", marker_thickness_, 0.01);

    ROS_INFO("Tag Family: %s", tag_family_name_.c_str());

    // Load tag specific configuration values.
    XmlRpc::XmlRpcValue tag_data;
    node_->param("tag_data", tag_data, tag_data);

    // Iterate through each tag in the configuration.
    XmlRpc::XmlRpcValue::ValueStruct::iterator it;
    for (it = tag_data.begin(); it != tag_data.end(); ++it)
    {
        // Retrieve the settings for the next tag.
        int tag_id = boost::lexical_cast<int>(it->first);
        XmlRpc::XmlRpcValue tag_values = it->second;

        // Load all the settings for this tag.
        if (tag_values.hasMember("size"))
        {
            tag_sizes_[tag_id] = static_cast<double>(tag_values["size"]);
            ROS_DEBUG("Setting tag%d to size %f m.", tag_id, tag_sizes_[tag_id]);
        }
    }
}

void SetupPublisher()
{
    ros::SubscriberStatusCallback connect_callback = &ConnectCallback;
    ros::SubscriberStatusCallback disconnect_callback = &DisconnectCallback;

    // Publisher
    marker_publisher_ = node_->advertise<visualization_msgs::MarkerArray>(
            DEFAULT_MARKER_TOPIC, 1, connect_callback,
            disconnect_callback);
    apriltag_publisher_ = node_->advertise<apriltags::AprilTagDetections>(
            DEFAULT_DETECTIONS_TOPIC, 1, connect_callback, disconnect_callback);

    if(publish_detections_image_)
    {
        image_publisher_ = (*image_).advertise(DEFAULT_DETECTIONS_IMAGE_TOPIC, 1);
    }
}

void InitializeTags()
{
    tag_params.newQuadAlgorithm = 1;
    family_ = new TagFamily(tag_family_name_);
    detector_ = new TagDetector(*family_, tag_params);
}

void InitializeROSNode(int argc, char **argv)
{
    ros::init(argc, argv, "apriltags");
    node_ =  boost::make_shared<ros::NodeHandle>("~");
    image_ = boost::make_shared<image_transport::ImageTransport>(*node_);
}

int main(int argc, char **argv)
{
    InitializeROSNode(argc,argv);
    GetParameterValues();
    SetupPublisher();
    InitializeTags();

    if(viewer_){
        cvNamedWindow("AprilTags");
        cvStartWindowThread();
    }

    ROS_INFO("AprilTags node started.");
    running_ = false;
    has_camera_info_ = false;
    ros::spin();
    ROS_INFO("AprilTags node stopped.");

    //Destroying Stuff
    cvDestroyWindow("AprilTags");
    delete detector_;
    delete family_;

    return EXIT_SUCCESS;
}
<commit_msg>Remove un-used variable<commit_after>/***********************************************************************

Copyright (c) 2014, Carnegie Mellon University
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are
met:

  Redistributions of source code must retain the above copyright
  notice, this list of conditions and the following disclaimer.

  Redistributions in binary form must reproduce the above copyright
  notice, this list of conditions and the following disclaimer in the
  documentation and/or other materials provided with the distribution.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*************************************************************************/
#include <ros/ros.h>
#include <ros/forwards.h>
#include <ros/single_subscriber_publisher.h>
#include <sensor_msgs/Image.h>
#include <image_transport/image_transport.h>
#include <visualization_msgs/Marker.h>
#include <opencv/cv.h>
#include <opencv/highgui.h>
#include <cv_bridge/cv_bridge.h>

#include <src/TagDetector.h>
#include <src/TagDetection.h>
#include <src/TagFamily.h>

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <visualization_msgs/MarkerArray.h>
#include "yaml-cpp/yaml.h"
#include <sstream>
#include <fstream>

#include <boost/unordered_set.hpp>
#include <boost/unordered_map.hpp>
#include <boost/make_shared.hpp>

#include "apriltags.h"
#include <apriltags/AprilTagDetections.h>

using namespace std;

// Functions

double GetTagSize(int tag_id)
{
    boost::unordered_map<size_t, double>::iterator tag_sizes_it =
        tag_sizes_.find(tag_id);
    if(tag_sizes_it != tag_sizes_.end()) {
        return tag_sizes_it->second;
    } else {
        return default_tag_size_;
    }
}

Eigen::Matrix4d GetDetectionTransform(TagDetection detection)
{
    // Check if fx,fy or cx,cy are not set
    if ((camera_info_.K[0] == 0.0) || (camera_info_.K[4] == 0.0) || (camera_info_.K[2] == 0.0) || (camera_info_.K[5] == 0.0))
    {
        ROS_WARN("Warning: Camera intrinsic matrix K is not set, can't recover 3D pose");
    }

    double tag_size = GetTagSize(detection.id);

    std::vector<cv::Point3f> object_pts;
    std::vector<cv::Point2f> image_pts;
    double tag_radius = tag_size/2.;

    object_pts.push_back(cv::Point3f(-tag_radius, -tag_radius, 0));
    object_pts.push_back(cv::Point3f( tag_radius, -tag_radius, 0));
    object_pts.push_back(cv::Point3f( tag_radius,  tag_radius, 0));
    object_pts.push_back(cv::Point3f(-tag_radius,  tag_radius, 0));

    image_pts.push_back(detection.p[0]);
    image_pts.push_back(detection.p[1]);
    image_pts.push_back(detection.p[2]);
    image_pts.push_back(detection.p[3]);

    cv::Matx33f intrinsics(camera_info_.K[0], 0, camera_info_.K[2],
                           0, camera_info_.K[4], camera_info_.K[5],
                           0, 0, 1);
    cv::Vec4f distortion_coeff(camera_info_.D[0], camera_info_.D[1], camera_info_.D[2], camera_info_.D[3]);

    cv::Mat rvec, tvec;
    cv::solvePnP(object_pts, image_pts, intrinsics, distortion_coeff, rvec, tvec);

    cv::Matx33d r;
    cv::Rodrigues(rvec, r);
    Eigen::Matrix3d rot;
    rot << r(0,0), r(0,1), r(0,2),
           r(1,0), r(1,1), r(1,2),
           r(2,0), r(2,1), r(2,2);

    Eigen::Matrix4d T;
    T.topLeftCorner(3,3) = rot;
    T.col(3).head(3) <<
            tvec.at<double>(0), tvec.at<double>(1), tvec.at<double>(2);
    T.row(3) << 0,0,0,1;

    return T;
}

// Callback for camera info
void InfoCallback(const sensor_msgs::CameraInfoConstPtr& camera_info)
{
    camera_info_ = (*camera_info);
    has_camera_info_ = true;
}

// Callback for image data
void ImageCallback(const sensor_msgs::ImageConstPtr& msg)
{
    if(!has_camera_info_){
        ROS_WARN("No Camera Info Received Yet");
        return;
    }

    // Get the image
    cv_bridge::CvImagePtr subscribed_ptr;
    try
    {
        subscribed_ptr = cv_bridge::toCvCopy(msg, "mono8");
    }
    catch(cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }

    cv::Mat subscribed_gray = subscribed_ptr->image;
    cv::Point2d opticalCenter;

    if ((camera_info_.K[2] > 1.0) && (camera_info_.K[5] > 1.0))
    {
        // cx,cy from intrinsic matric look reasonable, so we'll use that
        opticalCenter = cv::Point2d(camera_info_.K[5], camera_info_.K[2]);
    }
    else
    {
        opticalCenter = cv::Point2d(0.5*subscribed_gray.rows, 0.5*subscribed_gray.cols);
    }

    TagDetectionArray detections;
    detector_->process(subscribed_gray, opticalCenter, detections);
    visualization_msgs::MarkerArray marker_transforms;
    apriltags::AprilTagDetections apriltag_detections;
    apriltag_detections.header.frame_id = msg->header.frame_id;
    apriltag_detections.header.stamp = msg->header.stamp;

    if(viewer_)
    {
        subscribed_gray = family_->superimposeDetections(subscribed_gray,
                                                         detections);
    }
    for(unsigned int i = 0; i < detections.size(); ++i)
    {
        // skip bad detections
        if(!detections[i].good)
        {
            continue;
        }

        Eigen::Matrix4d pose = GetDetectionTransform(detections[i]);

        // Get this info from earlier code, don't extract it again
        Eigen::Matrix3d R = pose.block<3,3>(0,0);
        Eigen::Quaternion<double> q(R);

        double tag_size = GetTagSize(detections[i].id);
        cout << tag_size << " " << detections[i].id << endl;

        visualization_msgs::Marker marker_transform;
        marker_transform.header.frame_id = msg->header.frame_id;
        marker_transform.header.stamp = msg->header.stamp;

        // Only publish marker for 0.5 seconds after it
        // was last seen
        marker_transform.lifetime = ros::Duration(0.5);

        stringstream convert;
        convert << "tag" << detections[i].id;
        marker_transform.ns = convert.str().c_str();
        marker_transform.id = detections[i].id;
        if(display_type_ == "ARROW"){
            marker_transform.type = visualization_msgs::Marker::ARROW;
            marker_transform.scale.x = tag_size; // arrow length
            marker_transform.scale.y = tag_size/10.0; // diameter
            marker_transform.scale.z = tag_size/10.0; // diameter
        }
        else if(display_type_ == "CUBE"){
            marker_transform.type = visualization_msgs::Marker::CUBE;
            marker_transform.scale.x = tag_size;
            marker_transform.scale.y = tag_size;
            marker_transform.scale.z = marker_thickness_;
        }
        marker_transform.action = visualization_msgs::Marker::ADD;
        marker_transform.pose.position.x = pose(0,3);
        marker_transform.pose.position.y = pose(1,3);
        marker_transform.pose.position.z = pose(2,3);
        marker_transform.pose.orientation.x = q.x();
        marker_transform.pose.orientation.y = q.y();
        marker_transform.pose.orientation.z = q.z();
        marker_transform.pose.orientation.w = q.w();

        marker_transform.color.r = 1.0;
        marker_transform.color.g = 0.0;
        marker_transform.color.b = 1.0;
        marker_transform.color.a = 1.0;
        marker_transforms.markers.push_back(marker_transform);

        // Fill in AprilTag detection.
        apriltags::AprilTagDetection apriltag_det;
        apriltag_det.header = marker_transform.header;
        apriltag_det.id = marker_transform.id;
        apriltag_det.tag_size = tag_size;
        apriltag_det.pose = marker_transform.pose;
        const TagDetection &det = detections[i];
        for(uint pt_i = 0; pt_i < 4; ++pt_i)
        {
            geometry_msgs::Point32 img_pt;
            img_pt.x = det.p[pt_i].x;
            img_pt.y = det.p[pt_i].y;
            img_pt.z = 1;
            apriltag_det.corners2d[pt_i] = img_pt;
        }
        apriltag_detections.detections.push_back(apriltag_det);
    }
    marker_publisher_.publish(marker_transforms);
    apriltag_publisher_.publish(apriltag_detections);

    if(publish_detections_image_)
    {
        subscribed_ptr->image = family_->superimposeDetections(subscribed_ptr->image, detections);
        image_publisher_.publish(subscribed_ptr->toImageMsg());
    }

    if(viewer_)
    {
        cv::imshow("AprilTags", subscribed_gray);
    }
}

void ConnectCallback(const ros::SingleSubscriberPublisher& info)
{
    // Check for subscribers.
    uint32_t subscribers = marker_publisher_.getNumSubscribers()
                           + apriltag_publisher_.getNumSubscribers();
    ROS_DEBUG("Subscription detected! (%d subscribers)", subscribers);

    if(subscribers && !running_)
    {
        ROS_DEBUG("New Subscribers, Connecting to Input Image Topic.");
        ros::TransportHints ros_transport_hints(ros::TransportHints().tcpNoDelay());
        image_transport::TransportHints image_transport_hint(image_transport::TransportHints(
                                "raw", ros_transport_hints, (*node_),
                                "image_transport"));

        image_subscriber = (*image_).subscribe(
                DEFAULT_IMAGE_TOPIC, 1, &ImageCallback,
                image_transport_hint);
        info_subscriber = (*node_).subscribe(
                DEFAULT_CAMERA_INFO_TOPIC, 10, &InfoCallback);
        running_ = true;
    }
}

void DisconnectHandler()
{
}

void DisconnectCallback(const ros::SingleSubscriberPublisher& info)
{
    // Check for subscribers.
    uint32_t subscribers = marker_publisher_.getNumSubscribers()
                           + apriltag_publisher_.getNumSubscribers();
    ROS_DEBUG("Unsubscription detected! (%d subscribers)", subscribers);

    if(!subscribers && running_)
    {
        ROS_DEBUG("No Subscribers, Disconnecting from Input Image Topic.");
        image_subscriber.shutdown();
        info_subscriber.shutdown();
        running_ = false;
    }
}

void GetParameterValues()
{
    // Load node-wide configuration values.
    node_->param("viewer", viewer_, false);
    node_->param("publish_detections_image", publish_detections_image_, false);
    node_->param("tag_family", tag_family_name_, DEFAULT_TAG_FAMILY);
    node_->param("default_tag_size", default_tag_size_, DEFAULT_TAG_SIZE);
    node_->param("display_type", display_type_, DEFAULT_DISPLAY_TYPE);
    node_->param("marker_thickness", marker_thickness_, 0.01);

    ROS_INFO("Tag Family: %s", tag_family_name_.c_str());

    // Load tag specific configuration values.
    XmlRpc::XmlRpcValue tag_data;
    node_->param("tag_data", tag_data, tag_data);

    // Iterate through each tag in the configuration.
    XmlRpc::XmlRpcValue::ValueStruct::iterator it;
    for (it = tag_data.begin(); it != tag_data.end(); ++it)
    {
        // Retrieve the settings for the next tag.
        int tag_id = boost::lexical_cast<int>(it->first);
        XmlRpc::XmlRpcValue tag_values = it->second;

        // Load all the settings for this tag.
        if (tag_values.hasMember("size"))
        {
            tag_sizes_[tag_id] = static_cast<double>(tag_values["size"]);
            ROS_DEBUG("Setting tag%d to size %f m.", tag_id, tag_sizes_[tag_id]);
        }
    }
}

void SetupPublisher()
{
    ros::SubscriberStatusCallback connect_callback = &ConnectCallback;
    ros::SubscriberStatusCallback disconnect_callback = &DisconnectCallback;

    // Publisher
    marker_publisher_ = node_->advertise<visualization_msgs::MarkerArray>(
            DEFAULT_MARKER_TOPIC, 1, connect_callback,
            disconnect_callback);
    apriltag_publisher_ = node_->advertise<apriltags::AprilTagDetections>(
            DEFAULT_DETECTIONS_TOPIC, 1, connect_callback, disconnect_callback);

    if(publish_detections_image_)
    {
        image_publisher_ = (*image_).advertise(DEFAULT_DETECTIONS_IMAGE_TOPIC, 1);
    }
}

void InitializeTags()
{
    tag_params.newQuadAlgorithm = 1;
    family_ = new TagFamily(tag_family_name_);
    detector_ = new TagDetector(*family_, tag_params);
}

void InitializeROSNode(int argc, char **argv)
{
    ros::init(argc, argv, "apriltags");
    node_ =  boost::make_shared<ros::NodeHandle>("~");
    image_ = boost::make_shared<image_transport::ImageTransport>(*node_);
}

int main(int argc, char **argv)
{
    InitializeROSNode(argc,argv);
    GetParameterValues();
    SetupPublisher();
    InitializeTags();

    if(viewer_){
        cvNamedWindow("AprilTags");
        cvStartWindowThread();
    }

    ROS_INFO("AprilTags node started.");
    running_ = false;
    has_camera_info_ = false;
    ros::spin();
    ROS_INFO("AprilTags node stopped.");

    //Destroying Stuff
    cvDestroyWindow("AprilTags");
    delete detector_;
    delete family_;

    return EXIT_SUCCESS;
}
<|endoftext|>