<commit_before>// Copyright 2014 Jeff Donahue

#include <fstream>
#include <map>
#include <string>
#include <sstream>

#include "caffe/util/insert_splits.hpp"

using std::map;
using std::ostringstream;

namespace caffe {

void insert_splits(const NetParameter& param, NetParameter* param_split) {
  // Initialize by copying from the input NetParameter.
  param_split->CopyFrom(param);
  param_split->clear_layers();
  map<string, int> blob_name_to_bottom_count;
  map<string, int> blob_name_to_bottom_split_idx;
  // Determine the number of times each blob is used as an input (bottom) blob.
  for (int i = 0; i < param.layers_size(); ++i) {
    const LayerConnection& layer_connection = param.layers(i);
    for (int j = 0; j < layer_connection.bottom_size(); ++j) {
      const string& blob_name = layer_connection.bottom(j);
      ++blob_name_to_bottom_count[blob_name];
    }
  }
  // Create split layer for any input blobs used by other layers as bottom
  // blobs more than once.
  for (int i = 0; i < param.input_size(); ++i) {
    const string& blob_name = param.input(i);
    const int split_count = blob_name_to_bottom_count[blob_name];
    if (split_count > 1) {
      LayerConnection* split_layer_connection = param_split->add_layers();
      configure_split_layer(blob_name, split_count, split_layer_connection);
    }
  }
  for (int i = 0; i < param.layers_size(); ++i) {
    LayerConnection* layer_connection = param_split->add_layers();
    layer_connection->CopyFrom(param.layers(i));
    // Replace any shared bottom blobs with split layer outputs.
    for (int j = 0; j < layer_connection->bottom_size(); ++j) {
      const string& blob_name = layer_connection->bottom(j);
      const int split_count = blob_name_to_bottom_count[blob_name];
      if (split_count > 1) {
        layer_connection->set_bottom(j, get_split_blob_name(blob_name,
            blob_name_to_bottom_split_idx[blob_name]++));
      }
    }
    // Create split layer for any top blobs used by other layers as bottom
    // blobs more than once.
    for (int j = 0; j < layer_connection->top_size(); ++j) {
      const string& blob_name = layer_connection->top(j);
      const int split_count = blob_name_to_bottom_count[blob_name];
      if (split_count > 1) {
        LayerConnection* split_layer_connection = param_split->add_layers();
        configure_split_layer(blob_name, split_count, split_layer_connection);
      }
    }
  }
}

void configure_split_layer(const string& blob_name,
    const int split_count, LayerConnection* split_layer_connection) {
  split_layer_connection->Clear();
  split_layer_connection->add_bottom(blob_name);
  LayerParameter* split_layer_param = split_layer_connection->mutable_layer();
  split_layer_param->set_name(blob_name + "_split");
  split_layer_param->set_type("split");
  for (int k = 0; k < split_count; ++k) {
    split_layer_connection->add_top(get_split_blob_name(blob_name, k));
  }
}

string get_split_blob_name(const string& blob_name, const int split_index) {
  // 0th split top blob is given the same name as the bottom blob so that
  // computation is done 'in-place', saving a bit of time and memory.
  if (split_index == 0) {
    return blob_name;
  }
  ostringstream split_blob_name;
  split_blob_name << blob_name << "_split_" << split_index;
  return split_blob_name.str();
}

}  // namespace caffe
<commit_msg>remove unnecessary include<commit_after>// Copyright 2014 Jeff Donahue

#include <map>
#include <string>
#include <sstream>

#include "caffe/util/insert_splits.hpp"

using std::map;
using std::ostringstream;

namespace caffe {

void insert_splits(const NetParameter& param, NetParameter* param_split) {
  // Initialize by copying from the input NetParameter.
  param_split->CopyFrom(param);
  param_split->clear_layers();
  map<string, int> blob_name_to_bottom_count;
  map<string, int> blob_name_to_bottom_split_idx;
  // Determine the number of times each blob is used as an input (bottom) blob.
  for (int i = 0; i < param.layers_size(); ++i) {
    const LayerConnection& layer_connection = param.layers(i);
    for (int j = 0; j < layer_connection.bottom_size(); ++j) {
      const string& blob_name = layer_connection.bottom(j);
      ++blob_name_to_bottom_count[blob_name];
    }
  }
  // Create split layer for any input blobs used by other layers as bottom
  // blobs more than once.
  for (int i = 0; i < param.input_size(); ++i) {
    const string& blob_name = param.input(i);
    const int split_count = blob_name_to_bottom_count[blob_name];
    if (split_count > 1) {
      LayerConnection* split_layer_connection = param_split->add_layers();
      configure_split_layer(blob_name, split_count, split_layer_connection);
    }
  }
  for (int i = 0; i < param.layers_size(); ++i) {
    LayerConnection* layer_connection = param_split->add_layers();
    layer_connection->CopyFrom(param.layers(i));
    // Replace any shared bottom blobs with split layer outputs.
    for (int j = 0; j < layer_connection->bottom_size(); ++j) {
      const string& blob_name = layer_connection->bottom(j);
      const int split_count = blob_name_to_bottom_count[blob_name];
      if (split_count > 1) {
        layer_connection->set_bottom(j, get_split_blob_name(blob_name,
            blob_name_to_bottom_split_idx[blob_name]++));
      }
    }
    // Create split layer for any top blobs used by other layers as bottom
    // blobs more than once.
    for (int j = 0; j < layer_connection->top_size(); ++j) {
      const string& blob_name = layer_connection->top(j);
      const int split_count = blob_name_to_bottom_count[blob_name];
      if (split_count > 1) {
        LayerConnection* split_layer_connection = param_split->add_layers();
        configure_split_layer(blob_name, split_count, split_layer_connection);
      }
    }
  }
}

void configure_split_layer(const string& blob_name,
    const int split_count, LayerConnection* split_layer_connection) {
  split_layer_connection->Clear();
  split_layer_connection->add_bottom(blob_name);
  LayerParameter* split_layer_param = split_layer_connection->mutable_layer();
  split_layer_param->set_name(blob_name + "_split");
  split_layer_param->set_type("split");
  for (int k = 0; k < split_count; ++k) {
    split_layer_connection->add_top(get_split_blob_name(blob_name, k));
  }
}

string get_split_blob_name(const string& blob_name, const int split_index) {
  // 0th split top blob is given the same name as the bottom blob so that
  // computation is done 'in-place', saving a bit of time and memory.
  if (split_index == 0) {
    return blob_name;
  }
  ostringstream split_blob_name;
  split_blob_name << blob_name << "_split_" << split_index;
  return split_blob_name.str();
}

}  // namespace caffe
<|endoftext|>


<commit_before>//
// Copyright (c) 2008-2016 the Urho3D project.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.
//

#include <Urho3D/Core/CoreEvents.h>
#include <Urho3D/Core/ProcessUtils.h>
#include <Urho3D/Input/Input.h>
#include <Urho3D/Network/HttpRequest.h>
#include <Urho3D/UI/Font.h>
#include <Urho3D/UI/Text.h>
#include <Urho3D/UI/UI.h>

#include "HttpRequestDemo.h"

#include <Urho3D/DebugNew.h>

URHO3D_DEFINE_APPLICATION_MAIN(HttpRequestDemo)

HttpRequestDemo::HttpRequestDemo(Context* context) :
    Sample(context)
{
}

void HttpRequestDemo::Start()
{
    // Execute base class startup
    Sample::Start();

    // Create the user interface
    CreateUI();

    // Subscribe to basic events such as update
    SubscribeToEvents();

    // Set the mouse mode to use in the sample
    Sample::InitMouseMode(MM_FREE);
}

void HttpRequestDemo::CreateUI()
{
    ResourceCache* cache = GetSubsystem<ResourceCache>();

    // Construct new Text object
    text_ = new Text(context_);

    // Set font and text color
    text_->SetFont(cache->GetResource<Font>("Fonts/Anonymous Pro.ttf"), 15);
    text_->SetColor(Color(1.0f, 1.0f, 0.0f));

    // Align Text center-screen
    text_->SetHorizontalAlignment(HA_CENTER);
    text_->SetVerticalAlignment(VA_CENTER);

    // Add Text instance to the UI root element
    GetSubsystem<UI>()->GetRoot()->AddChild(text_);
}

void HttpRequestDemo::SubscribeToEvents()
{
    // Subscribe HandleUpdate() function for processing HTTP request
    SubscribeToEvent(E_UPDATE, URHO3D_HANDLER(HttpRequestDemo, HandleUpdate));
}

void HttpRequestDemo::HandleUpdate(StringHash eventType, VariantMap& eventData)
{
    if (httpRequest_.Null())
        httpRequest_ = new HttpRequest("http://httpbin.org/ip", "GET", {}, "");
    else
    {
        // Initializing HTTP request
        if (httpRequest_->GetState() == HTTP_INITIALIZING)
            return;
        // An error has occured
        else if (httpRequest_->GetState() == HTTP_ERROR)
        {
            text_->SetText("An error has occured.");
            UnsubscribeFromEvent("Update");
        }
        // Get message data
        else
        {
            if (httpRequest_->GetAvailableSize() > 0)
                message_ += httpRequest_->ReadLine();
            else
            {
                text_->SetText("Processing...");

                SharedPtr<JSONFile> json(new JSONFile(context_));
                json->FromString(message_);

                JSONValue val = json->GetRoot().Get("origin");

                if (val.IsNull())
                    text_->SetText("Invalid string.");
                else
                    text_->SetText("Your IP is: " + val.GetString());

                UnsubscribeFromEvent("Update");
            }
        }
    }
}
<commit_msg>Fix Clang compilation errors.<commit_after>//
// Copyright (c) 2008-2016 the Urho3D project.
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.
//

#include <Urho3D/Core/CoreEvents.h>
#include <Urho3D/Core/ProcessUtils.h>
#include <Urho3D/Input/Input.h>
#include <Urho3D/Network/HttpRequest.h>
#include <Urho3D/UI/Font.h>
#include <Urho3D/UI/Text.h>
#include <Urho3D/UI/UI.h>

#include "HttpRequestDemo.h"

#include <Urho3D/DebugNew.h>

URHO3D_DEFINE_APPLICATION_MAIN(HttpRequestDemo)

HttpRequestDemo::HttpRequestDemo(Context* context) :
    Sample(context)
{
}

void HttpRequestDemo::Start()
{
    // Execute base class startup
    Sample::Start();

    // Create the user interface
    CreateUI();

    // Subscribe to basic events such as update
    SubscribeToEvents();

    // Set the mouse mode to use in the sample
    Sample::InitMouseMode(MM_FREE);
}

