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

  Program:   Visualization Toolkit
  Module:    vtkPCorrelativeStatistics.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.

=========================================================================*/
#include "vtkToolkits.h"

#include "vtkPCorrelativeStatistics.h"

#include "vtkCommunicator.h"
#include "vtkInformation.h"
#include "vtkInformationVector.h"
#include "vtkObjectFactory.h"
#include "vtkMultiProcessController.h"
#include "vtkTable.h"
#include "vtkVariant.h"

vtkStandardNewMacro(vtkPCorrelativeStatistics);
vtkCxxRevisionMacro(vtkPCorrelativeStatistics, "1.6");
vtkCxxSetObjectMacro(vtkPCorrelativeStatistics, Controller, vtkMultiProcessController);
//-----------------------------------------------------------------------------
vtkPCorrelativeStatistics::vtkPCorrelativeStatistics()
{
  this->Controller = 0;
  this->SetController( vtkMultiProcessController::GetGlobalController() );
}

//-----------------------------------------------------------------------------
vtkPCorrelativeStatistics::~vtkPCorrelativeStatistics()
{
  this->SetController( 0 );
}

//-----------------------------------------------------------------------------
void vtkPCorrelativeStatistics::PrintSelf(ostream& os, vtkIndent indent)
{
  this->Superclass::PrintSelf(os, indent);
  os << indent << "Controller: " << this->Controller << endl;
}

// ----------------------------------------------------------------------
void vtkPCorrelativeStatistics::Learn( vtkTable* inData,
                                              vtkTable* inParameters,
                                              vtkDataObject* outMetaDO )
{
  vtkTable* outMeta = vtkTable::SafeDownCast( outMetaDO );
  if ( ! outMeta )
    {
    return;
    }

  // First calculate correlative statistics on local data set
  this->Superclass::Learn( inData, inParameters, outMeta );

  vtkIdType nRow = outMeta->GetNumberOfRows();
  if ( ! nRow )
    {
    // No statistics were calculated.
    return;
    }

  // Make sure that parallel updates are needed, otherwise leave it at that.
  int np = this->Controller->GetNumberOfProcesses();
  if ( np < 2 )
    {
    return;
    }

  // Now get ready for parallel calculations
  vtkCommunicator* com = this->Controller->GetCommunicator();
  if ( ! com )
    {
    vtkErrorMacro("No parallel communicator.");
    }

  // (All) gather all sample sizes
  int n_l = outMeta->GetValueByName( 0, "Cardinality" ).ToInt(); // Cardinality
  int* n_g = new int[np];
  com->AllGather( &n_l, n_g, 1 );

  // Iterate over all parameter rows
  for ( int r = 0; r < nRow; ++ r )
    {
    // (All) gather all local M statistics
    double M_l[5];
    M_l[0] = outMeta->GetValueByName( r, "Mean X" ).ToDouble();
    M_l[1] = outMeta->GetValueByName( r, "Mean Y" ).ToDouble();
    M_l[2] = outMeta->GetValueByName( r, "M2 X" ).ToDouble();
    M_l[3] = outMeta->GetValueByName( r, "M2 Y" ).ToDouble();
    M_l[4] = outMeta->GetValueByName( r, "M XY" ).ToDouble();
    double* M_g = new double[5 * np];
    com->AllGather( M_l, M_g, 5 );

    // Aggregate all local quintuples of M statistics into global ones
    int ns = n_g[0];
    double meanX = M_g[0];
    double meanY = M_g[1];
    double mom2X = M_g[2];
    double mom2Y = M_g[3];
    double momXY = M_g[4];

    for ( int i = 1; i < np; ++ i )
      {
      int ns_l = n_g[i];
      ns += ns_l;

      int o = 5 * i;
      double meanX_part = M_g[o];
      double meanY_part = M_g[o + 1];
      double mom2X_part = M_g[o + 2];
      double mom2Y_part = M_g[o + 3];
      double momXY_part = M_g[o + 4];

      double deltaX = meanX_part - meanX;
      double deltaX_sur_n = deltaX / static_cast<double>( ns );

      double deltaY = meanY_part - meanY;
      double deltaY_sur_n = deltaY / static_cast<double>( ns );

      int prod_ns = ns * ns_l;

      mom2X += mom2X_part
        + prod_ns * deltaX * deltaX_sur_n;

      mom2Y += mom2Y_part
        + prod_ns * deltaY * deltaY_sur_n;

      momXY += momXY_part
        + prod_ns * deltaX * deltaY_sur_n;

      meanX += ns_l * deltaX_sur_n;

      meanY += ns_l * deltaY_sur_n;
      }

    outMeta->SetValueByName( r, "Mean X", meanX );
    outMeta->SetValueByName( r, "Mean Y", meanY );
    outMeta->SetValueByName( r, "M2 X", mom2X );
    outMeta->SetValueByName( r, "M2 Y", mom2Y );
    outMeta->SetValueByName( r, "M XY", momXY );

    // Set global statistics
    outMeta->SetValueByName( r, "Cardinality", ns );

    // Clean-up
    delete [] M_g;
    }
  delete [] n_g;
}
<commit_msg>BUG: caught an error in the implementation of the parallel update formulas (cf. Sandia report SAND2008-6212 for notations): in the implementation, n was used instead of n. NB: this bug was not found earlier because it does not result in a calculation error when the input data set displays similar statistical properties (mean, in particular) across processes. This was the case for all parallel test cases.<commit_after>/*=========================================================================

  Program:   Visualization Toolkit
  Module:    vtkPCorrelativeStatistics.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.

=========================================================================*/
#include "vtkToolkits.h"

#include "vtkPCorrelativeStatistics.h"

#include "vtkCommunicator.h"
#include "vtkInformation.h"
#include "vtkInformationVector.h"
#include "vtkObjectFactory.h"
#include "vtkMultiProcessController.h"
#include "vtkTable.h"
#include "vtkVariant.h"

vtkStandardNewMacro(vtkPCorrelativeStatistics);
vtkCxxRevisionMacro(vtkPCorrelativeStatistics, "1.7");
vtkCxxSetObjectMacro(vtkPCorrelativeStatistics, Controller, vtkMultiProcessController);
//-----------------------------------------------------------------------------
vtkPCorrelativeStatistics::vtkPCorrelativeStatistics()
{
  this->Controller = 0;
  this->SetController( vtkMultiProcessController::GetGlobalController() );
}

//-----------------------------------------------------------------------------
vtkPCorrelativeStatistics::~vtkPCorrelativeStatistics()
{
  this->SetController( 0 );
}

//-----------------------------------------------------------------------------
void vtkPCorrelativeStatistics::PrintSelf(ostream& os, vtkIndent indent)
{
  this->Superclass::PrintSelf(os, indent);
  os << indent << "Controller: " << this->Controller << endl;
}

// ----------------------------------------------------------------------
void vtkPCorrelativeStatistics::Learn( vtkTable* inData,
                                              vtkTable* inParameters,
                                              vtkDataObject* outMetaDO )
{
  vtkTable* outMeta = vtkTable::SafeDownCast( outMetaDO );
  if ( ! outMeta )
    {
    return;
    }

  // First calculate correlative statistics on local data set
  this->Superclass::Learn( inData, inParameters, outMeta );

  vtkIdType nRow = outMeta->GetNumberOfRows();
  if ( ! nRow )
    {
    // No statistics were calculated.
    return;
    }

  // Make sure that parallel updates are needed, otherwise leave it at that.
  int np = this->Controller->GetNumberOfProcesses();
  if ( np < 2 )
    {
    return;
    }

  // Now get ready for parallel calculations
  vtkCommunicator* com = this->Controller->GetCommunicator();
  if ( ! com )
    {
    vtkErrorMacro("No parallel communicator.");
    }

  // (All) gather all sample sizes
  int n_l = outMeta->GetValueByName( 0, "Cardinality" ).ToInt(); // Cardinality
  int* n_g = new int[np];
  com->AllGather( &n_l, n_g, 1 );

  // Iterate over all parameter rows
  for ( int r = 0; r < nRow; ++ r )
    {
    // (All) gather all local M statistics
    double M_l[5];
    M_l[0] = outMeta->GetValueByName( r, "Mean X" ).ToDouble();
    M_l[1] = outMeta->GetValueByName( r, "Mean Y" ).ToDouble();
    M_l[2] = outMeta->GetValueByName( r, "M2 X" ).ToDouble();
    M_l[3] = outMeta->GetValueByName( r, "M2 Y" ).ToDouble();
    M_l[4] = outMeta->GetValueByName( r, "M XY" ).ToDouble();
    double* M_g = new double[5 * np];
    com->AllGather( M_l, M_g, 5 );

    // Aggregate all local quintuples of M statistics into global ones
    int ns = n_g[0];
    double meanX = M_g[0];
    double meanY = M_g[1];
    double mom2X = M_g[2];
    double mom2Y = M_g[3];
    double momXY = M_g[4];

    for ( int i = 1; i < np; ++ i )
      {
      int ns_l = n_g[i];
      int N = ns + ns_l;

      int o = 5 * i;
      double meanX_part = M_g[o];
      double meanY_part = M_g[o + 1];
      double mom2X_part = M_g[o + 2];
      double mom2Y_part = M_g[o + 3];
      double momXY_part = M_g[o + 4];

      double invN = 1. / static_cast<double>( N );

      double deltaX = meanX_part - meanX;
      double deltaX_sur_N = deltaX * invN;

      double deltaY = meanY_part - meanY;
      double deltaY_sur_N = deltaY * invN;

      int prod_ns = ns * ns_l;

      mom2X += mom2X_part
        + prod_ns * deltaX * deltaX_sur_N;

      mom2Y += mom2Y_part
        + prod_ns * deltaY * deltaY_sur_N;

      momXY += momXY_part
        + prod_ns * deltaX * deltaY_sur_N;

      meanX += ns_l * deltaX_sur_N;

      meanY += ns_l * deltaY_sur_N;

      ns = N;
      }

    outMeta->SetValueByName( r, "Mean X", meanX );
    outMeta->SetValueByName( r, "Mean Y", meanY );
    outMeta->SetValueByName( r, "M2 X", mom2X );
    outMeta->SetValueByName( r, "M2 Y", mom2Y );
    outMeta->SetValueByName( r, "M XY", momXY );

    // Set global statistics
    outMeta->SetValueByName( r, "Cardinality", ns );

    // Clean-up
    delete [] M_g;
    }
  delete [] n_g;
}
<|endoftext|>


<commit_before>#include "CrashHandler.h"

#include "SkTypes.h"

#include <stdlib.h>

// Disable SetupCrashHandler() unless SK_CRASH_HANDLER is defined.
#ifndef SK_CRASH_HANDLER
    void SetupCrashHandler() { }

