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

  Program:   Visualization Toolkit
  Module:    vtkUnstructuredGridHomogeneousRayIntegrator.cxx

  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notice for more information.

=========================================================================*/

/*
 * Copyright 2004 Sandia Corporation.
 * Under the terms of Contract DE-AC04-94AL85000, there is a non-exclusive
 * license for use of this work by or on behalf of the
 * U.S. Government. Redistribution and use in source and binary forms, with
 * or without modification, are permitted provided that this Notice and any
 * statement of authorship are reproduced on all copies.
 */

#include "vtkUnstructuredGridHomogeneousRayIntegrator.h"

#include "vtkObjectFactory.h"
#include "vtkVolumeProperty.h"
#include "vtkVolume.h"
#include "vtkAbstractVolumeMapper.h"
#include "vtkUnstructuredGrid.h"
#include "vtkDoubleArray.h"
#include "vtkColorTransferFunction.h"
#include "vtkPiecewiseFunction.h"

#include <math.h>

//-----------------------------------------------------------------------------

vtkCxxRevisionMacro(vtkUnstructuredGridHomogeneousRayIntegrator, "1.1");
vtkStandardNewMacro(vtkUnstructuredGridHomogeneousRayIntegrator);

//-----------------------------------------------------------------------------

vtkUnstructuredGridHomogeneousRayIntegrator::vtkUnstructuredGridHomogeneousRayIntegrator()
{
  this->Property = NULL;

  this->NumComponents = 0;
  this->ColorTable = NULL;
  this->AttenuationTable = NULL;
  this->TableShift = NULL;
  this->TableScale = NULL;

  this->UseAverageColor = 0;
  this->TransferFunctionTableSize = 1024;
}

vtkUnstructuredGridHomogeneousRayIntegrator::~vtkUnstructuredGridHomogeneousRayIntegrator()
{
  for (int i = 0; i < this->NumComponents; i++)
    {
    delete[] this->ColorTable[i];
    delete[] this->AttenuationTable[i];
    }
  delete[] this->ColorTable;
  delete[] this->AttenuationTable;
  delete[] this->TableShift;
  delete[] this->TableScale;
}

void vtkUnstructuredGridHomogeneousRayIntegrator::PrintSelf(ostream &os, vtkIndent indent)
{
  this->Superclass::PrintSelf(os, indent);

  os << indent << "UseAverageColor: " << this->UseAverageColor << endl;
  os << indent << "TransferFunctionTableSize: "
     << this->TransferFunctionTableSize << endl;
}

//-----------------------------------------------------------------------------

void vtkUnstructuredGridHomogeneousRayIntegrator::GetTransferFunctionTables(vtkDataArray *scalars)
{
  for (int i = 0; i < this->NumComponents; i++)
    {
    delete[] this->ColorTable[i];
    delete[] this->AttenuationTable[i];
    }
  delete[] this->ColorTable;
  delete[] this->AttenuationTable;
  delete[] this->TableShift;
  delete[] this->TableScale;

  this->NumComponents = scalars->GetNumberOfComponents();

  this->ColorTable = new float*[this->NumComponents];
  this->AttenuationTable = new float*[this->NumComponents];
  this->TableShift = new double[this->NumComponents];
  this->TableScale = new double[this->NumComponents];

  for (int c = 0; c < this->NumComponents; c++)
    {
    double range[2];
    scalars->GetRange(range, c);
    if (range[0] >= range[1])
      {
      range[1] = range[0] + 1;
      }
    this->TableScale[c] = this->TransferFunctionTableSize/(range[1]-range[0]);
    this->TableShift[c]
      = -range[0]*this->TransferFunctionTableSize/(range[1]-range[0]);

    this->ColorTable[c] = new float[3*this->TransferFunctionTableSize];
    if (this->Property->GetColorChannels(c) == 1)
      {
      // Get gray values.  Store temporarily in allocated RGB array.
      this->Property->GetGrayTransferFunction(c)
        ->GetTable(range[0], range[1], this->TransferFunctionTableSize,
                   this->ColorTable[c]);
      // Convert gray into RGB.  Copy backward so that we can use the same
      // array.
      for (int i = this->TransferFunctionTableSize-1; i >= 0; i--)
        {
        this->ColorTable[c][3*i + 0]
          = this->ColorTable[c][3*i + 1]
          = this->ColorTable[c][3*i + 2] = this->ColorTable[c][i];
        }
      }
    else
      {
      this->Property->GetRGBTransferFunction(c)
        ->GetTable(range[0], range[1], this->TransferFunctionTableSize,
                   this->ColorTable[c]);
      }

    this->AttenuationTable[c] = new float[this->TransferFunctionTableSize];
    this->Property->GetScalarOpacity(c)
      ->GetTable(range[0], range[1], this->TransferFunctionTableSize,
                 this->AttenuationTable[c]);

    // Adjust attenuation by scalar unit length.  This will make the unit
    // lenth the same as the model.
    float unitlength = this->Property->GetScalarOpacityUnitDistance(c);
    for (int i = 0; i < this->TransferFunctionTableSize; i++)
      {
      this->AttenuationTable[c][i] /= unitlength;
      }
    }

  this->TablesBuilt.Modified();
}

//-----------------------------------------------------------------------------

void vtkUnstructuredGridHomogeneousRayIntegrator::Initialize(vtkVolume *volume,
                                                   vtkDataArray *scalars)
{
  vtkVolumeProperty *property = volume->GetProperty();

  if (   (property == this->Property)
      && (this->TablesBuilt > property->GetMTime())
      && (this->TablesBuilt > this->MTime) )
    {
    // Nothing changed from the last time Initialize was run.
    return;
    }

  this->Property = property;
  this->Volume = volume;

  if (property->GetIndependentComponents())
    {
    this->GetTransferFunctionTables(scalars);
    }
}

//-----------------------------------------------------------------------------

void vtkUnstructuredGridHomogeneousRayIntegrator::Integrate(
                                     vtkDoubleArray *intersectionLengths,
                                     vtkDataArray *nearIntersections,
                                     vtkDataArray *vtkNotUsed(farIntersections),
                                     float color[4])
{
  vtkIdType numIntersections = intersectionLengths->GetNumberOfTuples();

  if (this->Property->GetIndependentComponents())
    {
    if (this->NumComponents == 1)
      {
      // Optimize for what I think is one of the most common uses.
      for (vtkIdType i = 0; i < numIntersections; i++)
        {
        int table_index
          = (int)(  this->TableScale[0]*nearIntersections->GetComponent(i, 0)
                  + this->TableShift[0] );
        if (table_index < 0)
          {
          table_index = 0;
          }
        if (table_index >= this->TransferFunctionTableSize)
          {
          table_index = this->TransferFunctionTableSize-1;
          }
        float *c = this->ColorTable[0] + 3*table_index;
        float tau = this->AttenuationTable[0][table_index];
        float alpha = 1-(float)exp(-intersectionLengths->GetComponent(i,0)*tau);
        color[0] += c[0]*alpha*(1-color[3]);
        color[1] += c[1]*alpha*(1-color[3]);
        color[2] += c[2]*alpha*(1-color[3]);
        color[3] += alpha*(1-color[3]);
        }
      }
    else
      {
      // Generic case.
      for (vtkIdType i = 0; i < numIntersections; i++)
        {
        float newcolor[4];
        int table_index
          = (int)(  this->TableScale[0]*nearIntersections->GetComponent(i, 0)
                  + this->TableShift[0] );
        if (table_index < 0)
          {
          table_index = 0;
          }
        if (table_index >= this->TransferFunctionTableSize)
          {
          table_index = this->TransferFunctionTableSize-1;
          }
        float *c = this->ColorTable[0] + 3*table_index;
        float tau = this->AttenuationTable[0][table_index];
        newcolor[0] = c[0];  newcolor[1] = c[1];
        newcolor[2] = c[2];  newcolor[3] = tau;
        for (int component = 1; component < this->NumComponents; component++)
          {
          table_index
            = (int)(  this->TableScale[component]
                      *nearIntersections->GetComponent(i, component)
                    + this->TableShift[component] );
          if (table_index < 0)
            {
            table_index = 0;
            }
          if (table_index >= this->TransferFunctionTableSize)
            {
            table_index = this->TransferFunctionTableSize-1;
            }
          c = this->ColorTable[component] + 3*table_index;
          tau = this->AttenuationTable[component][table_index];
          // Here we handle the mixing of material properties.  This never
          // seems to be defined very clearly.  I handle this by assuming
          // that each scalar represents a cloud of particles of a certian
          // color and a certain density.  We mix the scalars in the same
          // way as mixing these particles together.  By necessity, the
          // density becomes greater.  The "opacity" parameter is really
          // interpreted as the attenuation coefficient (which is
          // proportional to density) and can therefore easily be greater
          // than one.  The opacity of the resulting color will, however,
          // always be scaled between 0 and 1.
          if (tau + newcolor[3] > 1.0e-8f)
            {
            newcolor[0] *= newcolor[3]/(tau + newcolor[3]);
            newcolor[1] *= newcolor[3]/(tau + newcolor[3]);
            newcolor[2] *= newcolor[3]/(tau + newcolor[3]);
            newcolor[0] += c[0]*tau/(tau + newcolor[3]);
            newcolor[1] += c[1]*tau/(tau + newcolor[3]);
            newcolor[2] += c[2]*tau/(tau + newcolor[3]);
            newcolor[3] += c[3]*tau/(tau + newcolor[3]);
            }
          }
        float alpha = 1 - (float)exp(-intersectionLengths->GetComponent(i,0)
                                     *newcolor[3]);
        color[0] += newcolor[0]*alpha*(1-color[3]);
        color[1] += newcolor[1]*alpha*(1-color[3]);
        color[2] += newcolor[2]*alpha*(1-color[3]);
        color[3] += alpha*(1-color[3]);
        }
      }
    }
  else
    {
    int numComponents = nearIntersections->GetNumberOfComponents();
    for (vtkIdType i = 0; i < numIntersections; i++)
      {
      double c[4];
      if (numComponents == 4)
        {
        nearIntersections->GetTuple(i, c);
        }
      else
        {
        double *lt = nearIntersections->GetTuple(i);
        c[0] = c[1] = c[2] = lt[0];
        c[3] = lt[1];
        }
      float alpha = 1-(float)exp(-intersectionLengths->GetComponent(i,0)*c[3]);
      color[0] += (float)c[0]*alpha*(1-color[3]);
      color[1] += (float)c[1]*alpha*(1-color[3]);
      color[2] += (float)c[2]*alpha*(1-color[3]);
      color[3] += alpha*(1-color[3]);
      }
    }
}

<commit_msg>BUG: Colors in multiple transfer functions not mixed right.<commit_after>/*=========================================================================

  Program:   Visualization Toolkit
  Module:    vtkUnstructuredGridHomogeneousRayIntegrator.cxx

  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notice for more information.

=========================================================================*/

/*
 * Copyright 2004 Sandia Corporation.
 * Under the terms of Contract DE-AC04-94AL85000, there is a non-exclusive
 * license for use of this work by or on behalf of the
 * U.S. Government. Redistribution and use in source and binary forms, with
 * or without modification, are permitted provided that this Notice and any
 * statement of authorship are reproduced on all copies.
 */

#include "vtkUnstructuredGridHomogeneousRayIntegrator.h"