void HttpRequestDemo::CreateUI()
{
    ResourceCache* cache = GetSubsystem<ResourceCache>();

    // Construct new Text object
    text_ = new Text(context_);

    // Set font and text color
    text_->SetFont(cache->GetResource<Font>("Fonts/Anonymous Pro.ttf"), 15);
    text_->SetColor(Color(1.0f, 1.0f, 0.0f));

    // Align Text center-screen
    text_->SetHorizontalAlignment(HA_CENTER);
    text_->SetVerticalAlignment(VA_CENTER);

    // Add Text instance to the UI root element
    GetSubsystem<UI>()->GetRoot()->AddChild(text_);
}

void HttpRequestDemo::SubscribeToEvents()
{
    // Subscribe HandleUpdate() function for processing HTTP request
    SubscribeToEvent(E_UPDATE, URHO3D_HANDLER(HttpRequestDemo, HandleUpdate));
}

void HttpRequestDemo::HandleUpdate(StringHash eventType, VariantMap& eventData)
{
    if (httpRequest_.Null())
        httpRequest_ = new HttpRequest("http://httpbin.org/ip", "GET", Vector<String>(), String::EMPTY);
    else
    {
        // Initializing HTTP request
        if (httpRequest_->GetState() == HTTP_INITIALIZING)
            return;
        // An error has occured
        else if (httpRequest_->GetState() == HTTP_ERROR)
        {
            text_->SetText("An error has occured.");
            UnsubscribeFromEvent("Update");
        }
        // Get message data
        else
        {
            if (httpRequest_->GetAvailableSize() > 0)
                message_ += httpRequest_->ReadLine();
            else
            {
                text_->SetText("Processing...");

                SharedPtr<JSONFile> json(new JSONFile(context_));
                json->FromString(message_);

                JSONValue val = json->GetRoot().Get("origin");

                if (val.IsNull())
                    text_->SetText("Invalid string.");
                else
                    text_->SetText("Your IP is: " + val.GetString());

                UnsubscribeFromEvent("Update");
            }
        }
    }
}
<|endoftext|>


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

  Program:   Insight Segmentation & Registration Toolkit
  Module:    itkPlaheImageFilterTest.cxx
  Language:  C++
  Date:      $Date$
  Version:   $Revision$

  Copyright (c) 2002 Insight Consortium. All rights reserved.
  See ITKCopyright.txt or http://www.itk.org/HTML/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 notices for more information.

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

// Disable warning for long symbol names in this file only
#ifdef _MSC_VER
#pragma warning ( disable : 4786 )
#endif

#include "itkImage.h"
#include "itkPlaheImageFilter.h"
#include "itkGaussianImageSource.h"

int itkPlaheImageFilterTest(int, char* [] )
{

  // Define the dimension of the images
  const unsigned int myDimension = 2;

  // Define the pixel type of the images
  typedef   float           myPixelType;

  // Declare the types of the images
  typedef itk::Image<myPixelType, myDimension>  myImageType;

  // Declare the type of the index to access images
  typedef myImageType::IndexType          myIndexType;

  // Declare the type of the size
  typedef myImageType::SizeType           mySizeType;

  // Declare the type of the Region
  typedef myImageType::RegionType         myRegionType;

  // Declare the type for the filter
  typedef itk::PlaheImageFilter< myImageType > myFilterType;

  // Declare the type for the image source
  typedef itk::GaussianImageSource< myImageType > mySourceType;

  // Declare the pointers to images
  typedef myImageType::Pointer    myImageTypePointer;

  // Declare the pointers to the filter
  typedef myFilterType::Pointer   myFilterTypePointer;

  // Declare the pointers to the image source
  typedef mySourceType::Pointer   mySourceTypePointer;

  // Create the image source
  mySourceTypePointer imageSource = mySourceType::New();

  // Define their size, and start index
  unsigned long usize[ myDimension ];
  usize[0] = 20;
  usize[1] = 20;

  mySizeType size;
  size[0] = usize[0];
  size[1] = usize[1];

  myIndexType start;
  start[0] = 0;
  start[1] = 0;

  float spacing[ myDimension ];
  spacing[0] = 1.0;
  spacing[1] = 1.0;

  float origin[ myDimension ];
  origin[0] = 1.0;
  origin[1] = 1.0;

  myRegionType region;
  region.SetIndex( start );
  region.SetSize( size );

  imageSource->SetSize( usize );
  imageSource->SetOrigin( origin );
  imageSource->SetSpacing( spacing );

  // Create the Filter
  myFilterTypePointer filter = myFilterType::New();

  // window[] is a neighborhood of a evaluated pixel
  unsigned int pixelWindow[ myDimension ];
  pixelWindow[0] = 9;
  pixelWindow[1] = 9;

  filter->SetWindow( pixelWindow );
  filter->SetAlpha( 0.3 );
  filter->SetBeta(  0.3 );

  // Connect the input images
  filter->SetInput( imageSource->GetOutput() );

  // Get the Smart Pointer to the Filter Output
  myImageTypePointer outputImage = filter->GetOutput();

  // Execute the filter
  filter->Update();

  // All objects should be automatically destroyed at this point
  return EXIT_SUCCESS;

}

