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

  Program:   Insight Segmentation & Registration Toolkit (ITK)
  Module:    itkFastMarchingTest.cxx
  Language:  C++
  Date:      $Date$
  Version:   $Revision$

Copyright (c) 2000 National Library of Medicine
All rights reserved.

See COPYRIGHT.txt for copyright details.

=========================================================================*/
#include "itkFastMarchingImageFilter.h"
#include "itkImage.h"
#include "itkScalar.h"
#include "itkImageRegionIterator.h"
#include "itkPixelTraits.h"

#include "vnl/vnl_math.h"

int main()
{
  // create a fastmarching object
  typedef itk::Scalar<float> PixelType;
  typedef itk::Image<PixelType,2> FloatImage;
  typedef itk::FastMarchingImageFilter<FloatImage> FloatFMType;

  FloatFMType::Pointer marcher = FloatFMType::New();

  // setup trial points
  typedef FloatFMType::NodeType NodeType;
  typedef FloatFMType::NodeContainer NodeContainer;

  NodeContainer::Pointer trialPoints = NodeContainer::New();

  NodeType node;

  FloatImage::OffsetType index0 = {{28,35}};

  node.value = 0.0;
  node.index += index0;
  trialPoints->InsertElement(0, node);

  marcher->SetTrialPoints( trialPoints );

  // specify the size of the output image
  FloatImage::SizeType size = {{64,64}};
  marcher->SetOutputSize( size );

  // update the marcher
  marcher->Update();

  // check the results
  FloatImage::Pointer output = marcher->GetOutput();
  itk::ImageRegionIterator<FloatImage>
    iterator( output, output->GetBufferedRegion() );

  iterator = iterator.Begin();
  bool passed = true;

  for( ; !iterator.IsAtEnd(); ++iterator )
    {

    FloatImage::IndexType tempIndex;
    double distance;
    float output;

    tempIndex = iterator.GetIndex();
    tempIndex -= index0;
    distance = 0.0;
    for( int j = 0; j < 2; j++ )
      {
        distance += tempIndex[j] * tempIndex[j];
      }
    distance = vnl_math_sqrt( distance );

    output = itk::ScalarTraits<PixelType>::GetScalar( iterator.Get() );

    if( vnl_math_abs( output ) / distance > 1.42 )
      {
        std::cout << iterator.GetIndex() << " ";
        std::cout << vnl_math_abs( output ) / distance << " ";
        std::cout << vnl_math_abs( output ) << " " << distance << std::endl;
        passed = false;
      }

    }

  if( passed )
    {
    std::cout << "Fast Marching test passed" << std::endl;
    return EXIT_SUCCESS;
    }
  else
    {
    std::cout << "Fast Marching test failed" << std::endl;
    return EXIT_FAILURE;
    }

}
<commit_msg>ENH: Initialize index to 0.<commit_after>/*=========================================================================

  Program:   Insight Segmentation & Registration Toolkit (ITK)
  Module:    itkFastMarchingTest.cxx
  Language:  C++
  Date:      $Date$
  Version:   $Revision$

Copyright (c) 2000 National Library of Medicine
All rights reserved.

See COPYRIGHT.txt for copyright details.

=========================================================================*/
#include "itkFastMarchingImageFilter.h"
#include "itkImage.h"
#include "itkScalar.h"
#include "itkImageRegionIterator.h"
#include "itkPixelTraits.h"

#include "vnl/vnl_math.h"

int main()
{
  // create a fastmarching object
  typedef itk::Scalar<float> PixelType;
  typedef itk::Image<PixelType,2> FloatImage;
  typedef itk::FastMarchingImageFilter<FloatImage> FloatFMType;

  FloatFMType::Pointer marcher = FloatFMType::New();

  // setup trial points
  typedef FloatFMType::NodeType NodeType;
  typedef FloatFMType::NodeContainer NodeContainer;

  NodeContainer::Pointer trialPoints = NodeContainer::New();

  NodeType node;

  FloatImage::OffsetType offset0 = {{28,35}};

  node.value = 0.0;
  node.index += offset0;
  node.index = itk::Index<2>::ZeroIndex + offset0;
  trialPoints->InsertElement(0, node);

  marcher->SetTrialPoints( trialPoints );

  // specify the size of the output image
  FloatImage::SizeType size = {{64,64}};
  marcher->SetOutputSize( size );

  // update the marcher
  marcher->Update();

  // check the results
  FloatImage::Pointer output = marcher->GetOutput();
  itk::ImageRegionIterator<FloatImage>
    iterator( output, output->GetBufferedRegion() );

  iterator = iterator.Begin();
  bool passed = true;

  for( ; !iterator.IsAtEnd(); ++iterator )
    {

    FloatImage::IndexType tempIndex;
    double distance;
    float output;

    tempIndex = iterator.GetIndex();
    tempIndex -= offset0;
    distance = 0.0;
    for( int j = 0; j < 2; j++ )
      {
        distance += tempIndex[j] * tempIndex[j];
      }
    distance = vnl_math_sqrt( distance );

    output = itk::ScalarTraits<PixelType>::GetScalar( iterator.Get() );

    if( vnl_math_abs( output ) / distance > 1.42 )
      {
        std::cout << iterator.GetIndex() << " ";
        std::cout << vnl_math_abs( output ) / distance << " ";
        std::cout << vnl_math_abs( output ) << " " << distance << std::endl;
        passed = false;
      }

    }

  if( passed )
    {
    std::cout << "Fast Marching test passed" << std::endl;
    return EXIT_SUCCESS;
    }
  else
    {
    std::cout << "Fast Marching test failed" << std::endl;
    return EXIT_FAILURE;
    }

}
<|endoftext|>


<commit_before>/* Copyright (C) 2004 MySQL AB

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; version 2 of the License.

   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program; if not, write to the Free Software
   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA */

/*
  Utility program that encapsulates process creation, monitoring
  and bulletproof process cleanup

  Usage:
    safe_process [options to safe_process] -- progname arg1 ... argn

  To safeguard mysqld you would invoke safe_process with a few options
  for safe_process itself followed by a double dash to indicate start
  of the command line for the program you really want to start

  $> safe_process --output=output.log -- mysqld --datadir=var/data1 ...

  This would redirect output to output.log and then start mysqld,
  once it has done that it will continue to monitor the child as well
  as the parent.

  The safe_process then checks the follwing things:
  1. Child exits, propagate the childs return code to the parent
     by exiting with the same return code as the child.

  2. Parent dies, immediately kill the child and exit, thus the
     parent does not need to properly cleanup any child, it is handled
     automatically.

  3. Signal's recieced by the process will trigger same action as 2)

  4. The named event "safe_process[pid]" can be signaled and will
     trigger same action as 2)

  WARNING! Be careful when using ProcessExplorer, since it will open
           a handle to each process(and maybe also the Job), the process
           spawned by safe_process will not be closed off when safe_process
           is killed.
*/

/* Requires Windows 2000 or higher */
#define _WIN32_WINNT 0x0500

#include <windows.h>
#include <stdio.h>
#include <tlhelp32.h>
#include <signal.h>

static int verbose= 0;
static char safe_process_name[32]= {0};

static void message(const char* fmt, ...)
{
  if (!verbose)
    return;
  va_list args;
  fprintf(stderr, "%s: ", safe_process_name);
  va_start(args, fmt);
  vfprintf(stderr, fmt, args);
  fprintf(stderr, "\n");
  va_end(args);
  fflush(stderr);
}

static void die(const char* fmt, ...)
{
  DWORD last_err= GetLastError();
  va_list args;
  fprintf(stderr, "%s: FATAL ERROR, ", safe_process_name);
  va_start(args, fmt);
  vfprintf(stderr, fmt, args);
  fprintf(stderr, "\n");
  va_end(args);
  if (last_err)
  {
    char *message_text;
    if (FormatMessage(FORMAT_MESSAGE_FROM_SYSTEM | FORMAT_MESSAGE_ALLOCATE_BUFFER
        |FORMAT_MESSAGE_IGNORE_INSERTS, NULL, last_err , 0, (LPSTR)&message_text,
        0, NULL))
    {
      fprintf(stderr,"error: %d, %s\n",last_err, message_text);
      LocalFree(message_text);
    }
    else
    {
      /* FormatMessage failed, print error code only */
      fprintf(stderr,"error:%d\n", last_err);
    }
  }
  fflush(stderr);
  exit(1);
}

DWORD get_parent_pid(DWORD pid)
{
  HANDLE snapshot;
  DWORD parent_pid= -1;
  PROCESSENTRY32 pe32;
  pe32.dwSize= sizeof(PROCESSENTRY32);

  snapshot= CreateToolhelp32Snapshot(TH32CS_SNAPPROCESS, 0);
  if (snapshot == INVALID_HANDLE_VALUE)
    die("CreateToolhelp32Snapshot failed");

  if (!Process32First(snapshot, &pe32))
  {
    CloseHandle(snapshot);
    die("Process32First failed");
  }

  do
  {
    if (pe32.th32ProcessID == pid)
      parent_pid= pe32.th32ParentProcessID;
  } while(Process32Next( snapshot, &pe32));
  CloseHandle(snapshot);

  if (parent_pid == -1)
    die("Could not find parent pid");

  return parent_pid;
}

enum {
  PARENT,
  CHILD,
  EVENT,
  NUM_HANDLES
};

HANDLE shutdown_event;
void handle_signal (int signal)
{
  message("Got signal: %d", signal);
  if(SetEvent(shutdown_event) == 0) {
    /* exit safe_process and (hopefully) kill off the child */
    die("Failed to SetEvent");
  }
}