#include "vtkObjectFactory.h"
#include "vtkVolumeProperty.h"
#include "vtkVolume.h"
#include "vtkAbstractVolumeMapper.h"
#include "vtkUnstructuredGrid.h"
#include "vtkDoubleArray.h"
#include "vtkColorTransferFunction.h"
#include "vtkPiecewiseFunction.h"

#include <math.h>

//-----------------------------------------------------------------------------

vtkCxxRevisionMacro(vtkUnstructuredGridHomogeneousRayIntegrator, "1.2");
vtkStandardNewMacro(vtkUnstructuredGridHomogeneousRayIntegrator);

//-----------------------------------------------------------------------------

vtkUnstructuredGridHomogeneousRayIntegrator::vtkUnstructuredGridHomogeneousRayIntegrator()
{
  this->Property = NULL;

  this->NumComponents = 0;
  this->ColorTable = NULL;
  this->AttenuationTable = NULL;
  this->TableShift = NULL;
  this->TableScale = NULL;

  this->UseAverageColor = 0;
  this->TransferFunctionTableSize = 1024;
}

vtkUnstructuredGridHomogeneousRayIntegrator::~vtkUnstructuredGridHomogeneousRayIntegrator()
{
  for (int i = 0; i < this->NumComponents; i++)
    {
    delete[] this->ColorTable[i];
    delete[] this->AttenuationTable[i];
    }
  delete[] this->ColorTable;
  delete[] this->AttenuationTable;
  delete[] this->TableShift;
  delete[] this->TableScale;
}

void vtkUnstructuredGridHomogeneousRayIntegrator::PrintSelf(ostream &os, vtkIndent indent)
{
  this->Superclass::PrintSelf(os, indent);

  os << indent << "UseAverageColor: " << this->UseAverageColor << endl;
  os << indent << "TransferFunctionTableSize: "
     << this->TransferFunctionTableSize << endl;
}

//-----------------------------------------------------------------------------

void vtkUnstructuredGridHomogeneousRayIntegrator::GetTransferFunctionTables(vtkDataArray *scalars)
{
  for (int i = 0; i < this->NumComponents; i++)
    {
    delete[] this->ColorTable[i];
    delete[] this->AttenuationTable[i];
    }
  delete[] this->ColorTable;
  delete[] this->AttenuationTable;
  delete[] this->TableShift;
  delete[] this->TableScale;

  this->NumComponents = scalars->GetNumberOfComponents();

  this->ColorTable = new float*[this->NumComponents];
  this->AttenuationTable = new float*[this->NumComponents];
  this->TableShift = new double[this->NumComponents];
  this->TableScale = new double[this->NumComponents];

  for (int c = 0; c < this->NumComponents; c++)
    {
    double range[2];
    scalars->GetRange(range, c);
    if (range[0] >= range[1])
      {
      range[1] = range[0] + 1;
      }
    this->TableScale[c] = this->TransferFunctionTableSize/(range[1]-range[0]);
    this->TableShift[c]
      = -range[0]*this->TransferFunctionTableSize/(range[1]-range[0]);

    this->ColorTable[c] = new float[3*this->TransferFunctionTableSize];
    if (this->Property->GetColorChannels(c) == 1)
      {
      // Get gray values.  Store temporarily in allocated RGB array.
      this->Property->GetGrayTransferFunction(c)
        ->GetTable(range[0], range[1], this->TransferFunctionTableSize,
                   this->ColorTable[c]);
      // Convert gray into RGB.  Copy backward so that we can use the same
      // array.
      for (int i = this->TransferFunctionTableSize-1; i >= 0; i--)
        {
        this->ColorTable[c][3*i + 0]
          = this->ColorTable[c][3*i + 1]
          = this->ColorTable[c][3*i + 2] = this->ColorTable[c][i];
        }
      }
    else
      {
      this->Property->GetRGBTransferFunction(c)
        ->GetTable(range[0], range[1], this->TransferFunctionTableSize,
                   this->ColorTable[c]);
      }

    this->AttenuationTable[c] = new float[this->TransferFunctionTableSize];
    this->Property->GetScalarOpacity(c)
      ->GetTable(range[0], range[1], this->TransferFunctionTableSize,
                 this->AttenuationTable[c]);

    // Adjust attenuation by scalar unit length.  This will make the unit
    // lenth the same as the model.
    float unitlength = this->Property->GetScalarOpacityUnitDistance(c);
    for (int i = 0; i < this->TransferFunctionTableSize; i++)
      {
      this->AttenuationTable[c][i] /= unitlength;
      }
    }

  this->TablesBuilt.Modified();
}

//-----------------------------------------------------------------------------

void vtkUnstructuredGridHomogeneousRayIntegrator::Initialize(vtkVolume *volume,
                                                   vtkDataArray *scalars)
{
  vtkVolumeProperty *property = volume->GetProperty();

  if (   (property == this->Property)
      && (this->TablesBuilt > property->GetMTime())
      && (this->TablesBuilt > this->MTime) )
    {
    // Nothing changed from the last time Initialize was run.
    return;
    }

  this->Property = property;
  this->Volume = volume;

  if (property->GetIndependentComponents())
    {
    this->GetTransferFunctionTables(scalars);
    }
}

//-----------------------------------------------------------------------------

void vtkUnstructuredGridHomogeneousRayIntegrator::Integrate(
                                     vtkDoubleArray *intersectionLengths,
                                     vtkDataArray *nearIntersections,
                                     vtkDataArray *vtkNotUsed(farIntersections),
                                     float color[4])
{
  vtkIdType numIntersections = intersectionLengths->GetNumberOfTuples();

  if (this->Property->GetIndependentComponents())
    {
    if (this->NumComponents == 1)
      {
      // Optimize for what I think is one of the most common uses.
      for (vtkIdType i = 0; i < numIntersections; i++)
        {
        int table_index
          = (int)(  this->TableScale[0]*nearIntersections->GetComponent(i, 0)
                  + this->TableShift[0] );
        if (table_index < 0)
          {
          table_index = 0;
          }
        if (table_index >= this->TransferFunctionTableSize)
          {
          table_index = this->TransferFunctionTableSize-1;
          }
        float *c = this->ColorTable[0] + 3*table_index;
        float tau = this->AttenuationTable[0][table_index];
        float alpha = 1-(float)exp(-intersectionLengths->GetComponent(i,0)*tau);
        color[0] += c[0]*alpha*(1-color[3]);
        color[1] += c[1]*alpha*(1-color[3]);
        color[2] += c[2]*alpha*(1-color[3]);
        color[3] += alpha*(1-color[3]);
        }
      }
    else
      {
      // Generic case.
      for (vtkIdType i = 0; i < numIntersections; i++)
        {
        float newcolor[4];
        int table_index
          = (int)(  this->TableScale[0]*nearIntersections->GetComponent(i, 0)
                  + this->TableShift[0] );
        if (table_index < 0)
          {
          table_index = 0;
          }
        if (table_index >= this->TransferFunctionTableSize)
          {
          table_index = this->TransferFunctionTableSize-1;
          }
        float *c = this->ColorTable[0] + 3*table_index;
        float tau = this->AttenuationTable[0][table_index];
        newcolor[0] = c[0];  newcolor[1] = c[1];
        newcolor[2] = c[2];  newcolor[3] = tau;
        for (int component = 1; component < this->NumComponents; component++)
          {
          table_index
            = (int)(  this->TableScale[component]
                      *nearIntersections->GetComponent(i, component)
                    + this->TableShift[component] );
          if (table_index < 0)
            {
            table_index = 0;
            }
          if (table_index >= this->TransferFunctionTableSize)
            {
            table_index = this->TransferFunctionTableSize-1;
            }
          c = this->ColorTable[component] + 3*table_index;
          tau = this->AttenuationTable[component][table_index];
          // Here we handle the mixing of material properties.  This never
          // seems to be defined very clearly.  I handle this by assuming
          // that each scalar represents a cloud of particles of a certian
          // color and a certain density.  We mix the scalars in the same
          // way as mixing these particles together.  By necessity, the
          // density becomes greater.  The "opacity" parameter is really
          // interpreted as the attenuation coefficient (which is
          // proportional to density) and can therefore easily be greater
          // than one.  The opacity of the resulting color will, however,
          // always be scaled between 0 and 1.
          if (tau + newcolor[3] > 1.0e-8f)
            {
            newcolor[0] *= newcolor[3]/(tau + newcolor[3]);
            newcolor[1] *= newcolor[3]/(tau + newcolor[3]);
            newcolor[2] *= newcolor[3]/(tau + newcolor[3]);
            newcolor[0] += c[0]*tau/(tau + newcolor[3]);
            newcolor[1] += c[1]*tau/(tau + newcolor[3]);
            newcolor[2] += c[2]*tau/(tau + newcolor[3]);
            newcolor[3] += tau;
            }
          }
        float alpha = 1 - (float)exp(-intersectionLengths->GetComponent(i,0)
                                     *newcolor[3]);
        color[0] += newcolor[0]*alpha*(1-color[3]);
        color[1] += newcolor[1]*alpha*(1-color[3]);
        color[2] += newcolor[2]*alpha*(1-color[3]);
        color[3] += alpha*(1-color[3]);
        }
      }
    }
  else
    {
    int numComponents = nearIntersections->GetNumberOfComponents();
    for (vtkIdType i = 0; i < numIntersections; i++)
      {
      double c[4];
      if (numComponents == 4)
        {
        nearIntersections->GetTuple(i, c);
        }
      else
        {
        double *lt = nearIntersections->GetTuple(i);
        c[0] = c[1] = c[2] = lt[0];
        c[3] = lt[1];
        }
      float alpha = 1-(float)exp(-intersectionLengths->GetComponent(i,0)*c[3]);
      color[0] += (float)c[0]*alpha*(1-color[3]);
      color[1] += (float)c[1]*alpha*(1-color[3]);
      color[2] += (float)c[2]*alpha*(1-color[3]);
      color[3] += alpha*(1-color[3]);
      }
    }
}

<|endoftext|>


<commit_before>/*
   Tc 1: CSpin 9
   Tc 2: CSpin 8
   Tc 3: CSpin 7
   Tc Heat: CSpin 6
   H_en: pin 5
*/

#include <SPI.h>
#include "RFM69.h"
#include "Adafruit_MAX31855.h"

#define NODE_ID         2
#define NETWORK_ID      105
#define GATEWAY_ID      0
#define FREQUENCY       RF69_433MHZ
#define KEY             "p6ZNvTmGfdY2hUXb" //has to be same 16 characters/bytes on all nodes, not more not less!
#define ACK_TIME        100  // # of ms to wait for an ack
#define NB_ATTEMPTS_ACK 5 //number of attempts to try before giving up

#define Tc1_CS 9
#define Tc2_CS 8
#define Tc3_CS 7
#define TcH_CS 6
#define H_en 5

void sendPacket();
void printPacket();

Adafruit_MAX31855 Tc1(Tc1_CS);
Adafruit_MAX31855 Tc2(Tc2_CS);
Adafruit_MAX31855 Tc3(Tc3_CS);
Adafruit_MAX31855 TcH(TcH_CS);
RFM69 radio;

long TRANSMIT_PERIOD = 30000; //transmit a packet to gateway so often (in ms)
int TRANSMIT_PERIOD_MINUTES = 1;
int ACK_FAIL_WAIT_PERIOD = 500;

long currentTime;
long h_saved_time;
long h_interval;
const long h_pulse_on = 6000;
const long h_pulse_off = 30 * 60000;
//const long h_pulse_off = 60000;
int h_status = 1;

