<commit_before>/*=========================================================================
 *
 *  Copyright RTK 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 rtkRegularizedConjugateGradientConeBeamReconstructionFilter_hxx
#define rtkRegularizedConjugateGradientConeBeamReconstructionFilter_hxx

#include "rtkRegularizedConjugateGradientConeBeamReconstructionFilter.h"

namespace rtk
{

template< typename TImage >
RegularizedConjugateGradientConeBeamReconstructionFilter<TImage>::RegularizedConjugateGradientConeBeamReconstructionFilter()
{
  // Set the default values of member parameters
  m_GammaTV = 0.00005;
  m_Gamma = 0; // Laplacian regularization
  m_Tikhonov = 0; // Tikhonov regularization
  m_SoftThresholdWavelets = 0.001;
  m_SoftThresholdOnImage = 0.001;
  m_Preconditioned = false;
  m_RegularizedCG = false;

  m_TV_iterations=10;
  m_MainLoop_iterations=10;
  m_CG_iterations=4;

  // Default pipeline: CG, positivity, spatial TV
  m_PerformPositivity = true;
  m_PerformTVSpatialDenoising = true;
  m_PerformWaveletsSpatialDenoising = false;
  m_PerformSoftThresholdOnImage= false;

  // Dimensions processed for TV, default is all
  for (unsigned int i=0; i<TImage::ImageDimension; i++)
    m_DimensionsProcessedForTV[i]=true;

  // Other parameters
  m_CudaConjugateGradient = true; // 3D volumes of usual size fit on GPUs
  m_Order = 5;
  m_NumberOfLevels = 3;
  m_DisableDisplacedDetectorFilter = false;
  m_IterationCosts = false;

  // Create the filters
  m_CGFilter = CGFilterType::New();
  m_PositivityFilter = ThresholdFilterType::New();
  m_TVDenoising = TVDenoisingFilterType::New();
  m_WaveletsDenoising = WaveletsDenoisingFilterType::New();
  m_SoftThresholdFilter = SoftThresholdFilterType::New();
}

template< typename TImage >
void
RegularizedConjugateGradientConeBeamReconstructionFilter<TImage>
::SetInputVolume(const TImage* Volume)
{
  this->SetPrimaryInput(const_cast<TImage*>(Volume));
}

template< typename TImage >
void
RegularizedConjugateGradientConeBeamReconstructionFilter<TImage>
::SetInputProjectionStack(const TImage* Projection)
{
  this->SetInput("ProjectionStack", const_cast<TImage*>(Projection));
}

template< typename TImage >
void
RegularizedConjugateGradientConeBeamReconstructionFilter<TImage>
::SetInputWeights(const TImage* Weights)
{
  this->SetInput("Weights", const_cast<TImage*>(Weights));
}

template< typename TOutputImage>
void
RegularizedConjugateGradientConeBeamReconstructionFilter<TOutputImage>::
SetSupportMask(const TOutputImage *SupportMask)
{
  this->SetInput("SupportMask", const_cast<TOutputImage*>(SupportMask));
}

template< typename TImage >
typename TImage::ConstPointer
RegularizedConjugateGradientConeBeamReconstructionFilter<TImage>
::GetInputVolume()
{
  return static_cast< const TImage * >
          ( this->itk::ProcessObject::GetInput("Primary") );
}

template< typename TImage >
typename TImage::Pointer
RegularizedConjugateGradientConeBeamReconstructionFilter<TImage>
::GetInputProjectionStack()
{
  return static_cast< TImage * >
          ( this->itk::ProcessObject::GetInput("ProjectionStack") );
}

template< typename TImage >
typename TImage::Pointer
RegularizedConjugateGradientConeBeamReconstructionFilter<TImage>
::GetInputWeights()
{
  return static_cast< TImage * >
          ( this->itk::ProcessObject::GetInput("Weights") );
}

template< typename TOutputImage>
typename TOutputImage::ConstPointer
RegularizedConjugateGradientConeBeamReconstructionFilter<TOutputImage>::
GetSupportMask()
{
  return static_cast< const TOutputImage * >
          ( this->itk::ProcessObject::GetInput("SupportMask") );
}

template< typename TImage >
void
RegularizedConjugateGradientConeBeamReconstructionFilter<TImage>
::SetForwardProjectionFilter(ForwardProjectionType _arg)
{
  if( _arg != this->GetForwardProjectionFilter() )
    {
    Superclass::SetForwardProjectionFilter( _arg );
    m_CGFilter->SetForwardProjectionFilter( _arg );
    }
}

template< typename TImage >
void
RegularizedConjugateGradientConeBeamReconstructionFilter<TImage>
::SetBackProjectionFilter(BackProjectionType _arg)
{
  if( _arg != this->GetBackProjectionFilter() )
    {
    Superclass::SetBackProjectionFilter( _arg );
    m_CGFilter->SetBackProjectionFilter( _arg );
    }
}

template< typename TImage >
void
RegularizedConjugateGradientConeBeamReconstructionFilter<TImage>
::GenerateInputRequestedRegion()
{
  //Call the superclass' implementation of this method
  Superclass::GenerateInputRequestedRegion();

  // Let the CG subfilters compute the requested regions for the projections
  // stack and the input volume
  m_CGFilter->PropagateRequestedRegion(m_CGFilter->GetOutput());
}

template< typename TImage >
void
RegularizedConjugateGradientConeBeamReconstructionFilter<TImage>
::GenerateOutputInformation()
{
  // Construct the pipeline, adding regularization filters if the user wants them
  // Connect the last filter's output to the next filter's input using the currentDownstreamFilter pointer
  typename itk::ImageToImageFilter<TImage, TImage>::Pointer currentDownstreamFilter;

  // The conjugate gradient filter is the only part that must be in the pipeline
  // whatever was the user wants
  m_CGFilter->SetInput(0, this->GetInputVolume());
  m_CGFilter->SetInput(1, this->GetInputProjectionStack());
  m_CGFilter->SetInput(2, this->GetInputWeights());
  m_CGFilter->SetSupportMask(this->GetSupportMask());
  m_CGFilter->SetGeometry(this->m_Geometry);
  m_CGFilter->SetNumberOfIterations(this->m_CG_iterations);
  m_CGFilter->SetCudaConjugateGradient(this->GetCudaConjugateGradient());
  m_CGFilter->SetGamma(this->m_Gamma);
  m_CGFilter->SetTikhonov(this->m_Tikhonov);
//  m_CGFilter->SetIterationCosts(m_IterationCosts);
  m_CGFilter->SetDisableDisplacedDetectorFilter(m_DisableDisplacedDetectorFilter);

  currentDownstreamFilter = m_CGFilter;

  // Plug the positivity filter if requested
  if (m_PerformPositivity)
    {
    m_PositivityFilter->SetInPlace(true);

    m_PositivityFilter->SetOutsideValue(0.0);
    m_PositivityFilter->ThresholdBelow(0.0);
    m_PositivityFilter->SetInput(currentDownstreamFilter->GetOutput());

    currentDownstreamFilter = m_PositivityFilter;
    }

  if (m_PerformTVSpatialDenoising)
    {
    currentDownstreamFilter->ReleaseDataFlagOn();

    m_TVDenoising->SetInput(currentDownstreamFilter->GetOutput());
    m_TVDenoising->SetNumberOfIterations(this->m_TV_iterations);
    m_TVDenoising->SetGamma(this->m_GammaTV);
    m_TVDenoising->SetDimensionsProcessed(this->m_DimensionsProcessedForTV);

    currentDownstreamFilter = m_TVDenoising;
    }

  if (m_PerformWaveletsSpatialDenoising)
    {
    currentDownstreamFilter->ReleaseDataFlagOn();

    m_WaveletsDenoising->SetInput(currentDownstreamFilter->GetOutput());
    m_WaveletsDenoising->SetOrder(m_Order);
    m_WaveletsDenoising->SetThreshold(m_SoftThresholdWavelets);
    m_WaveletsDenoising->SetNumberOfLevels(m_NumberOfLevels);

    currentDownstreamFilter = m_WaveletsDenoising;
    }

  if (m_PerformSoftThresholdOnImage)
    {
    currentDownstreamFilter->ReleaseDataFlagOn();

    m_SoftThresholdFilter->SetInput(currentDownstreamFilter->GetOutput());
    m_SoftThresholdFilter->SetThreshold(m_SoftThresholdOnImage);

    currentDownstreamFilter = m_SoftThresholdFilter;
    }

  // Have the last filter calculate its output information
  currentDownstreamFilter->ReleaseDataFlagOff();
  currentDownstreamFilter->UpdateOutputInformation();

  // Copy it as the output information of the composite filter
  this->GetOutput()->CopyInformation( currentDownstreamFilter->GetOutput() );
}

template< typename TImage >
void
RegularizedConjugateGradientConeBeamReconstructionFilter<TImage>
::GenerateData()
{
  // Declare the pointer that will be used to plug the output back as input
  typename itk::ImageToImageFilter<TImage, TImage>::Pointer currentDownstreamFilter;
  typename TImage::Pointer pimg;

  for (int i=0; i<m_MainLoop_iterations; i++)
    {
    // After the first iteration, we need to use the output as input
    if (i>0)
      {
      pimg = currentDownstreamFilter->GetOutput();

      pimg->DisconnectPipeline();
      m_CGFilter->SetInput(0, pimg);

      // The input volume is no longer needed on the GPU, so we transfer it back to the CPU
      this->GetInputVolume()->GetBufferPointer();
      }

    currentDownstreamFilter = m_CGFilter;
    if (m_PerformPositivity)
      {
      currentDownstreamFilter = m_PositivityFilter;
      }
    if (m_PerformTVSpatialDenoising)
      {
      currentDownstreamFilter = m_TVDenoising;
      }
    if (m_PerformWaveletsSpatialDenoising)
      {
      currentDownstreamFilter = m_WaveletsDenoising;
      }
    if (m_PerformSoftThresholdOnImage)
      {
      currentDownstreamFilter = m_SoftThresholdFilter;
      }

    currentDownstreamFilter->Update();
    }

  this->GraftOutput( currentDownstreamFilter->GetOutput() );
}

}// end namespace

#endif
<commit_msg>Removed some releaseDataFlag which caused RegularizedConjugateGradientConeBeamReconstructionFilter to update the rtkConjugateGradientImageFilter twice<commit_after>/*=========================================================================
 *
 *  Copyright RTK 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 rtkRegularizedConjugateGradientConeBeamReconstructionFilter_hxx
#define rtkRegularizedConjugateGradientConeBeamReconstructionFilter_hxx

#include "rtkRegularizedConjugateGradientConeBeamReconstructionFilter.h"

namespace rtk
{

template< typename TImage >
RegularizedConjugateGradientConeBeamReconstructionFilter<TImage>::RegularizedConjugateGradientConeBeamReconstructionFilter()
{
  // Set the default values of member parameters
  m_GammaTV = 0.00005;
  m_Gamma = 0; // Laplacian regularization
  m_Tikhonov = 0; // Tikhonov regularization
  m_SoftThresholdWavelets = 0.001;
  m_SoftThresholdOnImage = 0.001;
  m_Preconditioned = false;
  m_RegularizedCG = false;

  m_TV_iterations=10;
  m_MainLoop_iterations=10;
  m_CG_iterations=4;

  // Default pipeline: CG, positivity, spatial TV
  m_PerformPositivity = true;
  m_PerformTVSpatialDenoising = true;
  m_PerformWaveletsSpatialDenoising = false;
  m_PerformSoftThresholdOnImage= false;

  // Dimensions processed for TV, default is all
  for (unsigned int i=0; i<TImage::ImageDimension; i++)
    m_DimensionsProcessedForTV[i]=true;

  // Other parameters
  m_CudaConjugateGradient = true; // 3D volumes of usual size fit on GPUs
  m_Order = 5;
  m_NumberOfLevels = 3;
  m_DisableDisplacedDetectorFilter = false;
  m_IterationCosts = false;

  // Create the filters
  m_CGFilter = CGFilterType::New();
  m_PositivityFilter = ThresholdFilterType::New();
  m_TVDenoising = TVDenoisingFilterType::New();
  m_WaveletsDenoising = WaveletsDenoisingFilterType::New();
  m_SoftThresholdFilter = SoftThresholdFilterType::New();
}

template< typename TImage >
void
RegularizedConjugateGradientConeBeamReconstructionFilter<TImage>
::SetInputVolume(const TImage* Volume)
{
  this->SetPrimaryInput(const_cast<TImage*>(Volume));
}

template< typename TImage >
void
RegularizedConjugateGradientConeBeamReconstructionFilter<TImage>
::SetInputProjectionStack(const TImage* Projection)
{
  this->SetInput("ProjectionStack", const_cast<TImage*>(Projection));
}

template< typename TImage >
void
RegularizedConjugateGradientConeBeamReconstructionFilter<TImage>
::SetInputWeights(const TImage* Weights)
{
  this->SetInput("Weights", const_cast<TImage*>(Weights));
}

template< typename TOutputImage>
void
RegularizedConjugateGradientConeBeamReconstructionFilter<TOutputImage>::
SetSupportMask(const TOutputImage *SupportMask)
{
  this->SetInput("SupportMask", const_cast<TOutputImage*>(SupportMask));
}

template< typename TImage >
typename TImage::ConstPointer
RegularizedConjugateGradientConeBeamReconstructionFilter<TImage>
::GetInputVolume()
{
  return static_cast< const TImage * >
          ( this->itk::ProcessObject::GetInput("Primary") );
}

template< typename TImage >
typename TImage::Pointer
RegularizedConjugateGradientConeBeamReconstructionFilter<TImage>
::GetInputProjectionStack()
{
  return static_cast< TImage * >
          ( this->itk::ProcessObject::GetInput("ProjectionStack") );
}

template< typename TImage >
typename TImage::Pointer
RegularizedConjugateGradientConeBeamReconstructionFilter<TImage>
::GetInputWeights()
{
  return static_cast< TImage * >
          ( this->itk::ProcessObject::GetInput("Weights") );
}

template< typename TOutputImage>
typename TOutputImage::ConstPointer
RegularizedConjugateGradientConeBeamReconstructionFilter<TOutputImage>::
GetSupportMask()
{
  return static_cast< const TOutputImage * >
          ( this->itk::ProcessObject::GetInput("SupportMask") );
}

template< typename TImage >
void
RegularizedConjugateGradientConeBeamReconstructionFilter<TImage>
::SetForwardProjectionFilter(ForwardProjectionType _arg)
{
  if( _arg != this->GetForwardProjectionFilter() )
    {
    Superclass::SetForwardProjectionFilter( _arg );
    m_CGFilter->SetForwardProjectionFilter( _arg );
    }
}

template< typename TImage >
void
RegularizedConjugateGradientConeBeamReconstructionFilter<TImage>
::SetBackProjectionFilter(BackProjectionType _arg)
{
  if( _arg != this->GetBackProjectionFilter() )
    {
    Superclass::SetBackProjectionFilter( _arg );
    m_CGFilter->SetBackProjectionFilter( _arg );
    }
}

template< typename TImage >
void
RegularizedConjugateGradientConeBeamReconstructionFilter<TImage>
::GenerateInputRequestedRegion()
{
  //Call the superclass' implementation of this method
  Superclass::GenerateInputRequestedRegion();

  // Let the CG subfilters compute the requested regions for the projections
  // stack and the input volume
  m_CGFilter->PropagateRequestedRegion(m_CGFilter->GetOutput());
}

template< typename TImage >
void
RegularizedConjugateGradientConeBeamReconstructionFilter<TImage>
::GenerateOutputInformation()
{
  // Construct the pipeline, adding regularization filters if the user wants them
  // Connect the last filter's output to the next filter's input using the currentDownstreamFilter pointer
  typename itk::ImageToImageFilter<TImage, TImage>::Pointer currentDownstreamFilter;

  // The conjugate gradient filter is the only part that must be in the pipeline
  // whatever was the user wants
  m_CGFilter->SetInput(0, this->GetInputVolume());
  m_CGFilter->SetInput(1, this->GetInputProjectionStack());
  m_CGFilter->SetInput(2, this->GetInputWeights());
  m_CGFilter->SetSupportMask(this->GetSupportMask());
  m_CGFilter->SetGeometry(this->m_Geometry);
  m_CGFilter->SetNumberOfIterations(this->m_CG_iterations);
  m_CGFilter->SetCudaConjugateGradient(this->GetCudaConjugateGradient());
  m_CGFilter->SetGamma(this->m_Gamma);
  m_CGFilter->SetTikhonov(this->m_Tikhonov);
//  m_CGFilter->SetIterationCosts(m_IterationCosts);
  m_CGFilter->SetDisableDisplacedDetectorFilter(m_DisableDisplacedDetectorFilter);

  currentDownstreamFilter = m_CGFilter;

  // Plug the positivity filter if requested
  if (m_PerformPositivity)
    {
    m_PositivityFilter->SetInPlace(false);

    m_PositivityFilter->SetOutsideValue(0.0);
    m_PositivityFilter->ThresholdBelow(0.0);
    m_PositivityFilter->SetInput(currentDownstreamFilter->GetOutput());

    currentDownstreamFilter = m_PositivityFilter;
    }

  if (m_PerformTVSpatialDenoising)
    {
    currentDownstreamFilter->ReleaseDataFlagOn();

    m_TVDenoising->SetInput(currentDownstreamFilter->GetOutput());
    m_TVDenoising->SetNumberOfIterations(this->m_TV_iterations);
    m_TVDenoising->SetGamma(this->m_GammaTV);
    m_TVDenoising->SetDimensionsProcessed(this->m_DimensionsProcessedForTV);

    currentDownstreamFilter = m_TVDenoising;
    }

  if (m_PerformWaveletsSpatialDenoising)
    {
    m_WaveletsDenoising->SetInput(currentDownstreamFilter->GetOutput());
    m_WaveletsDenoising->SetOrder(m_Order);
    m_WaveletsDenoising->SetThreshold(m_SoftThresholdWavelets);
    m_WaveletsDenoising->SetNumberOfLevels(m_NumberOfLevels);

    currentDownstreamFilter = m_WaveletsDenoising;
    }

  if (m_PerformSoftThresholdOnImage)
    {
    currentDownstreamFilter->ReleaseDataFlagOn();

    m_SoftThresholdFilter->SetInput(currentDownstreamFilter->GetOutput());
    m_SoftThresholdFilter->SetThreshold(m_SoftThresholdOnImage);

    currentDownstreamFilter = m_SoftThresholdFilter;
    }

  // Have the last filter calculate its output information
  currentDownstreamFilter->ReleaseDataFlagOff();
  currentDownstreamFilter->UpdateOutputInformation();

  // Copy it as the output information of the composite filter
  this->GetOutput()->CopyInformation( currentDownstreamFilter->GetOutput() );
}

template< typename TImage >
void
RegularizedConjugateGradientConeBeamReconstructionFilter<TImage>
::GenerateData()
{
  // Declare the pointer that will be used to plug the output back as input
  typename itk::ImageToImageFilter<TImage, TImage>::Pointer currentDownstreamFilter;
  typename TImage::Pointer pimg;

  for (int i=0; i<m_MainLoop_iterations; i++)
    {
    // After the first iteration, we need to use the output as input
    if (i>0)
      {
      pimg = currentDownstreamFilter->GetOutput();

      pimg->DisconnectPipeline();
      m_CGFilter->SetInput(0, pimg);

      // The input volume is no longer needed on the GPU, so we transfer it back to the CPU
      this->GetInputVolume()->GetBufferPointer();
      }

    currentDownstreamFilter = m_CGFilter;
    if (m_PerformPositivity)
      {
      currentDownstreamFilter = m_PositivityFilter;
      }
    if (m_PerformTVSpatialDenoising)
      {
      currentDownstreamFilter = m_TVDenoising;
      }
    if (m_PerformWaveletsSpatialDenoising)
      {
      currentDownstreamFilter = m_WaveletsDenoising;
      }
    if (m_PerformSoftThresholdOnImage)
      {
      currentDownstreamFilter = m_SoftThresholdFilter;
      }

    currentDownstreamFilter->Update();
    }

  this->GraftOutput( currentDownstreamFilter->GetOutput() );
}

}// end namespace

#endif
<|endoftext|>


<commit_before>/**********************************************************************
 *
 * GEOS - Geometry Engine Open Source
 * http://geos.osgeo.org
 *
 * Copyright (C) 2001-2002 Vivid Solutions Inc.
 * Copyright (C) 2005 Refractions Research Inc.
 *
 * This is free software; you can redistribute and/or modify it under
 * the terms of the GNU Lesser General Public Licence as published
 * by the Free Software Foundation.
 * See the COPYING file for more information.
 *
 **********************************************************************
 *
 * Last port: geomgraph/TopologyLocation.java r428 (JTS-1.12+)
 *
 **********************************************************************/