#else

    #if defined(SK_BUILD_FOR_MAC)

        // We only use local unwinding, so we can define this to select a faster implementation.
        #define UNW_LOCAL_ONLY
        #include <libunwind.h>
        #include <cxxabi.h>

        static void handler(int sig) {
            unw_context_t context;
            unw_getcontext(&context);

            unw_cursor_t cursor;
            unw_init_local(&cursor, &context);

            SkDebugf("\nSignal %d:\n", sig);
            while (unw_step(&cursor) > 0) {
                static const size_t kMax = 256;
                char mangled[kMax], demangled[kMax];
                unw_word_t offset;
                unw_get_proc_name(&cursor, mangled, kMax, &offset);

                int ok;
                size_t len = kMax;
                abi::__cxa_demangle(mangled, demangled, &len, &ok);

                SkDebugf("%s (+0x%zx)\n", ok == 0 ? demangled : mangled, (size_t)offset);
            }
            SkDebugf("\n");

            // Exit NOW.  Don't notify other threads, don't call anything registered with atexit().
            _Exit(sig);
        }

    #elif defined(SK_BUILD_FOR_UNIX) && !defined(SK_BUILD_FOR_NACL)  // NACL doesn't have backtrace.

        // We'd use libunwind here too, but it's a pain to get installed for
        // both 32 and 64 bit on bots.  Doesn't matter much: catchsegv is best anyway.
        #include <execinfo.h>

        static void handler(int sig) {
            static const int kMax = 64;
            void* stack[kMax];
            const int count = backtrace(stack, kMax);

            SkDebugf("\nSignal %d [%s]:\n", sig, strsignal(sig));
            backtrace_symbols_fd(stack, count, 2/*stderr*/);

            // Exit NOW.  Don't notify other threads, don't call anything registered with atexit().
            _Exit(sig);
        }

    #endif

    #if (defined(SK_BUILD_FOR_MAC) || (defined(SK_BUILD_FOR_UNIX) && !defined(SK_BUILD_FOR_NACL)))
        #include <signal.h>

        void SetupCrashHandler() {
            static const int kSignals[] = {
                SIGABRT,
                SIGBUS,
                SIGFPE,
                SIGILL,
                SIGSEGV,
            };

            for (size_t i = 0; i < sizeof(kSignals) / sizeof(kSignals[0]); i++) {
                // Register our signal handler unless something's already done so (e.g. catchsegv).
                void (*prev)(int) = signal(kSignals[i], handler);
                if (prev != SIG_DFL) {
                    signal(kSignals[i], prev);
                }
            }
        }

    #elif defined(SK_CRASH_HANDLER) && defined(SK_BUILD_FOR_WIN)

        #include <DbgHelp.h>

        static const struct {
            const char* name;
            int code;
        } kExceptions[] = {
        #define _(E) {#E, E}
            _(EXCEPTION_ACCESS_VIOLATION),
            _(EXCEPTION_BREAKPOINT),
            _(EXCEPTION_INT_DIVIDE_BY_ZERO),
            _(EXCEPTION_STACK_OVERFLOW),
            // TODO: more?
        #undef _
        };

        static LONG WINAPI handler(EXCEPTION_POINTERS* e) {
            const DWORD code = e->ExceptionRecord->ExceptionCode;
            SkDebugf("\nCaught exception %u", code);
            for (size_t i = 0; i < SK_ARRAY_COUNT(kExceptions); i++) {
                if (kExceptions[i].code == code) {
                    SkDebugf(" %s", kExceptions[i].name);
                }
            }
            SkDebugf("\n");

            // We need to run SymInitialize before doing any of the stack walking below.
            HANDLE hProcess = GetCurrentProcess();
            SymInitialize(hProcess, 0, true);

            STACKFRAME64 frame;
            sk_bzero(&frame, sizeof(frame));
            // Start frame off from the frame that triggered the exception.
            CONTEXT* c = e->ContextRecord;
            frame.AddrPC.Mode      = AddrModeFlat;
            frame.AddrStack.Mode   = AddrModeFlat;
            frame.AddrFrame.Mode   = AddrModeFlat;
        #if defined(_X86_)
            frame.AddrPC.Offset    = c->Eip;
            frame.AddrStack.Offset = c->Esp;
            frame.AddrFrame.Offset = c->Ebp;
            const DWORD machineType = IMAGE_FILE_MACHINE_I386;
        #elif defined(_AMD64_)
            frame.AddrPC.Offset    = c->Rip;
            frame.AddrStack.Offset = c->Rsp;
            frame.AddrFrame.Offset = c->Rbp;
            const DWORD machineType = IMAGE_FILE_MACHINE_AMD64;
        #endif

            while (StackWalk64(machineType,
                               GetCurrentProcess(),
                               GetCurrentThread(),
                               &frame,
                               c,
                               NULL,
                               SymFunctionTableAccess64,
                               SymGetModuleBase64,
                               NULL)) {
                // Buffer to store symbol name in.
                static const int kMaxNameLength = 1024;
                uint8_t buffer[sizeof(IMAGEHLP_SYMBOL64) + kMaxNameLength];
                sk_bzero(buffer, sizeof(buffer));

                // We have to place IMAGEHLP_SYMBOL64 at the front, and fill in
                // how much space it can use.
                IMAGEHLP_SYMBOL64* symbol = reinterpret_cast<IMAGEHLP_SYMBOL64*>(&buffer);
                symbol->SizeOfStruct = sizeof(IMAGEHLP_SYMBOL64);
                symbol->MaxNameLength = kMaxNameLength - 1;

                // Translate the current PC into a symbol and byte offset from the symbol.
                DWORD64 offset;
                SymGetSymFromAddr64(hProcess, frame.AddrPC.Offset, &offset, symbol);

                SkDebugf("%s +%x\n", symbol->Name, offset);
            }

            // Exit NOW.  Don't notify other threads, don't call anything registered with atexit().
            _exit(1);

            // The compiler wants us to return something.  This is what we'd do
            // if we didn't _exit().
            return EXCEPTION_EXECUTE_HANDLER;
        }

        void SetupCrashHandler() {
            SetUnhandledExceptionFilter(handler);
        }

    #else  // We asked for SK_CRASH_HANDLER, but it's not Mac, Linux, or Windows.  Sorry!

        void SetupCrashHandler() { }

    #endif
#endif // SK_CRASH_HANDLER
<commit_msg>upstream hooking into google3 crash handler for better stacktraces<commit_after>#include "CrashHandler.h"

#include "SkTypes.h"

#include <stdlib.h>

// Disable SetupCrashHandler() unless SK_CRASH_HANDLER is defined.
#ifndef SK_CRASH_HANDLER
    void SetupCrashHandler() { }

#elif defined(GOOGLE3)
    #include "base/process_state.h"
    void SetupCrashHandler() { InstallSignalHandlers(); }

#else

    #if defined(SK_BUILD_FOR_MAC)

        // We only use local unwinding, so we can define this to select a faster implementation.
        #define UNW_LOCAL_ONLY
        #include <libunwind.h>
        #include <cxxabi.h>

        static void handler(int sig) {
            unw_context_t context;
            unw_getcontext(&context);

            unw_cursor_t cursor;
            unw_init_local(&cursor, &context);

            SkDebugf("\nSignal %d:\n", sig);
            while (unw_step(&cursor) > 0) {
                static const size_t kMax = 256;
                char mangled[kMax], demangled[kMax];
                unw_word_t offset;
                unw_get_proc_name(&cursor, mangled, kMax, &offset);

                int ok;
                size_t len = kMax;
                abi::__cxa_demangle(mangled, demangled, &len, &ok);

                SkDebugf("%s (+0x%zx)\n", ok == 0 ? demangled : mangled, (size_t)offset);
            }
            SkDebugf("\n");

            // Exit NOW.  Don't notify other threads, don't call anything registered with atexit().
            _Exit(sig);
        }

    #elif defined(SK_BUILD_FOR_UNIX) && !defined(SK_BUILD_FOR_NACL)  // NACL doesn't have backtrace.

        // We'd use libunwind here too, but it's a pain to get installed for
        // both 32 and 64 bit on bots.  Doesn't matter much: catchsegv is best anyway.
        #include <execinfo.h>

        static void handler(int sig) {
            static const int kMax = 64;
            void* stack[kMax];
            const int count = backtrace(stack, kMax);

            SkDebugf("\nSignal %d [%s]:\n", sig, strsignal(sig));
            backtrace_symbols_fd(stack, count, 2/*stderr*/);

            // Exit NOW.  Don't notify other threads, don't call anything registered with atexit().
            _Exit(sig);
        }

    #endif

    #if (defined(SK_BUILD_FOR_MAC) || (defined(SK_BUILD_FOR_UNIX) && !defined(SK_BUILD_FOR_NACL)))
        #include <signal.h>

        void SetupCrashHandler() {
            static const int kSignals[] = {
                SIGABRT,
                SIGBUS,
                SIGFPE,
                SIGILL,
                SIGSEGV,
            };

            for (size_t i = 0; i < sizeof(kSignals) / sizeof(kSignals[0]); i++) {
                // Register our signal handler unless something's already done so (e.g. catchsegv).
                void (*prev)(int) = signal(kSignals[i], handler);
                if (prev != SIG_DFL) {
                    signal(kSignals[i], prev);
                }
            }
        }

    #elif defined(SK_CRASH_HANDLER) && defined(SK_BUILD_FOR_WIN)

        #include <DbgHelp.h>

        static const struct {
            const char* name;
            int code;
        } kExceptions[] = {
        #define _(E) {#E, E}
            _(EXCEPTION_ACCESS_VIOLATION),
            _(EXCEPTION_BREAKPOINT),
            _(EXCEPTION_INT_DIVIDE_BY_ZERO),
            _(EXCEPTION_STACK_OVERFLOW),
            // TODO: more?
        #undef _
        };

        static LONG WINAPI handler(EXCEPTION_POINTERS* e) {
            const DWORD code = e->ExceptionRecord->ExceptionCode;
            SkDebugf("\nCaught exception %u", code);
            for (size_t i = 0; i < SK_ARRAY_COUNT(kExceptions); i++) {
                if (kExceptions[i].code == code) {
                    SkDebugf(" %s", kExceptions[i].name);
                }
            }
            SkDebugf("\n");

            // We need to run SymInitialize before doing any of the stack walking below.
            HANDLE hProcess = GetCurrentProcess();
            SymInitialize(hProcess, 0, true);

            STACKFRAME64 frame;
            sk_bzero(&frame, sizeof(frame));
            // Start frame off from the frame that triggered the exception.
            CONTEXT* c = e->ContextRecord;
            frame.AddrPC.Mode      = AddrModeFlat;
            frame.AddrStack.Mode   = AddrModeFlat;
            frame.AddrFrame.Mode   = AddrModeFlat;
        #if defined(_X86_)
            frame.AddrPC.Offset    = c->Eip;
            frame.AddrStack.Offset = c->Esp;
            frame.AddrFrame.Offset = c->Ebp;
            const DWORD machineType = IMAGE_FILE_MACHINE_I386;
        #elif defined(_AMD64_)
            frame.AddrPC.Offset    = c->Rip;
            frame.AddrStack.Offset = c->Rsp;
            frame.AddrFrame.Offset = c->Rbp;
            const DWORD machineType = IMAGE_FILE_MACHINE_AMD64;
        #endif

            while (StackWalk64(machineType,
                               GetCurrentProcess(),
                               GetCurrentThread(),
                               &frame,
                               c,
                               NULL,
                               SymFunctionTableAccess64,
                               SymGetModuleBase64,
                               NULL)) {
                // Buffer to store symbol name in.
                static const int kMaxNameLength = 1024;
                uint8_t buffer[sizeof(IMAGEHLP_SYMBOL64) + kMaxNameLength];
                sk_bzero(buffer, sizeof(buffer));

                // We have to place IMAGEHLP_SYMBOL64 at the front, and fill in
                // how much space it can use.
                IMAGEHLP_SYMBOL64* symbol = reinterpret_cast<IMAGEHLP_SYMBOL64*>(&buffer);
                symbol->SizeOfStruct = sizeof(IMAGEHLP_SYMBOL64);
                symbol->MaxNameLength = kMaxNameLength - 1;

                // Translate the current PC into a symbol and byte offset from the symbol.
                DWORD64 offset;
                SymGetSymFromAddr64(hProcess, frame.AddrPC.Offset, &offset, symbol);

                SkDebugf("%s +%x\n", symbol->Name, offset);
            }

            // Exit NOW.  Don't notify other threads, don't call anything registered with atexit().
            _exit(1);

            // The compiler wants us to return something.  This is what we'd do
            // if we didn't _exit().
            return EXCEPTION_EXECUTE_HANDLER;
        }

        void SetupCrashHandler() {
            SetUnhandledExceptionFilter(handler);
        }

    #else  // We asked for SK_CRASH_HANDLER, but it's not Mac, Linux, or Windows.  Sorry!

        void SetupCrashHandler() { }

    #endif