long log_saved_time;
int log_interval = 6000;

typedef struct {
  int node_id;
  float Tc1_t;
  float Tc2_t;
  float Tc3_t;
  float TcH_t;
}
Payload;
Payload payload;

void setup() {
  Serial.begin(9600);
  pinMode(H_en, OUTPUT);
  digitalWrite(H_en, LOW);
  payload.node_id = NODE_ID;
  radio.initialize(FREQUENCY, NODE_ID, NETWORK_ID); //Begin Radio
  radio.setHighPower(); //uncomment only for RFM69HW!
  radio.encrypt(KEY); //Encrypt
  radio.sleep(); //Sleep radio (saves power)
  char buff[50];
  sprintf(buff, "Listening at %d Mhz...", FREQUENCY == RF69_433MHZ ? 433 : FREQUENCY == RF69_868MHZ ? 868 : 915);
}

void loop() {
  currentTime = millis();
  if (h_saved_time + h_interval < currentTime) {
    //if heater is on, wait 6 seconds
    if (h_status == 1) {
      digitalWrite(H_en, HIGH);
      h_interval = h_pulse_on;
      h_status = 0; //set status for next round.
      Serial.print("Heat interval ");
      Serial.print(h_pulse_on / 1000);
      Serial.println("s");
    }
    else if (h_status == 0) {
      digitalWrite(H_en, LOW);
      h_interval = h_pulse_off;
      h_status = 1; //set status for next round
      Serial.print("Heat interval ");
      Serial.print(h_pulse_off / 1000);
      Serial.println("s");
    }
    h_saved_time = currentTime;
  }
  if (log_saved_time + log_interval < currentTime) {
    payload.Tc1_t = Tc1.readCelsius();
    payload.Tc2_t = Tc2.readCelsius();
    payload.Tc3_t = Tc3.readCelsius();
    payload.TcH_t = TcH.readCelsius();
    sendPacket();
    printPacket();
    log_saved_time = currentTime;
  }
}

void sendPacket() {
  int nAttempt = 0;
  bool flag_ACK_received = false;
  while (nAttempt < NB_ATTEMPTS_ACK && !flag_ACK_received) {
    if (radio.sendWithRetry(GATEWAY_ID, (const void*)(&payload), sizeof(payload))) {
      flag_ACK_received = true;
    }
    else {
      if (radio.sendWithRetry(GATEWAY_ID, (const void*)(&payload), sizeof(payload))) {
        flag_ACK_received = true;
      }
      nAttempt++;
      ACK_FAIL_WAIT_PERIOD = random(300, 600);
      delay(ACK_FAIL_WAIT_PERIOD);
    }
  }
}
void printPacket() {
  Serial.print(payload.Tc1_t);
  Serial.print(", ");
  Serial.print(payload.Tc2_t);
  Serial.print(", ");
  Serial.print(payload.Tc3_t);
  Serial.print(", ");
  Serial.print(payload.TcH_t);
  Serial.println();
}
<commit_msg>testing git plugin<commit_after>/*
   Tc 1: CSpin 9
   Tc 2: CSpin 8
   Tc 3: CSpin 7
   Tc Heat: CSpin 6
   H_en: pin 5
*/

#include <SPI.h>
#include "RFM69.h"
#include "Adafruit_MAX31855.h"

#define NODE_ID         2
#define NETWORK_ID      105
#define GATEWAY_ID      0
#define FREQUENCY       RF69_433MHZ
#define KEY             "p6ZNvTmGfdY2hUXb" //has to be same 16 characters/bytes on all nodes, not more not less!
#define ACK_TIME        100  // # of ms to wait for an ack
#define NB_ATTEMPTS_ACK 5 //number of attempts to try before giving up

#define Tc1_CS 9
#define Tc2_CS 8
#define Tc3_CS 7
#define TcH_CS 6
#define H_en 5

void sendPacket();
void printPacket();

Adafruit_MAX31855 Tc1(Tc1_CS);
Adafruit_MAX31855 Tc2(Tc2_CS);
Adafruit_MAX31855 Tc3(Tc3_CS);
Adafruit_MAX31855 TcH(TcH_CS);
RFM69 radio;

long TRANSMIT_PERIOD = 30000; //transmit a packet to gateway so often (in ms)
int TRANSMIT_PERIOD_MINUTES = 1;
int ACK_FAIL_WAIT_PERIOD = 500;

long currentTime;
long h_saved_time;
long h_interval;
const long h_pulse_on = 6000;
const long h_pulse_off = 30 * 60000;
//const long h_pulse_off = 60000;
int h_status = 1;

long log_saved_time;
int log_interval = 6000;

typedef struct {
  int node_id;
  float Tc1_t;
  float Tc2_t;
  float Tc3_t;
  float TcH_t;
}
Payload;
Payload payload;

void setup() {
  Serial.begin(9600);
  pinMode(H_en, OUTPUT);
  digitalWrite(H_en, LOW);
  payload.node_id = NODE_ID;
  radio.initialize(FREQUENCY, NODE_ID, NETWORK_ID); //Begin Radio
  radio.setHighPower(); //uncomment only for RFM69HW!
  radio.encrypt(KEY); //Encrypt
  radio.sleep(); //Sleep radio (saves power)
  char buff[50];
  sprintf(buff, "Listening at %d Mhz...", FREQUENCY == RF69_433MHZ ? 433 : FREQUENCY == RF69_868MHZ ? 868 : 915);
}

void loop() {
  currentTime = millis();
  if (h_saved_time + h_interval < currentTime) {
    //if heater is on, wait 6 seconds
    if (h_status == 1) {
      digitalWrite(H_en, HIGH);
      h_interval = h_pulse_on;
      h_status = 0; //set status for next round.
      Serial.print("Heat interval ");
      Serial.print(h_pulse_on / 1000);
      Serial.println("s");
    }
    else if (h_status == 0) {
      digitalWrite(H_en, LOW);
      h_interval = h_pulse_off;
      h_status = 1; //set status for next round
      Serial.print("Heat interval ");
      Serial.print(h_pulse_off / 1000);
      Serial.println("s");
    }
    h_saved_time = currentTime;
  }
  if (log_saved_time + log_interval < currentTime) {
    payload.Tc1_t = Tc1.readCelsius();
    payload.Tc2_t = Tc2.readCelsius();
    payload.Tc3_t = Tc3.readCelsius();
    payload.TcH_t = TcH.readCelsius();
    sendPacket();
    printPacket();
    log_saved_time = currentTime;
  }
}

void sendPacket() {
  int nAttempt = 0;
  bool flag_ACK_received = false;
  while (nAttempt < NB_ATTEMPTS_ACK && !flag_ACK_received) {
    if (radio.sendWithRetry(GATEWAY_ID, (const void*)(&payload), sizeof(payload))) {
      flag_ACK_received = true;
    }
    else {
      if (radio.sendWithRetry(GATEWAY_ID, (const void*)(&payload), sizeof(payload))) {
        flag_ACK_received = true;
      }
      nAttempt++;
      ACK_FAIL_WAIT_PERIOD = random(300, 600);
      delay(ACK_FAIL_WAIT_PERIOD);
    }
  }
}

void printPacket() {
  Serial.print(payload.Tc1_t);
  Serial.print(", ");
  Serial.print(payload.Tc2_t);
  Serial.print(", ");
  Serial.print(payload.Tc3_t);
  Serial.print(", ");
  Serial.print(payload.TcH_t);
  Serial.println();
}
<|endoftext|>


<commit_before>// Copyright (C) 2010 and 2011 Marcin Arkadiusz Skrobiranda.
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
// 2. 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.
// 3. Neither the name of the project nor the names of its contributors
//    may be used to endorse or promote products derived from this software
//    without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE PROJECT 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 PROJECT 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 "TickEpochOperator.hpp"

using namespace GameServer::Achievement;
using namespace GameServer::Persistence;
using namespace GameServer::Turn;
using namespace GameServer::World;
using namespace std;

namespace GameServer
{
namespace Epoch
{

TickEpochOperator::TickEpochOperator(
    IEpochPersistenceFacadeShrPtr a_epoch_persistence_facade,
    IWorldPersistenceFacadeShrPtr a_world_persistence_facade,
    IAchievementManagerShrPtr     a_achievement_manager,
    ITurnManagerShrPtr            a_turn_manager
)
    : m_epoch_persistence_facade(a_epoch_persistence_facade),
      m_world_persistence_facade(a_world_persistence_facade),
      m_achievement_manager(a_achievement_manager),
      m_turn_manager(a_turn_manager)
{
}

TickEpochOperatorExitCode TickEpochOperator::tickEpoch(
    ITransactionShrPtr       a_transaction,
    string             const a_world_name
)
{
    try
    {
        // Verify if the world exists.
        IWorldShrPtr world = m_world_persistence_facade->getWorld(a_transaction, a_world_name);

        if (!world)
        {
            return TickEpochOperatorExitCode(TICK_EPOCH_OPERATOR_EXIT_CODE_WORLD_DOES_NOT_EXIST);
        }

        // Verify if the epoch exists.
        IEpochShrPtr epoch = m_epoch_persistence_facade->getEpoch(a_transaction, a_world_name);

        if (!epoch)
        {
            return TickEpochOperatorExitCode(TICK_EPOCH_OPERATOR_EXIT_CODE_EPOCH_DOES_NOT_EXIST);
        }

        // Verify if the epoch has been finished.
        if (epoch->getFinished())
        {
            return TickEpochOperatorExitCode(TICK_EPOCH_OPERATOR_EXIT_CODE_EPOCH_HAS_BEEN_FINISHED);
        }

        // Verify if the epoch is active.
        if (epoch->getActive())
        {
            return TickEpochOperatorExitCode(TICK_EPOCH_OPERATOR_EXIT_CODE_EPOCH_IS_ACTIVE);
        }

        bool const result_turn = m_turn_manager->turn(a_transaction, world);

        bool const result_achievement = m_achievement_manager->grantAchievements(a_transaction, world);

        return (result_turn and result_achievement)
                   ? TickEpochOperatorExitCode(TICK_EPOCH_OPERATOR_EXIT_CODE_EPOCH_HAS_BEEN_TACK)
                   : TickEpochOperatorExitCode(TICK_EPOCH_OPERATOR_EXIT_CODE_EPOCH_HAS_NOT_BEEN_TACK);
    }
    catch (...)
    {
        return TickEpochOperatorExitCode(TICK_EPOCH_OPERATOR_EXIT_CODE_UNEXPECTED_ERROR);
    }
}

} // namespace Epoch
} // namespace GameServer
<commit_msg>Fix: incrementing the number of ticks of an epoch.<commit_after>// Copyright (C) 2010 and 2011 Marcin Arkadiusz Skrobiranda.
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
// 2. 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.
// 3. Neither the name of the project nor the names of its contributors
//    may be used to endorse or promote products derived from this software
//    without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE PROJECT 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 PROJECT 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 "TickEpochOperator.hpp"

using namespace GameServer::Achievement;
using namespace GameServer::Persistence;
using namespace GameServer::Turn;
using namespace GameServer::World;
using namespace std;