int main(int argc, const char** argv )
{
  char child_args[4096]= {0};
  DWORD pid= GetCurrentProcessId();
  DWORD parent_pid= get_parent_pid(pid);
  HANDLE job_handle;
  HANDLE wait_handles[NUM_HANDLES]= {0};
  PROCESS_INFORMATION process_info= {0};

  sprintf(safe_process_name, "safe_process[%d]", pid);

  /* Create an event for the signal handler */
  if ((shutdown_event=
       CreateEvent(NULL, TRUE, FALSE, safe_process_name)) == NULL)
    die("Failed to create shutdown_event");
  wait_handles[EVENT]= shutdown_event;

  signal(SIGINT,   handle_signal);
  signal(SIGBREAK, handle_signal);
  signal(SIGTERM,  handle_signal);

  message("Started");

  /* Parse arguments */
  for (int i= 1; i < argc; i++) {
    const char* arg= argv[i];
	char* to= child_args;
    if (strcmp(arg, "--") == 0 && strlen(arg) == 2) {
      /* Got the "--" delimiter */
      if (i >= argc)
        die("No real args -> nothing to do");
      /* Copy the remaining args to child_arg */
      for (int j= i+1; j < argc; j++) {
        to+= _snprintf(to, child_args + sizeof(child_args) - to, "%s ", argv[j]);
      }
      break;
    } else {
      if ( strcmp(arg, "--verbose") == 0 )
        verbose++;
	  else if ( strncmp(arg, "--parent-pid", 10) == 0 )
	  {
	    /* Override parent_pid with a value provided by user */
		const char* start;
        if ((start= strstr(arg, "=")) == NULL)
		  die("Could not find start of option value in '%s'", arg);
		start++; /* Step past = */
		if ((parent_pid= atoi(start)) == 0)
		  die("Invalid value '%s' passed to --parent-id", start);
	  }
      else
        die("Unknown option: %s", arg);
    }
  }
  if (*child_args == '\0')
    die("nothing to do");

  /* Open a handle to the parent process */
  message("parent_pid: %d", parent_pid);
  if (parent_pid == pid)
    die("parent_pid is equal to own pid!");

  if ((wait_handles[PARENT]=
       OpenProcess(SYNCHRONIZE, FALSE, parent_pid)) == NULL)
    die("Failed to open parent process with pid: %d", parent_pid);

  /* Create the child process in a job */
  JOBOBJECT_EXTENDED_LIMIT_INFORMATION jeli = { 0 };
  STARTUPINFO                          si   = { 0 };
  si.cb = sizeof(si);

  /*
    Create the job object to make it possible to kill the process
    and all of it's children in one go
  */
  if ((job_handle= CreateJobObject(NULL, NULL)) == NULL)
    die("CreateJobObject failed");

  /*
    Make all processes associated with the job terminate when the
    last handle to the job is closed.
  */
  jeli.BasicLimitInformation.LimitFlags = JOB_OBJECT_LIMIT_KILL_ON_JOB_CLOSE;
  if (SetInformationJobObject(job_handle, JobObjectExtendedLimitInformation,
                              &jeli, sizeof(jeli)) == 0)
    message("SetInformationJobObject failed, continue anyway...");

#if 0
  /* Setup stdin, stdout and stderr redirect */
  si.dwFlags= STARTF_USESTDHANDLES;
  si.hStdInput= GetStdHandle(STD_INPUT_HANDLE);
  si.hStdOutput= GetStdHandle(STD_OUTPUT_HANDLE);
  si.hStdError= GetStdHandle(STD_ERROR_HANDLE);
#endif

  /*
    Create the process suspended to make sure it's assigned to the
    Job before it creates any process of it's own

    Allow the new process to break away from any job that this
    process is part of so that it can be assigned to the new JobObject
    we just created. This is safe since the new JobObject is created with
    the JOB_OBJECT_LIMIT_KILL_ON_JOB_CLOSE flag, making sure it will be
    terminated when the last handle to it is closed(which is owned by
    this process).
  */
  if (CreateProcess(NULL, (LPSTR)child_args,
                    NULL,
                    NULL,
                    TRUE, /* inherit handles */
                    CREATE_SUSPENDED | CREATE_BREAKAWAY_FROM_JOB,
                    NULL,
                    NULL,
                    &si,
                    &process_info) == 0)
    die("CreateProcess failed");

  if (AssignProcessToJobObject(job_handle, process_info.hProcess) == 0)
  {
    TerminateProcess(process_info.hProcess, 200);
    die("AssignProcessToJobObject failed");
  }
  ResumeThread(process_info.hThread);
  CloseHandle(process_info.hThread);

  wait_handles[CHILD]= process_info.hProcess;

  message("Started child %d", process_info.dwProcessId);

  /* Monitor loop */
  DWORD child_exit_code= 1;
  DWORD wait_res= WaitForMultipleObjects(NUM_HANDLES, wait_handles,
                                         FALSE, INFINITE);
  switch (wait_res)
  {
    case WAIT_OBJECT_0 + PARENT:
      message("Parent exit");
      break;
    case WAIT_OBJECT_0 + CHILD:
      if (GetExitCodeProcess(wait_handles[CHILD], &child_exit_code) == 0)
        message("Child exit: could not get exit_code");
      else
        message("Child exit: exit_code: %d", child_exit_code);
      break;
    case WAIT_OBJECT_0 + EVENT:
      message("Wake up from shutdown_event");
      break;

    default:
      message("Unexpected result %d from WaitForMultipleObjects", wait_res);
      break;
  }
  message("Exiting, child: %d", process_info.dwProcessId);

  if (TerminateJobObject(job_handle, 201) == 0)
    message("TerminateJobObject failed");
  CloseHandle(job_handle);
  message("Job terminated and closed");
  if (wait_res != WAIT_OBJECT_0 + CHILD)
  {
    /* The child has not yet returned, wait for it */
    message("waiting for child to exit");
    if ((wait_res= WaitForSingleObject(wait_handles[CHILD], INFINITE))
        != WAIT_OBJECT_0)
    {
      message("child wait failed: %d", wait_res);
    }
    else
    {
      message("child wait succeeded");
    }
    /* Child's exit code should now be 201, no need to get it */
  }

  message("Closing handles");
  for (int i= 0; i < NUM_HANDLES; i++)
    CloseHandle(wait_handles[i]);

  message("Exiting, exit_code: %d", child_exit_code);
  exit(child_exit_code);
}

<commit_msg>Bug #44775 MTR fails to bootstrap mysqld on Windows in Pushbuild 2.<commit_after>/* Copyright (C) 2004 MySQL AB

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; version 2 of the License.

   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program; if not, write to the Free Software
   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA */

/*
  Utility program that encapsulates process creation, monitoring
  and bulletproof process cleanup

  Usage:
    safe_process [options to safe_process] -- progname arg1 ... argn

  To safeguard mysqld you would invoke safe_process with a few options
  for safe_process itself followed by a double dash to indicate start
  of the command line for the program you really want to start

  $> safe_process --output=output.log -- mysqld --datadir=var/data1 ...

  This would redirect output to output.log and then start mysqld,
  once it has done that it will continue to monitor the child as well
  as the parent.

  The safe_process then checks the follwing things:
  1. Child exits, propagate the childs return code to the parent
     by exiting with the same return code as the child.

  2. Parent dies, immediately kill the child and exit, thus the
     parent does not need to properly cleanup any child, it is handled
     automatically.

  3. Signal's recieced by the process will trigger same action as 2)

  4. The named event "safe_process[pid]" can be signaled and will
     trigger same action as 2)

  WARNING! Be careful when using ProcessExplorer, since it will open
           a handle to each process(and maybe also the Job), the process
           spawned by safe_process will not be closed off when safe_process
           is killed.
*/

/* Requires Windows 2000 or higher */
#define _WIN32_WINNT 0x0500

#include <windows.h>
#include <stdio.h>
#include <tlhelp32.h>
#include <signal.h>

static int verbose= 0;
static char safe_process_name[32]= {0};

static void message(const char* fmt, ...)
{
  if (!verbose)
    return;
  va_list args;
  fprintf(stderr, "%s: ", safe_process_name);
  va_start(args, fmt);
  vfprintf(stderr, fmt, args);
  fprintf(stderr, "\n");
  va_end(args);
  fflush(stderr);
}

static void die(const char* fmt, ...)
{
  DWORD last_err= GetLastError();
  va_list args;
  fprintf(stderr, "%s: FATAL ERROR, ", safe_process_name);
  va_start(args, fmt);
  vfprintf(stderr, fmt, args);
  fprintf(stderr, "\n");
  va_end(args);
  if (last_err)
  {
    char *message_text;
    if (FormatMessage(FORMAT_MESSAGE_FROM_SYSTEM | FORMAT_MESSAGE_ALLOCATE_BUFFER
        |FORMAT_MESSAGE_IGNORE_INSERTS, NULL, last_err , 0, (LPSTR)&message_text,
        0, NULL))
    {
      fprintf(stderr,"error: %d, %s\n",last_err, message_text);
      LocalFree(message_text);
    }
    else
    {
      /* FormatMessage failed, print error code only */
      fprintf(stderr,"error:%d\n", last_err);
    }
  }
  fflush(stderr);
  exit(1);
}

DWORD get_parent_pid(DWORD pid)
{
  HANDLE snapshot;
  DWORD parent_pid= -1;
  PROCESSENTRY32 pe32;
  pe32.dwSize= sizeof(PROCESSENTRY32);

  snapshot= CreateToolhelp32Snapshot(TH32CS_SNAPPROCESS, 0);
  if (snapshot == INVALID_HANDLE_VALUE)
    die("CreateToolhelp32Snapshot failed");

  if (!Process32First(snapshot, &pe32))
  {
    CloseHandle(snapshot);
    die("Process32First failed");
  }

  do
  {
    if (pe32.th32ProcessID == pid)
      parent_pid= pe32.th32ParentProcessID;
  } while(Process32Next( snapshot, &pe32));
  CloseHandle(snapshot);

  if (parent_pid == -1)
    die("Could not find parent pid");

  return parent_pid;
}

enum {
  PARENT,
  CHILD,
  EVENT,
  NUM_HANDLES
};

HANDLE shutdown_event;
void handle_signal (int signal)
{
  message("Got signal: %d", signal);
  if(SetEvent(shutdown_event) == 0) {
    /* exit safe_process and (hopefully) kill off the child */
    die("Failed to SetEvent");
  }
}