#endif // SK_CRASH_HANDLER
<|endoftext|>


<commit_before>#ifndef __SIARCOSTMAP_HPP__
#define __SIARCOSTMAP_HPP__

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud2_iterator.h>
#include <pcl_ros/transforms.h>
#include <nav_msgs/OccupancyGrid.h>

class SiarCostmap
{

public:

	// Class constructor
    SiarCostmap(std::string nodeName)
    {
		// Load parameters
		ros::NodeHandle lnh("~");
		if(!lnh.getParam("hz", m_hz))
			m_hz = 10.0;
		if(!lnh.getParam("obstacle_height", m_obstacleHeight))
			m_obstacleHeight = 0.05;
		if(!lnh.getParam("exp_decay", m_expDecay))
			m_expDecay = 0.0;
		if(!lnh.getParam("resolution", m_resolution))
			m_resolution = 0.05;
		if(!lnh.getParam("width", m_width))
			m_width = 3.0;
		if(!lnh.getParam("height", m_height))
			m_height = 4.0;
		if(!lnh.getParam("base_frame_id", m_baseFrameId))
			m_baseFrameId = "base_link";

		// Setup subscription to sensor data
		m_sub0 = m_nh.subscribe("cloud0", 1, &SiarCostmap::cloud0Callback, this);
		m_sub1 = m_nh.subscribe("cloud1", 1, &SiarCostmap::cloud1Callback, this);
		m_sub2 = m_nh.subscribe("cloud2", 1, &SiarCostmap::cloud2Callback, this);
		m_sub3 = m_nh.subscribe("cloud3", 1, &SiarCostmap::cloud3Callback, this);
		m_sub4 = m_nh.subscribe("cloud4", 1, &SiarCostmap::cloud4Callback, this);
		m_sub5 = m_nh.subscribe("cloud5", 1, &SiarCostmap::cloud5Callback, this);
		for(int i=0; i<6; i++)
			m_cloudNew[i] = false;

		// Setup publisher
		m_pub = m_nh.advertise<nav_msgs::OccupancyGrid>(nodeName+"/costmap", 0);

		// Setup timer for data processing
		timer = m_nh.createTimer(ros::Duration(1.0/m_hz), &SiarCostmap::updateTimer, this);

		// Initialize costmap
		m_costmap.header.frame_id = m_baseFrameId;
		m_costmap.header.seq = 0;
		m_costmap.header.stamp = ros::Time::now();
		m_costmap.info.map_load_time = m_costmap.header.stamp;
		m_costmap.info.resolution = m_resolution;
		m_costmap.info.width = (int)(m_width/m_resolution);
		m_costmap.info.height = (int)(m_height/m_resolution);
		m_costmap.info.origin.position.x = -m_height/2.0;
		m_costmap.info.origin.position.y = m_width/2.0;
		m_costmap.info.origin.position.z = 0.0;
		m_costmap.info.origin.orientation.x = 0.0;
		m_costmap.info.origin.orientation.y = 0.0;
		m_costmap.info.origin.orientation.z = -0.7071;
		m_costmap.info.origin.orientation.w = 0.7071;
		m_costmap.data.resize(m_costmap.info.width*m_costmap.info.height);
		m_maxX = m_height/2.0;
		m_minX = -m_height/2.0;
		m_maxY = m_width/2.0;
		m_minY = -m_width/2.0;
		m_divRes = 1/m_resolution;
    }

    // Class destructor
    ~SiarCostmap(void)
    {
	}

	nav_msgs::OccupancyGrid &updateCostmap(void)
	{
		// Time stamp
		ros::Time t = ros::Time::now();

		// Initialize all cells in the grid to unkown value
		for(int i=0; i<m_costmap.data.size(); i++)
			m_costmap.data[i] = 0;//-128;

		// Process the latest cloud received in each topic
		for(int i=0; i<6; i++)
		{
			// Check if we have new data
			if(!m_cloudNew[i])
				continue;

			// Transform point-cloud to base frame
			sensor_msgs::PointCloud2 cloud;
			try
			{
				pcl_ros::transformPointCloud(m_baseFrameId, m_cloudTf[i], m_cloud[i], cloud);
			}
			catch (tf::TransformException ex)
			{
				continue;
			}

			// Evaluate every point into the cloud
			sensor_msgs::PointCloud2Iterator<float> iterX(cloud, "x");
			sensor_msgs::PointCloud2Iterator<float> iterY(cloud, "y");
			sensor_msgs::PointCloud2Iterator<float> iterZ(cloud, "z");
			for(int i=0; i<cloud.width; i++, ++iterX, ++iterY, ++iterZ)
			{
				float x = *iterX, y = *iterY, z = *iterZ;
				if(x > m_minX && x < m_maxX && y > m_minY && y < m_maxY)
				{
					int index = point2index(x,y);
					if(m_costmap.data[index] < 127)
					{
						if(fabs(z) > m_obstacleHeight)
							m_costmap.data[index] = 127;
						else
							m_costmap.data[index] = (int8_t)(z*100.0);
					}
				}
			}
		}

		// Update costmap info
		m_costmap.header.seq++;
		m_costmap.header.stamp = t;
		m_costmap.info.map_load_time = t;

		// Prepare next sensor reading
		for(int i=0; i<6; i++)
			m_cloudNew[i] = false;

		return m_costmap;
	}

	nav_msgs::OccupancyGrid &getCostmap(void)
	{
		return m_costmap;
	}

private:

    void cloud0Callback(const sensor_msgs::PointCloud2ConstPtr& msg)
    {
		processCloud(msg, 0);
	}

	void cloud1Callback(const sensor_msgs::PointCloud2ConstPtr& msg)
    {
		processCloud(msg, 1);
	}

	void cloud2Callback(const sensor_msgs::PointCloud2ConstPtr& msg)
    {
		processCloud(msg, 2);
	}

	void cloud3Callback(const sensor_msgs::PointCloud2ConstPtr& msg)
    {
		processCloud(msg, 3);
	}

	void cloud4Callback(const sensor_msgs::PointCloud2ConstPtr& msg)
    {
		processCloud(msg, 4);
	}

	void cloud5Callback(const sensor_msgs::PointCloud2ConstPtr& msg)
    {
		processCloud(msg, 5);
	}

	void updateTimer(const ros::TimerEvent& event)
	{
		// Update costmap
		updateCostmap();

		// Publish costmap
		m_pub.publish(m_costmap);
	}

	void processCloud(const sensor_msgs::PointCloud2ConstPtr& msg, int id)
	{
		static bool gotTf[6] = {false, false, false, false, false, false};

		if(!gotTf[id])
		{
			try
			{
				m_tfListener.lookupTransform(m_baseFrameId, msg->header.frame_id, ros::Time(0), m_cloudTf[id]);
				gotTf[id] = true;
			}
			catch (tf::TransformException ex)
			{
				ROS_ERROR("Error: %s", ex.what());
			}
		}
		else
		{
			m_cloud[id] = *msg;
			m_cloudNew[id] = true;
		}
	}

	inline int point2index(float &x, float &y)
	{
		//return (m_costmap.info.height - (int)((x-m_minX)*m_divRes))*m_costmap.info.width + m_costmap.info.width - (int)((y-m_minY)*m_divRes);
		return ((int)((x-m_minX)*m_divRes))*m_costmap.info.width + (int)((y-m_minY)*m_divRes);
	}

	// Params
	double m_hz, m_obstacleHeight, m_expDecay;
	double m_width, m_height, m_resolution;
	std::string m_baseFrameId;

	// ROS stuff
	ros::NodeHandle m_nh;
	ros::Timer timer;
	tf::TransformListener m_tfListener;
	ros::Subscriber m_sub0, m_sub1, m_sub2, m_sub3, m_sub4, m_sub5;
	ros::Publisher m_pub;

	// Sensor data
	sensor_msgs::PointCloud2 m_cloud[6];
	bool m_cloudNew[6];
	tf::StampedTransform m_cloudTf[6];

	// Compute costmap
	float m_minX, m_maxX, m_minY, m_maxY;
	float m_divRes;
	nav_msgs::OccupancyGrid m_costmap;
};

#endif

<commit_msg>Added new interface to costap<commit_after>#ifndef __SIARCOSTMAP_HPP__
#define __SIARCOSTMAP_HPP__

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud2_iterator.h>
#include <pcl_ros/transforms.h>
#include <nav_msgs/OccupancyGrid.h>

class SiarCostmap
{

public:

	// Point 2D
	struct Point2D
	{
		float x;
		float y;
	};

	// Class constructor
    SiarCostmap(std::string nodeName)
    {
		// Load parameters
		ros::NodeHandle lnh("~");
		if(!lnh.getParam("hz", m_hz))
			m_hz = 10.0;
		if(!lnh.getParam("obstacle_height", m_obstacleHeight))
			m_obstacleHeight = 0.05;
		if(!lnh.getParam("exp_decay", m_expDecay))
			m_expDecay = 0.0;
		if(!lnh.getParam("resolution", m_resolution))
			m_resolution = 0.05;
		if(!lnh.getParam("width", m_width))
			m_width = 3.0;
		if(!lnh.getParam("height", m_height))
			m_height = 4.0;
		if(!lnh.getParam("base_frame_id", m_baseFrameId))
			m_baseFrameId = "base_link";

		// Setup subscription to sensor data
		m_sub0 = m_nh.subscribe("cloud0", 1, &SiarCostmap::cloud0Callback, this);
		m_sub1 = m_nh.subscribe("cloud1", 1, &SiarCostmap::cloud1Callback, this);
		m_sub2 = m_nh.subscribe("cloud2", 1, &SiarCostmap::cloud2Callback, this);
		m_sub3 = m_nh.subscribe("cloud3", 1, &SiarCostmap::cloud3Callback, this);
		m_sub4 = m_nh.subscribe("cloud4", 1, &SiarCostmap::cloud4Callback, this);
		m_sub5 = m_nh.subscribe("cloud5", 1, &SiarCostmap::cloud5Callback, this);
		for(int i=0; i<6; i++)
			m_cloudNew[i] = false;

		// Setup publisher
		m_pub = m_nh.advertise<nav_msgs::OccupancyGrid>(nodeName+"/costmap", 0);

		// Setup timer for data processing
		timer = m_nh.createTimer(ros::Duration(1.0/m_hz), &SiarCostmap::updateTimer, this);

		// Initialize costmap
		m_costmap.header.frame_id = m_baseFrameId;
		m_costmap.header.seq = 0;
		m_costmap.header.stamp = ros::Time::now();
		m_costmap.info.map_load_time = m_costmap.header.stamp;
		m_costmap.info.resolution = m_resolution;
		m_costmap.info.width = (int)(m_width/m_resolution);
		m_costmap.info.height = (int)(m_height/m_resolution);
		m_costmap.info.origin.position.x = -m_height/2.0;
		m_costmap.info.origin.position.y = m_width/2.0;
		m_costmap.info.origin.position.z = 0.0;
		m_costmap.info.origin.orientation.x = 0.0;
		m_costmap.info.origin.orientation.y = 0.0;
		m_costmap.info.origin.orientation.z = -0.7071;
		m_costmap.info.origin.orientation.w = 0.7071;
		m_costmap.data.resize(m_costmap.info.width*m_costmap.info.height);
		m_maxX = m_height/2.0;
		m_minX = -m_height/2.0;
		m_maxY = m_width/2.0;
		m_minY = -m_width/2.0;
		m_divRes = 1/m_resolution;
    }

    // Class destructor
    ~SiarCostmap(void)
    {
	}