<commit_msg>FIX: API change<commit_after>/*=========================================================================

  Program:   Insight Segmentation & Registration Toolkit
  Module:    itkPlaheImageFilterTest.cxx
  Language:  C++
  Date:      $Date$
  Version:   $Revision$

  Copyright (c) 2002 Insight Consortium. All rights reserved.
  See ITKCopyright.txt or http://www.itk.org/HTML/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 notices for more information.

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

// Disable warning for long symbol names in this file only
#ifdef _MSC_VER
#pragma warning ( disable : 4786 )
#endif

#include "itkImage.h"
#include "itkPlaheImageFilter.h"
#include "itkGaussianImageSource.h"

int itkPlaheImageFilterTest(int, char* [] )
{

  // Define the dimension of the images
  const unsigned int myDimension = 2;

  // Define the pixel type of the images
  typedef   float           myPixelType;

  // Declare the types of the images
  typedef itk::Image<myPixelType, myDimension>  myImageType;

  // Declare the type of the index to access images
  typedef myImageType::IndexType          myIndexType;

  // Declare the type of the size
  typedef myImageType::SizeType           mySizeType;

  // Declare the type of the Region
  typedef myImageType::RegionType         myRegionType;

  // Declare the type for the filter
  typedef itk::PlaheImageFilter< myImageType > myFilterType;

  // Declare the type for the image source
  typedef itk::GaussianImageSource< myImageType > mySourceType;

  // Declare the pointers to images
  typedef myImageType::Pointer    myImageTypePointer;

  // Declare the pointers to the filter
  typedef myFilterType::Pointer   myFilterTypePointer;

  // Declare the pointers to the image source
  typedef mySourceType::Pointer   mySourceTypePointer;

  // Create the image source
  mySourceTypePointer imageSource = mySourceType::New();

  // Define their size, and start index
  unsigned long usize[ myDimension ];
  usize[0] = 20;
  usize[1] = 20;

  mySizeType size;
  size[0] = usize[0];
  size[1] = usize[1];

  myIndexType start;
  start[0] = 0;
  start[1] = 0;

  float spacing[ myDimension ];
  spacing[0] = 1.0;
  spacing[1] = 1.0;

  float origin[ myDimension ];
  origin[0] = 1.0;
  origin[1] = 1.0;

  myRegionType region;
  region.SetIndex( start );
  region.SetSize( size );

  imageSource->SetSize( usize );
  imageSource->SetOrigin( origin );
  imageSource->SetSpacing( spacing );

  // Create the Filter
  myFilterTypePointer filter = myFilterType::New();

  // window[] is a neighborhood of a evaluated pixel
  myFilterType::ImageSizeType radius;
  radius.Fill(9);

  filter->SetRadius( radius );
  filter->SetAlpha( 0.3 );
  filter->SetBeta(  0.3 );

  // Connect the input images
  filter->SetInput( imageSource->GetOutput() );

  // Get the Smart Pointer to the Filter Output
  myImageTypePointer outputImage = filter->GetOutput();

  // Execute the filter
  filter->Update();

  // All objects should be automatically destroyed at this point
  return EXIT_SUCCESS;

}

<|endoftext|>


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

  Program:   Visualization Toolkit
  Module:    vtkUnstructuredGridPreIntegration.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 "vtkUnstructuredGridPreIntegration.h"

#include "vtkObjectFactory.h"
#include "vtkVolumeProperty.h"
#include "vtkVolume.h"
#include "vtkAbstractVolumeMapper.h"
#include "vtkUnstructuredGrid.h"
#include "vtkDoubleArray.h"
#include "vtkUnstructuredGridPartialPreIntegration.h"

#include <vtkstd/algorithm>
#include <math.h>

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

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

vtkCxxSetObjectMacro(vtkUnstructuredGridPreIntegration, Integrator,
                     vtkUnstructuredGridVolumeRayIntegrator);

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

vtkUnstructuredGridPreIntegration::vtkUnstructuredGridPreIntegration()
{
  this->Integrator = vtkUnstructuredGridPartialPreIntegration::New();
  this->Property = NULL;

  this->NumComponents = 0;
  this->IntegrationTable = NULL;
  this->IntegrationTableScalarShift = NULL;
  this->IntegrationTableScalarScale = NULL;

  this->IntegrationTableScalarResolution = 128;
  this->IntegrationTableLengthResolution = 256;

  this->IncrementalPreIntegration = 1;
}

vtkUnstructuredGridPreIntegration::~vtkUnstructuredGridPreIntegration()
{
  this->SetIntegrator(NULL);

  if (this->IntegrationTable)
    {
    for (int i = 0; i < this->NumComponents; i++)
      {
      delete[] this->IntegrationTable[i];
      }
    delete[] this->IntegrationTable;
    }
  if (this->IntegrationTableScalarShift)
    {
    delete[] this->IntegrationTableScalarShift;
    }
  if (this->IntegrationTableScalarScale)
    {
    delete[] this->IntegrationTableScalarScale;
    }
}

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

  os << indent << "Integrator: " << this->Integrator << endl;

  os << indent << "IntegrationTableScalarResolution: "
     << this->IntegrationTableScalarResolution << endl;
  os << indent << "IntegrationTableLengthResolution: "
     << this->IntegrationTableLengthResolution << endl;

  os << indent << "IncrementalPreIntegration: "
     << this->IncrementalPreIntegration << endl;
}

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

double vtkUnstructuredGridPreIntegration::GetIntegrationTableScalarShift(int component)
{
  return this->IntegrationTableScalarShift[component];
}

double vtkUnstructuredGridPreIntegration::GetIntegrationTableScalarScale(int component)
{
  return this->IntegrationTableScalarScale[component];
}

double vtkUnstructuredGridPreIntegration::GetIntegrationTableLengthScale()
{
  return this->IntegrationTableLengthScale;
}

float *vtkUnstructuredGridPreIntegration::GetPreIntegrationTable(int component)
{
  return this->IntegrationTable[component];
}

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

void vtkUnstructuredGridPreIntegration::BuildPreIntegrationTables(vtkDataArray *scalars)
{
  // Establish temporary inputs to integrator.
  vtkVolume         *tmpVolume = vtkVolume::New();
  vtkVolumeProperty *tmpProperty = vtkVolumeProperty::New();
  vtkDataArray      *tmpScalars
    = vtkDataArray::CreateDataArray(scalars->GetDataType());

  tmpVolume->SetMapper(this->Volume->GetMapper());
  tmpVolume->SetProperty(tmpProperty);

  tmpProperty->IndependentComponentsOn();
  tmpProperty->SetInterpolationType(this->Property->GetInterpolationType());

  tmpScalars->SetNumberOfComponents(1);
  tmpScalars->SetNumberOfTuples(2);

  vtkDoubleArray *tmpIntersectionLengths = vtkDoubleArray::New();
  vtkDataArray   *tmpNearIntersections
    = vtkDataArray::CreateDataArray(scalars->GetDataType());
  vtkDataArray   *tmpFarIntersections
    = vtkDataArray::CreateDataArray(scalars->GetDataType());

  tmpIntersectionLengths->SetNumberOfComponents(1);
  tmpIntersectionLengths->SetNumberOfTuples(1);
  tmpNearIntersections->SetNumberOfComponents(1);
  tmpNearIntersections->SetNumberOfTuples(1);
  tmpFarIntersections->SetNumberOfComponents(1);
  tmpFarIntersections->SetNumberOfTuples(1);

  this->IntegrationTable = new float*[this->NumComponents];
  this->IntegrationTableScalarShift = new double[this->NumComponents];
  this->IntegrationTableScalarScale = new double[this->NumComponents];
  // Note that the scale set up such that a length of (this->MaxLength +
  // epsilon + 0.5) will scale to this->IntegrationTableLengthResolution-1.
  // Similar scaling is performed for the other dimensions of the
  // pre-integration table.
  this->IntegrationTableLengthScale
    = (this->IntegrationTableLengthResolution-2)/this->MaxLength;

  // We only do computations at one length.
  float d_length = (float)(1/this->IntegrationTableLengthScale);

  for (int component = 0; component < this->NumComponents; component++)
    {
    int d_idx, sb_idx, sf_idx;
    float *c;

    // Allocate table.
    this->IntegrationTable[component]
      = new float[4*this->IntegrationTableScalarResolution
                 *this->IntegrationTableScalarResolution
                 *this->IntegrationTableLengthResolution];
    // Determine scale and shift.
    double *range = scalars->GetRange(component);
    this->IntegrationTableScalarScale[component]
      = (this->IntegrationTableScalarResolution-2)/(range[1]-range[0]);
    this->IntegrationTableScalarShift[component]
      = -range[0]*this->IntegrationTableScalarScale[component];

    // Set values for d=0 (they are all zero).
    c = this->IntegrationTable[component];
    for (sb_idx = 0; sb_idx < this->IntegrationTableScalarResolution; sb_idx++)
      {
      for (sf_idx = 0; sf_idx < this->IntegrationTableScalarResolution;
           sf_idx++)
        {
        c[0] = c[1] = c[2] = c[3] = 0.0f;
        c += 4;
        }
      }

    // Initialize integrator.
    if (this->Property->GetColorChannels(component) == 3)
      {
      tmpProperty->SetColor(this->Property->GetRGBTransferFunction(component));
      }
    else
      {
      tmpProperty->SetColor(this->Property->GetGrayTransferFunction(component));
      }
    tmpProperty->SetScalarOpacity(this->Property->GetScalarOpacity(component));
    tmpProperty->SetScalarOpacityUnitDistance
      (this->Property->GetScalarOpacityUnitDistance(component));
    tmpProperty->SetShade(this->Property->GetShade(component));
    tmpProperty->SetAmbient(this->Property->GetAmbient(component));
    tmpProperty->SetDiffuse(this->Property->GetDiffuse(component));
    tmpProperty->SetSpecular(this->Property->GetSpecular(component));
    tmpProperty->SetSpecularPower(this->Property->GetSpecularPower(component));
    tmpScalars->SetTuple1(0, range[0]);
    tmpScalars->SetTuple1(1, range[1]);
    this->Integrator->Initialize(tmpVolume, tmpScalars);

    // Set values for next smallest d (the base values).
    tmpIntersectionLengths->SetTuple1(0, d_length);
    for (sb_idx = 0; sb_idx < this->IntegrationTableScalarResolution; sb_idx++)
      {
      tmpFarIntersections
        ->SetTuple1(0,
                    (  (sb_idx - this->IntegrationTableScalarShift[component])
                     / (this->IntegrationTableScalarScale[component]) ));
      for (sf_idx = 0; sf_idx < this->IntegrationTableScalarResolution;
           sf_idx++)
        {
        tmpNearIntersections
          ->SetTuple1(0,
                      (  (sf_idx - this->IntegrationTableScalarShift[component])
                       / (this->IntegrationTableScalarScale[component]) ));
        c[0] = c[1] = c[2] = c[3] = 0;
        this->Integrator->Integrate(tmpIntersectionLengths,
                                    tmpNearIntersections, tmpFarIntersections,
                                    c);
        c += 4;
        }
      }

    // Set rest of values using other values in table.
    if (this->IncrementalPreIntegration)
      {
      for (d_idx = 2; d_idx < this->IntegrationTableLengthResolution; d_idx++)
        {
        for (sb_idx = 0; sb_idx < this->IntegrationTableScalarResolution;
             sb_idx++)
          {
          for (sf_idx = 0; sf_idx < this->IntegrationTableScalarResolution;
               sf_idx++)
            {
            // We are going to perform incremental pre-integration.  To do
            // this, we compute the integration of a ray from sf to sb of
            // length d by combining two entries in the table.  The first
            // entry will be from sf to sm of length delta d (the smallest
            // non-zero length stored in the table).  The second entry will
            // be from sm to sb of length d - delta d.  See Weiler, et
            // al. "Hardware-Based Ray Casting for Tetrahedral Meshes" for
            // more details.
            int sm_idx = ((d_idx-1)*sf_idx + sb_idx + d_idx/2)/d_idx;

            float *colorf = this->GetIndexedTableEntry(sf_idx, sm_idx, 1,
                                                       component);
            float *colorb = this->GetIndexedTableEntry(sm_idx, sb_idx, d_idx-1,
                                                       component);

            c[0] = colorf[0] + colorb[0]*(1-colorf[3]);
            c[1] = colorf[1] + colorb[1]*(1-colorf[3]);
            c[2] = colorf[2] + colorb[2]*(1-colorf[3]);
            c[3] = colorf[3] + colorb[3]*(1-colorf[3]);
            c += 4;
            }
          }
        }
      }
    else
      {
      for (d_idx = 2; d_idx < this->IntegrationTableLengthResolution; d_idx++)
        {
        for (sb_idx = 0; sb_idx < this->IntegrationTableScalarResolution;
             sb_idx++)
          {
          for (sf_idx = 0; sf_idx < this->IntegrationTableScalarResolution;
               sf_idx++)
            {
            // Compute the integration table the old-fashioned slow way.
            float length = d_idx*d_length;
            float sb = (float)
              (  (sb_idx - this->IntegrationTableScalarShift[component])
               / (this->IntegrationTableScalarScale[component]) );
            float sf = (float)
              (  (sf_idx - this->IntegrationTableScalarShift[component])
               / (this->IntegrationTableScalarScale[component]) );
            tmpIntersectionLengths->SetTuple1(0, length);
            tmpFarIntersections->SetTuple1(0, sb);
            tmpNearIntersections->SetTuple1(0, sf);
            c[0] = c[1] = c[2] = c[3] = 0;
            this->Integrator->Integrate(tmpIntersectionLengths,
                                        tmpNearIntersections,
                                        tmpFarIntersections,
                                        c);
            c += 4;
            }
          }
        }
      }
    }

  // Get rid of temporary data.
  tmpVolume->Delete();
  tmpProperty->Delete();
  tmpScalars->Delete();

  tmpIntersectionLengths->Delete();
  tmpNearIntersections->Delete();
  tmpFarIntersections->Delete();
}

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

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

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

  // Delete old tables.
  for (i = 0; i < this->NumComponents; i++)
    {
    delete[] this->IntegrationTable[i];
    }
  delete[] this->IntegrationTable;
  delete[] this->IntegrationTableScalarShift;
  delete[] this->IntegrationTableScalarScale;

  this->NumComponents = scalars->GetNumberOfComponents();
  this->Property = property;
  this->Volume = volume;
  this->IntegrationTableBuilt.Modified();

  if (!property->GetIndependentComponents())
    {
    vtkErrorMacro("Cannot store dependent components in pre-integration table.");
    return;
    }

  // Determine the maximum possible length of a ray segment.
  vtkDataSet *input = volume->GetMapper()->GetDataSetInput();
  vtkIdType numcells = input->GetNumberOfCells();
  this->MaxLength = 0;
  for (i = 0; i < numcells; i++)
    {
    double cellbounds[6];
    input->GetCellBounds(i, cellbounds);
#define SQR(x)  ((x)*(x))
    double diagonal_length = sqrt(  SQR(cellbounds[1]-cellbounds[0])
                                  + SQR(cellbounds[3]-cellbounds[2])
                                  + SQR(cellbounds[5]-cellbounds[4]) );
#undef SQR
    if (diagonal_length > this->MaxLength)
      {
      this->MaxLength = diagonal_length;
      }
    }

  this->BuildPreIntegrationTables(scalars);
}

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

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

  for (vtkIdType i = 0; i < numIntersections; i++)
    {
    float newcolor[4];
    float *c;
    c = this->GetTableEntry(nearIntersections->GetComponent(i, 0),
                            farIntersections->GetComponent(i, 0),
                            intersectionLengths->GetComponent(i, 0), 0);
    newcolor[0] = c[0];  newcolor[1] = c[1];
    newcolor[2] = c[2];  newcolor[3] = c[3];
    for (int component = 1; component < this->NumComponents; component++)
      {
      c = this->GetTableEntry(nearIntersections->GetComponent(i, component),
                              farIntersections->GetComponent(i, component),
                              intersectionLengths->GetComponent(i, 0),
                              component);
      // The blending I'm using is a combination of porter and duff xors
      // and ins.
      float coef1=1-0.5f*c[3];
      float coef2=1-0.5f*newcolor[3];
      newcolor[0] = newcolor[0]*coef1 + c[0]*coef2;
      newcolor[1] = newcolor[1]*coef1 + c[1]*coef2;
      newcolor[2] = newcolor[2]*coef1 + c[2]*coef2;
      newcolor[3] = newcolor[3]*coef1 + c[3]*coef2;
      }

    float coef=1-color[3];
    color[0] += newcolor[0]*coef;
    color[1] += newcolor[1]*coef;
    color[2] += newcolor[2]*coef;
    color[3] += newcolor[3]*coef;
    }
}

<commit_msg>BUG: Handle transfer functions with a constant scalar.<commit_after>/*=========================================================================

  Program:   Visualization Toolkit
  Module:    vtkUnstructuredGridPreIntegration.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 "vtkUnstructuredGridPreIntegration.h"

#include "vtkObjectFactory.h"
#include "vtkVolumeProperty.h"
#include "vtkVolume.h"
#include "vtkAbstractVolumeMapper.h"
#include "vtkUnstructuredGrid.h"
#include "vtkDoubleArray.h"
#include "vtkUnstructuredGridPartialPreIntegration.h"

#include <vtkstd/algorithm>
#include <math.h>

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

vtkCxxRevisionMacro(vtkUnstructuredGridPreIntegration, "1.3");
vtkStandardNewMacro(vtkUnstructuredGridPreIntegration);

vtkCxxSetObjectMacro(vtkUnstructuredGridPreIntegration, Integrator,
                     vtkUnstructuredGridVolumeRayIntegrator);

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

vtkUnstructuredGridPreIntegration::vtkUnstructuredGridPreIntegration()
{
  this->Integrator = vtkUnstructuredGridPartialPreIntegration::New();
  this->Property = NULL;

  this->NumComponents = 0;
  this->IntegrationTable = NULL;
  this->IntegrationTableScalarShift = NULL;
  this->IntegrationTableScalarScale = NULL;

  this->IntegrationTableScalarResolution = 128;
  this->IntegrationTableLengthResolution = 256;

  this->IncrementalPreIntegration = 1;
}

vtkUnstructuredGridPreIntegration::~vtkUnstructuredGridPreIntegration()
{
  this->SetIntegrator(NULL);

  if (this->IntegrationTable)
    {
    for (int i = 0; i < this->NumComponents; i++)
      {
      delete[] this->IntegrationTable[i];
      }
    delete[] this->IntegrationTable;
    }
  if (this->IntegrationTableScalarShift)
    {
    delete[] this->IntegrationTableScalarShift;
    }
  if (this->IntegrationTableScalarScale)
    {
    delete[] this->IntegrationTableScalarScale;
    }
}

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

  os << indent << "Integrator: " << this->Integrator << endl;

  os << indent << "IntegrationTableScalarResolution: "
     << this->IntegrationTableScalarResolution << endl;
  os << indent << "IntegrationTableLengthResolution: "
     << this->IntegrationTableLengthResolution << endl;

  os << indent << "IncrementalPreIntegration: "
     << this->IncrementalPreIntegration << endl;
}

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

double vtkUnstructuredGridPreIntegration::GetIntegrationTableScalarShift(int component)
{
  return this->IntegrationTableScalarShift[component];
}

double vtkUnstructuredGridPreIntegration::GetIntegrationTableScalarScale(int component)
{
  return this->IntegrationTableScalarScale[component];
}

double vtkUnstructuredGridPreIntegration::GetIntegrationTableLengthScale()
{
  return this->IntegrationTableLengthScale;
}

float *vtkUnstructuredGridPreIntegration::GetPreIntegrationTable(int component)
{
  return this->IntegrationTable[component];
}

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

void vtkUnstructuredGridPreIntegration::BuildPreIntegrationTables(vtkDataArray *scalars)
{
  // Establish temporary inputs to integrator.
  vtkVolume         *tmpVolume = vtkVolume::New();
  vtkVolumeProperty *tmpProperty = vtkVolumeProperty::New();
  vtkDataArray      *tmpScalars
    = vtkDataArray::CreateDataArray(scalars->GetDataType());

  tmpVolume->SetMapper(this->Volume->GetMapper());
  tmpVolume->SetProperty(tmpProperty);

  tmpProperty->IndependentComponentsOn();
  tmpProperty->SetInterpolationType(this->Property->GetInterpolationType());

  tmpScalars->SetNumberOfComponents(1);
  tmpScalars->SetNumberOfTuples(2);

  vtkDoubleArray *tmpIntersectionLengths = vtkDoubleArray::New();
  vtkDataArray   *tmpNearIntersections
    = vtkDataArray::CreateDataArray(scalars->GetDataType());
  vtkDataArray   *tmpFarIntersections
    = vtkDataArray::CreateDataArray(scalars->GetDataType());

  tmpIntersectionLengths->SetNumberOfComponents(1);
  tmpIntersectionLengths->SetNumberOfTuples(1);
  tmpNearIntersections->SetNumberOfComponents(1);
  tmpNearIntersections->SetNumberOfTuples(1);
  tmpFarIntersections->SetNumberOfComponents(1);
  tmpFarIntersections->SetNumberOfTuples(1);

  this->IntegrationTable = new float*[this->NumComponents];
  this->IntegrationTableScalarShift = new double[this->NumComponents];
  this->IntegrationTableScalarScale = new double[this->NumComponents];
  // Note that the scale set up such that a length of (this->MaxLength +
  // epsilon + 0.5) will scale to this->IntegrationTableLengthResolution-1.
  // Similar scaling is performed for the other dimensions of the
  // pre-integration table.
  this->IntegrationTableLengthScale
    = (this->IntegrationTableLengthResolution-2)/this->MaxLength;

  // We only do computations at one length.
  float d_length = (float)(1/this->IntegrationTableLengthScale);

  for (int component = 0; component < this->NumComponents; component++)
    {
    int d_idx, sb_idx, sf_idx;
    float *c;

    // Allocate table.
    this->IntegrationTable[component]
      = new float[4*this->IntegrationTableScalarResolution
                 *this->IntegrationTableScalarResolution
                 *this->IntegrationTableLengthResolution];
    // Determine scale and shift.
    double *range = scalars->GetRange(component);
    if (range[0] == range[1])
      {
      // Unusual case where the scalars are all the same.
      this->IntegrationTableScalarScale[component] = 1.0;
      }
    else
      {
      this->IntegrationTableScalarScale[component]
        = (this->IntegrationTableScalarResolution-2)/(range[1]-range[0]);
      }
    this->IntegrationTableScalarShift[component]
      = -range[0]*this->IntegrationTableScalarScale[component];

    // Set values for d=0 (they are all zero).
    c = this->IntegrationTable[component];
    for (sb_idx = 0; sb_idx < this->IntegrationTableScalarResolution; sb_idx++)
      {
      for (sf_idx = 0; sf_idx < this->IntegrationTableScalarResolution;
           sf_idx++)
        {
        c[0] = c[1] = c[2] = c[3] = 0.0f;
        c += 4;
        }
      }

    // Initialize integrator.
    if (this->Property->GetColorChannels(component) == 3)
      {
      tmpProperty->SetColor(this->Property->GetRGBTransferFunction(component));
      }
    else
      {
      tmpProperty->SetColor(this->Property->GetGrayTransferFunction(component));
      }
    tmpProperty->SetScalarOpacity(this->Property->GetScalarOpacity(component));
    tmpProperty->SetScalarOpacityUnitDistance
      (this->Property->GetScalarOpacityUnitDistance(component));
    tmpProperty->SetShade(this->Property->GetShade(component));
    tmpProperty->SetAmbient(this->Property->GetAmbient(component));
    tmpProperty->SetDiffuse(this->Property->GetDiffuse(component));
    tmpProperty->SetSpecular(this->Property->GetSpecular(component));
    tmpProperty->SetSpecularPower(this->Property->GetSpecularPower(component));
    tmpScalars->SetTuple1(0, range[0]);
    tmpScalars->SetTuple1(1, range[1]);
    this->Integrator->Initialize(tmpVolume, tmpScalars);

    // Set values for next smallest d (the base values).
    tmpIntersectionLengths->SetTuple1(0, d_length);
    for (sb_idx = 0; sb_idx < this->IntegrationTableScalarResolution; sb_idx++)
      {
      tmpFarIntersections
        ->SetTuple1(0,
                    (  (sb_idx - this->IntegrationTableScalarShift[component])
                     / (this->IntegrationTableScalarScale[component]) ));
      for (sf_idx = 0; sf_idx < this->IntegrationTableScalarResolution;
           sf_idx++)
        {
        tmpNearIntersections
          ->SetTuple1(0,
                      (  (sf_idx - this->IntegrationTableScalarShift[component])
                       / (this->IntegrationTableScalarScale[component]) ));
        c[0] = c[1] = c[2] = c[3] = 0;
        this->Integrator->Integrate(tmpIntersectionLengths,
                                    tmpNearIntersections, tmpFarIntersections,
                                    c);
        c += 4;
        }
      }

    // Set rest of values using other values in table.
    if (this->IncrementalPreIntegration)
      {
      for (d_idx = 2; d_idx < this->IntegrationTableLengthResolution; d_idx++)
        {
        for (sb_idx = 0; sb_idx < this->IntegrationTableScalarResolution;
             sb_idx++)
          {
          for (sf_idx = 0; sf_idx < this->IntegrationTableScalarResolution;
               sf_idx++)
            {
            // We are going to perform incremental pre-integration.  To do
            // this, we compute the integration of a ray from sf to sb of
            // length d by combining two entries in the table.  The first
            // entry will be from sf to sm of length delta d (the smallest
            // non-zero length stored in the table).  The second entry will
            // be from sm to sb of length d - delta d.  See Weiler, et
            // al. "Hardware-Based Ray Casting for Tetrahedral Meshes" for
            // more details.
            int sm_idx = ((d_idx-1)*sf_idx + sb_idx + d_idx/2)/d_idx;

            float *colorf = this->GetIndexedTableEntry(sf_idx, sm_idx, 1,
                                                       component);
            float *colorb = this->GetIndexedTableEntry(sm_idx, sb_idx, d_idx-1,
                                                       component);

            c[0] = colorf[0] + colorb[0]*(1-colorf[3]);
            c[1] = colorf[1] + colorb[1]*(1-colorf[3]);
            c[2] = colorf[2] + colorb[2]*(1-colorf[3]);
            c[3] = colorf[3] + colorb[3]*(1-colorf[3]);
            c += 4;
            }
          }
        }
      }
    else
      {
      for (d_idx = 2; d_idx < this->IntegrationTableLengthResolution; d_idx++)
        {
        for (sb_idx = 0; sb_idx < this->IntegrationTableScalarResolution;
             sb_idx++)
          {
          for (sf_idx = 0; sf_idx < this->IntegrationTableScalarResolution;
               sf_idx++)
            {
            // Compute the integration table the old-fashioned slow way.
            float length = d_idx*d_length;
            float sb = (float)
              (  (sb_idx - this->IntegrationTableScalarShift[component])
               / (this->IntegrationTableScalarScale[component]) );
            float sf = (float)
              (  (sf_idx - this->IntegrationTableScalarShift[component])
               / (this->IntegrationTableScalarScale[component]) );
            tmpIntersectionLengths->SetTuple1(0, length);
            tmpFarIntersections->SetTuple1(0, sb);
            tmpNearIntersections->SetTuple1(0, sf);
            c[0] = c[1] = c[2] = c[3] = 0;
            this->Integrator->Integrate(tmpIntersectionLengths,
                                        tmpNearIntersections,
                                        tmpFarIntersections,
                                        c);
            c += 4;
            }
          }
        }
      }
    }

  // Get rid of temporary data.
  tmpVolume->Delete();
  tmpProperty->Delete();
  tmpScalars->Delete();

  tmpIntersectionLengths->Delete();
  tmpNearIntersections->Delete();
  tmpFarIntersections->Delete();
}

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

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

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

  // Delete old tables.
  for (i = 0; i < this->NumComponents; i++)
    {
    delete[] this->IntegrationTable[i];
    }
  delete[] this->IntegrationTable;
  delete[] this->IntegrationTableScalarShift;
  delete[] this->IntegrationTableScalarScale;

  this->NumComponents = scalars->GetNumberOfComponents();
  this->Property = property;
  this->Volume = volume;
  this->IntegrationTableBuilt.Modified();

  if (!property->GetIndependentComponents())
    {
    vtkErrorMacro("Cannot store dependent components in pre-integration table.");
    return;
    }

  // Determine the maximum possible length of a ray segment.
  vtkDataSet *input = volume->GetMapper()->GetDataSetInput();
  vtkIdType numcells = input->GetNumberOfCells();
  this->MaxLength = 0;
  for (i = 0; i < numcells; i++)
    {
    double cellbounds[6];
    input->GetCellBounds(i, cellbounds);
#define SQR(x)  ((x)*(x))
    double diagonal_length = sqrt(  SQR(cellbounds[1]-cellbounds[0])
                                  + SQR(cellbounds[3]-cellbounds[2])
                                  + SQR(cellbounds[5]-cellbounds[4]) );
#undef SQR
    if (diagonal_length > this->MaxLength)
      {
      this->MaxLength = diagonal_length;
      }
    }

  this->BuildPreIntegrationTables(scalars);
}

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

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

  for (vtkIdType i = 0; i < numIntersections; i++)
    {
    float newcolor[4];
    float *c;
    c = this->GetTableEntry(nearIntersections->GetComponent(i, 0),
                            farIntersections->GetComponent(i, 0),
                            intersectionLengths->GetComponent(i, 0), 0);
    newcolor[0] = c[0];  newcolor[1] = c[1];
    newcolor[2] = c[2];  newcolor[3] = c[3];
    for (int component = 1; component < this->NumComponents; component++)
      {
      c = this->GetTableEntry(nearIntersections->GetComponent(i, component),
                              farIntersections->GetComponent(i, component),
                              intersectionLengths->GetComponent(i, 0),
                              component);
      // The blending I'm using is a combination of porter and duff xors
      // and ins.
      float coef1=1-0.5f*c[3];
      float coef2=1-0.5f*newcolor[3];
      newcolor[0] = newcolor[0]*coef1 + c[0]*coef2;
      newcolor[1] = newcolor[1]*coef1 + c[1]*coef2;
      newcolor[2] = newcolor[2]*coef1 + c[2]*coef2;
      newcolor[3] = newcolor[3]*coef1 + c[3]*coef2;
      }

    float coef=1-color[3];
    color[0] += newcolor[0]*coef;
    color[1] += newcolor[1]*coef;
    color[2] += newcolor[2]*coef;
    color[3] += newcolor[3]*coef;
    }
}

<|endoftext|>


<commit_before>/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2008, Willow Garage, Inc.
*  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.
********************************************************************/