int main(int argc, const char** argv )
{
  char child_args[4096]= {0};
  DWORD pid= GetCurrentProcessId();
  DWORD parent_pid= get_parent_pid(pid);
  HANDLE job_handle;
  HANDLE wait_handles[NUM_HANDLES]= {0};
  PROCESS_INFORMATION process_info= {0};

  sprintf(safe_process_name, "safe_process[%d]", pid);

  /* Create an event for the signal handler */
  if ((shutdown_event=
       CreateEvent(NULL, TRUE, FALSE, safe_process_name)) == NULL)
    die("Failed to create shutdown_event");
  wait_handles[EVENT]= shutdown_event;

  signal(SIGINT,   handle_signal);
  signal(SIGBREAK, handle_signal);
  signal(SIGTERM,  handle_signal);

  message("Started");

  /* Parse arguments */
  for (int i= 1; i < argc; i++) {
    const char* arg= argv[i];
	char* to= child_args;
    if (strcmp(arg, "--") == 0 && strlen(arg) == 2) {
      /* Got the "--" delimiter */
      if (i >= argc)
        die("No real args -> nothing to do");
      /* Copy the remaining args to child_arg */
      for (int j= i+1; j < argc; j++) {
        to+= _snprintf(to, child_args + sizeof(child_args) - to, "%s ", argv[j]);
      }
      break;
    } else {
      if ( strcmp(arg, "--verbose") == 0 )
        verbose++;
	  else if ( strncmp(arg, "--parent-pid", 10) == 0 )
	  {
	    /* Override parent_pid with a value provided by user */
		const char* start;
        if ((start= strstr(arg, "=")) == NULL)
		  die("Could not find start of option value in '%s'", arg);
		start++; /* Step past = */
		if ((parent_pid= atoi(start)) == 0)
		  die("Invalid value '%s' passed to --parent-id", start);
	  }
      else
        die("Unknown option: %s", arg);
    }
  }
  if (*child_args == '\0')
    die("nothing to do");

  /* Open a handle to the parent process */
  message("parent_pid: %d", parent_pid);
  if (parent_pid == pid)
    die("parent_pid is equal to own pid!");

  if ((wait_handles[PARENT]=
       OpenProcess(SYNCHRONIZE, FALSE, parent_pid)) == NULL)
    die("Failed to open parent process with pid: %d", parent_pid);

  /* Create the child process in a job */
  JOBOBJECT_EXTENDED_LIMIT_INFORMATION jeli = { 0 };
  STARTUPINFO                          si   = { 0 };
  si.cb = sizeof(si);

  /*
    Create the job object to make it possible to kill the process
    and all of it's children in one go
  */
  if ((job_handle= CreateJobObject(NULL, NULL)) == NULL)
    die("CreateJobObject failed");

  /*
    Make all processes associated with the job terminate when the
    last handle to the job is closed.
  */
  jeli.BasicLimitInformation.LimitFlags = JOB_OBJECT_LIMIT_KILL_ON_JOB_CLOSE;
  if (SetInformationJobObject(job_handle, JobObjectExtendedLimitInformation,
                              &jeli, sizeof(jeli)) == 0)
    message("SetInformationJobObject failed, continue anyway...");

#if 0
  /* Setup stdin, stdout and stderr redirect */
  si.dwFlags= STARTF_USESTDHANDLES;
  si.hStdInput= GetStdHandle(STD_INPUT_HANDLE);
  si.hStdOutput= GetStdHandle(STD_OUTPUT_HANDLE);
  si.hStdError= GetStdHandle(STD_ERROR_HANDLE);
#endif

  /*
    Create the process suspended to make sure it's assigned to the
    Job before it creates any process of it's own

    Allow the new process to break away from any job that this
    process is part of so that it can be assigned to the new JobObject
    we just created. This is safe since the new JobObject is created with
    the JOB_OBJECT_LIMIT_KILL_ON_JOB_CLOSE flag, making sure it will be
    terminated when the last handle to it is closed(which is owned by
    this process).

    If breakaway from job fails on some reason, fallback is to create a
    new process group. Process groups also allow to kill process and its
    descedants, subject to some restrictions (processes have to run within
    the same console,and must not ignore CTRL_BREAK)
  */
  DWORD create_flags[]= {CREATE_BREAKAWAY_FROM_JOB, CREATE_NEW_PROCESS_GROUP, 0};
  BOOL process_created= FALSE;
  BOOL jobobject_assigned= FALSE;

  for (int i=0; i < sizeof(create_flags)/sizeof(create_flags[0]); i++)
  {
    process_created= CreateProcess(NULL, (LPSTR)child_args,
                    NULL,
                    NULL,
                    TRUE, /* inherit handles */
                    CREATE_SUSPENDED | create_flags[i],
                    NULL,
                    NULL,
                    &si,
                    &process_info);
    if (process_created)
    {
     jobobject_assigned= AssignProcessToJobObject(job_handle, process_info.hProcess);
     break;
    }
  }

  if (!process_created)
  {
    die("CreateProcess failed");
  }
  ResumeThread(process_info.hThread);
  CloseHandle(process_info.hThread);

  wait_handles[CHILD]= process_info.hProcess;

  message("Started child %d", process_info.dwProcessId);

  /* Monitor loop */
  DWORD child_exit_code= 1;
  DWORD wait_res= WaitForMultipleObjects(NUM_HANDLES, wait_handles,
                                         FALSE, INFINITE);
  switch (wait_res)
  {
    case WAIT_OBJECT_0 + PARENT:
      message("Parent exit");
      break;
    case WAIT_OBJECT_0 + CHILD:
      if (GetExitCodeProcess(wait_handles[CHILD], &child_exit_code) == 0)
        message("Child exit: could not get exit_code");
      else
        message("Child exit: exit_code: %d", child_exit_code);
      break;
    case WAIT_OBJECT_0 + EVENT:
      message("Wake up from shutdown_event");
      break;

    default:
      message("Unexpected result %d from WaitForMultipleObjects", wait_res);
      break;
  }
  message("Exiting, child: %d", process_info.dwProcessId);

  if (TerminateJobObject(job_handle, 201) == 0)
    message("TerminateJobObject failed");
  CloseHandle(job_handle);
  message("Job terminated and closed");

  if (!jobobject_assigned)
  {
    GenerateConsoleCtrlEvent(CTRL_BREAK_EVENT, process_info.dwProcessId);
    TerminateProcess(process_info.hProcess, 202);
  }

  if (wait_res != WAIT_OBJECT_0 + CHILD)
  {
    /* The child has not yet returned, wait for it */
    message("waiting for child to exit");
    if ((wait_res= WaitForSingleObject(wait_handles[CHILD], INFINITE))
        != WAIT_OBJECT_0)
    {
      message("child wait failed: %d", wait_res);
    }
    else
    {
      message("child wait succeeded");
    }
    /* Child's exit code should now be 201, no need to get it */
  }

  message("Closing handles");
  for (int i= 0; i < NUM_HANDLES; i++)
    CloseHandle(wait_handles[i]);

  message("Exiting, exit_code: %d", child_exit_code);
  exit(child_exit_code);
}

<|endoftext|>


<commit_before>/*
* Copyright (C) 2017 by Godlike
* This code is licensed under the MIT license (MIT)
* (http://opensource.org/licenses/MIT)
*/

#include <unicorn/video/vulkan/VkTexture.hpp>
#include <unicorn/video/Texture.hpp>
#include <unicorn/utility/Logger.hpp>