	nav_msgs::OccupancyGrid &updateCostmap(void)
	{
		// Time stamp
		ros::Time t = ros::Time::now();

		// Initialize all cells in the grid to unkown value
		for(int i=0; i<m_costmap.data.size(); i++)
			m_costmap.data[i] = 0;//-128;

		// Process the latest cloud received in each topic
		for(int i=0; i<6; i++)
		{
			// Check if we have new data
			if(!m_cloudNew[i])
				continue;

			// Transform point-cloud to base frame
			sensor_msgs::PointCloud2 cloud;
			try
			{
				pcl_ros::transformPointCloud(m_baseFrameId, m_cloudTf[i], m_cloud[i], cloud);
			}
			catch (tf::TransformException ex)
			{
				continue;
			}

			// Evaluate every point into the cloud
			sensor_msgs::PointCloud2Iterator<float> iterX(cloud, "x");
			sensor_msgs::PointCloud2Iterator<float> iterY(cloud, "y");
			sensor_msgs::PointCloud2Iterator<float> iterZ(cloud, "z");
			for(int i=0; i<cloud.width; i++, ++iterX, ++iterY, ++iterZ)
			{
				float x = *iterX, y = *iterY, z = *iterZ;
				if(x > m_minX && x < m_maxX && y > m_minY && y < m_maxY)
				{
					int index = point2index(x,y);
					if(m_costmap.data[index] < 127)
					{
						if(fabs(z) > m_obstacleHeight)
							m_costmap.data[index] = 127;
						else
							m_costmap.data[index] = (int8_t)(z*100.0);
					}
				}
			}
		}

		// Update costmap info
		m_costmap.header.seq++;
		m_costmap.header.stamp = t;
		m_costmap.info.map_load_time = t;

		// Prepare next sensor reading
		for(int i=0; i<6; i++)
			m_cloudNew[i] = false;

		return m_costmap;
	}

	nav_msgs::OccupancyGrid &getCostmap(void)
	{
		return m_costmap;
	}

	inline float getCost(float x, float y)
	{
		if(x > m_minX && x < m_maxX && y > m_minY && y < m_maxY)
			return m_costmap.data[point2index(x,y)];
		else
			return -128;
	}

	inline std::vector<float> getCost(std::vector<SiarCostmap::Point2D> &points)
	{
		std::vector<float> cost(points.size(), -128);
		for(int i=0; i<points.size(); i++)
		{
			float x = points[i].x;
			float y = points[i].y;
			if(x > m_minX && x < m_maxX && y > m_minY && y < m_maxY)
				cost[i] = m_costmap.data[point2index(x, y)];
		}
		return cost;
	}

private:

    void cloud0Callback(const sensor_msgs::PointCloud2ConstPtr& msg)
    {
		processCloud(msg, 0);
	}

	void cloud1Callback(const sensor_msgs::PointCloud2ConstPtr& msg)
    {
		processCloud(msg, 1);
	}

	void cloud2Callback(const sensor_msgs::PointCloud2ConstPtr& msg)
    {
		processCloud(msg, 2);
	}

	void cloud3Callback(const sensor_msgs::PointCloud2ConstPtr& msg)
    {
		processCloud(msg, 3);
	}

	void cloud4Callback(const sensor_msgs::PointCloud2ConstPtr& msg)
    {
		processCloud(msg, 4);
	}

	void cloud5Callback(const sensor_msgs::PointCloud2ConstPtr& msg)
    {
		processCloud(msg, 5);
	}

	void updateTimer(const ros::TimerEvent& event)
	{
		// Update costmap
		updateCostmap();

		// Publish costmap
		m_pub.publish(m_costmap);
	}

	void processCloud(const sensor_msgs::PointCloud2ConstPtr& msg, int id)
	{
		static bool gotTf[6] = {false, false, false, false, false, false};

		if(!gotTf[id])
		{
			try
			{
				m_tfListener.lookupTransform(m_baseFrameId, msg->header.frame_id, ros::Time(0), m_cloudTf[id]);
				gotTf[id] = true;
			}
			catch (tf::TransformException ex)
			{
				ROS_ERROR("Error: %s", ex.what());
			}
		}
		else
		{
			m_cloud[id] = *msg;
			m_cloudNew[id] = true;
		}
	}

	inline int point2index(float &x, float &y)
	{
		//return (m_costmap.info.height - (int)((x-m_minX)*m_divRes))*m_costmap.info.width + m_costmap.info.width - (int)((y-m_minY)*m_divRes);
		return ((int)((x-m_minX)*m_divRes))*m_costmap.info.width + (int)((y-m_minY)*m_divRes);
	}

	// Params
	double m_hz, m_obstacleHeight, m_expDecay;
	double m_width, m_height, m_resolution;
	std::string m_baseFrameId;

	// ROS stuff
	ros::NodeHandle m_nh;
	ros::Timer timer;
	tf::TransformListener m_tfListener;
	ros::Subscriber m_sub0, m_sub1, m_sub2, m_sub3, m_sub4, m_sub5;
	ros::Publisher m_pub;

	// Sensor data
	sensor_msgs::PointCloud2 m_cloud[6];
	bool m_cloudNew[6];
	tf::StampedTransform m_cloudTf[6];

	// Compute costmap
	float m_minX, m_maxX, m_minY, m_maxY;
	float m_divRes;
	nav_msgs::OccupancyGrid m_costmap;
};

#endif

<|endoftext|>


<commit_before>#include <depth_calibration/depth_adjuster.h>
#include <pluginlib/class_list_macros.h>

namespace depth_calibration
{
void DepthAdjuster::onInit()
{
  ros::NodeHandle& nh = getNodeHandle();

  sub_depth_raw_ = nh.subscribe<sensor_msgs::Image>("input_depth_raw", 1, &DepthAdjuster::apply_calibration_cb, this);
  pub_calibrated_depth_raw_ = nh.advertise<sensor_msgs::Image>("output_depth_raw", 1);

  sub_camera_info_ = nh.subscribe<sensor_msgs::CameraInfo>("input_camera_info", 1, &DepthAdjuster::relay_camera_info,
                                                           this);
  pub_camera_info_relay_ = nh.advertise<sensor_msgs::CameraInfo>("output_camera_info_relay", 1);

  ros::NodeHandle& private_nh = getPrivateNodeHandle();
  bool enable;
  private_nh.param("enable", enable, true);

  std::string calibration_file_path;
  private_nh.param("calibration_file_path", calibration_file_path, std::string("camera_info/depth_calibration.yaml"));

  if (enable)
    load_calibration(calibration_file_path);
  else
    ROS_WARN("[Depth adjuster] Depth calibration disabled by launch file");

  private_nh.param("unknown_depth_distance", unknown_depth_distance_, 0.0);
  unknown_depth_distance_ = unknown_depth_distance_ * 1000; //convert to depth value, which are in mm

  private_nh.param("is_occluded_percentage", is_occluded_percentage_, 1.0);
  private_nh.param("occluded_distance", occluded_distance_, 0.0);

  private_nh.param("border_percentage_top", border_percentage_top_, 0.0);
  private_nh.param("border_percentage_bottom", border_percentage_bottom_, 0.0);
  private_nh.param("border_percentage_left", border_percentage_left_, 0.0);
  private_nh.param("border_percentage_right", border_percentage_right_, 0.0);
}

void DepthAdjuster::load_calibration(std::string file_path)
{
  cv::FileStorage file_storage(file_path, cv::FileStorage::READ);

  if (!file_storage.isOpened())
  {
    ROS_WARN("[Depth adjuster] Cannot load depth calibration file: No file at %s", file_path.c_str());
    return;
  }

  file_storage["depth_multiplier"] >> depth_multiplier_correction_;
  file_storage.release();

  if (depth_multiplier_correction_.empty())
    ROS_WARN("[Depth adjuster] Depth calibration couldn't be loaded: Couldn't find depth_multiplier in file");
  else
    ROS_INFO("[Depth adjuster] Depth calibration file loaded successfully");
}

void DepthAdjuster::apply_calibration_cb(const sensor_msgs::ImageConstPtr& depth_msg)
{
  if (depth_multiplier_correction_.empty())
  {
    //just relay to make sure stuff still works
    pub_calibrated_depth_raw_.publish(depth_msg);
    return;
  }

  if (depth_msg->width != depth_multiplier_correction_.cols || depth_msg->height != depth_multiplier_correction_.rows)
  {
    ROS_ERROR_STREAM("[Depth adjuster] Calibration file has different resolution than camera depth image");
    ROS_ERROR_STREAM(
        "[Depth adjuster] Calibration multiplier: " << depth_multiplier_correction_.cols << "x" << depth_multiplier_correction_.rows << ", depth image: " << depth_msg->width << "x" << depth_msg->height);
    ROS_WARN_STREAM("[Depth adjuster] Skipping depth adjustments.");
    depth_multiplier_correction_.release();
    pub_calibrated_depth_raw_.publish(depth_msg);
    return;
  }

  cv_bridge::CvImagePtr cv_depth_image;
  try
  {
    cv_depth_image = cv_bridge::toCvCopy(depth_msg, "16UC1");
  }
  catch (cv_bridge::Exception& e)
  {
    ROS_WARN_THROTTLE(1.0, "[Depth adjuster] cv_bridge exception with cv_depth_image: %s", e.what());
    return;
  }

  cv::Mat depth_double = cv_depth_image->image.clone();
  cv::Mat zero_addition = (depth_double == 0); //if true value is set to 255, so divide by 255 later

  depth_double.convertTo(depth_double, CV_64F);
  depth_double = (depth_double).mul(depth_multiplier_correction_);

  int unknown_distances_count = cv::countNonZero(zero_addition);

  if(unknown_distances_count <= depth_multiplier_correction_.total() * is_occluded_percentage_)
  {
    zero_addition.convertTo(zero_addition, CV_64F);
    depth_double += (zero_addition * unknown_depth_distance_ / 255.0);
  }
  else
  {
    ROS_WARN_THROTTLE(5.0, "3D sensors seems to be occluded");
    zero_addition.convertTo(zero_addition, CV_64F);
    depth_double += (zero_addition * occluded_distance_ * 1000 / 255.0);
  }

  if (border_percentage_top_ != 0.0 || border_percentage_bottom_ != 0.0 || border_percentage_left_ != 0.0
      || border_percentage_right_ != 0.0)
  {
    remove_borders(depth_double);
  }

  depth_double.convertTo(cv_depth_image->image, CV_16U);
  pub_calibrated_depth_raw_.publish(cv_depth_image->toImageMsg());
}

void DepthAdjuster::remove_borders(cv::Mat& image)
{
  int border_height_top = image.cols * border_percentage_top_;
  int border_height_bottom = image.cols * border_percentage_bottom_;
  int border_width_left = image.rows * border_percentage_left_;
  int border_width_right = image.rows * border_percentage_right_;
  cv::Mat borders(image.size(), CV_64F, 1.0);

  image(cv::Rect(0, 0, image.cols, border_height_top)).setTo(0); //top
  image(cv::Rect(0, image.rows - border_height_bottom, image.cols, border_height_bottom)).setTo(0); //bottom

  image(cv::Rect(0, 0, border_width_left, image.rows)).setTo(0); //left
  image(cv::Rect(image.cols - border_width_right, 0, border_width_right, image.rows)).setTo(0); //right
}

void DepthAdjuster::relay_camera_info(const sensor_msgs::CameraInfoConstPtr& info_msg)
{
  pub_camera_info_relay_.publish(info_msg);
}
}

PLUGINLIB_EXPORT_CLASS(depth_calibration::DepthAdjuster, nodelet::Nodelet)
<commit_msg>Apply border removal, even when no calibration file is given<commit_after>#include <depth_calibration/depth_adjuster.h>
#include <pluginlib/class_list_macros.h>