#include <geos/geomgraph/TopologyLocation.h>
#include <geos/geom/Position.h>
#include <geos/geom/Location.h>

#include <vector>
#include <sstream>
#include <iostream>
#include <cassert>

#ifndef GEOS_INLINE
# include <geos/geomgraph/TopologyLocation.inl>
#endif

using namespace geos::geom;

namespace geos {
namespace geomgraph { // geos.geomgraph

/*public*/
void
TopologyLocation::merge(const TopologyLocation& gl)
{
    // if the src is an Area label & and the dest is not, increase the dest to be an Area
    std::size_t sz = locationSize;
    std::size_t glsz = gl.locationSize;
    if(glsz > sz) {
        locationSize = 3;
        location[Position::LEFT] = Location::NONE;
        location[Position::RIGHT] = Location::NONE;
    }
    for(std::size_t i = 0; i < locationSize; ++i) {
        if(location[i] == Location::NONE && i < glsz) {
            location[i] = gl.location[i];
        }
    }
}

std::string
TopologyLocation::toString() const
{
    std::stringstream ss;
    ss << *this;
    return ss.str();
}

std::ostream&
operator<< (std::ostream& os, const TopologyLocation& tl)
{
    if(tl.locationSize > 1) {
        os << tl.location[Position::LEFT];
    }
    os << tl.location[Position::ON];
    if(tl.locationSize > 1) {
        os << tl.location[Position::RIGHT];
    }
    return os;
}

} // namespace geos.geomgraph
} // namespace geos