#include <time.h>
#include <sys/stat.h>
#include "ros/ros.h"
#include "rosrecord/Recorder.h"
#include "topic_tools/shape_shifter.h"

#include <boost/thread.hpp>

#include <sstream>
#include <string>
#include <set>
#include <queue>

#include "std_msgs/Empty.h"

class OutgoingMessage
{
public:
  OutgoingMessage(std::string _topic_name, topic_tools::ShapeShifter::ConstPtr _msg, ros::Time _time) :
    topic_name(_topic_name), msg(_msg), time(_time) {}

  std::string topic_name;
  topic_tools::ShapeShifter::ConstPtr msg;
  ros::Time time;
};

class OutgoingQueue
{
public:
  OutgoingQueue(std::string _fname, std::queue<OutgoingMessage>* _queue, ros::Time _time) :
    fname(_fname), queue(_queue), time(_time) {}

  std::string fname;
  std::queue<OutgoingMessage>* queue;
  ros::Time time;
};

//! Global verbose flag so we can easily use from callback
bool g_verbose = false;

//! Global blackbox flag so we can easily use from callback
bool g_blackbox = false;

// Global Eventual exit code
int  g_exit_code       = 0;

//! Global variable including the set of currenly recording topics
std::set<std::string> g_currently_recording;

//! Global variable sued for initialization of counting messages
int g_count = -1;