namespace depth_calibration
{
void DepthAdjuster::onInit()
{
  ros::NodeHandle& nh = getNodeHandle();

  sub_depth_raw_ = nh.subscribe<sensor_msgs::Image>("input_depth_raw", 1, &DepthAdjuster::apply_calibration_cb, this);
  pub_calibrated_depth_raw_ = nh.advertise<sensor_msgs::Image>("output_depth_raw", 1);

  sub_camera_info_ = nh.subscribe<sensor_msgs::CameraInfo>("input_camera_info", 1, &DepthAdjuster::relay_camera_info,
                                                           this);
  pub_camera_info_relay_ = nh.advertise<sensor_msgs::CameraInfo>("output_camera_info_relay", 1);

  ros::NodeHandle& private_nh = getPrivateNodeHandle();
  bool enable;
  private_nh.param("enable", enable, true);

  std::string calibration_file_path;
  private_nh.param("calibration_file_path", calibration_file_path, std::string("camera_info/depth_calibration.yaml"));

  if (enable)
    load_calibration(calibration_file_path);
  else
    ROS_WARN("[Depth adjuster] Depth calibration disabled by launch file");

  private_nh.param("unknown_depth_distance", unknown_depth_distance_, 0.0);
  unknown_depth_distance_ = unknown_depth_distance_ * 1000; //convert to depth value, which are in mm

  private_nh.param("is_occluded_percentage", is_occluded_percentage_, 1.0);
  private_nh.param("occluded_distance", occluded_distance_, 0.0);

  private_nh.param("border_percentage_top", border_percentage_top_, 0.0);
  private_nh.param("border_percentage_bottom", border_percentage_bottom_, 0.0);
  private_nh.param("border_percentage_left", border_percentage_left_, 0.0);
  private_nh.param("border_percentage_right", border_percentage_right_, 0.0);
}

void DepthAdjuster::load_calibration(std::string file_path)
{
  cv::FileStorage file_storage(file_path, cv::FileStorage::READ);

  if (!file_storage.isOpened())
  {
    ROS_WARN("[Depth adjuster] Cannot load depth calibration file: No file at %s", file_path.c_str());
    return;
  }

  file_storage["depth_multiplier"] >> depth_multiplier_correction_;
  file_storage.release();

  if (depth_multiplier_correction_.empty())
    ROS_WARN("[Depth adjuster] Depth calibration couldn't be loaded: Couldn't find depth_multiplier in file");
  else
    ROS_INFO("[Depth adjuster] Depth calibration file loaded successfully");
}

void DepthAdjuster::apply_calibration_cb(const sensor_msgs::ImageConstPtr& depth_msg)
{
  if (!depth_multiplier_correction_.empty() && (depth_msg->width != depth_multiplier_correction_.cols || depth_msg->height != depth_multiplier_correction_.rows))
  {
    ROS_ERROR_STREAM("[Depth adjuster] Calibration file has different resolution than camera depth image");
    ROS_ERROR_STREAM(
        "[Depth adjuster] Calibration multiplier: " << depth_multiplier_correction_.cols << "x" << depth_multiplier_correction_.rows << ", depth image: " << depth_msg->width << "x" << depth_msg->height);
    ROS_WARN_STREAM("[Depth adjuster] Skipping depth calibration adjustments.");
    depth_multiplier_correction_.release();
  }

  cv_bridge::CvImagePtr cv_depth_image;
  try
  {
    cv_depth_image = cv_bridge::toCvCopy(depth_msg, "16UC1");
  }
  catch (cv_bridge::Exception& e)
  {
    ROS_WARN_THROTTLE(1.0, "[Depth adjuster] cv_bridge exception with cv_depth_image: %s", e.what());
    return;
  }

  cv::Mat depth_double = cv_depth_image->image.clone();
  cv::Mat zero_addition = (depth_double == 0); //if true value is set to 255, so divide by 255 later

  depth_double.convertTo(depth_double, CV_64F);
  if (!depth_multiplier_correction_.empty())
  {
    depth_double = (depth_double).mul(depth_multiplier_correction_);
  }

  int unknown_distances_count = cv::countNonZero(zero_addition);

  if(unknown_distances_count <= depth_double.total() * is_occluded_percentage_)
  {
    zero_addition.convertTo(zero_addition, CV_64F);
    depth_double += (zero_addition * unknown_depth_distance_ / 255.0);
  }
  else
  {
    ROS_WARN_THROTTLE(5.0, "3D sensors seems to be occluded");
    zero_addition.convertTo(zero_addition, CV_64F);
    depth_double += (zero_addition * occluded_distance_ * 1000 / 255.0);
  }

  if (border_percentage_top_ != 0.0 || border_percentage_bottom_ != 0.0 || border_percentage_left_ != 0.0
      || border_percentage_right_ != 0.0)
  {
    remove_borders(depth_double);
  }

  depth_double.convertTo(cv_depth_image->image, CV_16U);
  pub_calibrated_depth_raw_.publish(cv_depth_image->toImageMsg());
}

void DepthAdjuster::remove_borders(cv::Mat& image)
{
  int border_height_top = image.cols * border_percentage_top_;
  int border_height_bottom = image.cols * border_percentage_bottom_;
  int border_width_left = image.rows * border_percentage_left_;
  int border_width_right = image.rows * border_percentage_right_;
  cv::Mat borders(image.size(), CV_64F, 1.0);

  image(cv::Rect(0, 0, image.cols, border_height_top)).setTo(0); //top
  image(cv::Rect(0, image.rows - border_height_bottom, image.cols, border_height_bottom)).setTo(0); //bottom

  image(cv::Rect(0, 0, border_width_left, image.rows)).setTo(0); //left
  image(cv::Rect(image.cols - border_width_right, 0, border_width_right, image.rows)).setTo(0); //right
}

void DepthAdjuster::relay_camera_info(const sensor_msgs::CameraInfoConstPtr& info_msg)
{
  pub_camera_info_relay_.publish(info_msg);
}
}

PLUGINLIB_EXPORT_CLASS(depth_calibration::DepthAdjuster, nodelet::Nodelet)
<|endoftext|>


<commit_before>/*
 * Copyright (c) 2010 ARM Limited
 * All rights reserved
 *
 * The license below extends only to copyright in the software and shall
 * not be construed as granting a license to any other intellectual
 * property including but not limited to intellectual property relating
 * to a hardware implementation of the functionality of the software
 * licensed hereunder.  You may use the software subject to the license
 * terms below provided that you ensure that this notice is replicated
 * unmodified and in its entirety in all distributions of the software,
 * modified or unmodified, in source code or in binary form.
 *
 * 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 the copyright holders 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.
 *
 * Authors: Ali Saidi
 */

#include "base/trace.hh"
#include "dev/arm/rv_ctrl.hh"
#include "mem/packet.hh"
#include "mem/packet_access.hh"

RealViewCtrl::RealViewCtrl(Params *p)
    : BasicPioDevice(p), flags(0)
{
    pioSize = 0xD4;
}

Tick
RealViewCtrl::read(PacketPtr pkt)
{
    assert(pkt->getAddr() >= pioAddr && pkt->getAddr() < pioAddr + pioSize);
    assert(pkt->getSize() == 4);
    Addr daddr = pkt->getAddr() - pioAddr;
    pkt->allocate();

    switch(daddr) {
      case ProcId0:
        pkt->set(params()->proc_id0);
        break;
      case ProcId1:
        pkt->set(params()->proc_id1);
        break;
      case Clock24:
        Tick clk;
        clk = (Tick)(curTick() / (24 * SimClock::Int::us));
        pkt->set((uint32_t)(clk));
        break;
      case Clock100:
        Tick clk100;
        clk100 = (Tick)(curTick() / (100 * SimClock::Int::us));
        pkt->set((uint32_t)(clk100));
        break;
      case Flash:
        pkt->set<uint32_t>(0);
        break;
      case Clcd:
        pkt->set<uint32_t>(0x00001F00);
        break;
      case Osc0:
        pkt->set<uint32_t>(0x00012C5C);
        break;
      case Osc1:
        pkt->set<uint32_t>(0x00002CC0);
        break;
      case Osc2:
        pkt->set<uint32_t>(0x00002C75);
        break;
      case Osc3:
        pkt->set<uint32_t>(0x00020211);
        break;
      case Osc4:
        pkt->set<uint32_t>(0x00002C75);
        break;
      case Lock:
        pkt->set<uint32_t>(sysLock);
        break;
      case Flags:
        pkt->set<uint32_t>(flags);
        break;
      case IdReg:
        pkt->set<uint32_t>(params()->idreg);
        break;
      case CfgStat:
        pkt->set<uint32_t>(1);
        break;
      default:
        warn("Tried to read RealView I/O at offset %#x that doesn't exist\n",
             daddr);
        break;
    }
    pkt->makeAtomicResponse();
    return pioDelay;

}

Tick
RealViewCtrl::write(PacketPtr pkt)
{
    assert(pkt->getAddr() >= pioAddr && pkt->getAddr() < pioAddr + pioSize);

    Addr daddr = pkt->getAddr() - pioAddr;
    switch (daddr) {
      case Flash:
      case Clcd:
      case Osc0:
      case Osc1:
      case Osc2:
      case Osc3:
      case Osc4:
        break;
      case Lock:
        sysLock.lockVal = pkt->get<uint16_t>();
        break;
      case Flags:
        flags = pkt->get<uint32_t>();
        break;
      case FlagsClr:
        flags = 0;
        break;
      default:
        warn("Tried to write RVIO at offset %#x that doesn't exist\n",
             daddr);
        break;
    }
    pkt->makeAtomicResponse();
    return pioDelay;
}

void
RealViewCtrl::serialize(std::ostream &os)
{
    SERIALIZE_SCALAR(flags);
}

void
RealViewCtrl::unserialize(Checkpoint *cp, const std::string &section)
{
    UNSERIALIZE_SCALAR(flags);
}

RealViewCtrl *
RealViewCtrlParams::create()
{
    return new RealViewCtrl(this);
}
<commit_msg>ARM: fix the calculation of the values in the RV clocks<commit_after>/*
 * Copyright (c) 2010 ARM Limited
 * All rights reserved
 *
 * The license below extends only to copyright in the software and shall
 * not be construed as granting a license to any other intellectual
 * property including but not limited to intellectual property relating
 * to a hardware implementation of the functionality of the software
 * licensed hereunder.  You may use the software subject to the license
 * terms below provided that you ensure that this notice is replicated
 * unmodified and in its entirety in all distributions of the software,
 * modified or unmodified, in source code or in binary form.
 *
 * 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 the copyright holders 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.
 *
 * Authors: Ali Saidi
 */

#include "base/trace.hh"
#include "dev/arm/rv_ctrl.hh"
#include "mem/packet.hh"
#include "mem/packet_access.hh"

RealViewCtrl::RealViewCtrl(Params *p)
    : BasicPioDevice(p), flags(0)
{
    pioSize = 0xD4;
}

Tick
RealViewCtrl::read(PacketPtr pkt)
{
    assert(pkt->getAddr() >= pioAddr && pkt->getAddr() < pioAddr + pioSize);
    assert(pkt->getSize() == 4);
    Addr daddr = pkt->getAddr() - pioAddr;
    pkt->allocate();

    switch(daddr) {
      case ProcId0:
        pkt->set(params()->proc_id0);
        break;
      case ProcId1:
        pkt->set(params()->proc_id1);
        break;
      case Clock24:
        Tick clk;
        clk = SimClock::Float::MHz * curTick() * 24;
        pkt->set((uint32_t)(clk));
        break;
      case Clock100:
        Tick clk100;
        clk100 = SimClock::Float::MHz * curTick() * 100;
        pkt->set((uint32_t)(clk100));
        break;
      case Flash:
        pkt->set<uint32_t>(0);
        break;
      case Clcd:
        pkt->set<uint32_t>(0x00001F00);
        break;
      case Osc0:
        pkt->set<uint32_t>(0x00012C5C);
        break;
      case Osc1:
        pkt->set<uint32_t>(0x00002CC0);
        break;
      case Osc2:
        pkt->set<uint32_t>(0x00002C75);
        break;
      case Osc3:
        pkt->set<uint32_t>(0x00020211);
        break;
      case Osc4:
        pkt->set<uint32_t>(0x00002C75);
        break;
      case Lock:
        pkt->set<uint32_t>(sysLock);
        break;
      case Flags:
        pkt->set<uint32_t>(flags);
        break;
      case IdReg:
        pkt->set<uint32_t>(params()->idreg);
        break;
      case CfgStat:
        pkt->set<uint32_t>(1);
        break;
      default:
        warn("Tried to read RealView I/O at offset %#x that doesn't exist\n",
             daddr);
        break;
    }
    pkt->makeAtomicResponse();
    return pioDelay;

}