<commit_msg>Avoid useless comparison against glsz inside loop<commit_after>/**********************************************************************
 *
 * GEOS - Geometry Engine Open Source
 * http://geos.osgeo.org
 *
 * Copyright (C) 2001-2002 Vivid Solutions Inc.
 * Copyright (C) 2005 Refractions Research Inc.
 *
 * This is free software; you can redistribute and/or modify it under
 * the terms of the GNU Lesser General Public Licence as published
 * by the Free Software Foundation.
 * See the COPYING file for more information.
 *
 **********************************************************************
 *
 * Last port: geomgraph/TopologyLocation.java r428 (JTS-1.12+)
 *
 **********************************************************************/

#include <geos/geomgraph/TopologyLocation.h>
#include <geos/geom/Position.h>
#include <geos/geom/Location.h>

#include <algorithm>
#include <vector>
#include <sstream>
#include <iostream>
#include <cassert>

#ifndef GEOS_INLINE
# include <geos/geomgraph/TopologyLocation.inl>
#endif

using namespace geos::geom;

namespace geos {
namespace geomgraph { // geos.geomgraph

/*public*/
void
TopologyLocation::merge(const TopologyLocation& gl)
{
    // if the src is an Area label & and the dest is not, increase the dest to be an Area
    std::size_t sz = locationSize;
    std::size_t glsz = gl.locationSize;
    if(glsz > sz) {
        locationSize = 3;
        location[Position::LEFT] = Location::NONE;
        location[Position::RIGHT] = Location::NONE;
    }
    const std::size_t maxIndex = std::min(static_cast<std::size_t>(locationSize), glsz);
    for(std::size_t i = 0; i < maxIndex; ++i) {
        if(location[i] == Location::NONE) {
            location[i] = gl.location[i];
        }
    }
}

std::string
TopologyLocation::toString() const
{
    std::stringstream ss;
    ss << *this;
    return ss.str();
}

std::ostream&
operator<< (std::ostream& os, const TopologyLocation& tl)
{
    if(tl.locationSize > 1) {
        os << tl.location[Position::LEFT];
    }
    os << tl.location[Position::ON];
    if(tl.locationSize > 1) {
        os << tl.location[Position::RIGHT];
    }
    return os;
}

} // namespace geos.geomgraph
} // namespace geos

<|endoftext|>


<commit_before>// -*- mode:C++; tab-width:8; c-basic-offset:2; indent-tabs-mode:t -*-
// vim: ts=8 sw=2 smarttab
/*
 * Ceph - scalable distributed file system
 *
 * Copyright (C) 2004-2006 Sage Weil <sage@newdream.net>
 *
 * This is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License version 2.1, as published by the Free Software
 * Foundation.  See file COPYING.
 *
 */

#include "include/librados.h"
#include "config.h"
#include "common/common_init.h"

#include <iostream>

#include <stdlib.h>
#include <time.h>

void usage()
{
  cerr << "usage: radostool [options] [commands]" << std::endl;
  /*  cerr << "If no commands are specified, enter interactive mode.\n";
  cerr << "Commands:" << std::endl;
  cerr << "   stop              -- cleanly shut down file system" << std::endl
       << "   (osd|pg|mds) stat -- get monitor subsystem status" << std::endl
       << "   ..." << std::endl;
  */
  cerr << "Options:" << std::endl;
  cerr << "   -i infile\n";
  cerr << "   -o outfile\n";
  cerr << "        specify input or output file (for certain commands)\n";
  generic_client_usage();
}

int main(int argc, const char **argv)
{
  DEFINE_CONF_VARS(usage);
  vector<const char*> args;
  argv_to_vec(argc, argv, args);
  env_to_vec(args);
  common_init(args, "rados", false);

  vector<const char*> nargs;
  bufferlist indata, outdata;
  const char *outfile = 0;

  const char *pool = 0;

  FOR_EACH_ARG(args) {
    if (CONF_ARG_EQ("out_file", 'o')) {
      CONF_SAFE_SET_ARG_VAL(&outfile, OPT_STR);
    } else if (CONF_ARG_EQ("in_data", 'i')) {
      const char *fname;
      CONF_SAFE_SET_ARG_VAL(&fname, OPT_STR);
      int r = indata.read_file(fname);
      if (r < 0) {
	cerr << "error reading " << fname << ": " << strerror(-r) << std::endl;
	exit(0);
      } else {
	cout << "read " << indata.length() << " bytes from " << fname << std::endl;
      }
    } else if (CONF_ARG_EQ("pool", 'p')) {
      CONF_SAFE_SET_ARG_VAL(&pool, OPT_STR);
    } else if (CONF_ARG_EQ("help", 'h')) {
      usage();
    } else if (args[i][0] == '-' && nargs.empty()) {
      cerr << "unrecognized option " << args[i] << std::endl;
      usage();
    } else
      nargs.push_back(args[i]);
  }

  if (nargs.empty())
    usage();

  // open rados
  Rados rados;
  if (rados.initialize(0, NULL) < 0) {
     cerr << "couldn't initialize rados!" << std::endl;
     exit(1);
  }

  // open pool?
  rados_pool_t p;
  if (pool) {
    int r = rados.open_pool(pool, &p);
    if (r < 0) {
      cerr << "error opening pool " << pool << ": " << strerror(-r) << std::endl;
      exit(0);
    }
  }

  // list pools?
  if (strcmp(nargs[0], "lspools") == 0) {
    vector<string> vec;
    rados.list_pools(vec);
    for (vector<string>::iterator i = vec.begin(); i != vec.end(); ++i)
      cout << *i << std::endl;

  } else if (strcmp(nargs[0], "ls") == 0) {
    if (!pool)
      usage();

    Rados::ListCtx ctx;
    while (1) {
      list<object_t> vec;
      int r = rados.list(p, 1 << 10, vec, ctx);
      cout << "list result=" << r << " entries=" << vec.size() << std::endl;
      if (r < 0) {
	cerr << "got error: " << strerror(-r) << std::endl;
	break;
      }
      if (vec.empty())
	break;
      for (list<object_t>::iterator iter = vec.begin(); iter != vec.end(); ++iter)
	cout << *iter << std::endl;
    }

  } else if (strcmp(nargs[0], "get") == 0) {
    if (!pool || nargs.size() < 2)
      usage();
    object_t oid(nargs[1]);
    int r = rados.read(p, oid, 0, outdata, 0);
    if (r < 0) {
      cerr << "error reading " << oid << " from pool " << pool << ": " << strerror(-r) << std::endl;
      exit(0);
    }

  } else if (strcmp(nargs[0], "put") == 0) {
    if (!pool || nargs.size() < 2)
      usage();
    if (!indata.length()) {
      cerr << "must specify input file" << std::endl;
      usage();
    }
    object_t oid(nargs[1]);
    int r = rados.write(p, oid, 0, indata, indata.length());
    if (r < 0) {
      cerr << "error writing " << oid << " to pool " << pool << ": " << strerror(-r) << std::endl;
      exit(0);
    }

  } else {
    cerr << "unrecognized command " << nargs[0] << std::endl;
    usage();
  }

  // write data?
  int len = outdata.length();
  if (len) {
    if (outfile) {
      if (strcmp(outfile, "-") == 0) {
	::write(1, outdata.c_str(), len);
      } else {
	outdata.write_file(outfile);
      }
      generic_dout(0) << "wrote " << len << " byte payload to " << outfile << dendl;
    } else {
      generic_dout(0) << "got " << len << " byte payload, discarding (specify -o <outfile)" << dendl;
    }
  }

  if (pool)
    rados.close_pool(p);

  rados_deinitialize();
  return 0;
}
<commit_msg>radostool now includes benchmarking functionality from testradosciopp.cc<commit_after>// -*- mode:C++; tab-width:8; c-basic-offset:2; indent-tabs-mode:t -*-
// vim: ts=8 sw=2 smarttab
/*
 * Ceph - scalable distributed file system
 *
 * Copyright (C) 2004-2006 Sage Weil <sage@newdream.net>
 *
 * This is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License version 2.1, as published by the Free Software
 * Foundation.  See file COPYING.
 *
 */

#include "include/librados.h"
#include "config.h"
#include "common/common_init.h"

#include <iostream>

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

int aio_bench(Rados& rados, rados_pool_t pool, int concurrentios, int secondsToRun,
	       int writeSize, int readOffResults);

void usage()
{
  cerr << "usage: radostool [options] [commands]" << std::endl;
  /*  cerr << "If no commands are specified, enter interactive mode.\n";
  cerr << "Commands:" << std::endl;
  cerr << "   stop              -- cleanly shut down file system" << std::endl
       << "   (osd|pg|mds) stat -- get monitor subsystem status" << std::endl
       << "   ..." << std::endl;
  */
  cerr << "Options:" << std::endl;
  cerr << "   -i infile\n";
  cerr << "   -o outfile\n";
  cerr << "        specify input or output file (for certain commands)\n";
  generic_client_usage();
}