namespace unicorn
{
namespace video
{
namespace vulkan
{
VkTexture::VkTexture(vk::Device device)
    : m_device(device)
    , m_vkImage(nullptr)
    , m_isInitialized(false)
{
}

const vk::DescriptorImageInfo& VkTexture::GetDescriptorImageInfo() const
{
    return m_imageInfo;
}

bool VkTexture::Create(const vk::PhysicalDevice& physicalDevice, const vk::Device& device,
                       const vk::CommandPool& commandPool, const vk::Queue& queue, const Texture* texture)
{
    if(!m_isInitialized)
    {
        Buffer imageStagingBuffer;
        bool result = imageStagingBuffer.Create(
            physicalDevice, device,
            vk::BufferUsageFlagBits::eTransferSrc,
            vk::MemoryPropertyFlagBits::eHostVisible | vk::MemoryPropertyFlagBits::eHostCoherent,
            texture->Size());
        if(!result)
        {
            LOG_ERROR("Can't allocate staging buffer for texture - %s", texture->Path().c_str());
            return false;
        }

        imageStagingBuffer.Map();
        imageStagingBuffer.Write(texture->Data());
        imageStagingBuffer.Unmap();

        m_vkImage = new Image(
            physicalDevice,
            device,
            vk::Format::eR8G8B8A8Unorm,
            vk::ImageUsageFlagBits::eTransferDst | vk::ImageUsageFlagBits::eSampled | vk::ImageUsageFlagBits::eColorAttachment,
            texture->Width(),
            texture->Height());
        if(!m_vkImage->IsInitialized())
        {
            LOG_ERROR("Can't allocate vulkan based image for texture - %s", texture->Path().c_str());
            return false;
        }

        m_vkImage->TransitionLayout(vk::Format::eR8G8B8A8Unorm, vk::ImageLayout::eUndefined,
                                    vk::ImageLayout::eTransferDstOptimal, commandPool, queue);
        imageStagingBuffer.CopyToImage(*m_vkImage, commandPool, queue);
        m_vkImage->TransitionLayout(vk::Format::eR8G8B8A8Unorm, vk::ImageLayout::eTransferDstOptimal,
                                    vk::ImageLayout::eShaderReadOnlyOptimal, commandPool, queue);

        imageStagingBuffer.Destroy();

        vk::SamplerCreateInfo samplerInfo;
        samplerInfo.setMagFilter(vk::Filter::eLinear);
        samplerInfo.setMinFilter(vk::Filter::eLinear);
        samplerInfo.setAddressModeU(vk::SamplerAddressMode::eRepeat);
        samplerInfo.setAddressModeV(vk::SamplerAddressMode::eRepeat);
        samplerInfo.setAddressModeW(vk::SamplerAddressMode::eRepeat);
        samplerInfo.setAnisotropyEnable(VK_TRUE);
        samplerInfo.setMaxAnisotropy(16);
        samplerInfo.setBorderColor(vk::BorderColor::eIntOpaqueBlack);
        samplerInfo.setUnnormalizedCoordinates(VK_FALSE);
        samplerInfo.setCompareEnable(VK_FALSE);
        samplerInfo.setCompareOp(vk::CompareOp::eAlways);
        samplerInfo.setMipmapMode(vk::SamplerMipmapMode::eLinear);
        samplerInfo.setMipLodBias(0.0f);
        samplerInfo.setMinLod(0.0f);
        samplerInfo.setMaxLod(0.0f);

        result = (device.createSampler(&samplerInfo, nullptr, &m_sampler) == vk::Result::eSuccess);

        if(!result)
        {
            LOG_ERROR("Can't create sampler for texture - %s", texture->Path());
            return false;
        }

        m_imageInfo.sampler = m_sampler;
        m_imageInfo.imageView = m_vkImage->GetVkImageView();
        m_isInitialized = true;
    }
    return true;
}

void VkTexture::Delete()
{
    if(m_isInitialized)
    {
        if(m_device && m_sampler)
        {
            m_device.destroySampler(m_sampler);
        }
        m_vkImage->Delete();
        m_isInitialized = false;
    }
}

bool VkTexture::IsInitialized() const
{
    return m_isInitialized;
}
}
}
}
<commit_msg>[#20] travis fix<commit_after>/*
* Copyright (C) 2017 by Godlike
* This code is licensed under the MIT license (MIT)
* (http://opensource.org/licenses/MIT)
*/

#include <unicorn/video/vulkan/VkTexture.hpp>
#include <unicorn/video/Texture.hpp>
#include <unicorn/utility/Logger.hpp>

namespace unicorn
{
namespace video
{
namespace vulkan
{
VkTexture::VkTexture(vk::Device device)
    : m_device(device)
    , m_vkImage(nullptr)
    , m_isInitialized(false)
{
}

const vk::DescriptorImageInfo& VkTexture::GetDescriptorImageInfo() const
{
    return m_imageInfo;
}

bool VkTexture::Create(const vk::PhysicalDevice& physicalDevice, const vk::Device& device,
                       const vk::CommandPool& commandPool, const vk::Queue& queue, const Texture* texture)
{
    if(!m_isInitialized)
    {
        Buffer imageStagingBuffer;
        bool result = imageStagingBuffer.Create(
            physicalDevice, device,
            vk::BufferUsageFlagBits::eTransferSrc,
            vk::MemoryPropertyFlagBits::eHostVisible | vk::MemoryPropertyFlagBits::eHostCoherent,
            texture->Size());
        if(!result)
        {
            LOG_ERROR("Can't allocate staging buffer for texture - %s", texture->Path().c_str());
            return false;
        }

        imageStagingBuffer.Map();
        imageStagingBuffer.Write(texture->Data());
        imageStagingBuffer.Unmap();

        m_vkImage = new Image(
            physicalDevice,
            device,
            vk::Format::eR8G8B8A8Unorm,
            vk::ImageUsageFlagBits::eTransferDst | vk::ImageUsageFlagBits::eSampled | vk::ImageUsageFlagBits::eColorAttachment,
            texture->Width(),
            texture->Height());
        if(!m_vkImage->IsInitialized())
        {
            LOG_ERROR("Can't allocate vulkan based image for texture - %s", texture->Path().c_str());
            return false;
        }

        m_vkImage->TransitionLayout(vk::Format::eR8G8B8A8Unorm, vk::ImageLayout::eUndefined,
                                    vk::ImageLayout::eTransferDstOptimal, commandPool, queue);
        imageStagingBuffer.CopyToImage(*m_vkImage, commandPool, queue);
        m_vkImage->TransitionLayout(vk::Format::eR8G8B8A8Unorm, vk::ImageLayout::eTransferDstOptimal,
                                    vk::ImageLayout::eShaderReadOnlyOptimal, commandPool, queue);

        imageStagingBuffer.Destroy();

        vk::SamplerCreateInfo samplerInfo;
        samplerInfo.setMagFilter(vk::Filter::eLinear);
        samplerInfo.setMinFilter(vk::Filter::eLinear);
        samplerInfo.setAddressModeU(vk::SamplerAddressMode::eRepeat);
        samplerInfo.setAddressModeV(vk::SamplerAddressMode::eRepeat);
        samplerInfo.setAddressModeW(vk::SamplerAddressMode::eRepeat);
        samplerInfo.setAnisotropyEnable(VK_TRUE);
        samplerInfo.setMaxAnisotropy(16);
        samplerInfo.setBorderColor(vk::BorderColor::eIntOpaqueBlack);
        samplerInfo.setUnnormalizedCoordinates(VK_FALSE);
        samplerInfo.setCompareEnable(VK_FALSE);
        samplerInfo.setCompareOp(vk::CompareOp::eAlways);
        samplerInfo.setMipmapMode(vk::SamplerMipmapMode::eLinear);
        samplerInfo.setMipLodBias(0.0f);
        samplerInfo.setMinLod(0.0f);
        samplerInfo.setMaxLod(0.0f);

        result = (device.createSampler(&samplerInfo, nullptr, &m_sampler) == vk::Result::eSuccess);

        if(!result)
        {
            LOG_ERROR("Can't create sampler for texture - %s", texture->Path().c_str());
            return false;
        }

        m_imageInfo.sampler = m_sampler;
        m_imageInfo.imageView = m_vkImage->GetVkImageView();
        m_isInitialized = true;
    }
    return true;
}

void VkTexture::Delete()
{
    if(m_isInitialized)
    {
        if(m_device && m_sampler)
        {
            m_device.destroySampler(m_sampler);
        }
        m_vkImage->Delete();
        m_isInitialized = false;
    }
}

bool VkTexture::IsInitialized() const
{
    return m_isInitialized;
}
}
}
}
<|endoftext|>


<commit_before>/****************************************************************************
 *
 *   Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name ECL 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.
 *
 ****************************************************************************/

/**
 * @file terrain_estimator.cpp
 * Function for fusing rangefinder measurements to estimate terrain vertical position/
 *
 * @author Paul Riseborough <p_riseborough@live.com.au>
 *
 */

#include "ekf.h"
#include <ecl.h>
#include <mathlib/mathlib.h>

bool Ekf::initHagl()
{
	bool initialized = false;
	// get most recent range measurement from buffer
	const rangeSample &latest_measurement = _range_buffer.get_newest();

	if (!_control_status.flags.in_air) {
		// if on ground, do not trust the range sensor, but assume a ground clearance
		_terrain_vpos = _state.pos(2) + _params.rng_gnd_clearance;
		// use the ground clearance value as our uncertainty
		_terrain_var = sq(_params.rng_gnd_clearance);
		initialized = true;

	} else if ((_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseRangeFinder)
		   && _range_sensor.isHealthy()
		   && isRecent(latest_measurement.time_us, (uint64_t)2e5)) {
		// if we have a fresh measurement, use it to initialise the terrain estimator
		// TODO: the latest measurment did not go through the checks and could be invalid!
		_terrain_vpos = _state.pos(2) + latest_measurement.rng * _range_sensor.getCosTilt();
		// initialise state variance to variance of measurement
		_terrain_var = sq(_params.range_noise);
		// success
		initialized = true;

	} else if ((_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseOpticalFlow)
		   && _flow_for_terrain_data_ready) {
		// initialise terrain vertical position to origin as this is the best guess we have
		_terrain_vpos = fmaxf(0.0f,  _state.pos(2));
		_terrain_var = 100.0f;
		initialized = true;

	} else {
		// no information - cannot initialise
	}

	if (initialized) {
		// has initialized with valid data
		_time_last_hagl_fuse = _time_last_imu;
	}

	return initialized;
}

void Ekf::runTerrainEstimator()
{
	// If we are on ground, store the local position and time to use as a reference
	if (!_control_status.flags.in_air) {
		_last_on_ground_posD = _state.pos(2);
	}

	// Perform initialisation check and
	// on ground, continuously reset the terrain estimator
	if (!_terrain_initialised || !_control_status.flags.in_air) {
		_terrain_initialised = initHagl();

	} else {

		// predict the state variance growth where the state is the vertical position of the terrain underneath the vehicle

		// process noise due to errors in vehicle height estimate
		_terrain_var += sq(_imu_sample_delayed.delta_vel_dt * _params.terrain_p_noise);

		// process noise due to terrain gradient
		_terrain_var += sq(_imu_sample_delayed.delta_vel_dt * _params.terrain_gradient)
				* (sq(_state.vel(0)) + sq(_state.vel(1)));

		// limit the variance to prevent it becoming badly conditioned
		_terrain_var = math::constrain(_terrain_var, 0.0f, 1e4f);

		// Fuse range finder data if available
		if (_range_sensor.isDataHealthy()) {
			fuseHagl();
		}

		if (_flow_for_terrain_data_ready) {
			fuseFlowForTerrain();
			_flow_for_terrain_data_ready = false;
		}

		// constrain _terrain_vpos to be a minimum of _params.rng_gnd_clearance larger than _state.pos(2)
		if (_terrain_vpos - _state.pos(2) < _params.rng_gnd_clearance) {
			_terrain_vpos = _params.rng_gnd_clearance + _state.pos(2);
		}
	}

	updateTerrainValidity();
}

void Ekf::fuseHagl()
{
	// get a height above ground measurement from the range finder assuming a flat earth
	const float meas_hagl = _range_sensor.getRange() * _range_sensor.getCosTilt();

	// predict the hagl from the vehicle position and terrain height
	const float pred_hagl = _terrain_vpos - _state.pos(2);

	// calculate the innovation
	_hagl_innov = pred_hagl - meas_hagl;

	// calculate the observation variance adding the variance of the vehicles own height uncertainty
	const float obs_variance = fmaxf(P(9,9) * _params.vehicle_variance_scaler, 0.0f)
			     + sq(_params.range_noise)
			     + sq(_params.range_noise_scaler * _range_sensor.getRange());

	// calculate the innovation variance - limiting it to prevent a badly conditioned fusion
	_hagl_innov_var = fmaxf(_terrain_var + obs_variance, obs_variance);

	// perform an innovation consistency check and only fuse data if it passes
	const float gate_size = fmaxf(_params.range_innov_gate, 1.0f);
	_hagl_test_ratio = sq(_hagl_innov) / (sq(gate_size) * _hagl_innov_var);

	if (_hagl_test_ratio <= 1.0f) {
		// calculate the Kalman gain
		float gain = _terrain_var / _hagl_innov_var;
		// correct the state
		_terrain_vpos -= gain * _hagl_innov;
		// correct the variance
		_terrain_var = fmaxf(_terrain_var * (1.0f - gain), 0.0f);
		// record last successful fusion event
		_time_last_hagl_fuse = _time_last_imu;
		_innov_check_fail_status.flags.reject_hagl = false;

	} else {
		// If we have been rejecting range data for too long, reset to measurement
		if (isTimedOut(_time_last_hagl_fuse, (uint64_t)10E6)) {
			_terrain_vpos = _state.pos(2) + meas_hagl;
			_terrain_var = obs_variance;

		} else {
			_innov_check_fail_status.flags.reject_hagl = true;
		}
	}
}

void Ekf::fuseFlowForTerrain()
{
	// calculate optical LOS rates using optical flow rates that have had the body angular rate contribution removed
	// correct for gyro bias errors in the data used to do the motion compensation
	// Note the sign convention used: A positive LOS rate is a RH rotation of the scene about that axis.
	const Vector2f opt_flow_rate = Vector2f{_flow_compensated_XY_rad} / _flow_sample_delayed.dt + Vector2f{_flow_gyro_bias};

	// get latest estimated orientation
	const float q0 = _state.quat_nominal(0);
	const float q1 = _state.quat_nominal(1);
	const float q2 = _state.quat_nominal(2);
	const float q3 = _state.quat_nominal(3);

	// calculate the optical flow observation variance
	const float R_LOS = calcOptFlowMeasVar();

	// get rotation matrix from earth to body
	const Dcmf earth_to_body = quat_to_invrotmat(_state.quat_nominal);

	// calculate the sensor position relative to the IMU
	const Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body;

	// calculate the velocity of the sensor relative to the imu in body frame
	// Note: _flow_sample_delayed.gyro_xyz is the negative of the body angular velocity, thus use minus sign
	const Vector3f vel_rel_imu_body = Vector3f(-_flow_sample_delayed.gyro_xyz / _flow_sample_delayed.dt) % pos_offset_body;

	// calculate the velocity of the sensor in the earth frame
	const Vector3f vel_rel_earth = _state.vel + _R_to_earth * vel_rel_imu_body;

	// rotate into body frame
	const Vector3f vel_body = earth_to_body * vel_rel_earth;

	const float t0 = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;

	// constrain terrain to minimum allowed value and predict height above ground
	_terrain_vpos = fmaxf(_terrain_vpos, _params.rng_gnd_clearance + _state.pos(2));
	const float pred_hagl = _terrain_vpos - _state.pos(2);

	// Calculate observation matrix for flow around the vehicle x axis
	const float Hx = vel_body(1) * t0 / (pred_hagl * pred_hagl);

	// Constrain terrain variance to be non-negative
	_terrain_var = fmaxf(_terrain_var, 0.0f);

	// Cacluate innovation variance
	_flow_innov_var[0] = Hx * Hx * _terrain_var + R_LOS;

	// calculate the kalman gain for the flow x measurement
	const float Kx = _terrain_var * Hx / _flow_innov_var[0];

	// calculate prediced optical flow about x axis
	const float pred_flow_x = vel_body(1) * earth_to_body(2, 2) / pred_hagl;

	// calculate flow innovation (x axis)
	_flow_innov[0] = pred_flow_x - opt_flow_rate(0);

	// calculate correction term for terrain variance
	const float KxHxP =  Kx * Hx * _terrain_var;

	// innovation consistency check
	const float gate_size = fmaxf(_params.flow_innov_gate, 1.0f);
	float flow_test_ratio = sq(_flow_innov[0]) / (sq(gate_size) * _flow_innov_var[0]);

	// do not perform measurement update if badly conditioned
	if (flow_test_ratio <= 1.0f) {
		_terrain_vpos += Kx * _flow_innov[0];
		// guard against negative variance
		_terrain_var = fmaxf(_terrain_var - KxHxP, 0.0f);
		_time_last_of_fuse = _time_last_imu;
	}

	// Calculate observation matrix for flow around the vehicle y axis
	const float Hy = -vel_body(0) * t0 / (pred_hagl * pred_hagl);

	// Calculuate innovation variance
	_flow_innov_var[1] = Hy * Hy * _terrain_var + R_LOS;

	// calculate the kalman gain for the flow y measurement
	const float Ky = _terrain_var * Hy / _flow_innov_var[1];

	// calculate prediced optical flow about y axis
	const float pred_flow_y = -vel_body(0) * earth_to_body(2, 2) / pred_hagl;

	// calculate flow innovation (y axis)
	_flow_innov[1] = pred_flow_y - opt_flow_rate(1);

	// calculate correction term for terrain variance
	const float KyHyP =  Ky * Hy * _terrain_var;

	// innovation consistency check
	flow_test_ratio = sq(_flow_innov[1]) / (sq(gate_size) * _flow_innov_var[1]);

	if (flow_test_ratio <= 1.0f) {
		_terrain_vpos += Ky * _flow_innov[1];
		// guard against negative variance
		_terrain_var = fmaxf(_terrain_var - KyHyP, 0.0f);
		_time_last_of_fuse = _time_last_imu;
	}
}

bool Ekf::isTerrainEstimateValid() const
{
	return _hagl_valid;
}

void Ekf::updateTerrainValidity()
{
	// we have been fusing range finder measurements in the last 5 seconds
	const bool recent_range_fusion = isRecent(_time_last_hagl_fuse, (uint64_t)5e6);

	// we have been fusing optical flow measurements for terrain estimation within the last 5 seconds
	// this can only be the case if the main filter does not fuse optical flow
	const bool recent_flow_for_terrain_fusion = isRecent(_time_last_of_fuse, (uint64_t)5e6)
						    && !_control_status.flags.opt_flow;

	_hagl_valid = (_terrain_initialised && (recent_range_fusion || recent_flow_for_terrain_fusion));
}

// get the estimated vertical position of the terrain relative to the NED origin
float Ekf::getTerrainVertPos() const
{
	return _terrain_vpos;
}
<commit_msg>terrain_estimator: initialize with valid delayed measurement<commit_after>/****************************************************************************
 *
 *   Copyright (c) 2015 Estimation and Control Library (ECL). All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name ECL 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.
 *
 ****************************************************************************/

/**
 * @file terrain_estimator.cpp
 * Function for fusing rangefinder measurements to estimate terrain vertical position/
 *
 * @author Paul Riseborough <p_riseborough@live.com.au>
 *
 */

#include "ekf.h"
#include <ecl.h>
#include <mathlib/mathlib.h>

bool Ekf::initHagl()
{
	bool initialized = false;

	if (!_control_status.flags.in_air) {
		// if on ground, do not trust the range sensor, but assume a ground clearance
		_terrain_vpos = _state.pos(2) + _params.rng_gnd_clearance;
		// use the ground clearance value as our uncertainty
		_terrain_var = sq(_params.rng_gnd_clearance);
		initialized = true;

	} else if ((_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseRangeFinder)
		   && _range_sensor.isDataHealthy()) {
		// if we have a fresh measurement, use it to initialise the terrain estimator
		_terrain_vpos = _state.pos(2) + _range_sensor.getRange() * _range_sensor.getCosTilt();
		// initialise state variance to variance of measurement
		_terrain_var = sq(_params.range_noise);
		// success
		initialized = true;

	} else if ((_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseOpticalFlow)
		   && _flow_for_terrain_data_ready) {
		// initialise terrain vertical position to origin as this is the best guess we have
		_terrain_vpos = fmaxf(0.0f,  _state.pos(2));
		_terrain_var = 100.0f;
		initialized = true;

	} else {
		// no information - cannot initialise
	}

	if (initialized) {
		// has initialized with valid data
		_time_last_hagl_fuse = _time_last_imu;
	}

	return initialized;
}

void Ekf::runTerrainEstimator()
{
	// If we are on ground, store the local position and time to use as a reference
	if (!_control_status.flags.in_air) {
		_last_on_ground_posD = _state.pos(2);
	}

	// Perform initialisation check and
	// on ground, continuously reset the terrain estimator
	if (!_terrain_initialised || !_control_status.flags.in_air) {
		_terrain_initialised = initHagl();

	} else {

		// predict the state variance growth where the state is the vertical position of the terrain underneath the vehicle

		// process noise due to errors in vehicle height estimate
		_terrain_var += sq(_imu_sample_delayed.delta_vel_dt * _params.terrain_p_noise);

		// process noise due to terrain gradient
		_terrain_var += sq(_imu_sample_delayed.delta_vel_dt * _params.terrain_gradient)
				* (sq(_state.vel(0)) + sq(_state.vel(1)));

		// limit the variance to prevent it becoming badly conditioned
		_terrain_var = math::constrain(_terrain_var, 0.0f, 1e4f);

		// Fuse range finder data if available
		if (_range_sensor.isDataHealthy()) {
			fuseHagl();
		}

		if (_flow_for_terrain_data_ready) {
			fuseFlowForTerrain();
			_flow_for_terrain_data_ready = false;
		}

		// constrain _terrain_vpos to be a minimum of _params.rng_gnd_clearance larger than _state.pos(2)
		if (_terrain_vpos - _state.pos(2) < _params.rng_gnd_clearance) {
			_terrain_vpos = _params.rng_gnd_clearance + _state.pos(2);
		}
	}

	updateTerrainValidity();
}

void Ekf::fuseHagl()
{
	// get a height above ground measurement from the range finder assuming a flat earth
	const float meas_hagl = _range_sensor.getRange() * _range_sensor.getCosTilt();

	// predict the hagl from the vehicle position and terrain height
	const float pred_hagl = _terrain_vpos - _state.pos(2);

	// calculate the innovation
	_hagl_innov = pred_hagl - meas_hagl;

	// calculate the observation variance adding the variance of the vehicles own height uncertainty
	const float obs_variance = fmaxf(P(9,9) * _params.vehicle_variance_scaler, 0.0f)
			     + sq(_params.range_noise)
			     + sq(_params.range_noise_scaler * _range_sensor.getRange());

	// calculate the innovation variance - limiting it to prevent a badly conditioned fusion
	_hagl_innov_var = fmaxf(_terrain_var + obs_variance, obs_variance);

	// perform an innovation consistency check and only fuse data if it passes
	const float gate_size = fmaxf(_params.range_innov_gate, 1.0f);
	_hagl_test_ratio = sq(_hagl_innov) / (sq(gate_size) * _hagl_innov_var);

	if (_hagl_test_ratio <= 1.0f) {
		// calculate the Kalman gain
		float gain = _terrain_var / _hagl_innov_var;
		// correct the state
		_terrain_vpos -= gain * _hagl_innov;
		// correct the variance
		_terrain_var = fmaxf(_terrain_var * (1.0f - gain), 0.0f);
		// record last successful fusion event
		_time_last_hagl_fuse = _time_last_imu;
		_innov_check_fail_status.flags.reject_hagl = false;

	} else {
		// If we have been rejecting range data for too long, reset to measurement
		if (isTimedOut(_time_last_hagl_fuse, (uint64_t)10E6)) {
			_terrain_vpos = _state.pos(2) + meas_hagl;
			_terrain_var = obs_variance;

		} else {
			_innov_check_fail_status.flags.reject_hagl = true;
		}
	}
}

void Ekf::fuseFlowForTerrain()
{
	// calculate optical LOS rates using optical flow rates that have had the body angular rate contribution removed
	// correct for gyro bias errors in the data used to do the motion compensation
	// Note the sign convention used: A positive LOS rate is a RH rotation of the scene about that axis.
	const Vector2f opt_flow_rate = Vector2f{_flow_compensated_XY_rad} / _flow_sample_delayed.dt + Vector2f{_flow_gyro_bias};

	// get latest estimated orientation
	const float q0 = _state.quat_nominal(0);
	const float q1 = _state.quat_nominal(1);
	const float q2 = _state.quat_nominal(2);
	const float q3 = _state.quat_nominal(3);

	// calculate the optical flow observation variance
	const float R_LOS = calcOptFlowMeasVar();

	// get rotation matrix from earth to body
	const Dcmf earth_to_body = quat_to_invrotmat(_state.quat_nominal);

	// calculate the sensor position relative to the IMU
	const Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body;

	// calculate the velocity of the sensor relative to the imu in body frame
	// Note: _flow_sample_delayed.gyro_xyz is the negative of the body angular velocity, thus use minus sign
	const Vector3f vel_rel_imu_body = Vector3f(-_flow_sample_delayed.gyro_xyz / _flow_sample_delayed.dt) % pos_offset_body;

	// calculate the velocity of the sensor in the earth frame
	const Vector3f vel_rel_earth = _state.vel + _R_to_earth * vel_rel_imu_body;

	// rotate into body frame
	const Vector3f vel_body = earth_to_body * vel_rel_earth;

	const float t0 = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;

	// constrain terrain to minimum allowed value and predict height above ground
	_terrain_vpos = fmaxf(_terrain_vpos, _params.rng_gnd_clearance + _state.pos(2));
	const float pred_hagl = _terrain_vpos - _state.pos(2);

	// Calculate observation matrix for flow around the vehicle x axis
	const float Hx = vel_body(1) * t0 / (pred_hagl * pred_hagl);

	// Constrain terrain variance to be non-negative
	_terrain_var = fmaxf(_terrain_var, 0.0f);

	// Cacluate innovation variance
	_flow_innov_var[0] = Hx * Hx * _terrain_var + R_LOS;

	// calculate the kalman gain for the flow x measurement
	const float Kx = _terrain_var * Hx / _flow_innov_var[0];

	// calculate prediced optical flow about x axis
	const float pred_flow_x = vel_body(1) * earth_to_body(2, 2) / pred_hagl;

	// calculate flow innovation (x axis)
	_flow_innov[0] = pred_flow_x - opt_flow_rate(0);

	// calculate correction term for terrain variance
	const float KxHxP =  Kx * Hx * _terrain_var;

	// innovation consistency check
	const float gate_size = fmaxf(_params.flow_innov_gate, 1.0f);
	float flow_test_ratio = sq(_flow_innov[0]) / (sq(gate_size) * _flow_innov_var[0]);

	// do not perform measurement update if badly conditioned
	if (flow_test_ratio <= 1.0f) {
		_terrain_vpos += Kx * _flow_innov[0];
		// guard against negative variance
		_terrain_var = fmaxf(_terrain_var - KxHxP, 0.0f);
		_time_last_of_fuse = _time_last_imu;
	}

	// Calculate observation matrix for flow around the vehicle y axis
	const float Hy = -vel_body(0) * t0 / (pred_hagl * pred_hagl);

	// Calculuate innovation variance
	_flow_innov_var[1] = Hy * Hy * _terrain_var + R_LOS;

	// calculate the kalman gain for the flow y measurement
	const float Ky = _terrain_var * Hy / _flow_innov_var[1];

	// calculate prediced optical flow about y axis
	const float pred_flow_y = -vel_body(0) * earth_to_body(2, 2) / pred_hagl;

	// calculate flow innovation (y axis)
	_flow_innov[1] = pred_flow_y - opt_flow_rate(1);

	// calculate correction term for terrain variance
	const float KyHyP =  Ky * Hy * _terrain_var;

	// innovation consistency check
	flow_test_ratio = sq(_flow_innov[1]) / (sq(gate_size) * _flow_innov_var[1]);

	if (flow_test_ratio <= 1.0f) {
		_terrain_vpos += Ky * _flow_innov[1];
		// guard against negative variance
		_terrain_var = fmaxf(_terrain_var - KyHyP, 0.0f);
		_time_last_of_fuse = _time_last_imu;
	}
}

bool Ekf::isTerrainEstimateValid() const
{
	return _hagl_valid;
}

void Ekf::updateTerrainValidity()
{
	// we have been fusing range finder measurements in the last 5 seconds
	const bool recent_range_fusion = isRecent(_time_last_hagl_fuse, (uint64_t)5e6);

	// we have been fusing optical flow measurements for terrain estimation within the last 5 seconds
	// this can only be the case if the main filter does not fuse optical flow
	const bool recent_flow_for_terrain_fusion = isRecent(_time_last_of_fuse, (uint64_t)5e6)
						    && !_control_status.flags.opt_flow;

	_hagl_valid = (_terrain_initialised && (recent_range_fusion || recent_flow_for_terrain_fusion));
}

// get the estimated vertical position of the terrain relative to the NED origin
float Ekf::getTerrainVertPos() const
{
	return _terrain_vpos;
}
<|endoftext|>


<commit_before>// Copyright (c) 2010, Lawrence Livermore National Security, LLC. Produced at
// the Lawrence Livermore National Laboratory. LLNL-CODE-443211. All Rights
// reserved. See file COPYRIGHT for details.
//
// This file is part of the MFEM library. For more information and source code
// availability see http://mfem.org.
//
// MFEM is free software; you can redistribute it and/or modify it under the
// terms of the GNU Lesser General Public License (as published by the Free
// Software Foundation) version 2.1 dated February 1999.

#ifndef MFEM_PNONLINEARFORM
#define MFEM_PNONLINEARFORM

#include "../config/config.hpp"

#ifdef MFEM_USE_MPI

#include "pgridfunc.hpp"
#include "nonlinearform.hpp"

namespace mfem
{

/// Parallel non-linear operator on the true dofs
class ParNonlinearForm : public NonlinearForm
{
protected:
   mutable ParGridFunction X, Y;
   mutable OperatorHandle pGrad;

public:
   ParNonlinearForm(ParFiniteElementSpace *pf);

   ParFiniteElementSpace *ParFESpace() const
   { return (ParFiniteElementSpace *)fes; }

   /// Compute the enery corresponding to the state @a x.
   /** In general, @a x may have non-homogeneous essential boundary values.

       The state @a x must be a "GridFunction size" vector, i.e. its size must
       be fes->GetVSize(). */
   double GetParGridFunctionEnergy(const Vector &x) const;

   /// Compute the energy of a ParGridFunction
   double GetEnergy(const ParGridFunction &x) const
   { return GetParGridFunctionEnergy(x); }

   virtual double GetEnergy(const Vector &x) const
   { return GetParGridFunctionEnergy(Prolongate(x)); }

   virtual void Mult(const Vector &x, Vector &y) const;

   /// Return the local gradient matrix for the given true-dof vector x.
   /** The returned matrix does NOT have any boundary conditions imposed. */
   const SparseMatrix &GetLocalGradient(const Vector &x) const;

   virtual Operator &GetGradient(const Vector &x) const;

   /// Set the operator type id for the parallel gradient matrix/operator.
   void SetGradientType(Operator::Type tid) { pGrad.SetType(tid); }

   /** @brief Update the ParNonlinearForm to propagate updates of the associated
       parallel FE space. */
   /** After calling this method, the essential boundary conditions need to be
       set again. */
   virtual void Update();

   virtual ~ParNonlinearForm() { }
};

}

#endif // MFEM_USE_MPI

#endif
<commit_msg>Typo.<commit_after>// Copyright (c) 2010, Lawrence Livermore National Security, LLC. Produced at
// the Lawrence Livermore National Laboratory. LLNL-CODE-443211. All Rights
// reserved. See file COPYRIGHT for details.
//
// This file is part of the MFEM library. For more information and source code
// availability see http://mfem.org.
//
// MFEM is free software; you can redistribute it and/or modify it under the
// terms of the GNU Lesser General Public License (as published by the Free
// Software Foundation) version 2.1 dated February 1999.

#ifndef MFEM_PNONLINEARFORM
#define MFEM_PNONLINEARFORM

#include "../config/config.hpp"

#ifdef MFEM_USE_MPI

#include "pgridfunc.hpp"
#include "nonlinearform.hpp"

namespace mfem
{

/// Parallel non-linear operator on the true dofs
class ParNonlinearForm : public NonlinearForm
{
protected:
   mutable ParGridFunction X, Y;
   mutable OperatorHandle pGrad;

public:
   ParNonlinearForm(ParFiniteElementSpace *pf);

   ParFiniteElementSpace *ParFESpace() const
   { return (ParFiniteElementSpace *)fes; }

   /// Compute the energy corresponding to the state @a x.
   /** In general, @a x may have non-homogeneous essential boundary values.

       The state @a x must be a "GridFunction size" vector, i.e. its size must
       be fes->GetVSize(). */
   double GetParGridFunctionEnergy(const Vector &x) const;

   /// Compute the energy of a ParGridFunction
   double GetEnergy(const ParGridFunction &x) const
   { return GetParGridFunctionEnergy(x); }

   virtual double GetEnergy(const Vector &x) const
   { return GetParGridFunctionEnergy(Prolongate(x)); }

   virtual void Mult(const Vector &x, Vector &y) const;

   /// Return the local gradient matrix for the given true-dof vector x.
   /** The returned matrix does NOT have any boundary conditions imposed. */
   const SparseMatrix &GetLocalGradient(const Vector &x) const;

   virtual Operator &GetGradient(const Vector &x) const;

   /// Set the operator type id for the parallel gradient matrix/operator.
   void SetGradientType(Operator::Type tid) { pGrad.SetType(tid); }

   /** @brief Update the ParNonlinearForm to propagate updates of the associated
       parallel FE space. */
   /** After calling this method, the essential boundary conditions need to be
       set again. */
   virtual void Update();

   virtual ~ParNonlinearForm() { }
};

}

#endif // MFEM_USE_MPI

#endif
<|endoftext|>


<commit_before>//===- SyntheticSections.cpp ----------------------------------------------===//
//
//                             The LLVM Linker
//
// This file is distributed under the University of Illinois Open Source
// License. See LICENSE.TXT for details.
//
//===----------------------------------------------------------------------===//
//
// This file contains linker-synthesized sections. Currently,
// synthetic sections are created either output sections or input sections,
// but we are rewriting code so that all synthetic sections are created as
// input sections.
//
//===----------------------------------------------------------------------===//

#include "SyntheticSections.h"
#include "Config.h"
#include "Error.h"
#include "InputFiles.h"
#include "Memory.h"
#include "OutputSections.h"
#include "Strings.h"
#include "SymbolTable.h"

#include "lld/Core/Parallel.h"
#include "llvm/Support/Endian.h"
#include "llvm/Support/MD5.h"
#include "llvm/Support/RandomNumberGenerator.h"
#include "llvm/Support/SHA1.h"
#include "llvm/Support/xxhash.h"

using namespace llvm;
using namespace llvm::ELF;
using namespace llvm::object;
using namespace llvm::support;
using namespace llvm::support::endian;

using namespace lld;
using namespace lld::elf;

template <class ELFT> static std::vector<DefinedCommon *> getCommonSymbols() {
  std::vector<DefinedCommon *> V;
  for (Symbol *S : Symtab<ELFT>::X->getSymbols())
    if (auto *B = dyn_cast<DefinedCommon>(S->body()))
      V.push_back(B);
  return V;
}

// Find all common symbols and allocate space for them.
template <class ELFT>
CommonSection<ELFT>::CommonSection()
    : InputSection<ELFT>(SHF_ALLOC | SHF_WRITE, SHT_NOBITS, 1,
                         ArrayRef<uint8_t>(), "COMMON") {
  this->Live = true;

  // Sort the common symbols by alignment as an heuristic to pack them better.
  std::vector<DefinedCommon *> Syms = getCommonSymbols<ELFT>();
  std::stable_sort(Syms.begin(), Syms.end(),
                   [](const DefinedCommon *A, const DefinedCommon *B) {
                     return A->Alignment > B->Alignment;
                   });

  // Assign offsets to symbols.
  size_t Size = 0;
  size_t Alignment = 1;
  for (DefinedCommon *Sym : Syms) {
    Alignment = std::max<size_t>(Alignment, Sym->Alignment);
    Size = alignTo(Size, Sym->Alignment);

    // Compute symbol offset relative to beginning of input section.
    Sym->Offset = Size;
    Size += Sym->Size;
  }
  this->Alignment = Alignment;
  this->Data = makeArrayRef<uint8_t>(nullptr, Size);
}

static ArrayRef<uint8_t> createInterp() {
  // StringSaver guarantees that the returned string ends with '\0'.
  StringRef S = Saver.save(Config->DynamicLinker);
  return {(const uint8_t *)S.data(), S.size() + 1};
}

template <class ELFT>
InterpSection<ELFT>::InterpSection()
    : InputSection<ELFT>(SHF_ALLOC, SHT_PROGBITS, 1, createInterp(),
                         ".interp") {
  this->Live = true;
}

template <class ELFT>
BuildIdSection<ELFT>::BuildIdSection(size_t HashSize)
    : InputSection<ELFT>(SHF_ALLOC, SHT_NOTE, 1, ArrayRef<uint8_t>(),
                         ".note.gnu.build-id"),
      HashSize(HashSize) {
  this->Live = true;

  Buf.resize(16 + HashSize);
  const endianness E = ELFT::TargetEndianness;
  write32<E>(Buf.data(), 4);                   // Name size
  write32<E>(Buf.data() + 4, HashSize);        // Content size
  write32<E>(Buf.data() + 8, NT_GNU_BUILD_ID); // Type
  memcpy(Buf.data() + 12, "GNU", 4);           // Name string
  this->Data = ArrayRef<uint8_t>(Buf);
}

template <class ELFT>
uint8_t *BuildIdSection<ELFT>::getOutputLoc(uint8_t *Start) const {
  return Start + this->OutSec->getFileOffset() + this->OutSecOff;
}

static std::vector<ArrayRef<uint8_t>> split(ArrayRef<uint8_t> Arr,
                                            size_t ChunkSize) {
  std::vector<ArrayRef<uint8_t>> Ret;
  while (Arr.size() > ChunkSize) {
    Ret.push_back(Arr.take_front(ChunkSize));
    Arr = Arr.drop_front(ChunkSize);
  }
  if (!Arr.empty())
    Ret.push_back(Arr);
  return Ret;
}

template <class ELFT>
void BuildIdSection<ELFT>::computeHash(
    llvm::ArrayRef<uint8_t> Data,
    std::function<void(ArrayRef<uint8_t> Arr, uint8_t *Hash)> Hash) {
  std::vector<ArrayRef<uint8_t>> Chunks = split(Data, 1024 * 1024);
  std::vector<uint8_t> HashList(Chunks.size() * HashSize);

  if (Config->Threads)
    parallel_for_each(Chunks.begin(), Chunks.end(),
                      [&](ArrayRef<uint8_t> &Chunk) {
                        size_t Id = &Chunk - Chunks.data();
                        Hash(Chunk, HashList.data() + Id * HashSize);
                      });
  else
    std::for_each(Chunks.begin(), Chunks.end(), [&](ArrayRef<uint8_t> &Chunk) {
      size_t Id = &Chunk - Chunks.data();
      Hash(Chunk, HashList.data() + Id * HashSize);
    });

  Hash(HashList, this->getOutputLoc((uint8_t *)Data.begin()) + 16);
}

template <class ELFT>
void BuildIdFastHash<ELFT>::writeBuildId(MutableArrayRef<uint8_t> Buf) {
  computeHash(Buf, [](ArrayRef<uint8_t> Arr, uint8_t *Dest) {
    uint64_t Hash = xxHash64(toStringRef(Arr));
    write64<ELFT::TargetEndianness>(Dest, Hash);
  });
}

template <class ELFT>
void BuildIdMd5<ELFT>::writeBuildId(MutableArrayRef<uint8_t> Buf) {
  computeHash(Buf, [&](ArrayRef<uint8_t> Arr, uint8_t *Dest) {
    MD5 Hash;
    Hash.update(Arr);
    MD5::MD5Result Res;
    Hash.final(Res);
    memcpy(Dest, Res, HashSize);
  });
}

template <class ELFT>
void BuildIdSha1<ELFT>::writeBuildId(MutableArrayRef<uint8_t> Buf) {
  computeHash(Buf, [&](ArrayRef<uint8_t> Arr, uint8_t *Dest) {
    SHA1 Hash;
    Hash.update(Arr);
    memcpy(Dest, Hash.final().data(), HashSize);
  });
}

template <class ELFT>
void BuildIdUuid<ELFT>::writeBuildId(MutableArrayRef<uint8_t> Buf) {
  if (getRandomBytes(this->getOutputLoc(Buf.begin()) + 16, 16))
    error("entropy source failure");
}

template <class ELFT>
BuildIdHexstring<ELFT>::BuildIdHexstring()
    : BuildIdSection<ELFT>(Config->BuildIdVector.size()) {}

template <class ELFT>
void BuildIdHexstring<ELFT>::writeBuildId(MutableArrayRef<uint8_t> Buf) {
  memcpy(this->getOutputLoc(Buf.begin()) + 16, Config->BuildIdVector.data(),
         Config->BuildIdVector.size());
}

template class elf::CommonSection<ELF32LE>;
template class elf::CommonSection<ELF32BE>;
template class elf::CommonSection<ELF64LE>;
template class elf::CommonSection<ELF64BE>;

template class elf::InterpSection<ELF32LE>;
template class elf::InterpSection<ELF32BE>;
template class elf::InterpSection<ELF64LE>;
template class elf::InterpSection<ELF64BE>;

template class elf::BuildIdSection<ELF32LE>;
template class elf::BuildIdSection<ELF32BE>;
template class elf::BuildIdSection<ELF64LE>;
template class elf::BuildIdSection<ELF64BE>;

template class elf::BuildIdFastHash<ELF32LE>;
template class elf::BuildIdFastHash<ELF32BE>;
template class elf::BuildIdFastHash<ELF64LE>;
template class elf::BuildIdFastHash<ELF64BE>;

template class elf::BuildIdMd5<ELF32LE>;
template class elf::BuildIdMd5<ELF32BE>;
template class elf::BuildIdMd5<ELF64LE>;
template class elf::BuildIdMd5<ELF64BE>;

template class elf::BuildIdSha1<ELF32LE>;
template class elf::BuildIdSha1<ELF32BE>;
template class elf::BuildIdSha1<ELF64LE>;
template class elf::BuildIdSha1<ELF64BE>;

template class elf::BuildIdUuid<ELF32LE>;
template class elf::BuildIdUuid<ELF32BE>;
template class elf::BuildIdUuid<ELF64LE>;
template class elf::BuildIdUuid<ELF64BE>;

template class elf::BuildIdHexstring<ELF32LE>;
template class elf::BuildIdHexstring<ELF32BE>;
template class elf::BuildIdHexstring<ELF64LE>;
template class elf::BuildIdHexstring<ELF64BE>;
<commit_msg>[ELF] - Fix linux BB after r286061.<commit_after>//===- SyntheticSections.cpp ----------------------------------------------===//
//
//                             The LLVM Linker
//
// This file is distributed under the University of Illinois Open Source
// License. See LICENSE.TXT for details.
//
//===----------------------------------------------------------------------===//
//
// This file contains linker-synthesized sections. Currently,
// synthetic sections are created either output sections or input sections,
// but we are rewriting code so that all synthetic sections are created as
// input sections.
//
//===----------------------------------------------------------------------===//

#include "SyntheticSections.h"
#include "Config.h"
#include "Error.h"
#include "InputFiles.h"
#include "Memory.h"
#include "OutputSections.h"
#include "Strings.h"
#include "SymbolTable.h"

#include "lld/Core/Parallel.h"
#include "llvm/Support/Endian.h"
#include "llvm/Support/MD5.h"
#include "llvm/Support/RandomNumberGenerator.h"
#include "llvm/Support/SHA1.h"
#include "llvm/Support/xxhash.h"

using namespace llvm;
using namespace llvm::ELF;
using namespace llvm::object;
using namespace llvm::support;
using namespace llvm::support::endian;

using namespace lld;
using namespace lld::elf;

template <class ELFT> static std::vector<DefinedCommon *> getCommonSymbols() {
  std::vector<DefinedCommon *> V;
  for (Symbol *S : Symtab<ELFT>::X->getSymbols())
    if (auto *B = dyn_cast<DefinedCommon>(S->body()))
      V.push_back(B);
  return V;
}

// Find all common symbols and allocate space for them.
template <class ELFT>
CommonSection<ELFT>::CommonSection()
    : InputSection<ELFT>(SHF_ALLOC | SHF_WRITE, SHT_NOBITS, 1,
                         ArrayRef<uint8_t>(), "COMMON") {
  this->Live = true;

  // Sort the common symbols by alignment as an heuristic to pack them better.
  std::vector<DefinedCommon *> Syms = getCommonSymbols<ELFT>();
  std::stable_sort(Syms.begin(), Syms.end(),
                   [](const DefinedCommon *A, const DefinedCommon *B) {
                     return A->Alignment > B->Alignment;
                   });

  // Assign offsets to symbols.
  size_t Size = 0;
  size_t Alignment = 1;
  for (DefinedCommon *Sym : Syms) {
    Alignment = std::max<size_t>(Alignment, Sym->Alignment);
    Size = alignTo(Size, Sym->Alignment);

    // Compute symbol offset relative to beginning of input section.
    Sym->Offset = Size;
    Size += Sym->Size;
  }
  this->Alignment = Alignment;
  this->Data = makeArrayRef<uint8_t>(nullptr, Size);
}

static ArrayRef<uint8_t> createInterp() {
  // StringSaver guarantees that the returned string ends with '\0'.
  StringRef S = Saver.save(Config->DynamicLinker);
  return {(const uint8_t *)S.data(), S.size() + 1};
}

template <class ELFT>
InterpSection<ELFT>::InterpSection()
    : InputSection<ELFT>(SHF_ALLOC, SHT_PROGBITS, 1, createInterp(),
                         ".interp") {
  this->Live = true;
}

template <class ELFT>
BuildIdSection<ELFT>::BuildIdSection(size_t HashSize)
    : InputSection<ELFT>(SHF_ALLOC, SHT_NOTE, 1, ArrayRef<uint8_t>(),
                         ".note.gnu.build-id"),
      HashSize(HashSize) {
  this->Live = true;

  Buf.resize(16 + HashSize);
  const endianness E = ELFT::TargetEndianness;
  write32<E>(Buf.data(), 4);                   // Name size
  write32<E>(Buf.data() + 4, HashSize);        // Content size
  write32<E>(Buf.data() + 8, NT_GNU_BUILD_ID); // Type
  memcpy(Buf.data() + 12, "GNU", 4);           // Name string
  this->Data = ArrayRef<uint8_t>(Buf);
}

template <class ELFT>
uint8_t *BuildIdSection<ELFT>::getOutputLoc(uint8_t *Start) const {
  return Start + this->OutSec->getFileOffset() + this->OutSecOff;
}

static std::vector<ArrayRef<uint8_t>> split(ArrayRef<uint8_t> Arr,
                                            size_t ChunkSize) {
  std::vector<ArrayRef<uint8_t>> Ret;
  while (Arr.size() > ChunkSize) {
    Ret.push_back(Arr.take_front(ChunkSize));
    Arr = Arr.drop_front(ChunkSize);
  }
  if (!Arr.empty())
    Ret.push_back(Arr);
  return Ret;
}

template <class ELFT>
void BuildIdSection<ELFT>::computeHash(
    llvm::ArrayRef<uint8_t> Data,
    std::function<void(ArrayRef<uint8_t> Arr, uint8_t *Hash)> Hash) {
  std::vector<ArrayRef<uint8_t>> Chunks = split(Data, 1024 * 1024);
  std::vector<uint8_t> HashList(Chunks.size() * HashSize);

  if (Config->Threads)
    parallel_for_each(Chunks.begin(), Chunks.end(),
                      [&](ArrayRef<uint8_t> &Chunk) {
                        size_t Id = &Chunk - Chunks.data();
                        Hash(Chunk, HashList.data() + Id * HashSize);
                      });
  else
    std::for_each(Chunks.begin(), Chunks.end(), [&](ArrayRef<uint8_t> &Chunk) {
      size_t Id = &Chunk - Chunks.data();
      Hash(Chunk, HashList.data() + Id * HashSize);
    });

  Hash(HashList, this->getOutputLoc((uint8_t *)Data.begin()) + 16);
}

template <class ELFT>
void BuildIdFastHash<ELFT>::writeBuildId(MutableArrayRef<uint8_t> Buf) {
  this->computeHash(Buf, [](ArrayRef<uint8_t> Arr, uint8_t *Dest) {
    uint64_t Hash = xxHash64(toStringRef(Arr));
    write64<ELFT::TargetEndianness>(Dest, Hash);
  });
}

template <class ELFT>
void BuildIdMd5<ELFT>::writeBuildId(MutableArrayRef<uint8_t> Buf) {
  this->computeHash(Buf, [&](ArrayRef<uint8_t> Arr, uint8_t *Dest) {
    MD5 Hash;
    Hash.update(Arr);
    MD5::MD5Result Res;
    Hash.final(Res);
    memcpy(Dest, Res, this->HashSize);
  });
}

template <class ELFT>
void BuildIdSha1<ELFT>::writeBuildId(MutableArrayRef<uint8_t> Buf) {
  this->computeHash(Buf, [&](ArrayRef<uint8_t> Arr, uint8_t *Dest) {
    SHA1 Hash;
    Hash.update(Arr);
    memcpy(Dest, Hash.final().data(), this->HashSize);
  });
}

template <class ELFT>
void BuildIdUuid<ELFT>::writeBuildId(MutableArrayRef<uint8_t> Buf) {
  if (getRandomBytes(this->getOutputLoc(Buf.begin()) + 16, 16))
    error("entropy source failure");
}

template <class ELFT>
BuildIdHexstring<ELFT>::BuildIdHexstring()
    : BuildIdSection<ELFT>(Config->BuildIdVector.size()) {}

template <class ELFT>
void BuildIdHexstring<ELFT>::writeBuildId(MutableArrayRef<uint8_t> Buf) {
  memcpy(this->getOutputLoc(Buf.begin()) + 16, Config->BuildIdVector.data(),
         Config->BuildIdVector.size());
}

template class elf::CommonSection<ELF32LE>;
template class elf::CommonSection<ELF32BE>;
template class elf::CommonSection<ELF64LE>;
template class elf::CommonSection<ELF64BE>;

template class elf::InterpSection<ELF32LE>;
template class elf::InterpSection<ELF32BE>;
template class elf::InterpSection<ELF64LE>;
template class elf::InterpSection<ELF64BE>;

template class elf::BuildIdSection<ELF32LE>;
template class elf::BuildIdSection<ELF32BE>;
template class elf::BuildIdSection<ELF64LE>;
template class elf::BuildIdSection<ELF64BE>;

template class elf::BuildIdFastHash<ELF32LE>;
template class elf::BuildIdFastHash<ELF32BE>;
template class elf::BuildIdFastHash<ELF64LE>;
template class elf::BuildIdFastHash<ELF64BE>;

template class elf::BuildIdMd5<ELF32LE>;
template class elf::BuildIdMd5<ELF32BE>;
template class elf::BuildIdMd5<ELF64LE>;
template class elf::BuildIdMd5<ELF64BE>;

template class elf::BuildIdSha1<ELF32LE>;
template class elf::BuildIdSha1<ELF32BE>;
template class elf::BuildIdSha1<ELF64LE>;
template class elf::BuildIdSha1<ELF64BE>;

template class elf::BuildIdUuid<ELF32LE>;
template class elf::BuildIdUuid<ELF32BE>;
template class elf::BuildIdUuid<ELF64LE>;
template class elf::BuildIdUuid<ELF64BE>;

template class elf::BuildIdHexstring<ELF32LE>;
template class elf::BuildIdHexstring<ELF32BE>;
template class elf::BuildIdHexstring<ELF64LE>;
template class elf::BuildIdHexstring<ELF64BE>;
<|endoftext|>