Tick
RealViewCtrl::write(PacketPtr pkt)
{
    assert(pkt->getAddr() >= pioAddr && pkt->getAddr() < pioAddr + pioSize);

    Addr daddr = pkt->getAddr() - pioAddr;
    switch (daddr) {
      case Flash:
      case Clcd:
      case Osc0:
      case Osc1:
      case Osc2:
      case Osc3:
      case Osc4:
        break;
      case Lock:
        sysLock.lockVal = pkt->get<uint16_t>();
        break;
      case Flags:
        flags = pkt->get<uint32_t>();
        break;
      case FlagsClr:
        flags = 0;
        break;
      default:
        warn("Tried to write RVIO at offset %#x that doesn't exist\n",
             daddr);
        break;
    }
    pkt->makeAtomicResponse();
    return pioDelay;
}

void
RealViewCtrl::serialize(std::ostream &os)
{
    SERIALIZE_SCALAR(flags);
}

void
RealViewCtrl::unserialize(Checkpoint *cp, const std::string &section)
{
    UNSERIALIZE_SCALAR(flags);
}

RealViewCtrl *
RealViewCtrlParams::create()
{
    return new RealViewCtrl(this);
}
<|endoftext|>


<commit_before>/*=========================================================================
 *
 *  Copyright Insight Software Consortium
 *
 *  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.txt
 *
 *  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.
 *
 *=========================================================================*/
#ifndef __itkTimeVaryingVelocityFieldIntegrationImageFilter_hxx
#define __itkTimeVaryingVelocityFieldIntegrationImageFilter_hxx

#include "itkTimeVaryingVelocityFieldIntegrationImageFilter.h"

#include "itkImageRegionIteratorWithIndex.h"
#include "itkVectorLinearInterpolateImageFunction.h"

namespace itk
{

/*
 * TimeVaryingVelocityFieldIntegrationImageFilter class definitions
 */
template<class TTimeVaryingVelocityField, class TDisplacementField>
TimeVaryingVelocityFieldIntegrationImageFilter
  <TTimeVaryingVelocityField, TDisplacementField>
::TimeVaryingVelocityFieldIntegrationImageFilter()
{
  this->m_LowerTimeBound =  0.0,
  this->m_UpperTimeBound = 1.0,
  this->m_NumberOfIntegrationSteps = 100;
  this->m_NumberOfTimePoints = 0;
  this->SetNumberOfRequiredInputs( 1 );

  if( InputImageDimension - 1 != OutputImageDimension )
    {
    itkExceptionMacro( "The time-varying velocity field (input) should have "
      << "dimensionality of 1 greater than the deformation field (output). " );
    }

  typedef VectorLinearInterpolateImageFunction<TimeVaryingVelocityFieldType,
    RealType> DefaultVelocityFieldInterpolatorType;

  typename DefaultVelocityFieldInterpolatorType::Pointer
    velocityFieldInterpolator = DefaultVelocityFieldInterpolatorType::New();

  this->m_VelocityFieldInterpolator = velocityFieldInterpolator;

  typedef VectorLinearInterpolateImageFunction<DisplacementFieldType,
    RealType> DefaultDisplacementFieldInterpolatorType;

  typename DefaultDisplacementFieldInterpolatorType::Pointer
    deformationFieldInterpolator = DefaultDisplacementFieldInterpolatorType::New();

  this->m_DisplacementFieldInterpolator = deformationFieldInterpolator;
}

template<class TTimeVaryingVelocityField, class TDisplacementField>
TimeVaryingVelocityFieldIntegrationImageFilter
  <TTimeVaryingVelocityField, TDisplacementField>
::~TimeVaryingVelocityFieldIntegrationImageFilter()
{
}

template<class TTimeVaryingVelocityField, class TDisplacementField>
void
TimeVaryingVelocityFieldIntegrationImageFilter
  <TTimeVaryingVelocityField, TDisplacementField>
::GenerateOutputInformation()
{
  const TimeVaryingVelocityFieldType * input = this->GetInput();
  DisplacementFieldType * output = this->GetOutput();
  this->m_NumberOfTimePoints = input->GetLargestPossibleRegion().GetSize()[OutputImageDimension];
  if( !input || !output )
    {
    return;
    }

  //
  // The ImageBase::CopyInformation() method ca not be used here
  // because these two images have different dimensions. Therefore
  // the individual elements must be copied for the common dimensions.
  //
  typedef typename DisplacementFieldType::SizeType      SizeType;
  typedef typename DisplacementFieldType::SpacingType   SpacingType;
  typedef typename DisplacementFieldType::PointType     OriginType;
  typedef typename DisplacementFieldType::DirectionType DirectionType;
  typedef typename DisplacementFieldType::RegionType    RegionType;

  SizeType size;
  SpacingType spacing;
  OriginType origin;
  DirectionType direction;

  typedef typename TimeVaryingVelocityFieldType::SizeType       InputSizeType;
  typedef typename TimeVaryingVelocityFieldType::SpacingType    InputSpacingType;
  typedef typename TimeVaryingVelocityFieldType::PointType      InputOriginType;
  typedef typename TimeVaryingVelocityFieldType::DirectionType  InputDirectionType;
  typedef typename TimeVaryingVelocityFieldType::RegionType     InputRegionType;

  const InputSpacingType & inputSpacing = input->GetSpacing();
  const InputOriginType & inputOrigin = input->GetOrigin();
  const InputDirectionType & inputDirection = input->GetDirection();
  const InputRegionType requestedRegion = input->GetRequestedRegion();
  const InputSizeType requestedSize = requestedRegion.GetSize();

  for( unsigned int i = 0; i < OutputImageDimension; i++ )
    {
    size[i] = requestedSize[i];
    spacing[i] = inputSpacing[i];
    origin[i] = inputOrigin[i];

    for( unsigned int j = 0; j < OutputImageDimension; j++ )
      {
      direction[i][j] = inputDirection[i][j];
      }
    }

  output->SetOrigin( origin );
  output->SetSpacing( spacing );
  output->SetDirection( direction );
  output->SetRegions( size );
}

template<class TTimeVaryingVelocityField, class TDisplacementField>
void
TimeVaryingVelocityFieldIntegrationImageFilter
  <TTimeVaryingVelocityField, TDisplacementField>
::BeforeThreadedGenerateData()
{
  this->m_VelocityFieldInterpolator->SetInputImage( this->GetInput() );
  this->m_NumberOfTimePoints = this->GetInput()->GetLargestPossibleRegion().GetSize()[InputImageDimension-1];
  if( !this->m_InitialDiffeomorphism.IsNull() )
    {
    this->m_DisplacementFieldInterpolator->SetInputImage( this->m_InitialDiffeomorphism );
    }
}

template<class TTimeVaryingVelocityField, class TDisplacementField>
void
TimeVaryingVelocityFieldIntegrationImageFilter
  <TTimeVaryingVelocityField, TDisplacementField>
::ThreadedGenerateData( const OutputRegionType &region, ThreadIdType itkNotUsed( threadId ) )
{
  if( this->m_LowerTimeBound == this->m_UpperTimeBound )
    {
    return;
    }

  if( this->m_NumberOfIntegrationSteps == 0 )
    {
    return;
    }

  const TimeVaryingVelocityFieldType * inputField = this->GetInput();

  typename DisplacementFieldType::Pointer outputField = this->GetOutput();

  ImageRegionIteratorWithIndex<DisplacementFieldType> It( outputField, region );

  for( It.GoToBegin(); !It.IsAtEnd(); ++It )
    {
    PointType point;
    outputField->TransformIndexToPhysicalPoint( It.GetIndex(), point );
    VectorType displacement = this->IntegrateVelocityAtPoint( point, inputField );
    It.Set( displacement );
    }
}

template<class TTimeVaryingVelocityField, class TDisplacementField>
typename TimeVaryingVelocityFieldIntegrationImageFilter
  <TTimeVaryingVelocityField, TDisplacementField>::VectorType
TimeVaryingVelocityFieldIntegrationImageFilter
  <TTimeVaryingVelocityField, TDisplacementField>
::IntegrateVelocityAtPoint( const PointType & initialSpatialPoint,
                            const TimeVaryingVelocityFieldType *inputField )
{
  // Solve the initial value problem using fourth-order Runge-Kutta
  //    y' = f(t, y), y(t_0) = y_0

  VectorType zeroVector;
  zeroVector.Fill( 0.0 );

  // Initial conditions

  PointType startingSpatialPoint = initialSpatialPoint;
  if( !this->m_InitialDiffeomorphism.IsNull() )
    {
    if( this->m_DisplacementFieldInterpolator->IsInsideBuffer( startingSpatialPoint ) )
      {
      startingSpatialPoint += this->m_DisplacementFieldInterpolator->Evaluate( startingSpatialPoint );
      }
    }

  // Perform the integration
  // Need to know how to map the time dimension of the input image to the
  // assumed domain of [0,1].

  typename TimeVaryingVelocityFieldType::PointType spaceTimeOrigin = inputField->GetOrigin();

  typedef typename TimeVaryingVelocityFieldType::RegionType  RegionType;

  RegionType region = inputField->GetLargestPossibleRegion();

  typename RegionType::IndexType lastIndex = region.GetIndex();
  typename RegionType::SizeType size = region.GetSize();
  for( unsigned d = 0; d < InputImageDimension; d++ )
    {
    lastIndex[d] += ( size[d] - 1 );
    }

  typename TimeVaryingVelocityFieldType::PointType spaceTimeEnd;
  typename TimeVaryingVelocityFieldType::PointType pointIn2;
  typename TimeVaryingVelocityFieldType::PointType pointIn3;
  inputField->TransformIndexToPhysicalPoint( lastIndex, spaceTimeEnd );

  // Calculate the delta time used for integration
  const RealType deltaTime = vnl_math_abs( this->m_UpperTimeBound - this->m_LowerTimeBound ) /
    static_cast<RealType>( this->m_NumberOfIntegrationSteps );

  if( deltaTime == 0.0 )
    {
    return zeroVector;
    }

  const RealType timeOrigin = spaceTimeOrigin[InputImageDimension-1];
  const RealType timeEnd = spaceTimeEnd[InputImageDimension-1];
  const RealType timeSpan = timeEnd - timeOrigin;
  RealType timeSign = 1.0;
  if( this->m_UpperTimeBound < this->m_LowerTimeBound )
    {
    timeSign = -1.0;
    }

  VectorType displacement = zeroVector;

  RealType timePoint = timeOrigin + this->m_LowerTimeBound * timeSpan;

  RealType intervalTimePoint = ( timePoint + 1.0 ) / static_cast<RealType>( this->m_NumberOfTimePoints );

  PointType spatialPoint = startingSpatialPoint + displacement;
  for( unsigned int n = 0; n < this->m_NumberOfIntegrationSteps; n++ )
    {
    typename TimeVaryingVelocityFieldType::PointType x1;
    typename TimeVaryingVelocityFieldType::PointType x2;
    typename TimeVaryingVelocityFieldType::PointType x3;
    typename TimeVaryingVelocityFieldType::PointType x4;

    RealType intervalTimePointMinusDeltaTime = intervalTimePoint - timeSign * deltaTime;
    RealType intervalTimePointMinusHalfDeltaTime = intervalTimePoint - timeSign * deltaTime * 0.5;
    if( intervalTimePointMinusHalfDeltaTime < 0.0 )
      {
      intervalTimePointMinusHalfDeltaTime = 0.0;
      }
    if( intervalTimePointMinusHalfDeltaTime > 1.0 )
      {
      intervalTimePointMinusHalfDeltaTime = 1.0;
      }
    if( intervalTimePointMinusDeltaTime < 0.0 )
      {
      intervalTimePointMinusDeltaTime = 0.0;
      }
    if( intervalTimePointMinusDeltaTime > 1.0 )
      {
      intervalTimePointMinusDeltaTime = 1.0;
      }

    for( unsigned int d = 0; d < OutputImageDimension; d++ )
      {
      x1[d] = spatialPoint[d] + displacement[d];
      x2[d] = spatialPoint[d] + displacement[d];
      x3[d] = spatialPoint[d] + displacement[d];
      x4[d] = spatialPoint[d] + displacement[d];
      pointIn2[d] = spatialPoint[d] + displacement[d];
      }

    x1[OutputImageDimension] = intervalTimePointMinusDeltaTime * static_cast<RealType>( this->m_NumberOfTimePoints - 1 );
    x2[OutputImageDimension] = intervalTimePointMinusHalfDeltaTime * static_cast<RealType>( this->m_NumberOfTimePoints - 1 );
    x3[OutputImageDimension] = intervalTimePointMinusHalfDeltaTime * static_cast<RealType>( this->m_NumberOfTimePoints - 1 );
    x4[OutputImageDimension] = intervalTimePoint * static_cast<RealType>( this->m_NumberOfTimePoints - 1 );

    VectorType f1 = zeroVector;
    if ( this->m_VelocityFieldInterpolator->IsInsideBuffer( x1 ) )
      {
      f1 = this->m_VelocityFieldInterpolator->Evaluate( x1 );
      for( unsigned int jj = 0; jj < OutputImageDimension; jj++ )
        {
        x2[jj] += f1[jj] * deltaTime * 0.5;
        }
      }

    VectorType f2 = zeroVector;
    if ( this->m_VelocityFieldInterpolator->IsInsideBuffer( x2 ) )
      {
      f2 = this->m_VelocityFieldInterpolator->Evaluate( x2 );
      for( unsigned int jj = 0; jj < OutputImageDimension; jj++ )
        {
        x3[jj] += f2[jj] * deltaTime * 0.5;
        }
      }

    VectorType f3 = zeroVector;
    if ( this->m_VelocityFieldInterpolator->IsInsideBuffer( x3 ) )
      {
      f3 = this->m_VelocityFieldInterpolator->Evaluate( x3 );
      for( unsigned int jj = 0; jj < OutputImageDimension; jj++ )
        {
        x4[jj] += f3[jj] * deltaTime;
        }
      }

    VectorType f4 = zeroVector;
    if( this->m_VelocityFieldInterpolator->IsInsideBuffer( x4 ) )
      {
      f4 = this->m_VelocityFieldInterpolator->Evaluate( x4 );
      }

    for( unsigned int jj = 0; jj < OutputImageDimension; jj++ )
      {
      pointIn3[jj] = pointIn2[jj] + timeSign * deltaTime/6.0 * ( f1[jj] + 2.0*f2[jj] + 2.0*f3[jj] + f4[jj] );
      displacement[jj] = pointIn3[jj] - startingSpatialPoint[jj];
      }
    pointIn3[OutputImageDimension] = intervalTimePoint * static_cast<RealType>( this->m_NumberOfTimePoints - 1 );

    intervalTimePoint += deltaTime * timeSign;
    }
  return displacement;
}

template<class TTimeVaryingVelocityField, class TDisplacementField>
void
TimeVaryingVelocityFieldIntegrationImageFilter
  <TTimeVaryingVelocityField, TDisplacementField>
::PrintSelf( std::ostream& os, Indent indent ) const
{
  Superclass::PrintSelf( os, indent );

  os << indent << "VelocityFieldInterpolator: " << this->m_VelocityFieldInterpolator << std::endl;
  os << indent << "LowerTimeBound: " << this->m_LowerTimeBound << std::endl;
  os << indent << "UpperTimeBound: " << this->m_UpperTimeBound << std::endl;
  os << indent << "NumberOfIntegrationSteps: " << this->m_NumberOfIntegrationSteps << std::endl;

  if( !this->m_InitialDiffeomorphism.IsNull() )
    {
    os << indent << "InitialDiffeomorphism: " << this->m_InitialDiffeomorphism << std::endl;
    os << indent << "DisplacementFieldInterpolator: " << this->m_DisplacementFieldInterpolator << std::endl;
    }
}

}  //end namespace itk