//! Global queue for storing
std::queue<OutgoingMessage>* g_queue;

//! Global queue size
uint64_t g_queue_size=0;

//! Global max queue size
uint64_t g_max_queue_size=1048576*256;

//! Queue of queues to be used by the blackbox recorders
std::queue<OutgoingQueue> g_queue_queue;

//! Mutex for global queue
boost::mutex g_queue_mutex;

//! Conditional variable for global queue
boost::condition_variable_any g_queue_condition;

template <class T>
std::string time_to_str(T ros_t)
{
  char buf[1024]      = "";
  time_t t = ros_t.sec;
  struct tm *tms = localtime(&t);
  strftime(buf, 1024, "%Y-%m-%d-%H-%M-%S", tms);
  return std::string(buf);
}

//! Helper function to print executable usage
void print_usage() {
  fprintf (stderr, "Usage: rosrecord [options] TOPIC1 [TOPIC2 TOPIC3...]\n"
                   "  rosrecord logs ROS message data to a file.\n"
                   );
}

//! Helper function to print executable options
void print_help() {
  print_usage();
  fprintf(stderr, "Options:\n");
  fprintf(stderr, " -c <num>    : Only receive <num> messages on each topic\n");
  fprintf(stderr, " -f <prefix> : Prepend file prefix to beginning of bag name (name will always end with date stamp)\n");
  fprintf(stderr, " -F <fname>  : Record to a file named exactly <fname>.bag\n");
  fprintf(stderr, " -a          : Record all published messages.\n");
  fprintf(stderr, " -v          : Display a message every time a message is received on a topic\n");
  fprintf(stderr, " -m          : Maximize internal buffer size in MB (Default: 256MB)  0 = infinite.\n");
  fprintf(stderr, " -b          : Enable black box recording (don't write to file unless triggered)\n");
  fprintf(stderr, " -h          : Display this help message\n");
}

//! Callback to be invoked to save messages into a queue
void do_queue(topic_tools::ShapeShifter::ConstPtr msg,
               std::string topic_name,
               boost::shared_ptr<ros::Subscriber> subscriber,
               boost::shared_ptr<int> count)
{
  ros::Time rectime = ros::Time::now();

  if (g_verbose)
    std::cout << "Received message on topic " << subscriber->getTopic() << std::endl;

  OutgoingMessage out(topic_name, msg, rectime);

  {
    boost::mutex::scoped_lock lock(g_queue_mutex);
    g_queue->push(out);
    g_queue_size += out.msg->msgBufUsed;

    while (g_max_queue_size > 0 && g_queue_size > g_max_queue_size)
    {
      OutgoingMessage drop = g_queue->front();
      g_queue->pop();
      g_queue_size -= drop.msg->msgBufUsed;
      if (!g_blackbox)
      {
        static ros::Time last = ros::Time();
        ros::Time now = ros::Time::now();
        if (now > last + ros::Duration(5.0))
        {
          ROS_WARN("Rosrecord buffer exceeded.  Dropping oldest queued message.");
          last = now;
        }
      }
    }
  }

  if (!g_blackbox)
    g_queue_condition.notify_all();

  // If we are book-keeping count, decrement and possibly shutdown
  if ((*count) > 0)
  {
    (*count)--;
    if ((*count) == 0)
      subscriber->shutdown();
  }
}

//! Callback to be invoked to actually do the recording
void blackbox_trigger(std_msgs::Empty::ConstPtr trigger, std::string prefix, bool add_date)
{
  ros::WallTime rectime = ros::WallTime::now();

  std::vector<std::string> join;

  if (prefix.length() > 0)
    join.push_back(prefix);
  if (add_date)
    join.push_back(time_to_str(rectime));

  std::string tgt_fname = join[0];
  for (size_t i = 1; i < join.size(); i++)
    tgt_fname = tgt_fname + "_" + join[i];

  tgt_fname = tgt_fname + std::string(".bag");

  ROS_INFO("Triggered blackbox recording with name %s.", tgt_fname.c_str());

  {
    boost::mutex::scoped_lock lock(g_queue_mutex);

    OutgoingQueue out(tgt_fname, g_queue, ros::Time::now());

    g_queue = new std::queue<OutgoingMessage>;
    g_queue_size=0;

    g_queue_queue.push(out);
  }
  g_queue_condition.notify_all();
}