namespace GameServer
{
namespace Epoch
{

TickEpochOperator::TickEpochOperator(
    IEpochPersistenceFacadeShrPtr a_epoch_persistence_facade,
    IWorldPersistenceFacadeShrPtr a_world_persistence_facade,
    IAchievementManagerShrPtr     a_achievement_manager,
    ITurnManagerShrPtr            a_turn_manager
)
    : m_epoch_persistence_facade(a_epoch_persistence_facade),
      m_world_persistence_facade(a_world_persistence_facade),
      m_achievement_manager(a_achievement_manager),
      m_turn_manager(a_turn_manager)
{
}

TickEpochOperatorExitCode TickEpochOperator::tickEpoch(
    ITransactionShrPtr       a_transaction,
    string             const a_world_name
)
{
    try
    {
        // Verify if the world exists.
        IWorldShrPtr world = m_world_persistence_facade->getWorld(a_transaction, a_world_name);

        if (!world)
        {
            return TickEpochOperatorExitCode(TICK_EPOCH_OPERATOR_EXIT_CODE_WORLD_DOES_NOT_EXIST);
        }

        // Verify if the epoch exists.
        IEpochShrPtr epoch = m_epoch_persistence_facade->getEpoch(a_transaction, a_world_name);

        if (!epoch)
        {
            return TickEpochOperatorExitCode(TICK_EPOCH_OPERATOR_EXIT_CODE_EPOCH_DOES_NOT_EXIST);
        }

        // Verify if the epoch has been finished.
        if (epoch->getFinished())
        {
            return TickEpochOperatorExitCode(TICK_EPOCH_OPERATOR_EXIT_CODE_EPOCH_HAS_BEEN_FINISHED);
        }

        // Verify if the epoch is active.
        if (epoch->getActive())
        {
            return TickEpochOperatorExitCode(TICK_EPOCH_OPERATOR_EXIT_CODE_EPOCH_IS_ACTIVE);
        }

        bool const result_turn = m_turn_manager->turn(a_transaction, world);

        bool const result_achievement = m_achievement_manager->grantAchievements(a_transaction, world);

        // TODO: Change the interface (transaction, world).
        bool const result_tick = m_epoch_persistence_facade->tickEpoch(a_transaction, world->getWorldName());

        return (result_turn and result_achievement and result_tick)
                   ? TickEpochOperatorExitCode(TICK_EPOCH_OPERATOR_EXIT_CODE_EPOCH_HAS_BEEN_TACK)
                   : TickEpochOperatorExitCode(TICK_EPOCH_OPERATOR_EXIT_CODE_EPOCH_HAS_NOT_BEEN_TACK);
    }
    catch (...)
    {
        return TickEpochOperatorExitCode(TICK_EPOCH_OPERATOR_EXIT_CODE_UNEXPECTED_ERROR);
    }
}

} // namespace Epoch
} // namespace GameServer
<|endoftext|>


<commit_before>/**
 * @file hoeffding_tree_main.cpp
 * @author Ryan Curtin
 *
 * A command-line executable that can build a streaming decision tree.
 */
#include <mlpack/core.hpp>
#include <mlpack/methods/hoeffding_trees/hoeffding_tree.hpp>
#include <mlpack/methods/hoeffding_trees/binary_numeric_split.hpp>
#include <mlpack/methods/hoeffding_trees/information_gain.hpp>

using namespace std;
using namespace mlpack;
using namespace mlpack::tree;
using namespace mlpack::data;

PARAM_STRING("training_file", "Training dataset file.", "t", "");
PARAM_STRING("labels_file", "Labels for training dataset.", "l", "");

PARAM_DOUBLE("confidence", "Confidence before splitting (between 0 and 1).",
    "c", 0.95);
PARAM_INT("max_samples", "Maximum number of samples before splitting.", "n",
    5000);
PARAM_INT("min_samples", "Minimum number of samples before splitting.", "I",
    100);

PARAM_STRING("input_model_file", "File to load trained tree from.", "m", "");
PARAM_STRING("output_model_file", "File to save trained tree to.", "M", "");

PARAM_STRING("test_file", "File of testing data.", "T", "");
PARAM_STRING("test_labels_file", "Labels of test data.", "L", "");
PARAM_STRING("predictions_file", "File to output label predictions for test "
    "data into.", "p", "");
PARAM_STRING("probabilities_file", "In addition to predicting labels, provide "
    "prediction probabilities in this file.", "P", "");

PARAM_STRING("numeric_split_strategy", "The splitting strategy to use for "
    "numeric features: 'domingos' or 'binary'.", "N", "binary");
PARAM_FLAG("batch_mode", "If true, samples will be considered in batch instead "
    "of as a stream.  This generally results in better trees but at the cost of"
    " memory usage and runtime.", "b");
PARAM_FLAG("info_gain", "If set, information gain is used instead of Gini "
    "impurity for calculating Hoeffding bounds.", "i");
PARAM_INT("passes", "Number of passes to take over the dataset.", "s", 1);

PARAM_INT("bins", "If the 'domingos' split strategy is used, this specifies "
    "the number of bins for each numeric split.", "B", 10);
PARAM_INT("observations_before_binning", "If the 'domingos' split strategy is "
    "used, this specifies the number of samples observed before binning is "
    "performed.", "o", 100);

// Helper function for once we have chosen a tree type.
template<typename TreeType>
void PerformActions(const typename TreeType::NumericSplit& numericSplit =
    typename TreeType::NumericSplit(0));

int main(int argc, char** argv)
{
  CLI::ParseCommandLine(argc, argv);

  // Check input parameters for validity.
  const string trainingFile = CLI::GetParam<string>("training_file");
  const string labelsFile = CLI::GetParam<string>("labels_file");
  const string inputModelFile = CLI::GetParam<string>("input_model_file");
  const string testFile = CLI::GetParam<string>("test_file");
  const string predictionsFile = CLI::GetParam<string>("predictions_file");
  const string probabilitiesFile = CLI::GetParam<string>("probabilities_file");
  const string numericSplitStrategy =
      CLI::GetParam<string>("numeric_split_strategy");

  if ((!predictionsFile.empty() || !probabilitiesFile.empty()) &&
      testFile.empty())
    Log::Fatal << "--test_file must be specified if --predictions_file or "
        << "--probabilities_file is specified." << endl;

  if (trainingFile.empty() && inputModelFile.empty())
    Log::Fatal << "One of --training_file or --input_model_file must be "
        << "specified!" << endl;

  if (!trainingFile.empty() && labelsFile.empty())
    Log::Fatal << "If --training_file is specified, --labels_file must be "
        << "specified too!" << endl;

  if (trainingFile.empty() && CLI::HasParam("batch_mode"))
    Log::Warn << "--batch_mode (-b) ignored; no training set provided." << endl;

  if (CLI::HasParam("passes") && CLI::HasParam("batch_mode"))
    Log::Warn << "--batch_mode (-b) ignored because --passes was specified."
        << endl;

  if (CLI::HasParam("info_gain"))
  {
    if (numericSplitStrategy == "domingos")
    {
      const size_t bins = (size_t) CLI::GetParam<int>("bins");
      const size_t observationsBeforeBinning = (size_t)
          CLI::GetParam<int>("observations_before_binning");
      HoeffdingDoubleNumericSplit<InformationGain> ns(0, bins,
          observationsBeforeBinning);
      PerformActions<HoeffdingTree<InformationGain, HoeffdingDoubleNumericSplit,
          HoeffdingCategoricalSplit>>(ns);
    }
    else if (numericSplitStrategy == "binary")
    {
      PerformActions<HoeffdingTree<InformationGain, BinaryDoubleNumericSplit,
          HoeffdingCategoricalSplit>>();
    }
    else
    {
      Log::Fatal << "Unrecognized numeric split strategy ("
          << numericSplitStrategy << ")!  Must be 'domingos' or 'binary'."
          << endl;
    }
  }
  else
  {
    if (numericSplitStrategy == "domingos")
    {
      const size_t bins = (size_t) CLI::GetParam<int>("bins");
      const size_t observationsBeforeBinning = (size_t)
          CLI::GetParam<int>("observations_before_binning");
      HoeffdingDoubleNumericSplit<GiniImpurity> ns(0, bins,
          observationsBeforeBinning);
      PerformActions<HoeffdingTree<GiniImpurity, HoeffdingDoubleNumericSplit,
          HoeffdingCategoricalSplit>>(ns);
    }
    else if (numericSplitStrategy == "binary")
    {
      PerformActions<HoeffdingTree<GiniImpurity, BinaryDoubleNumericSplit,
          HoeffdingCategoricalSplit>>();
    }
    else
    {
      Log::Fatal << "Unrecognized numeric split strategy ("
          << numericSplitStrategy << ")!  Must be 'domingos' or 'binary'."
          << endl;
    }
  }
}

template<typename TreeType>
void PerformActions(const typename TreeType::NumericSplit& numericSplit)
{
  // Load necessary parameters.
  const string trainingFile = CLI::GetParam<string>("training_file");
  const string labelsFile = CLI::GetParam<string>("labels_file");
  const double confidence = CLI::GetParam<double>("confidence");
  const size_t maxSamples = (size_t) CLI::GetParam<int>("max_samples");
  const size_t minSamples = (size_t) CLI::GetParam<size_t>("min_samples");
  const string inputModelFile = CLI::GetParam<string>("input_model_file");
  const string outputModelFile = CLI::GetParam<string>("output_model_file");
  const string testFile = CLI::GetParam<string>("test_file");
  const string predictionsFile = CLI::GetParam<string>("predictions_file");
  const string probabilitiesFile = CLI::GetParam<string>("probabilities_file");
  bool batchTraining = CLI::HasParam("batch_mode");
  const size_t passes = (size_t) CLI::GetParam<int>("passes");
  if (passes > 1)
    batchTraining = false; // We already warned about this earlier.

  TreeType* tree = NULL;
  DatasetInfo datasetInfo;
  if (inputModelFile.empty())
  {
    arma::mat trainingSet;
    data::Load(trainingFile, trainingSet, datasetInfo, true);
    for (size_t i = 0; i < trainingSet.n_rows; ++i)
      Log::Info << datasetInfo.NumMappings(i) << " mappings in dimension "
          << i << "." << endl;

    arma::Col<size_t> labelsIn;
    data::Load(labelsFile, labelsIn, true, false);
    arma::Row<size_t> labels = labelsIn.t();

    // Now create the decision tree.
    Timer::Start("tree_training");
    if (passes > 1)
      Log::Info << "Taking " << passes << " passes over the dataset." << endl;

    tree = new TreeType(trainingSet, datasetInfo, labels, max(labels) + 1,
        batchTraining, confidence, maxSamples, 100, minSamples,
        typename TreeType::CategoricalSplit(0, 0), numericSplit);

    for (size_t i = 1; i < passes; ++i)
      tree->Train(trainingSet, labels, false);
    Timer::Stop("tree_training");
  }
  else
  {
    tree = new TreeType(datasetInfo, 1, 1);
    data::Load(inputModelFile, "streamingDecisionTree", *tree, true);

    if (!trainingFile.empty())
    {
      arma::mat trainingSet;
      data::Load(trainingFile, trainingSet, datasetInfo, true);
      for (size_t i = 0; i < trainingSet.n_rows; ++i)
        Log::Info << datasetInfo.NumMappings(i) << " mappings in dimension "
            << i << "." << endl;

      arma::Col<size_t> labelsIn;
      data::Load(labelsFile, labelsIn, true, false);
      arma::Row<size_t> labels = labelsIn.t();

      // Now create the decision tree.
      Timer::Start("tree_training");
      if (passes > 1)
      {
        Log::Info << "Taking " << passes << " passes over the dataset." << endl;
        for (size_t i = 0; i < passes; ++i)
          tree->Train(trainingSet, labels, false);
      }
      else
      {
        tree->Train(trainingSet, labels, batchTraining);
      }
      Timer::Stop("tree_training");
    }
  }

  if (!trainingFile.empty())
  {
    // Get training error.
    arma::mat trainingSet;
    data::Load(trainingFile, trainingSet, datasetInfo, true);
    arma::Row<size_t> predictions;
    tree->Classify(trainingSet, predictions);

    arma::Col<size_t> labelsIn;
    data::Load(labelsFile, labelsIn, true, false);
    arma::Row<size_t> labels = labelsIn.t();

    size_t correct = 0;
    for (size_t i = 0; i < labels.n_elem; ++i)
      if (labels[i] == predictions[i])
        ++correct;

    Log::Info << correct << " out of " << labels.n_elem << " correct "
        << "on training set (" << double(correct) / double(labels.n_elem) *
        100.0 << ")." << endl;

  }

  // The tree is trained or loaded.  Now do any testing if we need.
  if (!testFile.empty())
  {
    arma::mat testSet;
    data::Load(testFile, testSet, datasetInfo, true);

    arma::Row<size_t> predictions;
    arma::rowvec probabilities;

    Timer::Start("tree_testing");
    tree->Classify(testSet, predictions, probabilities);
    Timer::Stop("tree_testing");

    if (CLI::HasParam("test_labels_file"))
    {
      string testLabelsFile = CLI::GetParam<string>("test_labels_file");
      arma::Col<size_t> testLabelsIn;
      data::Load(testLabelsFile, testLabelsIn, true, false);
      arma::Row<size_t> testLabels = testLabelsIn.t();

      size_t correct = 0;
      for (size_t i = 0; i < testLabels.n_elem; ++i)
      {
        if (predictions[i] == testLabels[i])
          ++correct;
      }
      Log::Info << correct << " out of " << testLabels.n_elem << " correct "
          << "on test set (" << double(correct) / double(testLabels.n_elem) *
          100.0 << ")." << endl;
    }

    if (!predictionsFile.empty())
      data::Save(predictionsFile, predictions);

    if (!probabilitiesFile.empty())
      data::Save(probabilitiesFile, probabilities);
  }

  // Check the accuracy on the training set.
  if (!outputModelFile.empty())
    data::Save(outputModelFile, "streamingDecisionTree", tree, true);

  // Clean up memory.
  delete tree;
}
<commit_msg>Fix segfault.<commit_after>/**
 * @file hoeffding_tree_main.cpp
 * @author Ryan Curtin
 *
 * A command-line executable that can build a streaming decision tree.
 */