#endif
<commit_msg>COMP: The + operation was not registering on Win7-vc10-vce32rel<commit_after>/*=========================================================================
 *
 *  Copyright Insight Software Consortium
 *
 *  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.txt
 *
 *  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.
 *
 *=========================================================================*/
#ifndef __itkTimeVaryingVelocityFieldIntegrationImageFilter_hxx
#define __itkTimeVaryingVelocityFieldIntegrationImageFilter_hxx

#include "itkTimeVaryingVelocityFieldIntegrationImageFilter.h"

#include "itkImageRegionIteratorWithIndex.h"
#include "itkVectorLinearInterpolateImageFunction.h"

namespace itk
{

/*
 * TimeVaryingVelocityFieldIntegrationImageFilter class definitions
 */
template<class TTimeVaryingVelocityField, class TDisplacementField>
TimeVaryingVelocityFieldIntegrationImageFilter
  <TTimeVaryingVelocityField, TDisplacementField>
::TimeVaryingVelocityFieldIntegrationImageFilter()
{
  this->m_LowerTimeBound =  0.0,
  this->m_UpperTimeBound = 1.0,
  this->m_NumberOfIntegrationSteps = 100;
  this->m_NumberOfTimePoints = 0;
  this->SetNumberOfRequiredInputs( 1 );

  if( InputImageDimension - 1 != OutputImageDimension )
    {
    itkExceptionMacro( "The time-varying velocity field (input) should have "
      << "dimensionality of 1 greater than the deformation field (output). " );
    }

  typedef VectorLinearInterpolateImageFunction<TimeVaryingVelocityFieldType,
    RealType> DefaultVelocityFieldInterpolatorType;

  typename DefaultVelocityFieldInterpolatorType::Pointer
    velocityFieldInterpolator = DefaultVelocityFieldInterpolatorType::New();

  this->m_VelocityFieldInterpolator = velocityFieldInterpolator;

  typedef VectorLinearInterpolateImageFunction<DisplacementFieldType,
    RealType> DefaultDisplacementFieldInterpolatorType;

  typename DefaultDisplacementFieldInterpolatorType::Pointer
    deformationFieldInterpolator = DefaultDisplacementFieldInterpolatorType::New();

  this->m_DisplacementFieldInterpolator = deformationFieldInterpolator;
}

template<class TTimeVaryingVelocityField, class TDisplacementField>
TimeVaryingVelocityFieldIntegrationImageFilter
  <TTimeVaryingVelocityField, TDisplacementField>
::~TimeVaryingVelocityFieldIntegrationImageFilter()
{
}

template<class TTimeVaryingVelocityField, class TDisplacementField>
void
TimeVaryingVelocityFieldIntegrationImageFilter
  <TTimeVaryingVelocityField, TDisplacementField>
::GenerateOutputInformation()
{
  const TimeVaryingVelocityFieldType * input = this->GetInput();
  DisplacementFieldType * output = this->GetOutput();
  this->m_NumberOfTimePoints = input->GetLargestPossibleRegion().GetSize()[OutputImageDimension];
  if( !input || !output )
    {
    return;
    }

  //
  // The ImageBase::CopyInformation() method ca not be used here
  // because these two images have different dimensions. Therefore
  // the individual elements must be copied for the common dimensions.
  //
  typedef typename DisplacementFieldType::SizeType      SizeType;
  typedef typename DisplacementFieldType::SpacingType   SpacingType;
  typedef typename DisplacementFieldType::PointType     OriginType;
  typedef typename DisplacementFieldType::DirectionType DirectionType;
  typedef typename DisplacementFieldType::RegionType    RegionType;

  SizeType size;
  SpacingType spacing;
  OriginType origin;
  DirectionType direction;

  typedef typename TimeVaryingVelocityFieldType::SizeType       InputSizeType;
  typedef typename TimeVaryingVelocityFieldType::SpacingType    InputSpacingType;
  typedef typename TimeVaryingVelocityFieldType::PointType      InputOriginType;
  typedef typename TimeVaryingVelocityFieldType::DirectionType  InputDirectionType;
  typedef typename TimeVaryingVelocityFieldType::RegionType     InputRegionType;

  const InputSpacingType & inputSpacing = input->GetSpacing();
  const InputOriginType & inputOrigin = input->GetOrigin();
  const InputDirectionType & inputDirection = input->GetDirection();
  const InputRegionType requestedRegion = input->GetRequestedRegion();
  const InputSizeType requestedSize = requestedRegion.GetSize();

  for( unsigned int i = 0; i < OutputImageDimension; i++ )
    {
    size[i] = requestedSize[i];
    spacing[i] = inputSpacing[i];
    origin[i] = inputOrigin[i];

    for( unsigned int j = 0; j < OutputImageDimension; j++ )
      {
      direction[i][j] = inputDirection[i][j];
      }
    }

  output->SetOrigin( origin );
  output->SetSpacing( spacing );
  output->SetDirection( direction );
  output->SetRegions( size );
}

template<class TTimeVaryingVelocityField, class TDisplacementField>
void
TimeVaryingVelocityFieldIntegrationImageFilter
  <TTimeVaryingVelocityField, TDisplacementField>
::BeforeThreadedGenerateData()
{
  this->m_VelocityFieldInterpolator->SetInputImage( this->GetInput() );
  this->m_NumberOfTimePoints = this->GetInput()->GetLargestPossibleRegion().GetSize()[InputImageDimension-1];
  if( !this->m_InitialDiffeomorphism.IsNull() )
    {
    this->m_DisplacementFieldInterpolator->SetInputImage( this->m_InitialDiffeomorphism );
    }
}

template<class TTimeVaryingVelocityField, class TDisplacementField>
void
TimeVaryingVelocityFieldIntegrationImageFilter
  <TTimeVaryingVelocityField, TDisplacementField>
::ThreadedGenerateData( const OutputRegionType &region, ThreadIdType itkNotUsed( threadId ) )
{
  if( this->m_LowerTimeBound == this->m_UpperTimeBound )
    {
    return;
    }

  if( this->m_NumberOfIntegrationSteps == 0 )
    {
    return;
    }

  const TimeVaryingVelocityFieldType * inputField = this->GetInput();

  typename DisplacementFieldType::Pointer outputField = this->GetOutput();

  ImageRegionIteratorWithIndex<DisplacementFieldType> It( outputField, region );

  for( It.GoToBegin(); !It.IsAtEnd(); ++It )
    {
    PointType point;
    outputField->TransformIndexToPhysicalPoint( It.GetIndex(), point );
    VectorType displacement = this->IntegrateVelocityAtPoint( point, inputField );
    It.Set( displacement );
    }
}

template<class TTimeVaryingVelocityField, class TDisplacementField>
typename TimeVaryingVelocityFieldIntegrationImageFilter
  <TTimeVaryingVelocityField, TDisplacementField>::VectorType
TimeVaryingVelocityFieldIntegrationImageFilter
  <TTimeVaryingVelocityField, TDisplacementField>
::IntegrateVelocityAtPoint( const PointType & initialSpatialPoint,
                            const TimeVaryingVelocityFieldType *inputField )
{
  // Solve the initial value problem using fourth-order Runge-Kutta
  //    y' = f(t, y), y(t_0) = y_0

  VectorType zeroVector;
  zeroVector.Fill( 0.0 );

  // Initial conditions

  PointType startingSpatialPoint = initialSpatialPoint;
  if( !this->m_InitialDiffeomorphism.IsNull() )
    {
    if( this->m_DisplacementFieldInterpolator->IsInsideBuffer( startingSpatialPoint ) )
      {
      startingSpatialPoint += this->m_DisplacementFieldInterpolator->Evaluate( startingSpatialPoint );
      }
    }

  // Perform the integration
  // Need to know how to map the time dimension of the input image to the
  // assumed domain of [0,1].

  typename TimeVaryingVelocityFieldType::PointType spaceTimeOrigin = inputField->GetOrigin();

  typedef typename TimeVaryingVelocityFieldType::RegionType  RegionType;

  RegionType region = inputField->GetLargestPossibleRegion();

  typename RegionType::IndexType lastIndex = region.GetIndex();
  typename RegionType::SizeType size = region.GetSize();
  for( unsigned d = 0; d < InputImageDimension; d++ )
    {
    lastIndex[d] += ( size[d] - 1 );
    }

  typename TimeVaryingVelocityFieldType::PointType spaceTimeEnd;
  typename TimeVaryingVelocityFieldType::PointType pointIn2;
  typename TimeVaryingVelocityFieldType::PointType pointIn3;
  inputField->TransformIndexToPhysicalPoint( lastIndex, spaceTimeEnd );

  // Calculate the delta time used for integration
  const RealType deltaTime = vnl_math_abs( this->m_UpperTimeBound - this->m_LowerTimeBound ) /
    static_cast<RealType>( this->m_NumberOfIntegrationSteps );

  if( deltaTime == 0.0 )
    {
    return zeroVector;
    }

  const RealType timeOrigin = spaceTimeOrigin[InputImageDimension-1];
  const RealType timeEnd = spaceTimeEnd[InputImageDimension-1];
  const RealType timeSpan = timeEnd - timeOrigin;
  RealType timeSign = 1.0;
  if( this->m_UpperTimeBound < this->m_LowerTimeBound )
    {
    timeSign = -1.0;
    }

  VectorType displacement = zeroVector;

  RealType timePoint = timeOrigin + this->m_LowerTimeBound * timeSpan;

  RealType intervalTimePoint = ( timePoint + 1.0 ) / static_cast<RealType>( this->m_NumberOfTimePoints );

  /** Windows not registering + operation so use a loop explicitly */
  PointType spatialPoint = startingSpatialPoint;
  for( unsigned int d = 0; d < InputImageDimension; d++ )
    {
    spatialPoint[d] += displacement[d];
    }

  for( unsigned int n = 0; n < this->m_NumberOfIntegrationSteps; n++ )
    {
    typename TimeVaryingVelocityFieldType::PointType x1;
    typename TimeVaryingVelocityFieldType::PointType x2;
    typename TimeVaryingVelocityFieldType::PointType x3;
    typename TimeVaryingVelocityFieldType::PointType x4;

    RealType intervalTimePointMinusDeltaTime = intervalTimePoint - timeSign * deltaTime;
    RealType intervalTimePointMinusHalfDeltaTime = intervalTimePoint - timeSign * deltaTime * 0.5;
    if( intervalTimePointMinusHalfDeltaTime < 0.0 )
      {
      intervalTimePointMinusHalfDeltaTime = 0.0;
      }
    if( intervalTimePointMinusHalfDeltaTime > 1.0 )
      {
      intervalTimePointMinusHalfDeltaTime = 1.0;
      }
    if( intervalTimePointMinusDeltaTime < 0.0 )
      {
      intervalTimePointMinusDeltaTime = 0.0;
      }
    if( intervalTimePointMinusDeltaTime > 1.0 )
      {
      intervalTimePointMinusDeltaTime = 1.0;
      }

    for( unsigned int d = 0; d < OutputImageDimension; d++ )
      {
      x1[d] = spatialPoint[d] + displacement[d];
      x2[d] = spatialPoint[d] + displacement[d];
      x3[d] = spatialPoint[d] + displacement[d];
      x4[d] = spatialPoint[d] + displacement[d];
      pointIn2[d] = spatialPoint[d] + displacement[d];
      }

    x1[OutputImageDimension] = intervalTimePointMinusDeltaTime * static_cast<RealType>( this->m_NumberOfTimePoints - 1 );
    x2[OutputImageDimension] = intervalTimePointMinusHalfDeltaTime * static_cast<RealType>( this->m_NumberOfTimePoints - 1 );
    x3[OutputImageDimension] = intervalTimePointMinusHalfDeltaTime * static_cast<RealType>( this->m_NumberOfTimePoints - 1 );
    x4[OutputImageDimension] = intervalTimePoint * static_cast<RealType>( this->m_NumberOfTimePoints - 1 );

    VectorType f1 = zeroVector;
    if ( this->m_VelocityFieldInterpolator->IsInsideBuffer( x1 ) )
      {
      f1 = this->m_VelocityFieldInterpolator->Evaluate( x1 );
      for( unsigned int jj = 0; jj < OutputImageDimension; jj++ )
        {
        x2[jj] += f1[jj] * deltaTime * 0.5;
        }
      }

    VectorType f2 = zeroVector;
    if ( this->m_VelocityFieldInterpolator->IsInsideBuffer( x2 ) )
      {
      f2 = this->m_VelocityFieldInterpolator->Evaluate( x2 );
      for( unsigned int jj = 0; jj < OutputImageDimension; jj++ )
        {
        x3[jj] += f2[jj] * deltaTime * 0.5;
        }
      }

    VectorType f3 = zeroVector;
    if ( this->m_VelocityFieldInterpolator->IsInsideBuffer( x3 ) )
      {
      f3 = this->m_VelocityFieldInterpolator->Evaluate( x3 );
      for( unsigned int jj = 0; jj < OutputImageDimension; jj++ )
        {
        x4[jj] += f3[jj] * deltaTime;
        }
      }

    VectorType f4 = zeroVector;
    if( this->m_VelocityFieldInterpolator->IsInsideBuffer( x4 ) )
      {
      f4 = this->m_VelocityFieldInterpolator->Evaluate( x4 );
      }

    for( unsigned int jj = 0; jj < OutputImageDimension; jj++ )
      {
      pointIn3[jj] = pointIn2[jj] + timeSign * deltaTime/6.0 * ( f1[jj] + 2.0*f2[jj] + 2.0*f3[jj] + f4[jj] );
      displacement[jj] = pointIn3[jj] - startingSpatialPoint[jj];
      }
    pointIn3[OutputImageDimension] = intervalTimePoint * static_cast<RealType>( this->m_NumberOfTimePoints - 1 );

    intervalTimePoint += deltaTime * timeSign;
    }
  return displacement;
}

template<class TTimeVaryingVelocityField, class TDisplacementField>
void
TimeVaryingVelocityFieldIntegrationImageFilter
  <TTimeVaryingVelocityField, TDisplacementField>
::PrintSelf( std::ostream& os, Indent indent ) const
{
  Superclass::PrintSelf( os, indent );

  os << indent << "VelocityFieldInterpolator: " << this->m_VelocityFieldInterpolator << std::endl;
  os << indent << "LowerTimeBound: " << this->m_LowerTimeBound << std::endl;
  os << indent << "UpperTimeBound: " << this->m_UpperTimeBound << std::endl;
  os << indent << "NumberOfIntegrationSteps: " << this->m_NumberOfIntegrationSteps << std::endl;

  if( !this->m_InitialDiffeomorphism.IsNull() )
    {
    os << indent << "InitialDiffeomorphism: " << this->m_InitialDiffeomorphism << std::endl;
    os << indent << "DisplacementFieldInterpolator: " << this->m_DisplacementFieldInterpolator << std::endl;
    }
}

}  //end namespace itk