//! Thread that actually does writing to file.
void do_record(std::string prefix, bool add_date)
{
  ros::WallTime rectime = ros::WallTime::now();

  std::vector<std::string> join;

  if (prefix.length() > 0)
    join.push_back(prefix);
  if (add_date)
    join.push_back(time_to_str(rectime));

  std::string tgt_fname = join[0];
  for (size_t i = 1; i < join.size(); i++)
    tgt_fname = tgt_fname + "_" + join[i];

  tgt_fname = tgt_fname + std::string(".bag");

  std::string fname = tgt_fname + std::string(".active");

  ros::NodeHandle nh;
  ros::record::Recorder recorder;

  // Open our recorder and add available topics
  if (!recorder.open(std::string(fname)))
  {
    ROS_FATAL("Could not open output file: %s", fname.c_str());
    g_exit_code = 1;
    ros::shutdown();
  }

  // Technically the g_queue_mutex should be locked while checking empty
  // Except it should only get checked if the node is not ok, and thus
  // it shouldn't be in contention.
  while (nh.ok() || !g_queue->empty())
  {
    boost::unique_lock<boost::mutex> lock(g_queue_mutex);
    while(g_queue->empty())
    {
      if (!nh.ok())
        return;
      g_queue_condition.wait(lock);
    }

    OutgoingMessage out = g_queue->front();
    g_queue->pop();
    g_queue_size -= out.msg->msgBufUsed;

    lock.release()->unlock();

    recorder.record(out.topic_name, out.msg, out.time);
  }

  // Close the file nicely
  recorder.close();

  // Rename the file to the actual target name
  rename(fname.c_str(),tgt_fname.c_str());
}

void do_record_bb()
{
  ros::NodeHandle nh;

  while (nh.ok() || !g_queue_queue.empty())
  {
    boost::unique_lock<boost::mutex> lock(g_queue_mutex);
    while(g_queue_queue.empty())
    {
      if (!nh.ok())
        return;
      g_queue_condition.wait(lock);
    }

    OutgoingQueue out_queue = g_queue_queue.front();
    g_queue_queue.pop();

    lock.release()->unlock();

    std::string tgt_fname = out_queue.fname;
    std::string fname = tgt_fname + std::string(".active");

    ros::record::Recorder recorder;

    if (recorder.open(fname))
    {

      while(!out_queue.queue->empty())
      {
        OutgoingMessage out = out_queue.queue->front();
        out_queue.queue->pop();
        recorder.record(out.topic_name, out.msg, out.time);
      }

      // Close the file nicely
      recorder.close();

      // Rename the file to the actual target name
      rename(fname.c_str(),tgt_fname.c_str());
    } else {
      ROS_ERROR("Could not open file: %s", out_queue.fname.c_str());
    }
  }
}

void do_check_master(const ros::TimerEvent& e, ros::NodeHandle& node_handle)
{
  ros::master::V_TopicInfo all_topics;

  if (ros::master::getTopics(all_topics))
  {
    // For each topic
    for (ros::master::V_TopicInfo::iterator topic_iter = all_topics.begin();
         topic_iter != all_topics.end();
         topic_iter++)
    {
      // If we're not currently recording it, do so
      if (g_currently_recording.find(topic_iter->name) == g_currently_recording.end())
      {
        boost::shared_ptr<int> count(new int(g_count));
        boost::shared_ptr<ros::Subscriber> sub(new ros::Subscriber);
        *sub = node_handle.subscribe<topic_tools::ShapeShifter>(topic_iter->name, 100, boost::bind(&do_queue, _1, topic_iter->name, sub, count));
        g_currently_recording.insert(topic_iter->name);
      }
    }
  }
}

void do_check_subscribers_left(const ros::TimerEvent& e)
{
  ros::V_string subscribed_topics;
  ros::this_node::getSubscribedTopics(subscribed_topics);

  // If there is only 1 topic left (time), we can shutdown
  if (subscribed_topics.size() == 1)
    ros::shutdown();
}

//! Main function
int main(int argc, char **argv)
{
  ros::init(argc, argv, "rosrecord", ros::init_options::AnonymousName);

  // Variables
  bool check_master    = false; // Whether master should be checked periodically

  bool add_date = true;;
  std::string prefix("");   // Prefix

  // Parse options
  int option_char;

  while ((option_char = getopt(argc,argv,"f:F:c:m:abhv")) != -1)
  {
    switch (option_char)
    {
    case 'f': prefix = std::string(optarg); break;
    case 'F': prefix = std::string(optarg); add_date = false; break;
    case 'c': g_count = atoi(optarg); break;
    case 'a': check_master = true; break;
    case 'b': g_blackbox = true; break;
    case 'v': g_verbose = true; break;
    case 'm':
      {
        int m=0;
        m=atoi(optarg);
        if (m < 0)
        {
          fprintf(stderr, "Buffer size must be 0 or positive.\n");
          return 1;
        }
        g_max_queue_size =  1048576*m;
      }
      break;
    case 'h': print_help(); return 1;
    case '?': print_usage(); return 1;
    }
  }

  // Logic to make sure count is not specified with automatic topic
  // subscription (implied by no listed topics)
  if ((argc - optind) < 1)
  {
    if (g_count > 0)
    {
      fprintf(stderr, "Specifing a count is not valid with automatic topic subscription.\n");
      return 1;
    }
    if (!check_master)
    {
      ROS_WARN("Running rosrecord with no arguments has been deprecated.  Please use 'rosrecord -a' instead\n");
      check_master = true;
    }
  }

  g_queue = new std::queue<OutgoingMessage>;

  // Get a node_handle
  ros::NodeHandle node_handle;

  // Only set up recording if we actually got a useful nodehandle.
  if (node_handle.ok())
  {
    boost::thread record_thread;

    // Spin up a thread for actually writing to file
    if (!g_blackbox)
      record_thread = boost::thread(boost::bind(&do_record, prefix, add_date));
    else
      record_thread = boost::thread(boost::bind(&do_record_bb));

    ros::Subscriber trigger = node_handle.subscribe<std_msgs::Empty>("blackbox_trigger", 100, boost::bind(&blackbox_trigger, _1, prefix, add_date));

    // Every non-processed argument is assumed to be a topic
    for (int i = optind; i < argc; i++)
    {
      boost::shared_ptr<int> count(new int(g_count));
      boost::shared_ptr<ros::Subscriber> sub(new ros::Subscriber);
      *sub = node_handle.subscribe<topic_tools::ShapeShifter>(argv[i], 100, boost::bind(&do_queue, _1, argv[i], sub, count));
    }

    ros::Timer check_master_timer;
    if (check_master)
      check_master_timer = node_handle.createTimer(ros::Duration(1.0), boost::bind(&do_check_master, _1, boost::ref(node_handle)));

    ros::Timer check_subscribers_left_timer;
    if (g_count >= 0)
      check_subscribers_left_timer = node_handle.createTimer(ros::Duration(0.1), &do_check_subscribers_left);

    ros::MultiThreadedSpinner s(10);
    ros::spin(s);

    g_queue_condition.notify_all();

    record_thread.join();

  }

  delete g_queue;

  // Return our exit code
  return g_exit_code;
}
<commit_msg>Actually renaming the file correctly after close.<commit_after>/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2008, Willow Garage, Inc.
*  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.
********************************************************************/

#include <time.h>
#include <sys/stat.h>
#include "ros/ros.h"
#include "rosrecord/Recorder.h"
#include "topic_tools/shape_shifter.h"

#include <boost/thread.hpp>

#include <sstream>
#include <string>
#include <set>
#include <queue>

#include "std_msgs/Empty.h"

class OutgoingMessage
{
public:
  OutgoingMessage(std::string _topic_name, topic_tools::ShapeShifter::ConstPtr _msg, ros::Time _time) :
    topic_name(_topic_name), msg(_msg), time(_time) {}

  std::string topic_name;
  topic_tools::ShapeShifter::ConstPtr msg;
  ros::Time time;
};

class OutgoingQueue
{
public:
  OutgoingQueue(std::string _fname, std::queue<OutgoingMessage>* _queue, ros::Time _time) :
    fname(_fname), queue(_queue), time(_time) {}

  std::string fname;
  std::queue<OutgoingMessage>* queue;
  ros::Time time;
};

//! Global verbose flag so we can easily use from callback
bool g_verbose = false;

//! Global blackbox flag so we can easily use from callback
bool g_blackbox = false;

// Global Eventual exit code
int  g_exit_code       = 0;

//! Global variable including the set of currenly recording topics
std::set<std::string> g_currently_recording;

//! Global variable sued for initialization of counting messages
int g_count = -1;

//! Global queue for storing
std::queue<OutgoingMessage>* g_queue;

//! Global queue size
uint64_t g_queue_size=0;

//! Global max queue size
uint64_t g_max_queue_size=1048576*256;

//! Queue of queues to be used by the blackbox recorders
std::queue<OutgoingQueue> g_queue_queue;

//! Mutex for global queue
boost::mutex g_queue_mutex;

//! Conditional variable for global queue
boost::condition_variable_any g_queue_condition;

template <class T>
std::string time_to_str(T ros_t)
{
  char buf[1024]      = "";
  time_t t = ros_t.sec;
  struct tm *tms = localtime(&t);
  strftime(buf, 1024, "%Y-%m-%d-%H-%M-%S", tms);
  return std::string(buf);
}

//! Helper function to print executable usage
void print_usage() {
  fprintf (stderr, "Usage: rosrecord [options] TOPIC1 [TOPIC2 TOPIC3...]\n"
                   "  rosrecord logs ROS message data to a file.\n"
                   );
}

//! Helper function to print executable options
void print_help() {
  print_usage();
  fprintf(stderr, "Options:\n");
  fprintf(stderr, " -c <num>    : Only receive <num> messages on each topic\n");
  fprintf(stderr, " -f <prefix> : Prepend file prefix to beginning of bag name (name will always end with date stamp)\n");
  fprintf(stderr, " -F <fname>  : Record to a file named exactly <fname>.bag\n");
  fprintf(stderr, " -a          : Record all published messages.\n");
  fprintf(stderr, " -v          : Display a message every time a message is received on a topic\n");
  fprintf(stderr, " -m          : Maximize internal buffer size in MB (Default: 256MB)  0 = infinite.\n");
  fprintf(stderr, " -b          : Enable black box recording (don't write to file unless triggered)\n");
  fprintf(stderr, " -h          : Display this help message\n");
}