#include <mlpack/core.hpp>
#include <mlpack/methods/hoeffding_trees/hoeffding_tree.hpp>
#include <mlpack/methods/hoeffding_trees/binary_numeric_split.hpp>
#include <mlpack/methods/hoeffding_trees/information_gain.hpp>

using namespace std;
using namespace mlpack;
using namespace mlpack::tree;
using namespace mlpack::data;

PARAM_STRING("training_file", "Training dataset file.", "t", "");
PARAM_STRING("labels_file", "Labels for training dataset.", "l", "");

PARAM_DOUBLE("confidence", "Confidence before splitting (between 0 and 1).",
    "c", 0.95);
PARAM_INT("max_samples", "Maximum number of samples before splitting.", "n",
    5000);
PARAM_INT("min_samples", "Minimum number of samples before splitting.", "I",
    100);

PARAM_STRING("input_model_file", "File to load trained tree from.", "m", "");
PARAM_STRING("output_model_file", "File to save trained tree to.", "M", "");

PARAM_STRING("test_file", "File of testing data.", "T", "");
PARAM_STRING("test_labels_file", "Labels of test data.", "L", "");
PARAM_STRING("predictions_file", "File to output label predictions for test "
    "data into.", "p", "");
PARAM_STRING("probabilities_file", "In addition to predicting labels, provide "
    "prediction probabilities in this file.", "P", "");

PARAM_STRING("numeric_split_strategy", "The splitting strategy to use for "
    "numeric features: 'domingos' or 'binary'.", "N", "binary");
PARAM_FLAG("batch_mode", "If true, samples will be considered in batch instead "
    "of as a stream.  This generally results in better trees but at the cost of"
    " memory usage and runtime.", "b");
PARAM_FLAG("info_gain", "If set, information gain is used instead of Gini "
    "impurity for calculating Hoeffding bounds.", "i");
PARAM_INT("passes", "Number of passes to take over the dataset.", "s", 1);

PARAM_INT("bins", "If the 'domingos' split strategy is used, this specifies "
    "the number of bins for each numeric split.", "B", 10);
PARAM_INT("observations_before_binning", "If the 'domingos' split strategy is "
    "used, this specifies the number of samples observed before binning is "
    "performed.", "o", 100);

// Helper function for once we have chosen a tree type.
template<typename TreeType>
void PerformActions(const typename TreeType::NumericSplit& numericSplit =
    typename TreeType::NumericSplit(0));

int main(int argc, char** argv)
{
  CLI::ParseCommandLine(argc, argv);

  // Check input parameters for validity.
  const string trainingFile = CLI::GetParam<string>("training_file");
  const string labelsFile = CLI::GetParam<string>("labels_file");
  const string inputModelFile = CLI::GetParam<string>("input_model_file");
  const string testFile = CLI::GetParam<string>("test_file");
  const string predictionsFile = CLI::GetParam<string>("predictions_file");
  const string probabilitiesFile = CLI::GetParam<string>("probabilities_file");
  const string numericSplitStrategy =
      CLI::GetParam<string>("numeric_split_strategy");

  if ((!predictionsFile.empty() || !probabilitiesFile.empty()) &&
      testFile.empty())
    Log::Fatal << "--test_file must be specified if --predictions_file or "
        << "--probabilities_file is specified." << endl;

  if (trainingFile.empty() && inputModelFile.empty())
    Log::Fatal << "One of --training_file or --input_model_file must be "
        << "specified!" << endl;

  if (!trainingFile.empty() && labelsFile.empty())
    Log::Fatal << "If --training_file is specified, --labels_file must be "
        << "specified too!" << endl;

  if (trainingFile.empty() && CLI::HasParam("batch_mode"))
    Log::Warn << "--batch_mode (-b) ignored; no training set provided." << endl;

  if (CLI::HasParam("passes") && CLI::HasParam("batch_mode"))
    Log::Warn << "--batch_mode (-b) ignored because --passes was specified."
        << endl;

  if (CLI::HasParam("info_gain"))
  {
    if (numericSplitStrategy == "domingos")
    {
      const size_t bins = (size_t) CLI::GetParam<int>("bins");
      const size_t observationsBeforeBinning = (size_t)
          CLI::GetParam<int>("observations_before_binning");
      HoeffdingDoubleNumericSplit<InformationGain> ns(0, bins,
          observationsBeforeBinning);
      PerformActions<HoeffdingTree<InformationGain, HoeffdingDoubleNumericSplit,
          HoeffdingCategoricalSplit>>(ns);
    }
    else if (numericSplitStrategy == "binary")
    {
      PerformActions<HoeffdingTree<InformationGain, BinaryDoubleNumericSplit,
          HoeffdingCategoricalSplit>>();
    }
    else
    {
      Log::Fatal << "Unrecognized numeric split strategy ("
          << numericSplitStrategy << ")!  Must be 'domingos' or 'binary'."
          << endl;
    }
  }
  else
  {
    if (numericSplitStrategy == "domingos")
    {
      const size_t bins = (size_t) CLI::GetParam<int>("bins");
      const size_t observationsBeforeBinning = (size_t)
          CLI::GetParam<int>("observations_before_binning");
      HoeffdingDoubleNumericSplit<GiniImpurity> ns(0, bins,
          observationsBeforeBinning);
      PerformActions<HoeffdingTree<GiniImpurity, HoeffdingDoubleNumericSplit,
          HoeffdingCategoricalSplit>>(ns);
    }
    else if (numericSplitStrategy == "binary")
    {
      PerformActions<HoeffdingTree<GiniImpurity, BinaryDoubleNumericSplit,
          HoeffdingCategoricalSplit>>();
    }
    else
    {
      Log::Fatal << "Unrecognized numeric split strategy ("
          << numericSplitStrategy << ")!  Must be 'domingos' or 'binary'."
          << endl;
    }
  }
}

template<typename TreeType>
void PerformActions(const typename TreeType::NumericSplit& numericSplit)
{
  // Load necessary parameters.
  const string trainingFile = CLI::GetParam<string>("training_file");
  const string labelsFile = CLI::GetParam<string>("labels_file");
  const double confidence = CLI::GetParam<double>("confidence");
  const size_t maxSamples = (size_t) CLI::GetParam<int>("max_samples");
  const size_t minSamples = (size_t) CLI::GetParam<int>("min_samples");
  const string inputModelFile = CLI::GetParam<string>("input_model_file");
  const string outputModelFile = CLI::GetParam<string>("output_model_file");
  const string testFile = CLI::GetParam<string>("test_file");
  const string predictionsFile = CLI::GetParam<string>("predictions_file");
  const string probabilitiesFile = CLI::GetParam<string>("probabilities_file");
  bool batchTraining = CLI::HasParam("batch_mode");
  const size_t passes = (size_t) CLI::GetParam<int>("passes");
  if (passes > 1)
    batchTraining = false; // We already warned about this earlier.

  TreeType* tree = NULL;
  DatasetInfo datasetInfo;
  if (inputModelFile.empty())
  {
    arma::mat trainingSet;
    data::Load(trainingFile, trainingSet, datasetInfo, true);
    for (size_t i = 0; i < trainingSet.n_rows; ++i)
      Log::Info << datasetInfo.NumMappings(i) << " mappings in dimension "
          << i << "." << endl;

    arma::Col<size_t> labelsIn;
    data::Load(labelsFile, labelsIn, true, false);
    arma::Row<size_t> labels = labelsIn.t();

    // Now create the decision tree.
    Timer::Start("tree_training");
    if (passes > 1)
      Log::Info << "Taking " << passes << " passes over the dataset." << endl;

    tree = new TreeType(trainingSet, datasetInfo, labels, max(labels) + 1,
        batchTraining, confidence, maxSamples, 100, minSamples,
        typename TreeType::CategoricalSplit(0, 0), numericSplit);

    for (size_t i = 1; i < passes; ++i)
      tree->Train(trainingSet, labels, false);
    Timer::Stop("tree_training");
  }
  else
  {
    tree = new TreeType(datasetInfo, 1, 1);
    data::Load(inputModelFile, "streamingDecisionTree", *tree, true);

    if (!trainingFile.empty())
    {
      arma::mat trainingSet;
      data::Load(trainingFile, trainingSet, datasetInfo, true);
      for (size_t i = 0; i < trainingSet.n_rows; ++i)
        Log::Info << datasetInfo.NumMappings(i) << " mappings in dimension "
            << i << "." << endl;

      arma::Col<size_t> labelsIn;
      data::Load(labelsFile, labelsIn, true, false);
      arma::Row<size_t> labels = labelsIn.t();

      // Now create the decision tree.
      Timer::Start("tree_training");
      if (passes > 1)
      {
        Log::Info << "Taking " << passes << " passes over the dataset." << endl;
        for (size_t i = 0; i < passes; ++i)
          tree->Train(trainingSet, labels, false);
      }
      else
      {
        tree->Train(trainingSet, labels, batchTraining);
      }
      Timer::Stop("tree_training");
    }
  }

  if (!trainingFile.empty())
  {
    // Get training error.
    arma::mat trainingSet;
    data::Load(trainingFile, trainingSet, datasetInfo, true);
    arma::Row<size_t> predictions;
    tree->Classify(trainingSet, predictions);

    arma::Col<size_t> labelsIn;
    data::Load(labelsFile, labelsIn, true, false);
    arma::Row<size_t> labels = labelsIn.t();

    size_t correct = 0;
    for (size_t i = 0; i < labels.n_elem; ++i)
      if (labels[i] == predictions[i])
        ++correct;

    Log::Info << correct << " out of " << labels.n_elem << " correct "
        << "on training set (" << double(correct) / double(labels.n_elem) *
        100.0 << ")." << endl;

  }

  // The tree is trained or loaded.  Now do any testing if we need.
  if (!testFile.empty())
  {
    arma::mat testSet;
    data::Load(testFile, testSet, datasetInfo, true);

    arma::Row<size_t> predictions;
    arma::rowvec probabilities;

    Timer::Start("tree_testing");
    tree->Classify(testSet, predictions, probabilities);
    Timer::Stop("tree_testing");

    if (CLI::HasParam("test_labels_file"))
    {
      string testLabelsFile = CLI::GetParam<string>("test_labels_file");
      arma::Col<size_t> testLabelsIn;
      data::Load(testLabelsFile, testLabelsIn, true, false);
      arma::Row<size_t> testLabels = testLabelsIn.t();

      size_t correct = 0;
      for (size_t i = 0; i < testLabels.n_elem; ++i)
      {
        if (predictions[i] == testLabels[i])
          ++correct;
      }
      Log::Info << correct << " out of " << testLabels.n_elem << " correct "
          << "on test set (" << double(correct) / double(testLabels.n_elem) *
          100.0 << ")." << endl;
    }

    if (!predictionsFile.empty())
      data::Save(predictionsFile, predictions);

    if (!probabilitiesFile.empty())
      data::Save(probabilitiesFile, probabilities);
  }

  // Check the accuracy on the training set.
  if (!outputModelFile.empty())
    data::Save(outputModelFile, "streamingDecisionTree", tree, true);

  // Clean up memory.
  delete tree;
}
<|endoftext|>