#endif
<|endoftext|>


<commit_before>/*
 * Simple program to extract "interesting" query strings
 * values from HTTP log files or other types of logs
 * Not really for prime-time.
 *
 */
#include <iostream>
#include <fstream>
#include <string>

#include "modp_burl.h"
#include "modp_ascii.h"
#include "modp_qsiter.h"

#include "sqli_fingerprints.h"
#include "sqlparse.h"

using namespace std;

static bool is_special(std::string& str) {
    string nval(str);
    //size_t len = sqli_qs_normalize(const_cast<char*>(nval.data()), nval.size());
    //nval.erase(len, std::string::npos);
    //  / *
    if (string::npos == nval.find_first_not_of("ABCDEFGHIJKLMNOPQRSTUVWXYZ _-+")) {
        // all alpha, a few symbols can't be string
        return false;
    } else if (string::npos == nval.find_first_not_of("0123456789. ()[]{}_-+:;,")) {
        // all numbers and random symbols
        return false;
    } else if (string::npos == nval.find_first_not_of("ABCDEFGHIJKLMNOPQRSTUVWXYZ0123456789._-+:;,")) {
        // big blobs of text without whitespace
        return false;
    } else {
        return true;
    }
}

class LineIterator {
private:
    const string fname;
    istream* myfile;
    int linenum;
    string line;

public:
    LineIterator(const char* filename, istream* is)
        : fname(filename), myfile(is), linenum(0)
        {

        }

    LineIterator(const string& filename, istream* is)
        : fname(filename), myfile(is), linenum(0)
        {

        }

    const string& getFilename() const {
        return this->fname;
    }

    int getLinenum() const {
        return this->linenum;
    }

    const string& getLine() const {
        return this->line;
    }

    bool next() {
        while (myfile->good()) {
            linenum +=1;
            getline(*myfile, line);
            if (line.length() > 0 && line.at(0) != '#') {
                return true;
            }
        }
        return false;
    }
};

class tester {

public:
    tester()
        {
        }

    void test_positive(const char* fname, istream* is)  {
        sfilter sf;

        bool invert = false;

        LineIterator li(fname, is);

        qsiter_t qsi;

        while (li.next()) {
            string orig(li.getLine());
            if (false) {
                // LOG FILE MODE
                size_t pos = orig.find("GET ");
                if (pos == string::npos) {
                    pos = orig.find("POST ");
                    if (pos == string::npos) {
                        continue;
                    } else {
                        pos += 5;
                    }
                } else {
                    pos += 4;
                }
                size_t endpos = orig.find(' ', pos);
                if (endpos == string::npos) {
                    continue;
                }

                pos = orig.find('?', pos);
                if (pos == string::npos || pos > endpos) {
                    continue;
                }
                pos += 1;

                qsiter_reset(&qsi, orig.data() + pos, endpos-pos);
                bool issqli = false;
                while (!issqli && qsiter_next(&qsi)) {
                    string val(qsi.val, qsi.vallen);
                    string tmp(val);
                    tmp.erase(sqli_qs_normalize(const_cast<char*>(tmp.data()), tmp.size()), std::string::npos);
                    issqli = is_sqli(&sf, tmp.data(), tmp.size(), NULL);
                }
                if (issqli) {
                    cout << orig << "\n";
                }
            } else if (true) {
                bool issqli = false;
                string tmp(orig);
                // RAW MODE -- line is full input to be evalutated.
                if (is_special(orig)) {
                    if (invert) {
                        //
                    } else {
                        tmp.erase(sqli_qs_normalize(const_cast<char*>(tmp.data()), tmp.size()), std::string::npos);
                        issqli = is_sqli(&sf, tmp.data(), tmp.size(), NULL);
                        //cout << tmp << "    |    " << orig << endl;
                        //cout << orig << endl;
                    }
                } else {
                    if (invert) {
                        cout << orig << endl;
                    } else {
                        //
                    }
                }
                if (issqli) {
                    cout << tmp << "\n";
                }
            }
        } /* while li.next() */
    } /* tester */
}; /* class */

int main( int argc, const char* argv[] ) {
    tester atest;

    if (argc == 1) {
        atest.test_positive("stdin", &(cin));
    } else {
        for (int i = 2; i < argc; ++i) {
            ifstream is(argv[i]);
            atest.test_positive(argv[1], &is);
        }
    }

    return 0;
}

<commit_msg>remove dead code<commit_after><|endoftext|>