int main(int argc, const char **argv)
{
  DEFINE_CONF_VARS(usage);
  vector<const char*> args;
  argv_to_vec(argc, argv, args);
  env_to_vec(args);
  common_init(args, "rados", false);

  vector<const char*> nargs;
  bufferlist indata, outdata;
  const char *outfile = 0;

  const char *pool = 0;

  FOR_EACH_ARG(args) {
    if (CONF_ARG_EQ("out_file", 'o')) {
      CONF_SAFE_SET_ARG_VAL(&outfile, OPT_STR);
    } else if (CONF_ARG_EQ("in_data", 'i')) {
      const char *fname;
      CONF_SAFE_SET_ARG_VAL(&fname, OPT_STR);
      int r = indata.read_file(fname);
      if (r < 0) {
	cerr << "error reading " << fname << ": " << strerror(-r) << std::endl;
	exit(0);
      } else {
	cout << "read " << indata.length() << " bytes from " << fname << std::endl;
      }
    } else if (CONF_ARG_EQ("pool", 'p')) {
      CONF_SAFE_SET_ARG_VAL(&pool, OPT_STR);
    } else if (CONF_ARG_EQ("help", 'h')) {
      usage();
    } else if (args[i][0] == '-' && nargs.empty()) {
      cerr << "unrecognized option " << args[i] << std::endl;
      usage();
    } else
      nargs.push_back(args[i]);
  }

  if (nargs.empty())
    usage();

  // open rados
  Rados rados;
  if (rados.initialize(0, NULL) < 0) {
     cerr << "couldn't initialize rados!" << std::endl;
     exit(1);
  }

  // open pool?
  rados_pool_t p;
  if (pool) {
    int r = rados.open_pool(pool, &p);
    if (r < 0) {
      cerr << "error opening pool " << pool << ": " << strerror(-r) << std::endl;
      exit(0);
    }
  }

  // list pools?
  if (strcmp(nargs[0], "lspools") == 0) {
    vector<string> vec;
    rados.list_pools(vec);
    for (vector<string>::iterator i = vec.begin(); i != vec.end(); ++i)
      cout << *i << std::endl;

  } else if (strcmp(nargs[0], "ls") == 0) {
    if (!pool)
      usage();

    Rados::ListCtx ctx;
    while (1) {
      list<object_t> vec;
      int r = rados.list(p, 1 << 10, vec, ctx);
      cout << "list result=" << r << " entries=" << vec.size() << std::endl;
      if (r < 0) {
	cerr << "got error: " << strerror(-r) << std::endl;
	break;
      }
      if (vec.empty())
	break;
      for (list<object_t>::iterator iter = vec.begin(); iter != vec.end(); ++iter)
	cout << *iter << std::endl;
    }

  } else if (strcmp(nargs[0], "get") == 0) {
    if (!pool || nargs.size() < 2)
      usage();
    object_t oid(nargs[1]);
    int r = rados.read(p, oid, 0, outdata, 0);
    if (r < 0) {
      cerr << "error reading " << oid << " from pool " << pool << ": " << strerror(-r) << std::endl;
      exit(0);
    }

  } else if (strcmp(nargs[0], "put") == 0) {
    if (!pool || nargs.size() < 2)
      usage();
    if (!indata.length()) {
      cerr << "must specify input file" << std::endl;
      usage();
    }
    object_t oid(nargs[1]);
    int r = rados.write(p, oid, 0, indata, indata.length());
    if (r < 0) {
      cerr << "error writing " << oid << " to pool " << pool << ": " << strerror(-r) << std::endl;
      exit(0);
    }

  } else if (strcmp(nargs[0], "bench") == 0) {
    if ( nargs.size() < 5) {
      cerr << "bench requires 4 arguments: Concurrent writes," << std::endl
	   << "minimum seconds to run, write size, and consistency checking" << std::endl
	   << "(1 for yes, 0 for no)." << std::endl;
    }
    else {
    aio_bench(rados, p, atoi(nargs[1]), atoi(nargs[2]), atoi(nargs[3]), atoi(nargs[4]));
    }
  }
  else {
    cerr << "unrecognized command " << nargs[0] << std::endl;
    usage();
  }

  // write data?
  int len = outdata.length();
  if (len) {
    if (outfile) {
      if (strcmp(outfile, "-") == 0) {
	::write(1, outdata.c_str(), len);
      } else {
	outdata.write_file(outfile);
      }
      generic_dout(0) << "wrote " << len << " byte payload to " << outfile << dendl;
    } else {
      generic_dout(0) << "got " << len << " byte payload, discarding (specify -o <outfile)" << dendl;
    }
  }

  if (pool)
    rados.close_pool(p);

  rados_deinitialize();
  return 0;
}

/**********************************************

**********************************************/

int aio_bench (Rados& rados, rados_pool_t pool, int concurrentios, int secondsToRun,
  int writeSize, int readOffResults) {

  cout << "Maintaining " << concurrentios << " concurrent writes of " << writeSize
       << " bytes for at least " << secondsToRun << " seconds.\n";

  Rados::AioCompletion* completions[concurrentios];
  char* name[concurrentios];
  bufferlist* contents[concurrentios];
  char contentsChars[writeSize];
  double totalLatency = 0;
  utime_t maxLatency;
  utime_t startTimes[concurrentios];
  int writesMade = 0;
  int writesCompleted = 0;
  time_t initialTime;
  utime_t startTime;
  utime_t stopTime;

  time(&initialTime);
  stringstream initialTimeS("");
  initialTimeS << initialTime;
  const char* iTime = initialTimeS.str().c_str();
  maxLatency.set_from_double(0);
  //set up writes so I can start them together
  for (int i = 0; i<concurrentios; ++i) {
    name[i] = new char[128];
    contents[i] = new bufferlist();
    snprintf(name[i], 128, "Object %s:%d", iTime, i);
    snprintf(contentsChars, writeSize, "I'm the %dth object!", i);
    contents[i]->append(contentsChars, writeSize);
  }

  //set up the pool, get start time, and go!
  cout << "open pool result = " << rados.open_pool("data",&pool) << " pool = " << pool << std::endl;

  startTime = g_clock.now();

  for (int i = 0; i<concurrentios; ++i) {
    startTimes[i] = g_clock.now();
    rados.aio_write(pool, name[i], 0, *contents[i], writeSize, &completions[i]);
    ++writesMade;
  }
  cerr << "Finished writing first objects\n";

  //keep on adding new writes as old ones complete until we've passed minimum time
  int slot;
  bufferlist* newContents;
  char* newName;
  utime_t currentLatency;
  utime_t runtime;
  runtime.set_from_double(secondsToRun);
  stopTime = startTime + runtime;
  while( g_clock.now() < stopTime ) {
    slot = writesCompleted % concurrentios;
    //create new contents and name on the heap, and fill them
    newContents = new bufferlist();
    newName = new char[128];
    snprintf(newName, 128, "Object %s:%d", iTime, writesMade);
    snprintf(contentsChars, writeSize, "I'm the %dth object!", writesMade);
    newContents->append(contentsChars, writeSize);
    completions[slot]->wait_for_complete();
    currentLatency = g_clock.now() - startTimes[slot];
    totalLatency += currentLatency;
    if( currentLatency > maxLatency) maxLatency = currentLatency;
    ++writesCompleted;
    completions[slot]->release();
    //write new stuff to rados, then delete old stuff
    //and save locations of new stuff for later deletion
    startTimes[slot] = g_clock.now();
    rados.aio_write(pool, newName, 0, *newContents, writeSize, &completions[slot]);
    ++writesMade;
    delete name[slot];
    delete contents[slot];
    name[slot] = newName;
    contents[slot] = newContents;
  }

  cerr << "Waiting for last writes to finish\n";
  while (writesCompleted < writesMade) {
    slot = writesCompleted % concurrentios;
    completions[slot]->wait_for_complete();
    currentLatency = g_clock.now() - startTimes[slot];
    totalLatency += currentLatency;
    if (currentLatency > maxLatency) maxLatency = currentLatency;
    completions[slot]-> release();
    ++writesCompleted;
    delete name[slot];
    delete contents[slot];
  }

  utime_t timePassed = g_clock.now() - startTime;

  //check objects for consistency if requested
  int errors = 0;
  if (readOffResults) {
    char matchName[128];
    object_t oid;
    bufferlist actualContents;
    for (int i = 0; i < writesCompleted; ++i ) {
      snprintf(matchName, 128, "Object %s:%d", iTime, i);
      oid = object_t(matchName);
      snprintf(contentsChars, writeSize, "I'm the %dth object!", i);
      rados.read(pool, oid, 0, actualContents, writeSize);
      if (strcmp(contentsChars, actualContents.c_str()) != 0 ) {
	cerr << "Object " << matchName << " is not correct!";
	++errors;
      }
      actualContents.clear();
    }
  }

  char bw[20];
  double bandwidth = ((double)writesCompleted)*((double)writeSize)/(double)timePassed;
  bandwidth = bandwidth/(1024*1024); // we want it in MB/sec
  sprintf(bw, "%.3lf \n", bandwidth);

  double averageLatency = totalLatency / writesCompleted;

  cout << "Total time run:        " << timePassed << std::endl
       << "Total writes made:     " << writesCompleted << std::endl
       << "Write size:            " << writeSize << std::endl
       << "Bandwidth (MB/sec):    " << bw << std::endl
       << "Average Latency:       " << averageLatency << std::endl
       << "Max latency:           " << maxLatency << std::endl
       << "Time waiting for Rados:" << totalLatency/concurrentios << std::endl;

  if (readOffResults) {
    if (errors) cout << "WARNING: There were " << errors << " total errors in copying!\n";
    else cout << "No errors in copying!\n";
  }
  return 0;
}
<|endoftext|>