<commit_before>/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2014 Max-Planck-Institute for Intelligent Systems,
 *                     University of Southern California,
 *                     Karlsruhe Institute of Technology
 *    Jan Issac (jan.issac@gmail.com)
 *
 *  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.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  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 OWNER 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.
 *
 */

/**
 * @date 05/04/2014
 * @author Jan Issac (jan.issac@gmail.com)
 * Max-Planck-Institute for Intelligent Systems, University of Southern California (USC),
 *   Karlsruhe Institute of Technology (KIT)
 */

#include "oni_vicon_player/vicon_player.hpp"

#include <iostream>
#include <fstream>

#include <visualization_msgs/Marker.h>

using namespace oni_vicon_player;

ViconPlayer::ViconPlayer(ros::NodeHandle& node_handle)
{
    object_publisher_ =
            node_handle.advertise<visualization_msgs::Marker>("vicon_object_pose", 0);
}

ViconPlayer::~ViconPlayer()
{

}

bool ViconPlayer::load(const std::string& source_file,
                       const ViconPlayer::CalibrationTransform& calibration_transform,
                       boost::function<void(int64_t)> updateCb)
{
    std::ifstream data_file(source_file.c_str());

//    std::ofstream process_data((source_file + ".processed").c_str());

    if (!data_file.is_open())
    {
        return false;
    }

    raw_data_.clear();
    data_.clear();

    RawRecord previous_record;
    RawRecord record;
    RawRecord selected_record;
    while (data_file.peek() != EOF)
    {
        previous_record = record;

        data_file >> record.vicon_time;
        data_file >> record.vicon_frame;
        data_file >> record.depth_sensor_time;
        data_file >> record.depth_sensor_frame;
        data_file >> record.vicon_frame_id;
        data_file >> record.translation_x;
        data_file >> record.translation_y;
        data_file >> record.translation_z;
        data_file >> record.orientation_w;
        data_file >> record.orientation_x;
        data_file >> record.orientation_y;
        data_file >> record.orientation_z;

        raw_data_.push_back(record);

        if (record.depth_sensor_frame == 0)
        {
            previous_record = record;
            continue;
        }

        if (record.depth_sensor_frame == previous_record.depth_sensor_frame)
        {
            continue;
        }

        PoseRecord pose_record;
        tf::Vector3 translation;
        tf::Quaternion orientation;

        selected_record = closestViconFrame(record);

        translation.setX(selected_record.translation_x);
        translation.setY(selected_record.translation_y);
        translation.setZ(selected_record.translation_z);

        orientation.setW(selected_record.orientation_w);
        orientation.setX(selected_record.orientation_x);
        orientation.setY(selected_record.orientation_y);
        orientation.setZ(selected_record.orientation_z);
/*
        process_data << selected_record.vicon_time << " ";
        process_data << selected_record.vicon_frame << " ";
        process_data << record.depth_sensor_time << " ";
        process_data << record.depth_sensor_frame << " ";
        process_data << selected_record.vicon_frame_id << " ";
        process_data << selected_record.translation_x << " ";
        process_data << selected_record.translation_y << " ";
        process_data << selected_record.translation_z << " ";
        process_data << selected_record.orientation_w << " ";
        process_data << selected_record.orientation_x << " ";
        process_data << selected_record.orientation_y << " ";
        process_data << selected_record.orientation_z;
        process_data << std::endl;
*/
        pose_record.stamp.fromSec(record.depth_sensor_time * 1e-3);
        pose_record.pose.setOrigin(translation);
        pose_record.pose.setRotation(orientation);

        // transform into depth sensor frame
        pose_record.pose = calibration_transform.viconToDepthSensor(pose_record.pose);

        data_[record.depth_sensor_frame] = pose_record;

        updateCb(data_.size());

        if (countDepthSensorFrames() != record.depth_sensor_frame)
        {
            ROS_WARN("%ld !- %ld", countDepthSensorFrames(), record.depth_sensor_frame);
        }
    }

    data_file.close();

//    process_data.close();

    return true;
}

const ViconPlayer::PoseRecord& ViconPlayer::poseRecord(int64_t frame)
{
    return data_[frame];
}

int64_t ViconPlayer::countViconFrames()
{
    return raw_data_.size();
}

int64_t ViconPlayer::countDepthSensorFrames()
{
    return data_.size();
}

ViconPlayer::RawRecord ViconPlayer::closestViconFrame(const ViconPlayer::RawRecord& oni_frame)
{
    std::vector<RawRecord>::reverse_iterator rit = raw_data_.rbegin();

    RawRecord selected_record;

    for (rit = raw_data_.rbegin(); rit != raw_data_.rend(); ++rit)
    {
        if (rit == raw_data_.rbegin())
        {
            selected_record = *rit;
        }
        else if (std::abs(oni_frame.depth_sensor_time - rit->vicon_time) >=
                 std::abs(oni_frame.depth_sensor_time - selected_record.vicon_time))
        {
            break;
        }
        else
        {
            selected_record = *rit;
        }
    }

    return selected_record;
}

void ViconPlayer::publish(const ViconPlayer::PoseRecord& pose_record,
                          sensor_msgs::ImagePtr corresponding_image,
                          const std::string& object_display)
{
    visualization_msgs::Marker marker;
    marker.header.frame_id = corresponding_image->header.frame_id;
    marker.header.stamp =  corresponding_image->header.stamp;
    marker.ns = "vicon_object_pose";
    marker.id = 0;
    marker.scale.x = 1.0;
    marker.scale.y = 1.0;
    marker.scale.z = 1.0;
    marker.color.r = 1;
    marker.color.g = 0;
    marker.color.b = 0;
    marker.color.a = 1;

    marker.type = visualization_msgs::Marker::MESH_RESOURCE;
    marker.action = visualization_msgs::Marker::ADD;

    marker.pose.position.x = pose_record.pose.getOrigin().getX();
    marker.pose.position.y = pose_record.pose.getOrigin().getY();
    marker.pose.position.z = pose_record.pose.getOrigin().getZ();

    tf::Quaternion orientation = pose_record.pose.getRotation();
    marker.pose.orientation.w = orientation.getW();
    marker.pose.orientation.x = orientation.getX();
    marker.pose.orientation.y = orientation.getY();
    marker.pose.orientation.z = orientation.getZ();

    marker.mesh_resource = object_display;

    object_publisher_.publish(marker);
}
<commit_msg>time ms vs ns detection of recorded data<commit_after>/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2014 Max-Planck-Institute for Intelligent Systems,
 *                     University of Southern California,
 *                     Karlsruhe Institute of Technology
 *    Jan Issac (jan.issac@gmail.com)
 *
 *  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.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  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 OWNER 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.
 *
 */

/**
 * @date 05/04/2014
 * @author Jan Issac (jan.issac@gmail.com)
 * Max-Planck-Institute for Intelligent Systems, University of Southern California (USC),
 *   Karlsruhe Institute of Technology (KIT)
 */

#include "oni_vicon_player/vicon_player.hpp"

#include <iostream>
#include <fstream>

#include <visualization_msgs/Marker.h>

using namespace oni_vicon_player;

ViconPlayer::ViconPlayer(ros::NodeHandle& node_handle)
{
    object_publisher_ =
            node_handle.advertise<visualization_msgs::Marker>("vicon_object_pose", 0);
}

ViconPlayer::~ViconPlayer()
{

}

bool ViconPlayer::load(const std::string& source_file,
                       const ViconPlayer::CalibrationTransform& calibration_transform,
                       boost::function<void(int64_t)> updateCb)
{
    std::ifstream data_file(source_file.c_str());

//    std::ofstream process_data((source_file + ".processed").c_str());

    if (!data_file.is_open())
    {
        return false;
    }

    raw_data_.clear();
    data_.clear();

    RawRecord previous_record;
    RawRecord record;
    RawRecord selected_record;
    while (data_file.peek() != EOF)
    {
        previous_record = record;

        data_file >> record.vicon_time;
        data_file >> record.vicon_frame;
        data_file >> record.depth_sensor_time;
        data_file >> record.depth_sensor_frame;
        data_file >> record.vicon_frame_id;
        data_file >> record.translation_x;
        data_file >> record.translation_y;
        data_file >> record.translation_z;
        data_file >> record.orientation_w;
        data_file >> record.orientation_x;
        data_file >> record.orientation_y;
        data_file >> record.orientation_z;

        raw_data_.push_back(record);

        if (record.depth_sensor_frame == 0)
        {
            if (record.vicon_time < 1e3)
            {
                time_is_in_ms_ = true;
            }

            previous_record = record;
            continue;
        }

        if (record.depth_sensor_frame == previous_record.depth_sensor_frame)
        {
            continue;
        }

        PoseRecord pose_record;
        tf::Vector3 translation;
        tf::Quaternion orientation;

        selected_record = closestViconFrame(record);

        translation.setX(selected_record.translation_x);
        translation.setY(selected_record.translation_y);
        translation.setZ(selected_record.translation_z);

        orientation.setW(selected_record.orientation_w);
        orientation.setX(selected_record.orientation_x);
        orientation.setY(selected_record.orientation_y);
        orientation.setZ(selected_record.orientation_z);
/*
        process_data << selected_record.vicon_time << " ";
        process_data << selected_record.vicon_frame << " ";
        process_data << record.depth_sensor_time << " ";
        process_data << record.depth_sensor_frame << " ";
        process_data << selected_record.vicon_frame_id << " ";
        process_data << selected_record.translation_x << " ";
        process_data << selected_record.translation_y << " ";
        process_data << selected_record.translation_z << " ";
        process_data << selected_record.orientation_w << " ";
        process_data << selected_record.orientation_x << " ";
        process_data << selected_record.orientation_y << " ";
        process_data << selected_record.orientation_z;
        process_data << std::endl;
*/
        pose_record.stamp.fromSec(record.depth_sensor_time * 1e-3);
        pose_record.pose.setOrigin(translation);
        pose_record.pose.setRotation(orientation);

        // transform into depth sensor frame
        pose_record.pose = calibration_transform.viconToDepthSensor(pose_record.pose);

        data_[record.depth_sensor_frame] = pose_record;

        updateCb(data_.size());

        if (countDepthSensorFrames() != record.depth_sensor_frame)
        {
            ROS_WARN("%ld !- %ld", countDepthSensorFrames(), record.depth_sensor_frame);
        }
    }

    data_file.close();

//    process_data.close();

    return true;
}