//! Callback to be invoked to save messages into a queue
void do_queue(topic_tools::ShapeShifter::ConstPtr msg,
               std::string topic_name,
               boost::shared_ptr<ros::Subscriber> subscriber,
               boost::shared_ptr<int> count)
{
  ros::Time rectime = ros::Time::now();

  if (g_verbose)
    std::cout << "Received message on topic " << subscriber->getTopic() << std::endl;

  OutgoingMessage out(topic_name, msg, rectime);

  {
    boost::mutex::scoped_lock lock(g_queue_mutex);
    g_queue->push(out);
    g_queue_size += out.msg->msgBufUsed;

    while (g_max_queue_size > 0 && g_queue_size > g_max_queue_size)
    {
      OutgoingMessage drop = g_queue->front();
      g_queue->pop();
      g_queue_size -= drop.msg->msgBufUsed;
      if (!g_blackbox)
      {
        static ros::Time last = ros::Time();
        ros::Time now = ros::Time::now();
        if (now > last + ros::Duration(5.0))
        {
          ROS_WARN("Rosrecord buffer exceeded.  Dropping oldest queued message.");
          last = now;
        }
      }
    }
  }

  if (!g_blackbox)
    g_queue_condition.notify_all();

  // If we are book-keeping count, decrement and possibly shutdown
  if ((*count) > 0)
  {
    (*count)--;
    if ((*count) == 0)
      subscriber->shutdown();
  }
}

//! Callback to be invoked to actually do the recording
void blackbox_trigger(std_msgs::Empty::ConstPtr trigger, std::string prefix, bool add_date)
{
  ros::WallTime rectime = ros::WallTime::now();

  std::vector<std::string> join;

  if (prefix.length() > 0)
    join.push_back(prefix);
  if (add_date)
    join.push_back(time_to_str(rectime));

  std::string tgt_fname = join[0];
  for (size_t i = 1; i < join.size(); i++)
    tgt_fname = tgt_fname + "_" + join[i];

  tgt_fname = tgt_fname + std::string(".bag");

  ROS_INFO("Triggered blackbox recording with name %s.", tgt_fname.c_str());

  {
    boost::mutex::scoped_lock lock(g_queue_mutex);

    OutgoingQueue out(tgt_fname, g_queue, ros::Time::now());

    g_queue = new std::queue<OutgoingMessage>;
    g_queue_size=0;

    g_queue_queue.push(out);
  }
  g_queue_condition.notify_all();
}

//! Thread that actually does writing to file.
void do_record(std::string prefix, bool add_date)
{
  ros::WallTime rectime = ros::WallTime::now();

  std::vector<std::string> join;

  if (prefix.length() > 0)
    join.push_back(prefix);
  if (add_date)
    join.push_back(time_to_str(rectime));

  std::string tgt_fname = join[0];
  for (size_t i = 1; i < join.size(); i++)
    tgt_fname = tgt_fname + "_" + join[i];

  tgt_fname = tgt_fname + std::string(".bag");

  ROS_INFO("Recording to %s.", tgt_fname.c_str());

  std::string fname = tgt_fname + std::string(".active");

  ros::NodeHandle nh;
  ros::record::Recorder recorder;

  // Open our recorder and add available topics
  if (!recorder.open(std::string(fname)))
  {
    ROS_FATAL("Could not open output file: %s", fname.c_str());
    g_exit_code = 1;
    ros::shutdown();
  }

  // Technically the g_queue_mutex should be locked while checking empty
  // Except it should only get checked if the node is not ok, and thus
  // it shouldn't be in contention.
  while (nh.ok() || !g_queue->empty())
  {
    boost::unique_lock<boost::mutex> lock(g_queue_mutex);
    while(g_queue->empty())
    {
      if (!nh.ok())
      {
        // Close the file nicely
        recorder.close();
        rename(fname.c_str(),tgt_fname.c_str());
        return;
      }
      g_queue_condition.wait(lock);
    }

    OutgoingMessage out = g_queue->front();
    g_queue->pop();
    g_queue_size -= out.msg->msgBufUsed;

    lock.release()->unlock();

    recorder.record(out.topic_name, out.msg, out.time);
  }

  // Close the file nicely
  recorder.close();
  rename(fname.c_str(),tgt_fname.c_str());
}

void do_record_bb()
{
  ros::NodeHandle nh;

  while (nh.ok() || !g_queue_queue.empty())
  {
    boost::unique_lock<boost::mutex> lock(g_queue_mutex);
    while(g_queue_queue.empty())
    {
      if (!nh.ok())
        return;
      g_queue_condition.wait(lock);
    }

    OutgoingQueue out_queue = g_queue_queue.front();
    g_queue_queue.pop();

    lock.release()->unlock();

    std::string tgt_fname = out_queue.fname;
    std::string fname = tgt_fname + std::string(".active");

    ros::record::Recorder recorder;

    if (recorder.open(fname))
    {

      while(!out_queue.queue->empty())
      {
        OutgoingMessage out = out_queue.queue->front();
        out_queue.queue->pop();
        recorder.record(out.topic_name, out.msg, out.time);
      }

      // Close the file nicely
      recorder.close();

      // Rename the file to the actual target name
      rename(fname.c_str(),tgt_fname.c_str());
    } else {
      ROS_ERROR("Could not open file: %s", out_queue.fname.c_str());
    }
  }
}

void do_check_master(const ros::TimerEvent& e, ros::NodeHandle& node_handle)
{
  ros::master::V_TopicInfo all_topics;

  if (ros::master::getTopics(all_topics))
  {
    // For each topic
    for (ros::master::V_TopicInfo::iterator topic_iter = all_topics.begin();
         topic_iter != all_topics.end();
         topic_iter++)
    {
      // If we're not currently recording it, do so
      if (g_currently_recording.find(topic_iter->name) == g_currently_recording.end())
      {
        boost::shared_ptr<int> count(new int(g_count));
        boost::shared_ptr<ros::Subscriber> sub(new ros::Subscriber);
        *sub = node_handle.subscribe<topic_tools::ShapeShifter>(topic_iter->name, 100, boost::bind(&do_queue, _1, topic_iter->name, sub, count));
        g_currently_recording.insert(topic_iter->name);
      }
    }
  }
}

void do_check_subscribers_left(const ros::TimerEvent& e)
{
  ros::V_string subscribed_topics;
  ros::this_node::getSubscribedTopics(subscribed_topics);

  // If there is only 1 topic left (time), we can shutdown
  if (subscribed_topics.size() == 1)
    ros::shutdown();
}

//! Main function
int main(int argc, char **argv)
{
  ros::init(argc, argv, "rosrecord", ros::init_options::AnonymousName);

  // Variables
  bool check_master    = false; // Whether master should be checked periodically

  bool add_date = true;;
  std::string prefix("");   // Prefix

  // Parse options
  int option_char;

  while ((option_char = getopt(argc,argv,"f:F:c:m:abhv")) != -1)
  {
    switch (option_char)
    {
    case 'f': prefix = std::string(optarg); break;
    case 'F': prefix = std::string(optarg); add_date = false; break;
    case 'c': g_count = atoi(optarg); break;
    case 'a': check_master = true; break;
    case 'b': g_blackbox = true; break;
    case 'v': g_verbose = true; break;
    case 'm':
      {
        int m=0;
        m=atoi(optarg);
        if (m < 0)
        {
          fprintf(stderr, "Buffer size must be 0 or positive.\n");
          return 1;
        }
        g_max_queue_size =  1048576*m;
      }
      break;
    case 'h': print_help(); return 1;
    case '?': print_usage(); return 1;
    }
  }

  // Logic to make sure count is not specified with automatic topic
  // subscription (implied by no listed topics)
  if ((argc - optind) < 1)
  {
    if (g_count > 0)
    {
      fprintf(stderr, "Specifing a count is not valid with automatic topic subscription.\n");
      return 1;
    }
    if (!check_master)
    {
      ROS_WARN("Running rosrecord with no arguments has been deprecated.  Please use 'rosrecord -a' instead\n");
      check_master = true;
    }
  }

  g_queue = new std::queue<OutgoingMessage>;

  // Get a node_handle
  ros::NodeHandle node_handle;

  // Only set up recording if we actually got a useful nodehandle.
  if (node_handle.ok())
  {
    boost::thread record_thread;

    // Spin up a thread for actually writing to file
    if (!g_blackbox)
      record_thread = boost::thread(boost::bind(&do_record, prefix, add_date));
    else
      record_thread = boost::thread(boost::bind(&do_record_bb));

    ros::Subscriber trigger = node_handle.subscribe<std_msgs::Empty>("blackbox_trigger", 100, boost::bind(&blackbox_trigger, _1, prefix, add_date));

    // Every non-processed argument is assumed to be a topic
    for (int i = optind; i < argc; i++)
    {
      boost::shared_ptr<int> count(new int(g_count));
      boost::shared_ptr<ros::Subscriber> sub(new ros::Subscriber);
      *sub = node_handle.subscribe<topic_tools::ShapeShifter>(argv[i], 100, boost::bind(&do_queue, _1, argv[i], sub, count));
    }

    ros::Timer check_master_timer;
    if (check_master)
      check_master_timer = node_handle.createTimer(ros::Duration(1.0), boost::bind(&do_check_master, _1, boost::ref(node_handle)));

    ros::Timer check_subscribers_left_timer;
    if (g_count >= 0)
      check_subscribers_left_timer = node_handle.createTimer(ros::Duration(0.1), &do_check_subscribers_left);

    ros::MultiThreadedSpinner s(10);
    ros::spin(s);

    g_queue_condition.notify_all();

    record_thread.join();

  }

  delete g_queue;

  // Return our exit code
  return g_exit_code;
}
<|endoftext|>


<commit_before><commit_msg>devd: Correct typo in comment.<commit_after><|endoftext|>


<commit_before>#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/point_cloud_conversion.h>
#include <tf/transform_listener.h>
#include <octomap/octomap.h>
#include <octomap/OcTree.h>
#include <octomap_msgs/Octomap.h>
#include <octomap_msgs/conversions.h>
#include <geometry_msgs/PointStamped.h>

#include <string>
#include <math.h>
#include <vector>

#include "dmath/geometry.h"