<commit_before>/******************************************************************************
 * Copyright 2017 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

#include "modules/perception/traffic_light/preprocessor/tl_preprocessor.h"

#include "modules/perception/lib/base/time_util.h"
#include "modules/perception/onboard/transform_input.h"
#include "modules/perception/traffic_light/base/tl_shared_data.h"
#include "modules/perception/traffic_light/base/utils.h"

namespace apollo {
namespace perception {
namespace traffic_light {

bool TLPreprocessor::Init() {
  ConfigManager *config_manager = ConfigManager::instance();
  const ModelConfig *model_config = NULL;
  if (!config_manager->GetModelConfig(name(), &model_config)) {
    AERROR << "not found model: " << name();
    return false;
  }

  // Read parameters from config file
  if (!model_config->GetValue("max_cached_lights_size",
                              &max_cached_lights_size_)) {
    AERROR << "max_cached_image_lights_array_size not found." << name();
    return false;
  }
  if (!model_config->GetValue("projection_image_cols",
                              &projection_image_cols_)) {
    AERROR << "projection_image_cols not found." << name();
    return false;
  }
  if (!model_config->GetValue("projection_image_rows",
                              &projection_image_rows_)) {
    AERROR << "projection_image_rows not found." << name();
    return false;
  }
  if (!model_config->GetValue("sync_interval_seconds",
                              &sync_interval_seconds_)) {
    AERROR << "sync_interval_seconds not found." << name();
    return false;
  }
  if (!model_config->GetValue("no_signals_interval_seconds",
                              &no_signals_interval_seconds_)) {
    AERROR << "no_signals_interval_seconds not found." << name();
    return false;
  }

  // init projection
  if (!projection_.Init()) {
    AERROR << "TLPreprocessor init projection failed.";
    return false;
  }
  AINFO << kCountCameraId;
  return true;
}

bool TLPreprocessor::CacheLightsProjections(const CarPose &pose,
                                            const std::vector<Signal> &signals,
                                            const double timestamp) {
  MutexLock lock(&mutex_);
  PERF_FUNCTION();

  AINFO << "TLPreprocessor has " << cached_lights_.size()
        << " lights projections cached.";

  // pop front if cached array'size > FLAGS_max_cached_image_lights_array_size
  while (cached_lights_.size() > static_cast<size_t>(max_cached_lights_size_)) {
    cached_lights_.erase(cached_lights_.begin());
  }

  // lights projection info. to be added in cached array
  std::shared_ptr<ImageLights> image_lights(new ImageLights);
  // default select long focus camera
  image_lights->camera_id = LONG_FOCUS;
  image_lights->timestamp = timestamp;
  image_lights->pose = pose;
  image_lights->is_pose_valid = true;

  AINFO << "TLPreprocessor Got signal number:" << signals.size()
        << ", ts: " << GLOG_TIMESTAMP(timestamp);
  for (const auto &signal : signals) {
    AINFO << "signal info:" << signal.ShortDebugString();
  }
  // lights projections info.

  std::vector<std::shared_ptr<LightPtrs>> lights_on_image(kCountCameraId);
  std::vector<std::shared_ptr<LightPtrs>> lights_outside_image(kCountCameraId);
  for (auto &light_ptrs : lights_on_image) {
    light_ptrs.reset(new LightPtrs);
  }
  for (auto &light_ptrs : lights_outside_image) {
    light_ptrs.reset(new LightPtrs);
  }
  if (signals.size() > 0) {
    // project light region on each camera's image plane
    for (int cam_id = 0; cam_id < kCountCameraId; ++cam_id) {
      if (!ProjectLights(pose, signals, static_cast<CameraId>(cam_id),
                         lights_on_image[cam_id].get(),
                         lights_outside_image[cam_id].get())) {
        AERROR << "add_cached_lights_projections project lights on "
               << kCameraIdToStr.at(static_cast<CameraId>(cam_id))
               << " image failed, "
               << "ts: " << GLOG_TIMESTAMP(timestamp) << ", camera_id: "
               << kCameraIdToStr.at(static_cast<CameraId>(cam_id));
        return false;
      }
    }

    // select which image to be used
    SelectImage(pose, lights_on_image, lights_outside_image,
                &(image_lights->camera_id));
    AINFO << "select camera: " << kCameraIdToStr.at(image_lights->camera_id);

  } else {
    last_no_signals_ts_ = timestamp;
  }
  image_lights->num_signals = signals.size();
  AINFO << "cached info with " << image_lights->num_signals << " signals";
  cached_lights_.push_back(image_lights);

  return true;
}

bool TLPreprocessor::SyncImage(const ImageSharedPtr &image,
                               ImageLightsPtr *image_lights, bool *should_pub) {
  MutexLock lock(&mutex_);
  PERF_FUNCTION();
  CameraId camera_id = image->camera_id();
  double image_ts = image->ts();
  bool sync_ok = false;

  PERF_FUNCTION();
  if (cached_lights_.size() == 0) {
    AINFO << "No cached light";
    return false;
  }
  const int cam_id = static_cast<int>(camera_id);
  if (cam_id < 0 || cam_id >= kCountCameraId) {
    AERROR << "SyncImage failed, "
           << "get unknown CameraId: " << camera_id;
    return false;
  }

  // find close enough(by timestamp difference)
  // lights projection from back to front

  bool find_loc = false;  // if pose is found
  auto cached_lights_ptr = cached_lights_.rbegin();
  for (; cached_lights_ptr != cached_lights_.rend(); ++cached_lights_ptr) {
    double light_ts = (*cached_lights_ptr)->timestamp;
    if (fabs(light_ts - image_ts) < sync_interval_seconds_) {
      find_loc = true;
      auto proj_cam_id = static_cast<int>((*cached_lights_ptr)->camera_id);
      auto image_cam_id = static_cast<int>(camera_id);
      auto proj_cam_id_str =
          (kCameraIdToStr.find(proj_cam_id) != kCameraIdToStr.end()
               ? kCameraIdToStr.at(proj_cam_id)
               : std::to_string(proj_cam_id));
      // found related pose but if camear ID doesn't match
      if (proj_cam_id != image_cam_id) {
        AWARN << "find appropriate localization, but camera_id not match"
              << ", cached projection's camera_id: " << proj_cam_id_str
              << " , image's camera_id: " << kCameraIdToStr.at(image_cam_id);
        continue;
      }
      if (image_ts < last_output_ts_) {
        AWARN << "TLPreprocessor reject the image pub ts:"
              << GLOG_TIMESTAMP(image_ts)
              << " which is earlier than last output ts:"
              << GLOG_TIMESTAMP(last_output_ts_)
              << ", image camera_id: " << kCameraIdToStr.at(image_cam_id);
        return false;
      }
      sync_ok = true;
      break;
    }
  }

  if (sync_ok) {
    *image_lights = *cached_lights_ptr;
    (*image_lights)->diff_image_pose_ts =
        image_ts - (*cached_lights_ptr)->timestamp;
    (*image_lights)->diff_image_sys_ts = image_ts - TimeUtil::GetCurrentTime();

    (*image_lights)->image = image;
    (*image_lights)->timestamp = image_ts;
    AINFO << "TLPreprocessor sync ok ts: " << GLOG_TIMESTAMP(image_ts)
          << " camera_id: " << kCameraIdToStr.at(camera_id);
    last_output_ts_ = image_ts;
    last_pub_camera_id_ = camera_id;
    *should_pub = true;
  } else {
    AINFO << "sync image with cached lights projection failed, "
          << "no valid pose, ts: " << GLOG_TIMESTAMP(image_ts)
          << " camera_id: " << kCameraIdToStr.at(camera_id);
    std::string cached_array_str = "cached lights";
    double diff_image_pose_ts = 0.0;
    double diff_image_sys_ts = 0.0;
    bool no_signal = false;
    if (fabs(image_ts - last_no_signals_ts_) < no_signals_interval_seconds_) {
      AINFO << "TLPreprocessor " << cached_array_str
            << " sync failed, image ts: " << GLOG_TIMESTAMP(image_ts)
            << " last_no_signals_ts: " << GLOG_TIMESTAMP(last_no_signals_ts_)
            << " (sync_time - last_no_signals_ts): "
            << GLOG_TIMESTAMP(image_ts - last_no_signals_ts_)
            << " query /tf in low frequence because no signals forward "
            << " camera_id: " << kCameraIdToStr.at(camera_id);
      no_signal = true;
    } else if (image_ts < cached_lights_.front()->timestamp) {
      double pose_ts = cached_lights_.front()->timestamp;
      double system_ts = TimeUtil::GetCurrentTime();
      AWARN << "TLPreprocessor " << cached_array_str
            << " sync failed, image ts: " << GLOG_TIMESTAMP(image_ts)
            << ", which is earlier than " << cached_array_str
            << ".front() ts: " << GLOG_TIMESTAMP(pose_ts)
            << ", diff between image and pose ts: "
            << GLOG_TIMESTAMP(image_ts - pose_ts)
            << "; system ts: " << GLOG_TIMESTAMP(system_ts)
            << ", diff between image and system ts: "
            << GLOG_TIMESTAMP(image_ts - system_ts)
            << ", camera_id: " << kCameraIdToStr.at(camera_id);
      // difference between image and pose timestamps
      diff_image_pose_ts = image_ts - pose_ts;
      diff_image_sys_ts = image_ts - system_ts;
    } else if (image_ts > cached_lights_.back()->timestamp) {
      double pose_ts = cached_lights_.back()->timestamp;
      double system_ts = TimeUtil::GetCurrentTime();
      AWARN << "TLPreprocessor " << cached_array_str
            << " sync failed, image ts: " << GLOG_TIMESTAMP(image_ts)
            << ", which is older than " << cached_array_str
            << ".back() ts: " << GLOG_TIMESTAMP(pose_ts)
            << ", diff between image and pose ts: "
            << GLOG_TIMESTAMP(image_ts - pose_ts)
            << "; system ts: " << GLOG_TIMESTAMP(system_ts)
            << ", diff between image and system ts: "
            << GLOG_TIMESTAMP(image_ts - system_ts)
            << ", camera_id: " << kCameraIdToStr.at(camera_id);
      diff_image_pose_ts = image_ts - pose_ts;
      diff_image_sys_ts = image_ts - system_ts;
    } else if (!find_loc) {
      // if no pose found, log warning msg
      AWARN << "TLPreprocessor " << cached_array_str
            << " sync failed, image ts: " << GLOG_TIMESTAMP(image_ts)
            << ", cannot find close enough timestamp, " << cached_array_str
            << ".front() ts: "
            << GLOG_TIMESTAMP(cached_lights_.front()->timestamp) << ", "
            << cached_array_str << ".back() ts: "
            << GLOG_TIMESTAMP(cached_lights_.back()->timestamp)
            << ", camera_id: " << kCameraIdToStr.at(camera_id);
    }
    if (image->camera_id() == LONG_FOCUS &&
        (no_signal || last_pub_camera_id_ == LONG_FOCUS)) {
      *should_pub = true;
      (*image_lights).reset(new ImageLights);
      (*image_lights)->image = image;
      (*image_lights)->timestamp = image_ts;
      (*image_lights)->diff_image_sys_ts = diff_image_sys_ts;
      (*image_lights)->diff_image_pose_ts = diff_image_pose_ts;
      (*image_lights)->is_pose_valid = no_signal;
      (*image_lights)->num_signals = 0;
    }
  }
  // sync fail may because:
  // 1. image is not selected
  // 2. timestamp drift
  // 3. [there is no tf]
  return sync_ok;
}

void TLPreprocessor::set_last_pub_camera_id(CameraId camera_id) {
  last_pub_camera_id_ = camera_id;
}

CameraId TLPreprocessor::last_pub_camera_id() const {
  return last_pub_camera_id_;
}

int TLPreprocessor::max_cached_lights_size() const {
  return max_cached_lights_size_;
}

void TLPreprocessor::SelectImage(const CarPose &pose,
                                 const LightsArray &lights_on_image_array,
                                 const LightsArray &lights_outside_image_array,
                                 CameraId *selection) {
  *selection = static_cast<CameraId>(kShortFocusIdx);

  // check from long focus to short focus
  for (int cam_id = 0; cam_id < kCountCameraId; ++cam_id) {
    if (!lights_outside_image_array[cam_id]->empty()) {
      continue;
    }
    bool ok = true;
    // find the short focus camera without range check
    if (cam_id != kShortFocusIdx) {
      for (const LightPtr &light : *(lights_on_image_array[cam_id])) {
        if (IsOnBorder(cv::Size(projection_image_cols_, projection_image_rows_),
                       light->region.projection_roi,
                       image_border_size[cam_id])) {
          ok = false;
          AINFO << "light project on image border region, "
                << "CameraId: " << kCameraIdToStr.at(cam_id);
          break;
        }
      }
    }
    if (ok) {
      *selection = static_cast<CameraId>(cam_id);
      break;
    }
  }

  AINFO << "select_image selection: " << *selection;
}

bool TLPreprocessor::ProjectLights(const CarPose &pose,
                                   const std::vector<Signal> &signals,
                                   const CameraId &camera_id,
                                   LightPtrs *lights_on_image,
                                   LightPtrs *lights_outside_image) {
  if (signals.empty()) {
    ADEBUG << "project_lights get empty signals.";
    return true;
  }

  const int cam_id = static_cast<int>(camera_id);
  if (cam_id < 0 || cam_id >= kCountCameraId) {
    AERROR << "project_lights get invalid CameraId: " << camera_id;
    return false;
  }

  for (size_t i = 0; i < signals.size(); ++i) {
    LightPtr light(new Light);
    light->info = signals[i];
    if (!projection_.Project(pose, ProjectOption(camera_id), light.get())) {
      lights_outside_image->push_back(light);
    } else {
      lights_on_image->push_back(light);
    }
  }

  return true;
}

bool TLPreprocessor::IsOnBorder(const cv::Size size, const cv::Rect &roi,
                                const int border_size) const {
  if (roi.x < border_size || roi.y < border_size) {
    return true;
  }
  if (roi.x + roi.width + border_size >= size.width ||
      roi.y + roi.height + border_size >= size.height) {
    return true;
  }
  return false;
}

int TLPreprocessor::GetMinFocalLenCameraId() { return kShortFocusIdx; }

int TLPreprocessor::GetMaxFocalLenCameraId() { return kLongFocusIdx; }

REGISTER_PREPROCESSOR(TLPreprocessor);

}  // namespace traffic_light
}  // namespace perception
}  // namespace apollo
<commit_msg>fix when there is no signal, traffic light give error info (#2304)<commit_after>/******************************************************************************
 * Copyright 2017 The Apollo Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

#include "modules/perception/traffic_light/preprocessor/tl_preprocessor.h"

#include "modules/perception/lib/base/time_util.h"
#include "modules/perception/onboard/transform_input.h"
#include "modules/perception/traffic_light/base/tl_shared_data.h"
#include "modules/perception/traffic_light/base/utils.h"

namespace apollo {
namespace perception {
namespace traffic_light {

bool TLPreprocessor::Init() {
  ConfigManager *config_manager = ConfigManager::instance();
  const ModelConfig *model_config = NULL;
  if (!config_manager->GetModelConfig(name(), &model_config)) {
    AERROR << "not found model: " << name();
    return false;
  }

  // Read parameters from config file
  if (!model_config->GetValue("max_cached_lights_size",
                              &max_cached_lights_size_)) {
    AERROR << "max_cached_image_lights_array_size not found." << name();
    return false;
  }
  if (!model_config->GetValue("projection_image_cols",
                              &projection_image_cols_)) {
    AERROR << "projection_image_cols not found." << name();
    return false;
  }
  if (!model_config->GetValue("projection_image_rows",
                              &projection_image_rows_)) {
    AERROR << "projection_image_rows not found." << name();
    return false;
  }
  if (!model_config->GetValue("sync_interval_seconds",
                              &sync_interval_seconds_)) {
    AERROR << "sync_interval_seconds not found." << name();
    return false;
  }
  if (!model_config->GetValue("no_signals_interval_seconds",
                              &no_signals_interval_seconds_)) {
    AERROR << "no_signals_interval_seconds not found." << name();
    return false;
  }

  // init projection
  if (!projection_.Init()) {
    AERROR << "TLPreprocessor init projection failed.";
    return false;
  }
  AINFO << kCountCameraId;
  return true;
}

bool TLPreprocessor::CacheLightsProjections(const CarPose &pose,
                                            const std::vector<Signal> &signals,
                                            const double timestamp) {
  MutexLock lock(&mutex_);
  PERF_FUNCTION();

  AINFO << "TLPreprocessor has " << cached_lights_.size()
        << " lights projections cached.";

  // pop front if cached array'size > FLAGS_max_cached_image_lights_array_size
  while (cached_lights_.size() > static_cast<size_t>(max_cached_lights_size_)) {
    cached_lights_.erase(cached_lights_.begin());
  }

  // lights projection info. to be added in cached array
  std::shared_ptr<ImageLights> image_lights(new ImageLights);
  // default select long focus camera
  image_lights->camera_id = LONG_FOCUS;
  image_lights->timestamp = timestamp;
  image_lights->pose = pose;
  image_lights->is_pose_valid = true;

  AINFO << "TLPreprocessor Got signal number:" << signals.size()
        << ", ts: " << GLOG_TIMESTAMP(timestamp);
  for (const auto &signal : signals) {
    AINFO << "signal info:" << signal.ShortDebugString();
  }
  // lights projections info.

  std::vector<std::shared_ptr<LightPtrs>> lights_on_image(kCountCameraId);
  std::vector<std::shared_ptr<LightPtrs>> lights_outside_image(kCountCameraId);
  for (auto &light_ptrs : lights_on_image) {
    light_ptrs.reset(new LightPtrs);
  }
  for (auto &light_ptrs : lights_outside_image) {
    light_ptrs.reset(new LightPtrs);
  }
  if (signals.size() > 0) {
    // project light region on each camera's image plane
    for (int cam_id = 0; cam_id < kCountCameraId; ++cam_id) {
      if (!ProjectLights(pose, signals, static_cast<CameraId>(cam_id),
                         lights_on_image[cam_id].get(),
                         lights_outside_image[cam_id].get())) {
        AERROR << "add_cached_lights_projections project lights on "
               << kCameraIdToStr.at(static_cast<CameraId>(cam_id))
               << " image failed, "
               << "ts: " << GLOG_TIMESTAMP(timestamp) << ", camera_id: "
               << kCameraIdToStr.at(static_cast<CameraId>(cam_id));
        return false;
      }
    }

    // select which image to be used
    SelectImage(pose, lights_on_image, lights_outside_image,
                &(image_lights->camera_id));
    AINFO << "select camera: " << kCameraIdToStr.at(image_lights->camera_id);

  } else {
    last_no_signals_ts_ = timestamp;
  }
  image_lights->num_signals = signals.size();
  AINFO << "cached info with " << image_lights->num_signals << " signals";
  cached_lights_.push_back(image_lights);

  return true;
}

bool TLPreprocessor::SyncImage(const ImageSharedPtr &image,
                               ImageLightsPtr *image_lights, bool *should_pub) {
  MutexLock lock(&mutex_);
  PERF_FUNCTION();
  CameraId camera_id = image->camera_id();
  double image_ts = image->ts();
  bool sync_ok = false;

  PERF_FUNCTION();
  if (cached_lights_.size() == 0) {
    AINFO << "No cached light";
    return false;
  }
  const int cam_id = static_cast<int>(camera_id);
  if (cam_id < 0 || cam_id >= kCountCameraId) {
    AERROR << "SyncImage failed, "
           << "get unknown CameraId: " << camera_id;
    return false;
  }

  // find close enough(by timestamp difference)
  // lights projection from back to front

  bool find_loc = false;  // if pose is found
  auto cached_lights_ptr = cached_lights_.rbegin();
  for (; cached_lights_ptr != cached_lights_.rend(); ++cached_lights_ptr) {
    double light_ts = (*cached_lights_ptr)->timestamp;
    if (fabs(light_ts - image_ts) < sync_interval_seconds_) {
      find_loc = true;
      auto proj_cam_id = static_cast<int>((*cached_lights_ptr)->camera_id);
      auto image_cam_id = static_cast<int>(camera_id);
      auto proj_cam_id_str =
          (kCameraIdToStr.find(proj_cam_id) != kCameraIdToStr.end()
               ? kCameraIdToStr.at(proj_cam_id)
               : std::to_string(proj_cam_id));
      // found related pose but if camear ID doesn't match
      if (proj_cam_id != image_cam_id) {
        AWARN << "find appropriate localization, but camera_id not match"
              << ", cached projection's camera_id: " << proj_cam_id_str
              << " , image's camera_id: " << kCameraIdToStr.at(image_cam_id);
        continue;
      }
      if (image_ts < last_output_ts_) {
        AWARN << "TLPreprocessor reject the image pub ts:"
              << GLOG_TIMESTAMP(image_ts)
              << " which is earlier than last output ts:"
              << GLOG_TIMESTAMP(last_output_ts_)
              << ", image camera_id: " << kCameraIdToStr.at(image_cam_id);
        return false;
      }
      sync_ok = true;
      break;
    }
  }

  if (sync_ok) {
    *image_lights = *cached_lights_ptr;
    (*image_lights)->diff_image_pose_ts =
        image_ts - (*cached_lights_ptr)->timestamp;
    (*image_lights)->diff_image_sys_ts = image_ts - TimeUtil::GetCurrentTime();

    (*image_lights)->image = image;
    (*image_lights)->timestamp = image_ts;
    AINFO << "TLPreprocessor sync ok ts: " << GLOG_TIMESTAMP(image_ts)
          << " camera_id: " << kCameraIdToStr.at(camera_id);
    last_output_ts_ = image_ts;
    last_pub_camera_id_ = camera_id;
    *should_pub = true;
  } else {
    AINFO << "sync image with cached lights projection failed, "
          << "no valid pose, ts: " << GLOG_TIMESTAMP(image_ts)
          << " camera_id: " << kCameraIdToStr.at(camera_id);
    std::string cached_array_str = "cached lights";
    double diff_image_pose_ts = 0.0;
    double diff_image_sys_ts = 0.0;
    bool no_signal = false;
    if (fabs(image_ts - last_no_signals_ts_) < no_signals_interval_seconds_) {
      AINFO << "TLPreprocessor " << cached_array_str
            << " sync failed, image ts: " << GLOG_TIMESTAMP(image_ts)
            << " last_no_signals_ts: " << GLOG_TIMESTAMP(last_no_signals_ts_)
            << " (sync_time - last_no_signals_ts): "
            << GLOG_TIMESTAMP(image_ts - last_no_signals_ts_)
            << " query /tf in low frequence because no signals forward "
            << " camera_id: " << kCameraIdToStr.at(camera_id);
      no_signal = true;
    } else if (image_ts < cached_lights_.front()->timestamp) {
      double pose_ts = cached_lights_.front()->timestamp;
      double system_ts = TimeUtil::GetCurrentTime();
      AWARN << "TLPreprocessor " << cached_array_str
            << " sync failed, image ts: " << GLOG_TIMESTAMP(image_ts)
            << ", which is earlier than " << cached_array_str
            << ".front() ts: " << GLOG_TIMESTAMP(pose_ts)
            << ", diff between image and pose ts: "
            << GLOG_TIMESTAMP(image_ts - pose_ts)
            << "; system ts: " << GLOG_TIMESTAMP(system_ts)
            << ", diff between image and system ts: "
            << GLOG_TIMESTAMP(image_ts - system_ts)
            << ", camera_id: " << kCameraIdToStr.at(camera_id);
      // difference between image and pose timestamps
      diff_image_pose_ts = image_ts - pose_ts;
      diff_image_sys_ts = image_ts - system_ts;
    } else if (image_ts > cached_lights_.back()->timestamp) {
      double pose_ts = cached_lights_.back()->timestamp;
      double system_ts = TimeUtil::GetCurrentTime();
      AWARN << "TLPreprocessor " << cached_array_str
            << " sync failed, image ts: " << GLOG_TIMESTAMP(image_ts)
            << ", which is older than " << cached_array_str
            << ".back() ts: " << GLOG_TIMESTAMP(pose_ts)
            << ", diff between image and pose ts: "
            << GLOG_TIMESTAMP(image_ts - pose_ts)
            << "; system ts: " << GLOG_TIMESTAMP(system_ts)
            << ", diff between image and system ts: "
            << GLOG_TIMESTAMP(image_ts - system_ts)
            << ", camera_id: " << kCameraIdToStr.at(camera_id);
      diff_image_pose_ts = image_ts - pose_ts;
      diff_image_sys_ts = image_ts - system_ts;
    } else if (!find_loc) {
      // if no pose found, log warning msg
      AWARN << "TLPreprocessor " << cached_array_str
            << " sync failed, image ts: " << GLOG_TIMESTAMP(image_ts)
            << ", cannot find close enough timestamp, " << cached_array_str
            << ".front() ts: "
            << GLOG_TIMESTAMP(cached_lights_.front()->timestamp) << ", "
            << cached_array_str << ".back() ts: "
            << GLOG_TIMESTAMP(cached_lights_.back()->timestamp)
            << ", camera_id: " << kCameraIdToStr.at(camera_id);
    }
    if (image->camera_id() == LONG_FOCUS &&
        (no_signal || last_pub_camera_id_ == LONG_FOCUS)) {
      *should_pub = true;
      (*image_lights).reset(new ImageLights);
      (*image_lights)->image = image;
      (*image_lights)->camera_id = image->camera_id();
      (*image_lights)->timestamp = image_ts;
      (*image_lights)->diff_image_sys_ts = diff_image_sys_ts;
      (*image_lights)->diff_image_pose_ts = diff_image_pose_ts;
      (*image_lights)->is_pose_valid = no_signal;
      (*image_lights)->num_signals = 0;
    }
  }
  // sync fail may because:
  // 1. image is not selected
  // 2. timestamp drift
  // 3. [there is no tf]
  return sync_ok;
}

void TLPreprocessor::set_last_pub_camera_id(CameraId camera_id) {
  last_pub_camera_id_ = camera_id;
}

CameraId TLPreprocessor::last_pub_camera_id() const {
  return last_pub_camera_id_;
}

int TLPreprocessor::max_cached_lights_size() const {
  return max_cached_lights_size_;
}

void TLPreprocessor::SelectImage(const CarPose &pose,
                                 const LightsArray &lights_on_image_array,
                                 const LightsArray &lights_outside_image_array,
                                 CameraId *selection) {
  *selection = static_cast<CameraId>(kShortFocusIdx);

  // check from long focus to short focus
  for (int cam_id = 0; cam_id < kCountCameraId; ++cam_id) {
    if (!lights_outside_image_array[cam_id]->empty()) {
      continue;
    }
    bool ok = true;
    // find the short focus camera without range check
    if (cam_id != kShortFocusIdx) {
      for (const LightPtr &light : *(lights_on_image_array[cam_id])) {
        if (IsOnBorder(cv::Size(projection_image_cols_, projection_image_rows_),
                       light->region.projection_roi,
                       image_border_size[cam_id])) {
          ok = false;
          AINFO << "light project on image border region, "
                << "CameraId: " << kCameraIdToStr.at(cam_id);
          break;
        }
      }
    }
    if (ok) {
      *selection = static_cast<CameraId>(cam_id);
      break;
    }
  }

  AINFO << "select_image selection: " << *selection;
}

bool TLPreprocessor::ProjectLights(const CarPose &pose,
                                   const std::vector<Signal> &signals,
                                   const CameraId &camera_id,
                                   LightPtrs *lights_on_image,
                                   LightPtrs *lights_outside_image) {
  if (signals.empty()) {
    ADEBUG << "project_lights get empty signals.";
    return true;
  }

  const int cam_id = static_cast<int>(camera_id);
  if (cam_id < 0 || cam_id >= kCountCameraId) {
    AERROR << "project_lights get invalid CameraId: " << camera_id;
    return false;
  }

  for (size_t i = 0; i < signals.size(); ++i) {
    LightPtr light(new Light);
    light->info = signals[i];
    if (!projection_.Project(pose, ProjectOption(camera_id), light.get())) {
      lights_outside_image->push_back(light);
    } else {
      lights_on_image->push_back(light);
    }
  }

  return true;
}

bool TLPreprocessor::IsOnBorder(const cv::Size size, const cv::Rect &roi,
                                const int border_size) const {
  if (roi.x < border_size || roi.y < border_size) {
    return true;
  }
  if (roi.x + roi.width + border_size >= size.width ||
      roi.y + roi.height + border_size >= size.height) {
    return true;
  }
  return false;
}

int TLPreprocessor::GetMinFocalLenCameraId() { return kShortFocusIdx; }

int TLPreprocessor::GetMaxFocalLenCameraId() { return kLongFocusIdx; }

REGISTER_PREPROCESSOR(TLPreprocessor);

}  // namespace traffic_light
}  // namespace perception
}  // namespace apollo
<|endoftext|>


<commit_before>//-----------------------------------------------------------------------bl-
//--------------------------------------------------------------------------
//
// QUESO - a library to support the Quantification of Uncertainty
// for Estimation, Simulation and Optimization
//
// Copyright (C) 2008-2015 The PECOS Development Team
//
// This library is free software; you can redistribute it and/or
// modify it under the terms of the Version 2.1 GNU Lesser General
// Public License as published by the Free Software Foundation.
//
// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc. 51 Franklin Street, Fifth Floor,
// Boston, MA  02110-1301  USA
//
//-----------------------------------------------------------------------el-

// This class
#include <queso/InterpolationSurrogateData.h>

// QUESO
#include <queso/GslVector.h>
#include <queso/GslMatrix.h>

// C++
#include <sstream>

namespace QUESO
{
  template<class V, class M>
  InterpolationSurrogateData<V,M>::InterpolationSurrogateData(const BoxSubset<V,M> & domain,
                                                              const std::vector<unsigned int>& n_points )
    : m_domain(domain),
      m_n_points(n_points)
  {
    // This checks that the dimension of n_points and the domain are consistent
    this->check_dim_consistency();

    // Size m_values
    this->init_values(this->m_n_points);
  }

  template<class V, class M>
  void InterpolationSurrogateData<V,M>::check_dim_consistency() const
  {
    if( this->dim() != this->m_n_points.size() )
      {
        std::stringstream vspace_dim;
        vspace_dim << this->m_domain.vectorSpace().dimGlobal();

        std::stringstream n_points_dim;
        n_points_dim << this->m_n_points.size();

        std::string error = "ERROR: Mismatch between dimension of parameter space and number of points\n.";
        error += "        domain dimension = " + vspace_dim.str() + "\n";
        error += "        points dimension = " + n_points_dim.str() + "\n";

        queso_error_msg(error);
      }
  }

  template<class V, class M>
  void InterpolationSurrogateData<V,M>::init_values( const std::vector<unsigned int>& n_points )
  {
    unsigned int n_total_points = 0;
    for( std::vector<unsigned int>::const_iterator it = n_points.begin();
         it != n_points.end(); ++it )
      {
        n_total_points += *it;
      }

    this->m_values.resize(n_total_points);
  }

  template<class V, class M>
  void InterpolationSurrogateData<V,M>::set_values( std::vector<double>& values )
  {
    queso_assert_equal_to( values.size(), m_values.size() );

    this->m_values = values;
  }

  template<class V, class M>
  void InterpolationSurrogateData<V,M>::set_value( unsigned int n, double value )
  {
    queso_assert_less( n, m_values.size() );

    this->m_values[n] = value;
  }

  template<class V, class M>
  double InterpolationSurrogateData<V,M>::spacing( unsigned int dim ) const
  {
    unsigned int n_intervals = this->m_n_points[dim]-1;
    double x_min = this->x_min(dim);
    double x_max = this->x_max(dim);

    return (x_max-x_min)/n_intervals;
  }

} // end namespace QUESO

// Instantiate
template class QUESO::InterpolationSurrogateData<QUESO::GslVector,QUESO::GslMatrix>;
<commit_msg>Fixed bug in InterpolationSurrogateData::init_values<commit_after>//-----------------------------------------------------------------------bl-
//--------------------------------------------------------------------------
//
// QUESO - a library to support the Quantification of Uncertainty
// for Estimation, Simulation and Optimization
//
// Copyright (C) 2008-2015 The PECOS Development Team
//
// This library is free software; you can redistribute it and/or
// modify it under the terms of the Version 2.1 GNU Lesser General
// Public License as published by the Free Software Foundation.
//
// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc. 51 Franklin Street, Fifth Floor,
// Boston, MA  02110-1301  USA
//
//-----------------------------------------------------------------------el-

// This class
#include <queso/InterpolationSurrogateData.h>

// QUESO
#include <queso/GslVector.h>
#include <queso/GslMatrix.h>

// C++
#include <sstream>

namespace QUESO
{
  template<class V, class M>
  InterpolationSurrogateData<V,M>::InterpolationSurrogateData(const BoxSubset<V,M> & domain,
                                                              const std::vector<unsigned int>& n_points )
    : m_domain(domain),
      m_n_points(n_points)
  {
    // This checks that the dimension of n_points and the domain are consistent
    this->check_dim_consistency();

    // Size m_values
    this->init_values(this->m_n_points);
  }

  template<class V, class M>
  void InterpolationSurrogateData<V,M>::check_dim_consistency() const
  {
    if( this->dim() != this->m_n_points.size() )
      {
        std::stringstream vspace_dim;
        vspace_dim << this->m_domain.vectorSpace().dimGlobal();

        std::stringstream n_points_dim;
        n_points_dim << this->m_n_points.size();

        std::string error = "ERROR: Mismatch between dimension of parameter space and number of points\n.";
        error += "        domain dimension = " + vspace_dim.str() + "\n";
        error += "        points dimension = " + n_points_dim.str() + "\n";

        queso_error_msg(error);
      }
  }

  template<class V, class M>
  void InterpolationSurrogateData<V,M>::init_values( const std::vector<unsigned int>& n_points )
  {
    unsigned int n_total_points = 1.0;
    for( std::vector<unsigned int>::const_iterator it = n_points.begin();
         it != n_points.end(); ++it )
      {
        n_total_points *= *it;
      }

    this->m_values.resize(n_total_points);
  }

  template<class V, class M>
  void InterpolationSurrogateData<V,M>::set_values( std::vector<double>& values )
  {
    queso_assert_equal_to( values.size(), m_values.size() );

    this->m_values = values;
  }

  template<class V, class M>
  void InterpolationSurrogateData<V,M>::set_value( unsigned int n, double value )
  {
    queso_assert_less( n, m_values.size() );

    this->m_values[n] = value;
  }

  template<class V, class M>
  double InterpolationSurrogateData<V,M>::spacing( unsigned int dim ) const
  {
    unsigned int n_intervals = this->m_n_points[dim]-1;
    double x_min = this->x_min(dim);
    double x_max = this->x_max(dim);

    return (x_max-x_min)/n_intervals;
  }

} // end namespace QUESO

// Instantiate
template class QUESO::InterpolationSurrogateData<QUESO::GslVector,QUESO::GslMatrix>;
<|endoftext|>


<commit_before>#include <cfloat>
#include <omp.h>
#include "renderer.h"

#define PI 3.141592653589793826433

void Camera::init(Vec3 from, Vec3 lookat, Vec3 up, double fov, double aspect, double aperture, double focus_dist)
{
	lens_radius_ = aperture / 2;
	double theta = fov * PI / 180;
	double half_height = tan(theta / 2);
	double half_width = aspect * half_height;
	origin_ = from;
	Vec3 d = from - lookat;
	w_ = d.normalize();
	u_ = cross(up, w_).normalize();
	v_ = cross(w_, u_);
	lower_left_corner_ = origin_ - u_ * (half_width * focus_dist) - v_ * (half_height * focus_dist) - w_ * focus_dist;
	horizontal_ = u_ * (2.0 * half_width * focus_dist);
	vertical_ = v_ * (2.0 * half_height * focus_dist);
}

renderer::renderer(int w, int h)
	:steps_(0)
{
	WIDTH = w;
	HEIGHT = h;

	// camera
	Vec3 from(13, 3, 3);
	Vec3 lookat(0, 0.5, 0);
	Vec3 up(0, 1, 0);
	double fov = 20.0;
	double aspect = (double)WIDTH / (double)HEIGHT;
	double dist_to_focus = 10.0;
	double aperture = 0.1;

	cam_.init(from, lookat, up, fov, aspect, aperture, dist_to_focus);

	// scene
	double R = cos(PI / 4);
	scene_.Append(new Sphere(Vec3(0, -1000, 0), 1000, new Lambertian(Vec3(0.5, 0.5, 0.5))));
	scene_.Append(new Sphere(Vec3(0, 1, 0), 1.0, new Dielectric(1.5)));
	scene_.Append(new Sphere(Vec3(-4, 1, 0), 1.0, new Lambertian(Vec3(0.4, 0.2, 0.1))));
	scene_.Append(new Sphere(Vec3(4, 1, 0), 1.0, new Metal(Vec3(0.7, 0.6, 0.5), 0.0)));
}

renderer::~renderer()
{
}

Vec3 renderer::raytrace(Ray r, int depth, my_rand &rnd)const
{
	// noise for debug
	return Vec3(rand_.get(), rand_.get(), rand_.get());

	HitRecord rec;
	if (scene_.hit(r, 0.001, DBL_MAX, rec)) {
		Ray scattered;
		Vec3 attenuation;
		if (depth < 50 && rec.mat_ptr->scatter(r, rec, attenuation, scattered, rnd)) {
			return attenuation * raytrace(scattered, depth + 1, rnd);
		}
		else {
			return Vec3(0, 0, 0);
		}
	}
	else {
		Vec3 unit_direction = r.direction().normalize();
		double t = 0.5*(unit_direction.y + 1.0);
		return (1.0 - t)*Vec3(1.0, 1.0, 1.0) + t * Vec3(0.5, 0.7, 1.0);
	}
}

Vec3 renderer::color(double u, double v, my_rand &rnd)const
{
	Ray r = cam_.get_ray(u, v, rnd);
	return raytrace(r, 0, rnd);
}

void renderer::update(const double *src, double *dest, my_rand *a_rand)const
{
	const double INV_WIDTH = 1.0 / (double)WIDTH;
	const double INV_HEIGHT = 1.0 / (double)HEIGHT;

	#pragma omp parallel for
	for (int y = 0; y < HEIGHT; y++) {
		my_rand &rnd = a_rand[omp_get_thread_num()];
		for (int x = 0; x < WIDTH; x++) {
			int index = 3 * (y * WIDTH + x);

			double u = ((double)x + rnd.get()) * INV_WIDTH;
			double v = 1.0 - ((double)y + rnd.get()) * INV_HEIGHT;

			Vec3 color = this->color(u, v, rnd);
			dest[index + 0] = src[index + 0] + color.x;
			dest[index + 1] = src[index + 1] + color.y;
			dest[index + 2] = src[index + 2] + color.z;
		}
	}
}
<commit_msg>Update renderer.cpp<commit_after>#include <cfloat>
#include <omp.h>
#include "renderer.h"

#define PI 3.141592653589793826433

void Camera::init(Vec3 from, Vec3 lookat, Vec3 up, double fov, double aspect, double aperture, double focus_dist)
{
	lens_radius_ = aperture / 2;
	double theta = fov * PI / 180;
	double half_height = tan(theta / 2);
	double half_width = aspect * half_height;
	origin_ = from;
	Vec3 d = from - lookat;
	w_ = d.normalize();
	u_ = cross(up, w_).normalize();
	v_ = cross(w_, u_);
	lower_left_corner_ = origin_ - u_ * (half_width * focus_dist) - v_ * (half_height * focus_dist) - w_ * focus_dist;
	horizontal_ = u_ * (2.0 * half_width * focus_dist);
	vertical_ = v_ * (2.0 * half_height * focus_dist);
}

renderer::renderer(int w, int h)
	:steps_(0)
{
	WIDTH = w;
	HEIGHT = h;

	// camera
	Vec3 from(13, 3, 3);
	Vec3 lookat(0, 0.5, 0);
	Vec3 up(0, 1, 0);
	double fov = 20.0;
	double aspect = (double)WIDTH / (double)HEIGHT;
	double dist_to_focus = 10.0;
	double aperture = 0.1;

	cam_.init(from, lookat, up, fov, aspect, aperture, dist_to_focus);

	// scene
	double R = cos(PI / 4);
	scene_.Append(new Sphere(Vec3(0, -1000, 0), 1000, new Lambertian(Vec3(0.5, 0.5, 0.5))));
	scene_.Append(new Sphere(Vec3(0, 1, 0), 1.0, new Dielectric(1.5)));
	scene_.Append(new Sphere(Vec3(-4, 1, 0), 1.0, new Lambertian(Vec3(0.4, 0.2, 0.1))));
	scene_.Append(new Sphere(Vec3(4, 1, 0), 1.0, new Metal(Vec3(0.7, 0.6, 0.5), 0.0)));
}

renderer::~renderer()
{
}

Vec3 renderer::raytrace(Ray r, int depth, my_rand &rnd)const
{
	// noise for debug
	return Vec3(0,0,0);
//	return Vec3(rand_.get(), rand_.get(), rand_.get());

	HitRecord rec;
	if (scene_.hit(r, 0.001, DBL_MAX, rec)) {
		Ray scattered;
		Vec3 attenuation;
		if (depth < 50 && rec.mat_ptr->scatter(r, rec, attenuation, scattered, rnd)) {
			return attenuation * raytrace(scattered, depth + 1, rnd);
		}
		else {
			return Vec3(0, 0, 0);
		}
	}
	else {
		Vec3 unit_direction = r.direction().normalize();
		double t = 0.5*(unit_direction.y + 1.0);
		return (1.0 - t)*Vec3(1.0, 1.0, 1.0) + t * Vec3(0.5, 0.7, 1.0);
	}
}

Vec3 renderer::color(double u, double v, my_rand &rnd)const
{
	Ray r = cam_.get_ray(u, v, rnd);
	return raytrace(r, 0, rnd);
}

void renderer::update(const double *src, double *dest, my_rand *a_rand)const
{
	const double INV_WIDTH = 1.0 / (double)WIDTH;
	const double INV_HEIGHT = 1.0 / (double)HEIGHT;

	#pragma omp parallel for
	for (int y = 0; y < HEIGHT; y++) {
		my_rand &rnd = a_rand[omp_get_thread_num()];
		for (int x = 0; x < WIDTH; x++) {
			int index = 3 * (y * WIDTH + x);

			double u = ((double)x + rnd.get()) * INV_WIDTH;
			double v = 1.0 - ((double)y + rnd.get()) * INV_HEIGHT;

			Vec3 color = this->color(u, v, rnd);
			dest[index + 0] = src[index + 0] + color.x;
			dest[index + 1] = src[index + 1] + color.y;
			dest[index + 2] = src[index + 2] + color.z;
		}
	}
}
<|endoftext|>