const ViconPlayer::PoseRecord& ViconPlayer::poseRecord(int64_t frame)
{
    return data_[frame];
}

int64_t ViconPlayer::countViconFrames()
{
    return raw_data_.size();
}

int64_t ViconPlayer::countDepthSensorFrames()
{
    return data_.size();
}

ViconPlayer::RawRecord ViconPlayer::closestViconFrame(const ViconPlayer::RawRecord& oni_frame)
{
    std::vector<RawRecord>::reverse_iterator rit = raw_data_.rbegin();

    RawRecord selected_record;

    for (rit = raw_data_.rbegin(); rit != raw_data_.rend(); ++rit)
    {
        if (rit == raw_data_.rbegin())
        {
            selected_record = *rit;
        }
        else if (std::abs(oni_frame.depth_sensor_time - rit->vicon_time) >=
                 std::abs(oni_frame.depth_sensor_time - selected_record.vicon_time))
        {
            break;
        }
        else
        {
            selected_record = *rit;
        }
    }

    return selected_record;
}

void ViconPlayer::publish(const ViconPlayer::PoseRecord& pose_record,
                          sensor_msgs::ImagePtr corresponding_image,
                          const std::string& object_display)
{
    visualization_msgs::Marker marker;
    marker.header.frame_id = corresponding_image->header.frame_id;
    marker.header.stamp =  corresponding_image->header.stamp;
    marker.ns = "vicon_object_pose";
    marker.id = 0;
    marker.scale.x = 1.0;
    marker.scale.y = 1.0;
    marker.scale.z = 1.0;
    marker.color.r = 1;
    marker.color.g = 0;
    marker.color.b = 0;
    marker.color.a = 1;

    marker.type = visualization_msgs::Marker::MESH_RESOURCE;
    marker.action = visualization_msgs::Marker::ADD;

    marker.pose.position.x = pose_record.pose.getOrigin().getX();
    marker.pose.position.y = pose_record.pose.getOrigin().getY();
    marker.pose.position.z = pose_record.pose.getOrigin().getZ();

    tf::Quaternion orientation = pose_record.pose.getRotation();
    marker.pose.orientation.w = orientation.getW();
    marker.pose.orientation.x = orientation.getX();
    marker.pose.orientation.y = orientation.getY();
    marker.pose.orientation.z = orientation.getZ();

    marker.mesh_resource = object_display;

    object_publisher_.publish(marker);
}
<|endoftext|>


<commit_before>/* Copyright 2019 The TensorFlow Authors. All Rights Reserved.

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 "tensorflow/lite/delegates/gpu/gl/kernels/add.h"

#include <algorithm>
#include <cstdint>
#include <cstring>
#include <string>
#include <vector>

#include "absl/memory/memory.h"
#include "absl/strings/str_cat.h"
#include "tensorflow/lite/delegates/gpu/common/data_type.h"
#include "tensorflow/lite/delegates/gpu/common/status.h"
#include "tensorflow/lite/delegates/gpu/common/types.h"

namespace tflite {
namespace gpu {
namespace gl {
namespace {

class Add : public NodeShader {
 public:
  Status GenerateCode(const GenerationContext& ctx,
                      GeneratedCode* generated_code) const final {
    auto attr = absl::any_cast<AddAttributes>(ctx.node->operation.attributes);
    auto adds = absl::get_if<Tensor<Linear, DataType::FLOAT32>>(&attr.param);
    auto scalar = absl::get_if<float>(&attr.param);
    auto inputs = ctx.graph->FindInputs(ctx.node->id);

    if (!adds && !scalar) {
      // check if it is a broadcast
      if (inputs.size() == 2 &&
          inputs[0]->tensor.shape != inputs[1]->tensor.shape &&
          inputs[1]->tensor.shape.h == 1 && inputs[1]->tensor.shape.w == 1 &&
          inputs[0]->tensor.shape.c == inputs[1]->tensor.shape.c) {
        // TODO(b/147771327): investigate why input_data_1[gid.z] worked before
        *generated_code = {
            /*parameters=*/{},
            /*objects=*/{},
            /*shared_variables=*/{},
            /*workload=*/uint3(),
            /*workgroup=*/uint3(),
            /*source_code=*/
            "value_0 = $input_data_0[gid.x, gid.y, gid.z]$ + "
            "          $input_data_1[gid.z]$;",
            /*input=*/IOStructure::ONLY_DEFINITIONS,
            /*output=*/IOStructure::AUTO,
        };
        return OkStatus();
      }

      std::string code = "value_0 = value_0";
      for (int index = 1; index < inputs.size(); ++index) {
        if (inputs[index]->tensor.shape != inputs[0]->tensor.shape) {
          return InvalidArgumentError("Shapes are not equal");
        }
        absl::StrAppend(&code, " + value_", index);
      }
      absl::StrAppend(&code, ";");
      *generated_code = {
          /*parameters=*/{},
          /*objects=*/{},
          /*shared_variables=*/{},
          /*workload=*/uint3(),
          /*workgroup=*/uint3(),
          /*source_code=*/std::move(code),
          /*input=*/IOStructure::AUTO,
          /*output=*/IOStructure::AUTO,
      };
      return OkStatus();
    }

    if (scalar) {
      *generated_code = {
          /*parameters=*/{{"scalar", *scalar}},
          /*objects=*/{},
          /*shared_variables=*/{},
          /*workload=*/uint3(),
          /*workgroup=*/uint3(),
          /*source_code=*/"value_0 += $scalar$;",
          /*input=*/IOStructure::AUTO,
          /*output=*/IOStructure::AUTO,
      };
    } else {
      auto shape = inputs[0]->tensor.shape;
      *generated_code = {
          /*parameters=*/{},
          /*objects=*/{{"add_buffer", MakeReadonlyObject(adds->data)}},
          /*shared_variables=*/{},
          // Declare workload explicitly because shader depends on gid.z.
          /*workload=*/
          uint3(shape.w, shape.h, IntegralDivideRoundUp(shape.c, 4)),
          /*workgroup=*/uint3(),
          /*source_code=*/"value_0 += $add_buffer[gid.z]$;",
          /*input=*/IOStructure::AUTO,
          /*output=*/IOStructure::AUTO,
      };
    }

    return OkStatus();
  }
};

}  // namespace

std::unique_ptr<NodeShader> NewAddNodeShader() {
  return absl::make_unique<Add>();
}

}  // namespace gl
}  // namespace gpu
}  // namespace tflite
<commit_msg>Fix broadcast case of add op for Android.<commit_after>/* Copyright 2019 The TensorFlow Authors. All Rights Reserved.

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 "tensorflow/lite/delegates/gpu/gl/kernels/add.h"

#include <algorithm>
#include <cstdint>
#include <cstring>
#include <string>
#include <vector>

#include "absl/memory/memory.h"
#include "absl/strings/str_cat.h"
#include "tensorflow/lite/delegates/gpu/common/data_type.h"
#include "tensorflow/lite/delegates/gpu/common/status.h"
#include "tensorflow/lite/delegates/gpu/common/types.h"

namespace tflite {
namespace gpu {
namespace gl {
namespace {

class Add : public NodeShader {
 public:
  Status GenerateCode(const GenerationContext& ctx,
                      GeneratedCode* generated_code) const final {
    auto attr = absl::any_cast<AddAttributes>(ctx.node->operation.attributes);
    auto adds = absl::get_if<Tensor<Linear, DataType::FLOAT32>>(&attr.param);
    auto scalar = absl::get_if<float>(&attr.param);
    auto inputs = ctx.graph->FindInputs(ctx.node->id);

    if (!adds && !scalar) {
      // check if it is a broadcast
      if (inputs.size() == 2 &&
          inputs[0]->tensor.shape != inputs[1]->tensor.shape &&
          inputs[1]->tensor.shape.h == 1 && inputs[1]->tensor.shape.w == 1 &&
          inputs[0]->tensor.shape.c == inputs[1]->tensor.shape.c) {
        // TODO(b/147771327): investigate why input_data_1[gid.z] worked before
        *generated_code = {
            /*parameters=*/{},
            /*objects=*/{},
            /*shared_variables=*/{},
            /*workload=*/uint3(),
            /*workgroup=*/uint3(),
            /*source_code=*/
            "value_0 = $input_data_0[gid.x, gid.y, gid.z]$ + "
            "          $input_data_1[0, 0, gid.z]$;",
            /*input=*/IOStructure::ONLY_DEFINITIONS,
            /*output=*/IOStructure::AUTO,
        };
        return OkStatus();
      }

      std::string code = "value_0 = value_0";
      for (int index = 1; index < inputs.size(); ++index) {
        if (inputs[index]->tensor.shape != inputs[0]->tensor.shape) {
          return InvalidArgumentError("Shapes are not equal");
        }
        absl::StrAppend(&code, " + value_", index);
      }
      absl::StrAppend(&code, ";");
      *generated_code = {
          /*parameters=*/{},
          /*objects=*/{},
          /*shared_variables=*/{},
          /*workload=*/uint3(),
          /*workgroup=*/uint3(),
          /*source_code=*/std::move(code),
          /*input=*/IOStructure::AUTO,
          /*output=*/IOStructure::AUTO,
      };
      return OkStatus();
    }

    if (scalar) {
      *generated_code = {
          /*parameters=*/{{"scalar", *scalar}},
          /*objects=*/{},
          /*shared_variables=*/{},
          /*workload=*/uint3(),
          /*workgroup=*/uint3(),
          /*source_code=*/"value_0 += $scalar$;",
          /*input=*/IOStructure::AUTO,
          /*output=*/IOStructure::AUTO,
      };
    } else {
      auto shape = inputs[0]->tensor.shape;
      *generated_code = {
          /*parameters=*/{},
          /*objects=*/{{"add_buffer", MakeReadonlyObject(adds->data)}},
          /*shared_variables=*/{},
          // Declare workload explicitly because shader depends on gid.z.
          /*workload=*/
          uint3(shape.w, shape.h, IntegralDivideRoundUp(shape.c, 4)),
          /*workgroup=*/uint3(),
          /*source_code=*/"value_0 += $add_buffer[gid.z]$;",
          /*input=*/IOStructure::AUTO,
          /*output=*/IOStructure::AUTO,
      };
    }

    return OkStatus();
  }
};

}  // namespace