class ArtificialPotentialField{
public:
    ArtificialPotentialField(ros::NodeHandle &node) :
        base_link_("base_link"),
        cmd_pub_(node.advertise<geometry_msgs::Twist>("cmd_vel", 10)),
        obs_sub_(node.subscribe("octomap_full", 10, &ArtificialPotentialField::obstacleCallback, this)),
        goal_sub_(node.subscribe("clicked_point", 10, &ArtificialPotentialField::goalCallback, this))

    {
        collision_map_.header.stamp = ros::Time(0);
    }

    void spin(){
        ros::Rate r(10);

        ros::Duration(1).sleep();
        geometry_msgs::Twist cmd;
        cmd.linear.z = 0.15;
        cmd_pub_.publish(cmd);
        ros::Duration(3).sleep();

        cmd.linear.z = 0;
        cmd_pub_.publish(cmd);
        ros::Duration(3).sleep();

        const double force = 0.025;

        while(ros::ok()){
            if(collision_map_.header.stamp != ros::Time(0)){
                std::string map_frame = collision_map_.header.frame_id;
                octomap::OcTree *tree = dynamic_cast<octomap::OcTree*>(octomap_msgs::msgToMap(collision_map_));
                octomap::OcTree::leaf_iterator const end_it = tree->end_leafs();

                double min_dist = 99999999;
                dmath::Vector3D min_obs;

                ros::Time now = ros::Time::now();
                tf_listener_.waitForTransform(map_frame, base_link_, now, ros::Duration(1));
                for(octomap::OcTree::leaf_iterator it = tree->begin_leafs(0); it != end_it; it++){
                    if(it->getOccupancy() < tree->getOccupancyThres()) continue;

                    geometry_msgs::PointStamped p_in, p_out;
                    p_in.header.frame_id = map_frame;
                    p_in.point.x = it.getX();
                    p_in.point.y = it.getY();
                    p_in.point.z = it.getZ();

                    try{
                        p_in.header.stamp = now;
                        tf_listener_.transformPoint(base_link_, p_in, p_out);
                        dmath::Vector3D obs(p_out.point.x, p_out.point.y, p_out.point.z);
                        double dist = magnitude(obs);
                        if(min_dist > dist){
                            min_dist = dist;
                            min_obs = -obs;
                        }

                    }catch(tf::TransformException &ex){
                        ROS_ERROR_STREAM("Exception trying to transform octomap: " << ex.what());
                    }
                }

                dmath::Vector3D Fs;
                Fs += get_potential_force(min_obs, 0, 3.0, 1.0, 4.0);

                geometry_msgs::PointStamped goal_msg_lc;
                dmath::Vector3D goal_lc;
                try{
                    tf_listener_.waitForTransform(goal_msg_gl_.header.frame_id, base_link_, now, ros::Duration(1));
                    goal_msg_gl_.header.stamp = now;
                    tf_listener_.transformPoint(base_link_, goal_msg_gl_, goal_msg_lc);
                    goal_lc = -dmath::Vector3D(goal_msg_lc.point.x, goal_msg_lc.point.y, goal_msg_lc.point.z);
                }catch(tf::TransformException &ex){
                    ROS_ERROR_STREAM("Exception trying to transform goal position: " << ex.what());
                    goal_lc = dmath::Vector3D();
                }

                Fs += get_potential_force(goal_lc, 100, 0, 1, 1);

                dmath::Vector3D vel = Fs * force;

                if(vel.x > 0.5) vel.x = 0.5;
                if(vel.x < -0.5) vel.x = -0.5;
                if(vel.y > 0.5) vel.y = 0.5;
                if(vel.y < -0.5) vel.y = -0.5;
                if(vel.z > 0.5) vel.z = 0.5;
                if(vel.z < -0.5) vel.z = -0.5;
                cmd.linear.x = vel.x;
                cmd.linear.y = vel.y;
                //cmd.linear.z = vel.z;

                cmd_pub_.publish(cmd);
            }
            r.sleep();
            ros::spinOnce();
        }
    }

private:
    dmath::Vector3D get_potential_force(const dmath::Vector3D &dest_lc, double A = 1, double B = 1, double n = 1, double m = 1){
        dmath::Vector3D u = dest_lc;
        u = normalize(u);

        const double d = magnitude(dest_lc);
        double U = 0;
        if(fabs(d) > dmath::tol){
            U = -A/pow(d, n) + B/pow(d, m);
        }

        return U * u;
    }

    void obstacleCallback(const octomap_msgs::OctomapPtr &obs_msg){
        collision_map_ = *obs_msg;
    }

    void goalCallback(const geometry_msgs::PointStamped &goal_msg){
        goal_msg_gl_ = goal_msg;
    }

    octomap_msgs::Octomap collision_map_;
    ros::Publisher cmd_pub_;
    ros::Subscriber obs_sub_, goal_sub_;
    tf::TransformListener tf_listener_;
    std::string base_link_;
    geometry_msgs::PointStamped goal_msg_gl_;
};

int main(int argc, char *argv[]){
    ros::init(argc, argv, "apf_planner");

    ros::NodeHandle node;
    ArtificialPotentialField apf(node);
    apf.spin();

    return 0;
}

<commit_msg>Improve a problem that calculation does not converge at goal position<commit_after>#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/point_cloud_conversion.h>
#include <tf/transform_listener.h>
#include <octomap/octomap.h>
#include <octomap/OcTree.h>
#include <octomap_msgs/Octomap.h>
#include <octomap_msgs/conversions.h>
#include <geometry_msgs/PointStamped.h>

#include <string>
#include <math.h>
#include <vector>

#include "dmath/geometry.h"

class ArtificialPotentialField{
public:
    ArtificialPotentialField(ros::NodeHandle &node) :
        base_link_("base_link"),
        cmd_pub_(node.advertise<geometry_msgs::Twist>("cmd_vel", 10)),
        obs_sub_(node.subscribe("octomap_full", 10, &ArtificialPotentialField::obstacleCallback, this)),
        goal_sub_(node.subscribe("clicked_point", 10, &ArtificialPotentialField::goalCallback, this))

    {
        collision_map_.header.stamp = ros::Time(0);
    }

    void spin(){
        ros::Rate r(10);

        ros::Duration(1).sleep();
        geometry_msgs::Twist cmd;
        cmd.linear.z = 0.15;
        cmd_pub_.publish(cmd);
        ros::Duration(3).sleep();

        cmd.linear.z = 0;
        cmd_pub_.publish(cmd);
        ros::Duration(3).sleep();

        const double force = 0.025;

        while(ros::ok()){
            if(collision_map_.header.stamp != ros::Time(0)){
                std::string map_frame = collision_map_.header.frame_id;
                octomap::OcTree *tree = dynamic_cast<octomap::OcTree*>(octomap_msgs::msgToMap(collision_map_));
                octomap::OcTree::leaf_iterator const end_it = tree->end_leafs();

                double min_dist = 99999999;
                dmath::Vector3D min_obs;

                ros::Time now = ros::Time::now();
                tf_listener_.waitForTransform(map_frame, base_link_, now, ros::Duration(1));
                for(octomap::OcTree::leaf_iterator it = tree->begin_leafs(0); it != end_it; it++){
                    if(it->getOccupancy() < tree->getOccupancyThres()) continue;

                    geometry_msgs::PointStamped p_in, p_out;
                    p_in.header.frame_id = map_frame;
                    p_in.point.x = it.getX();
                    p_in.point.y = it.getY();
                    p_in.point.z = it.getZ();

                    try{
                        p_in.header.stamp = now;
                        tf_listener_.transformPoint(base_link_, p_in, p_out);
                        dmath::Vector3D obs(p_out.point.x, p_out.point.y, p_out.point.z);
                        double dist = magnitude(obs);
                        if(min_dist > dist){
                            min_dist = dist;
                            min_obs = -obs;
                        }

                    }catch(tf::TransformException &ex){
                        ROS_ERROR_STREAM("Exception trying to transform octomap: " << ex.what());
                    }
                }

                dmath::Vector3D Fs;
                Fs += get_potential_force(min_obs, 0, 3.0, 1.0, 4.0);

                geometry_msgs::PointStamped goal_msg_lc;
                dmath::Vector3D goal_lc;
                try{
                    tf_listener_.waitForTransform(goal_msg_gl_.header.frame_id, base_link_, now, ros::Duration(1));
                    goal_msg_gl_.header.stamp = now;
                    tf_listener_.transformPoint(base_link_, goal_msg_gl_, goal_msg_lc);
                    goal_lc = -dmath::Vector3D(goal_msg_lc.point.x, goal_msg_lc.point.y, goal_msg_lc.point.z);
                }catch(tf::TransformException &ex){
                    ROS_ERROR_STREAM("Exception trying to transform goal position: " << ex.what());
                    goal_lc = dmath::Vector3D();
                }

                Fs += get_potential_force(goal_lc, 50, 0, 1, 1);

                dmath::Vector3D vel = Fs * force;

                if(vel.x > 0.5) vel.x = 0.5;
                if(vel.x < -0.5) vel.x = -0.5;
                if(vel.y > 0.5) vel.y = 0.5;
                if(vel.y < -0.5) vel.y = -0.5;
                if(vel.z > 0.5) vel.z = 0.5;
                if(vel.z < -0.5) vel.z = -0.5;
                cmd.linear.x = vel.x;
                cmd.linear.y = vel.y;
                //cmd.linear.z = vel.z;

                cmd_pub_.publish(cmd);
            }
            r.sleep();
            ros::spinOnce();
        }
    }

private:
    dmath::Vector3D get_potential_force(const dmath::Vector3D &dest_lc, double A = 1, double B = 1, double n = 1, double m = 1){
        dmath::Vector3D u = dest_lc;
        u = normalize(u);

        const double d = magnitude(dest_lc);
        double U = 0;
        if(fabs(d) > dmath::tol){
            U = -A/pow(d, n) + B/pow(d, m);
        }

        return U * u;
    }

    void obstacleCallback(const octomap_msgs::OctomapPtr &obs_msg){
        collision_map_ = *obs_msg;
    }

    void goalCallback(const geometry_msgs::PointStamped &goal_msg){
        goal_msg_gl_ = goal_msg;
    }

    octomap_msgs::Octomap collision_map_;
    ros::Publisher cmd_pub_;
    ros::Subscriber obs_sub_, goal_sub_;
    tf::TransformListener tf_listener_;
    std::string base_link_;
    geometry_msgs::PointStamped goal_msg_gl_;
};

int main(int argc, char *argv[]){
    ros::init(argc, argv, "apf_planner");

    ros::NodeHandle node;
    ArtificialPotentialField apf(node);
    apf.spin();

    return 0;
}

<|endoftext|>