std::unique_ptr<NodeShader> NewAddNodeShader() {
  return absl::make_unique<Add>();
}

}  // namespace gl
}  // namespace gpu
}  // namespace tflite
<|endoftext|>


<commit_before>//
// open horizon -- undefined_darkness@outlook.com
//

#include "hud.h"
#include "renderer/shared.h"
#include "scene/camera.h"

namespace gui
{

//------------------------------------------------------------

inline bool get_project_pos(const nya_math::vec3 &pos, nya_math::vec2 &result)
{
    nya_math::vec4 p(pos, 1.0);
    p = nya_scene::get_camera().get_view_matrix() * nya_render::get_projection_matrix() * p;
    if (p.z < 0)
        return false;

    result.x = (p.x / p.w * 0.5 + 0.5);
    result.y = (p.y / p.w * -0.5 + 0.5);
    return true;
}

//------------------------------------------------------------

void hud::load(const char *aircraft_name)
{
    *this = hud(); //release

    if (!aircraft_name)
        return;

    if (!m_common_loaded)
    {
        m_fonts.load("Hud/hudCommon.fhm");
        m_common.load("Hud/hudCommon.fhm");
        m_common_loaded = true;
    }

    m_aircraft.load(("Hud/aircraft/hudAircraft_" + std::string(aircraft_name) + ".fhm").c_str());
}

//------------------------------------------------------------

void hud::draw(const render &r)
{
    if (m_hide)
        return;

    const auto green = nya_math::vec4(103,223,144,255)/255.0;
    const auto red = nya_math::vec4(200,0,0,255)/255.0;
    const auto blue = nya_math::vec4(100,200,200,255)/255.0;

    wchar_t buf[255];
    swprintf(buf, sizeof(buf), L"%d", m_speed);
    m_fonts.draw_text(r, L"SPEED", "NowGE20", r.get_width()/2 - 210, r.get_height()/2 - 30, green);
    m_fonts.draw_text(r, buf, "NowGE20", r.get_width()/2 - 210, r.get_height()/2 - 7, green);
    swprintf(buf, sizeof(buf), L"%d", m_alt);
    m_fonts.draw_text(r, L"ALT", "NowGE20", r.get_width()/2 + 170, r.get_height()/2 - 30, green);
    m_fonts.draw_text(r, buf, "NowGE20", r.get_width()/2 + 170, r.get_height()/2 - 7, green);

    //m_common.debug_draw_tx(r);
    //m_aircraft.debug_draw_tx(r);
    //m_common.debug_draw(r, debug_variable::get()); static int last_idx = 0; if (last_idx != debug_variable::get()) printf("%d %d\n", debug_variable::get(), m_common.get_id(debug_variable::get())); last_idx = debug_variable::get();
    //m_aircraft.debug_draw(r, debug_variable::get()); static int last_idx = 0; if (last_idx != debug_variable::get()) printf("%d %d\n", debug_variable::get(), m_aircraft.get_id(debug_variable::get())); last_idx = debug_variable::get();

    //m_common.draw(r, 3, green);
    //m_common.draw(r, 158, green);
    //m_common.draw(r, 159, green);
    //m_common.draw(r, 214, green);

    nya_math::vec2 proj_pos;
    if (get_project_pos(m_project_pos, proj_pos))
    {
        //m_common.draw(r, 215, r.get_width() * proj_pos.x, r.get_height() * proj_pos.y, green);
        m_common.draw(r, 2, r.get_width() * proj_pos.x, r.get_height() * proj_pos.y, green);
        //ToDo
    }

    for (auto &t: m_targets)
    {
        if (!get_project_pos(t.pos, proj_pos))
            continue;

        int icon = 102;
        auto color = green;

        const bool far = (nya_scene::get_camera().get_pos() - t.pos).length() > 10000.0;
        if (far)
            icon = 121;

        if (t.target == target_air_lock)
        {
            color = red;
            m_common.draw(r, 100, r.get_width() * proj_pos.x, r.get_height() * proj_pos.y, red);
        }

        if (t.target == target_air_ally)
        {
            icon = 106;
            color = blue;
        }

        m_common.draw(r, icon, r.get_width() * proj_pos.x, r.get_height() * proj_pos.y, color);
        m_common.draw(r, icon + 1, r.get_width() * proj_pos.x, r.get_height() * proj_pos.y, color);

        if (t.select != select_not)
            m_common.draw(r, t.select == select_current ? 117 : 116, r.get_width() * proj_pos.x, r.get_height() * proj_pos.y, color);
    }

    m_common.draw(r, 10, r.get_width()/2 - 150, r.get_height()/2, green);
    m_common.draw(r, 16, r.get_width()/2 + 150, r.get_height()/2, green);

    m_aircraft.draw(r, m_missiles_icon + 401, r.get_width() - 110, r.get_height() - 60, green);
    m_aircraft.draw(r, m_missiles_icon + 406, r.get_width() - 110, r.get_height() - 60, green);
}

//------------------------------------------------------------

void hud::set_missiles(const char *id, int icon)
{
    //ToDo
    m_missiles_icon = icon;
}

//------------------------------------------------------------

void hud::set_missile_reload(int idx, float value)
{
    m_aircraft.set_progress(m_missiles_icon + 406, idx, value);
}

//------------------------------------------------------------

void hud::add_target(const nya_math::vec3 &pos, target_type target, select_type select)
{
    m_targets.push_back({pos, target, select});
}

//------------------------------------------------------------
}
<commit_msg>hud fix<commit_after>//
// open horizon -- undefined_darkness@outlook.com
//

#include "hud.h"
#include "renderer/shared.h"
#include "scene/camera.h"

namespace gui
{

//------------------------------------------------------------

inline bool get_project_pos(const render &r, const nya_math::vec3 &pos, nya_math::vec2 &result)
{
    nya_math::vec4 p(pos, 1.0);
    p = nya_scene::get_camera().get_view_matrix() * nya_render::get_projection_matrix() * p;
    if (p.z < 0)
        return false;

    const float aspect = float(r.get_width(true)) / r.get_height(true) / (float(r.get_width()) / r.get_height() );
    if (aspect > 1.0)
        p.x *= aspect;
    else
        p.y /= aspect;

    result.x = (p.x / p.w * 0.5 + 0.5) * r.get_width();
    result.y = (p.y / p.w * -0.5 + 0.5) * r.get_height();
    return true;
}

//------------------------------------------------------------

void hud::load(const char *aircraft_name)
{
    *this = hud(); //release

    if (!aircraft_name)
        return;

    if (!m_common_loaded)
    {
        m_fonts.load("Hud/hudCommon.fhm");
        m_common.load("Hud/hudCommon.fhm");
        m_common_loaded = true;
    }

    m_aircraft.load(("Hud/aircraft/hudAircraft_" + std::string(aircraft_name) + ".fhm").c_str());
}

//------------------------------------------------------------

void hud::draw(const render &r)
{
    if (m_hide)
        return;

    const auto green = nya_math::vec4(103,223,144,255)/255.0;
    const auto red = nya_math::vec4(200,0,0,255)/255.0;
    const auto blue = nya_math::vec4(100,200,200,255)/255.0;

    wchar_t buf[255];
    swprintf(buf, sizeof(buf), L"%d", m_speed);
    m_fonts.draw_text(r, L"SPEED", "NowGE20", r.get_width()/2 - 210, r.get_height()/2 - 30, green);
    m_fonts.draw_text(r, buf, "NowGE20", r.get_width()/2 - 210, r.get_height()/2 - 7, green);
    swprintf(buf, sizeof(buf), L"%d", m_alt);
    m_fonts.draw_text(r, L"ALT", "NowGE20", r.get_width()/2 + 170, r.get_height()/2 - 30, green);
    m_fonts.draw_text(r, buf, "NowGE20", r.get_width()/2 + 170, r.get_height()/2 - 7, green);

    //m_common.debug_draw_tx(r);
    //m_aircraft.debug_draw_tx(r);
    //m_common.debug_draw(r, debug_variable::get()); static int last_idx = 0; if (last_idx != debug_variable::get()) printf("%d %d\n", debug_variable::get(), m_common.get_id(debug_variable::get())); last_idx = debug_variable::get();
    //m_aircraft.debug_draw(r, debug_variable::get()); static int last_idx = 0; if (last_idx != debug_variable::get()) printf("%d %d\n", debug_variable::get(), m_aircraft.get_id(debug_variable::get())); last_idx = debug_variable::get();

    //m_common.draw(r, 3, green);
    //m_common.draw(r, 158, green);
    //m_common.draw(r, 159, green);
    //m_common.draw(r, 214, green);

    nya_math::vec2 proj_pos;
    if (get_project_pos(r, m_project_pos, proj_pos))
    {
        //m_common.draw(r, 215, proj_pos.x, proj_pos.y, green);
        m_common.draw(r, 2, proj_pos.x, proj_pos.y, green);
        //ToDo
    }

    for (auto &t: m_targets)
    {
        if (!get_project_pos(r, t.pos, proj_pos))
            continue;

        int icon = 102;
        auto color = green;

        const bool far = (nya_scene::get_camera().get_pos() - t.pos).length() > 10000.0;
        if (far)
            icon = 121;

        if (t.target == target_air_lock)
        {
            color = red;
            m_common.draw(r, 100, proj_pos.x, proj_pos.y, red);
        }

        if (t.target == target_air_ally)
        {
            icon = 106;
            color = blue;
        }

        m_common.draw(r, icon, proj_pos.x, proj_pos.y, color);
        m_common.draw(r, icon + 1, proj_pos.x, proj_pos.y, color);

        if (t.select != select_not)
            m_common.draw(r, t.select == select_current ? 117 : 116, proj_pos.x, proj_pos.y, color);
    }

    m_common.draw(r, 10, r.get_width()/2 - 150, r.get_height()/2, green);
    m_common.draw(r, 16, r.get_width()/2 + 150, r.get_height()/2, green);

    m_aircraft.draw(r, m_missiles_icon + 401, r.get_width() - 110, r.get_height() - 60, green);
    m_aircraft.draw(r, m_missiles_icon + 406, r.get_width() - 110, r.get_height() - 60, green);
}

//------------------------------------------------------------

void hud::set_missiles(const char *id, int icon)
{
    //ToDo
    m_missiles_icon = icon;
}

//------------------------------------------------------------

void hud::set_missile_reload(int idx, float value)
{
    m_aircraft.set_progress(m_missiles_icon + 406, idx, value);
}

//------------------------------------------------------------

void hud::add_target(const nya_math::vec3 &pos, target_type target, select_type select)
{
    m_targets.push_back({pos, target, select});
}

//------------------------------------------------------------
}
<|endoftext|>


<commit_before><commit_msg>Update Matrix.cpp<commit_after><|endoftext|>


<commit_before><commit_msg>INTEGRATION: CWS warnings01 (1.14.10); FILE MERGED 2005/11/21 08:42:39 sb 1.14.10.3: #i53898# Made code warning-free. 2005/09/22 20:44:11 sb 1.14.10.2: RESYNC: (1.14-1.15); FILE MERGED 2005/08/31 13:43:20 sb 1.14.10.1: #i53898# Made code warning-free.<commit_after><|endoftext|>