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

  Program:   Visualization Toolkit
  Module:    vtkAVIWriter.cxx

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

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

=========================================================================*/
#include "vtkWindows.h"
#include "vtkAVIWriter.h"

#include "vtkImageData.h"
#include "vtkObjectFactory.h"

#include "vtkErrorCode.h"

#ifdef _MSC_VER
#pragma warning (push, 3)
#endif

#include <vfw.h>

#ifdef _MSC_VER
#pragma warning (pop)
#endif

class vtkAVIWriterInternal
{
public:
  PAVISTREAM Stream;
  PAVISTREAM StreamCompressed;
  PAVIFILE AVIFile;
  LPBITMAPINFOHEADER  lpbi;       // pointer to BITMAPINFOHEADER
  HANDLE              hDIB;  // handle to DIB, temp handle
};

//---------------------------------------------------------------------------
vtkStandardNewMacro(vtkAVIWriter);

//---------------------------------------------------------------------------
vtkAVIWriter::vtkAVIWriter()
{
  this->Internals = new vtkAVIWriterInternal;
  this->Internals->Stream = NULL;
  this->Internals->StreamCompressed = NULL;
  this->Internals->AVIFile = NULL;
  this->Time = 0;
  this->Quality = 10000;
  this->Rate = 1000;
  this->Internals->hDIB = NULL;  // handle to DIB, temp handle
  this->PromptCompressionOptions = 0;
  this->CompressorFourCC = NULL;
  this->SetCompressorFourCC("MSVC");
}

//---------------------------------------------------------------------------
vtkAVIWriter::~vtkAVIWriter()
{
  if (this->Internals->AVIFile)
    {
    this->End();
    }
  delete this->Internals;
  this->SetCompressorFourCC(NULL);
}

//---------------------------------------------------------------------------
void vtkAVIWriter::Start()
{
  // Error checking
  this->Error = 1;
  if ( this->GetInput() == NULL )
    {
    vtkErrorMacro(<<"Write:Please specify an input!");
    this->SetErrorCode(vtkGenericMovieWriter::NoInputError);
    return;
    }
  if (!this->FileName)
    {
    vtkErrorMacro(<<"Write:Please specify a FileName");
    this->SetErrorCode(vtkErrorCode::NoFileNameError);
    return;
    }

  // Fill in image information.
  this->GetInputAlgorithm(0, 0)->UpdateInformation();
  int wExtent[6];
  this->GetInputInformation(0,0)->Get(
    vtkStreamingDemandDrivenPipeline::WHOLE_EXTENT(), wExtent);
  this->GetInputAlgorithm(0, 0)->SetUpdateExtentToWholeExtent();

  LONG hr;
  AVISTREAMINFO strhdr;

  AVIFileInit();
  // opens AVIFile library
  hr = AVIFileOpen(&this->Internals->AVIFile, this->FileName,
                   OF_WRITE | OF_CREATE, 0L);
  if (hr != 0)
    {
    vtkErrorMacro("Unable to open " << this->FileName);
    this->SetErrorCode(vtkErrorCode::CannotOpenFileError);
    return;
    }

  // Fill in the header for the video stream....
  // The video stream will run in 15ths of a second....
  memset(&strhdr, 0, sizeof(strhdr));
  strhdr.fccType                = streamtypeVIDEO;// stream type
  strhdr.fccHandler             = 0;
  strhdr.dwScale                = 1;
  strhdr.dwRate                 = this->Rate;
  strhdr.dwQuality              = (DWORD) -1;
  strhdr.dwSuggestedBufferSize  = (wExtent[1] - wExtent[0] + 1)*
    (wExtent[3] - wExtent[2] + 1)*3;
  SetRect(&strhdr.rcFrame, 0, 0, wExtent[1] - wExtent[0] + 1,
          wExtent[3] - wExtent[2] + 1);

  // And create the stream;
  AVIFileCreateStream(this->Internals->AVIFile,  // file pointer
                      &this->Internals->Stream,  // returned stream pointer
                      &strhdr);      // stream header

  // do not want to display this dialog
  AVICOMPRESSOPTIONS opts;
  AVICOMPRESSOPTIONS FAR * aopts[1] = {&opts};
  memset(&opts, 0, sizeof(opts));

  // need to setup opts
  opts.fccType = 0;
  char fourcc[4] = {' ', ' ', ' ', ' '};
  if (this->CompressorFourCC)
    {
    memcpy(fourcc, this->CompressorFourCC, strlen(this->CompressorFourCC));
    }
  opts.fccHandler=mmioFOURCC(fourcc[0], fourcc[1], fourcc[2], fourcc[3]);
  switch (this->GetQuality())
    {
    case 0:
      opts.dwQuality = 2500;
      break;
    case 1:
      opts.dwQuality = 5000;
      break;
    default:
      opts.dwQuality = 10000;
      break;
    }

  opts.dwBytesPerSecond = 0;
  opts.dwFlags = AVICOMPRESSF_VALID;

  if (this->PromptCompressionOptions)
    {
    if (!AVISaveOptions(NULL, 0,
                        1, &this->Internals->Stream,
                        (LPAVICOMPRESSOPTIONS FAR *) &aopts))
      {
      vtkErrorMacro("Unable to save " << this->FileName);
      return;
      }
    }

  if (AVIMakeCompressedStream(&this->Internals->StreamCompressed,
                              this->Internals->Stream,
                              &opts, NULL) != AVIERR_OK)
    {
    vtkErrorMacro("Unable to compress " << this->FileName);
    this->SetErrorCode(vtkGenericMovieWriter::CanNotCompress);
    return;
    }

  DWORD               dwLen;      // size of memory block
  int dataWidth = (((wExtent[1] - wExtent[0] + 1)*3+3)/4)*4;

  dwLen = sizeof(BITMAPINFOHEADER) + dataWidth*(wExtent[3] - wExtent[2] + 1);
  this->Internals->hDIB = ::GlobalAlloc(GHND, dwLen);
  this->Internals->lpbi = (LPBITMAPINFOHEADER) ::GlobalLock(this->Internals->hDIB);

  this->Internals->lpbi->biSize = sizeof(BITMAPINFOHEADER);
  this->Internals->lpbi->biWidth = wExtent[1] - wExtent[0] + 1;
  this->Internals->lpbi->biHeight = wExtent[3] - wExtent[2] + 1;
  this->Internals->lpbi->biPlanes = 1;
  this->Internals->lpbi->biBitCount = 24;
  this->Internals->lpbi->biCompression = BI_RGB;
  this->Internals->lpbi->biClrUsed = 0;
  this->Internals->lpbi->biClrImportant = 0;
  this->Internals->lpbi->biSizeImage = dataWidth*(wExtent[3] - wExtent[2] + 1);

  if (AVIStreamSetFormat(this->Internals->StreamCompressed, 0,
                         this->Internals->lpbi, this->Internals->lpbi->biSize))
    {
    vtkErrorMacro("Unable to format " << this->FileName << " Most likely this means that the video compression scheme you seleted could not handle the data. Try selecting a different compression scheme." );
    this->SetErrorCode(vtkGenericMovieWriter::CanNotFormat);
    return;
    }

  this->Error = 0;
  this->Time = 0;
}

//---------------------------------------------------------------------------
void vtkAVIWriter::Write()
{
  if (this->Error)
    {
    return;
    }

  // get the data
  vtkImageData* input = this->GetImageDataInput(0);
  this->GetInputAlgorithm(0, 0)->UpdateWholeExtent();
  int *wExtent = input->GetExtent();

  // get the pointer to the data
  unsigned char *ptr =
    (unsigned char *)(input->GetScalarPointer());

  int dataWidth = (((wExtent[1] - wExtent[0] + 1)*3+3)/4)*4;
  int srcWidth = (wExtent[1] - wExtent[0] + 1)*3;

  // copy the data to the clipboard
  unsigned char *dest
    = (unsigned char *)this->Internals->lpbi + this->Internals->lpbi->biSize;
  int i,j;
  for (i = 0; i < this->Internals->lpbi->biHeight; i++)
    {
    for (j = 0; j < this->Internals->lpbi->biWidth; j++)
      {
      *dest++ = ptr[2];
      *dest++ = ptr[1];
      *dest++ = *ptr;
      ptr += 3;
      }
    dest = dest + (dataWidth - srcWidth);
    }

  AVIStreamWrite(this->Internals->StreamCompressed,  // stream pointer
                 this->Time, // time of this frame
                 1,        // number to write
                 (LPBYTE) this->Internals->lpbi +    // pointer to data
                 this->Internals->lpbi->biSize,
                 this->Internals->lpbi->biSizeImage,  // size of this frame
                 AVIIF_KEYFRAME,       // flags....
                 NULL, NULL);
  this->Time++;
}

//---------------------------------------------------------------------------
void vtkAVIWriter::End()
{
  ::GlobalUnlock(this->Internals->hDIB);
  if (this->Internals->Stream)
    {
    AVIStreamClose(this->Internals->Stream);
    this->Internals->Stream = NULL;
    }

  if (this->Internals->StreamCompressed)
    {
    AVIStreamClose(this->Internals->StreamCompressed);
    this->Internals->StreamCompressed = NULL;
    }

  if (this->Internals->AVIFile)
    {
    AVIFileClose(this->Internals->AVIFile);
    this->Internals->AVIFile = NULL;
    }

  AVIFileExit();          // releases AVIFile library
}

//---------------------------------------------------------------------------
void vtkAVIWriter::PrintSelf(ostream& os, vtkIndent indent)
{
  this->Superclass::PrintSelf(os, indent);
  os << indent << "Rate: " << this->Rate << endl;
  os << indent << "Quality: " << this->Quality << endl;
  os << indent << "PromptCompressionOptions: " << (this->GetPromptCompressionOptions() ? "on":"off") << endl;
  os << indent << "CompressorFourCC: "
     << (this->CompressorFourCC ? this->CompressorFourCC : "(None)") << endl;
}

<commit_msg>Another attempt to mave vtkAVIWriter compile on Windows.<commit_after>/*=========================================================================

  Program:   Visualization Toolkit
  Module:    vtkAVIWriter.cxx

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

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

=========================================================================*/
#include "vtkWindows.h"
#include "vtkAVIWriter.h"

#include "vtkImageData.h"
#include "vtkInformation.h"
#include "vtkObjectFactory.h"
#include "vtkStreamingDemandDrivenPipeline.h"

#include "vtkErrorCode.h"

#ifdef _MSC_VER
#pragma warning (push, 3)
#endif

#include <vfw.h>

#ifdef _MSC_VER
#pragma warning (pop)
#endif

class vtkAVIWriterInternal
{
public:
  PAVISTREAM Stream;
  PAVISTREAM StreamCompressed;
  PAVIFILE AVIFile;
  LPBITMAPINFOHEADER  lpbi;       // pointer to BITMAPINFOHEADER
  HANDLE              hDIB;  // handle to DIB, temp handle
};

//---------------------------------------------------------------------------
vtkStandardNewMacro(vtkAVIWriter);

//---------------------------------------------------------------------------
vtkAVIWriter::vtkAVIWriter()
{
  this->Internals = new vtkAVIWriterInternal;
  this->Internals->Stream = NULL;
  this->Internals->StreamCompressed = NULL;
  this->Internals->AVIFile = NULL;
  this->Time = 0;
  this->Quality = 10000;
  this->Rate = 1000;
  this->Internals->hDIB = NULL;  // handle to DIB, temp handle
  this->PromptCompressionOptions = 0;
  this->CompressorFourCC = NULL;
  this->SetCompressorFourCC("MSVC");
}

//---------------------------------------------------------------------------
vtkAVIWriter::~vtkAVIWriter()
{
  if (this->Internals->AVIFile)
    {
    this->End();
    }
  delete this->Internals;
  this->SetCompressorFourCC(NULL);
}

//---------------------------------------------------------------------------
void vtkAVIWriter::Start()
{
  // Error checking
  this->Error = 1;
  if ( this->GetInput() == NULL )
    {
    vtkErrorMacro(<<"Write:Please specify an input!");
    this->SetErrorCode(vtkGenericMovieWriter::NoInputError);
    return;
    }
  if (!this->FileName)
    {
    vtkErrorMacro(<<"Write:Please specify a FileName");
    this->SetErrorCode(vtkErrorCode::NoFileNameError);
    return;
    }

  // Fill in image information.
  this->GetInputAlgorithm(0, 0)->UpdateInformation();
  int wExtent[6];
  this->GetInputInformation(0,0)->Get(
    vtkStreamingDemandDrivenPipeline::WHOLE_EXTENT(), wExtent);
  this->GetInputAlgorithm(0, 0)->SetUpdateExtentToWholeExtent();

  LONG hr;
  AVISTREAMINFO strhdr;

  AVIFileInit();
  // opens AVIFile library
  hr = AVIFileOpen(&this->Internals->AVIFile, this->FileName,
                   OF_WRITE | OF_CREATE, 0L);
  if (hr != 0)
    {
    vtkErrorMacro("Unable to open " << this->FileName);
    this->SetErrorCode(vtkErrorCode::CannotOpenFileError);
    return;
    }

  // Fill in the header for the video stream....
  // The video stream will run in 15ths of a second....
  memset(&strhdr, 0, sizeof(strhdr));
  strhdr.fccType                = streamtypeVIDEO;// stream type
  strhdr.fccHandler             = 0;
  strhdr.dwScale                = 1;
  strhdr.dwRate                 = this->Rate;
  strhdr.dwQuality              = (DWORD) -1;
  strhdr.dwSuggestedBufferSize  = (wExtent[1] - wExtent[0] + 1)*
    (wExtent[3] - wExtent[2] + 1)*3;
  SetRect(&strhdr.rcFrame, 0, 0, wExtent[1] - wExtent[0] + 1,
          wExtent[3] - wExtent[2] + 1);

  // And create the stream;
  AVIFileCreateStream(this->Internals->AVIFile,  // file pointer
                      &this->Internals->Stream,  // returned stream pointer
                      &strhdr);      // stream header

  // do not want to display this dialog
  AVICOMPRESSOPTIONS opts;
  AVICOMPRESSOPTIONS FAR * aopts[1] = {&opts};
  memset(&opts, 0, sizeof(opts));

  // need to setup opts
  opts.fccType = 0;
  char fourcc[4] = {' ', ' ', ' ', ' '};
  if (this->CompressorFourCC)
    {
    memcpy(fourcc, this->CompressorFourCC, strlen(this->CompressorFourCC));
    }
  opts.fccHandler=mmioFOURCC(fourcc[0], fourcc[1], fourcc[2], fourcc[3]);
  switch (this->GetQuality())
    {
    case 0:
      opts.dwQuality = 2500;
      break;
    case 1:
      opts.dwQuality = 5000;
      break;
    default:
      opts.dwQuality = 10000;
      break;
    }

  opts.dwBytesPerSecond = 0;
  opts.dwFlags = AVICOMPRESSF_VALID;

  if (this->PromptCompressionOptions)
    {
    if (!AVISaveOptions(NULL, 0,
                        1, &this->Internals->Stream,
                        (LPAVICOMPRESSOPTIONS FAR *) &aopts))
      {
      vtkErrorMacro("Unable to save " << this->FileName);
      return;
      }
    }

  if (AVIMakeCompressedStream(&this->Internals->StreamCompressed,
                              this->Internals->Stream,
                              &opts, NULL) != AVIERR_OK)
    {
    vtkErrorMacro("Unable to compress " << this->FileName);
    this->SetErrorCode(vtkGenericMovieWriter::CanNotCompress);
    return;
    }

  DWORD               dwLen;      // size of memory block
  int dataWidth = (((wExtent[1] - wExtent[0] + 1)*3+3)/4)*4;

  dwLen = sizeof(BITMAPINFOHEADER) + dataWidth*(wExtent[3] - wExtent[2] + 1);
  this->Internals->hDIB = ::GlobalAlloc(GHND, dwLen);
  this->Internals->lpbi = (LPBITMAPINFOHEADER) ::GlobalLock(this->Internals->hDIB);

  this->Internals->lpbi->biSize = sizeof(BITMAPINFOHEADER);
  this->Internals->lpbi->biWidth = wExtent[1] - wExtent[0] + 1;
  this->Internals->lpbi->biHeight = wExtent[3] - wExtent[2] + 1;
  this->Internals->lpbi->biPlanes = 1;
  this->Internals->lpbi->biBitCount = 24;
  this->Internals->lpbi->biCompression = BI_RGB;
  this->Internals->lpbi->biClrUsed = 0;
  this->Internals->lpbi->biClrImportant = 0;
  this->Internals->lpbi->biSizeImage = dataWidth*(wExtent[3] - wExtent[2] + 1);

  if (AVIStreamSetFormat(this->Internals->StreamCompressed, 0,
                         this->Internals->lpbi, this->Internals->lpbi->biSize))
    {
    vtkErrorMacro("Unable to format " << this->FileName << " Most likely this means that the video compression scheme you seleted could not handle the data. Try selecting a different compression scheme." );
    this->SetErrorCode(vtkGenericMovieWriter::CanNotFormat);
    return;
    }

  this->Error = 0;
  this->Time = 0;
}

//---------------------------------------------------------------------------
void vtkAVIWriter::Write()
{
  if (this->Error)
    {
    return;
    }

  // get the data
  vtkImageData* input = this->GetImageDataInput(0);
  this->GetInputAlgorithm(0, 0)->UpdateWholeExtent();
  int *wExtent = input->GetExtent();

  // get the pointer to the data
  unsigned char *ptr =
    (unsigned char *)(input->GetScalarPointer());

  int dataWidth = (((wExtent[1] - wExtent[0] + 1)*3+3)/4)*4;
  int srcWidth = (wExtent[1] - wExtent[0] + 1)*3;

  // copy the data to the clipboard
  unsigned char *dest
    = (unsigned char *)this->Internals->lpbi + this->Internals->lpbi->biSize;
  int i,j;
  for (i = 0; i < this->Internals->lpbi->biHeight; i++)
    {
    for (j = 0; j < this->Internals->lpbi->biWidth; j++)
      {
      *dest++ = ptr[2];
      *dest++ = ptr[1];
      *dest++ = *ptr;
      ptr += 3;
      }
    dest = dest + (dataWidth - srcWidth);
    }

  AVIStreamWrite(this->Internals->StreamCompressed,  // stream pointer
                 this->Time, // time of this frame
                 1,        // number to write
                 (LPBYTE) this->Internals->lpbi +    // pointer to data
                 this->Internals->lpbi->biSize,
                 this->Internals->lpbi->biSizeImage,  // size of this frame
                 AVIIF_KEYFRAME,       // flags....
                 NULL, NULL);
  this->Time++;
}

//---------------------------------------------------------------------------
void vtkAVIWriter::End()
{
  ::GlobalUnlock(this->Internals->hDIB);
  if (this->Internals->Stream)
    {
    AVIStreamClose(this->Internals->Stream);
    this->Internals->Stream = NULL;
    }

  if (this->Internals->StreamCompressed)
    {
    AVIStreamClose(this->Internals->StreamCompressed);
    this->Internals->StreamCompressed = NULL;
    }

  if (this->Internals->AVIFile)
    {
    AVIFileClose(this->Internals->AVIFile);
    this->Internals->AVIFile = NULL;
    }

  AVIFileExit();          // releases AVIFile library
}

//---------------------------------------------------------------------------
void vtkAVIWriter::PrintSelf(ostream& os, vtkIndent indent)
{
  this->Superclass::PrintSelf(os, indent);
  os << indent << "Rate: " << this->Rate << endl;
  os << indent << "Quality: " << this->Quality << endl;
  os << indent << "PromptCompressionOptions: " << (this->GetPromptCompressionOptions() ? "on":"off") << endl;
  os << indent << "CompressorFourCC: "
     << (this->CompressorFourCC ? this->CompressorFourCC : "(None)") << endl;
}

<|endoftext|>


<commit_before>// Copyright (c) 2015, 2016, 2017 LAAS-CNRS
// Authors: Florent Lamiraux
//
// This file is part of hpp-core.
// hpp-core 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, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-core 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 Lesser Public License for more details.  You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-core. If not, see <http://www.gnu.org/licenses/>.

#ifndef HPP_CORE_SRC_IMPLICIT_FUNCTION_HH
# define HPP_CORE_SRC_IMPLICIT_FUNCTION_HH

#include <Eigen/Geometry>
#include <hpp/pinocchio/device.hh>
#include <hpp/pinocchio/liegroup-space.hh>
#include <hpp/constraints/differentiable-function.hh>
#include <hpp/constraints/matrix-view.hh>
#include <hpp/constraints/tools.hh>
#include <hpp/core/comparison-type.hh>
#include <hpp/core/explicit-numerical-constraint.hh>

namespace hpp {
  namespace core {

    typedef hpp::pinocchio::liegroup::VectorSpaceOperation <3, false> R3;
    typedef hpp::pinocchio::liegroup::SpecialOrthogonalOperation <3> SO3;
    typedef hpp::pinocchio::liegroup::CartesianProductOperation <R3, SO3>
    R3xSO3;
    typedef se3::SpecialEuclideanOperation <3> SE3;
    typedef hpp::pinocchio::LiegroupType LiegroupType;

    struct JacobianVisitor : public boost::static_visitor <>
    {
      JacobianVisitor (vectorIn_t qOut, vectorIn_t f_qIn,
                       matrixIn_t Jf, const Eigen::MatrixBlocks <false, false>&
                       outJacobian, const Eigen::MatrixBlocks <false, false>&
                       inJacobian, matrixOut_t result) :
        qOut_ (qOut), f_qIn_ (f_qIn), Jf_ (Jf), outJacobian_ (outJacobian),
        inJacobian_ (inJacobian), result_ (result)
      {
      }

      template <typename LgT> void operator () (const LgT&);

      vectorIn_t qOut_, f_qIn_;
      matrixIn_t Jf_;
      const Eigen::MatrixBlocks <false, false>& outJacobian_;
      const Eigen::MatrixBlocks <false, false>& inJacobian_;
      matrixOut_t result_;
    }; // struct JacobianVisitor

    template <> inline void JacobianVisitor::operator () <R3xSO3 >
      (const R3xSO3&)
    {
      using Eigen::MatrixBlocks;
      using Eigen::BlockIndex;
      typedef hpp::constraints::BlockIndex BlockIndex;
      hppDout (info, "result_ = " << std::endl << result_);
      // Fill R^3 part
      assert (outJacobian_.nbRows () == 6);
      Eigen::MatrixBlocks <false, false> tmp (outJacobian_.block (0, 0, 3, 3));
      tmp.lview (result_) = matrix3_t::Identity ();
      hppDout (info, "result_ = " << std::endl << result_);
      // extract 3 top rows of inJacobian_
      segments_t cols (inJacobian_.cols ());
      MatrixBlocks <false, false> inJacobian
        (inJacobian_.block (0, 0, 3, BlockIndex::cardinal (cols)));
      inJacobian.lview (result_) = -Jf_.topRows <3> ();
      hppDout (info, "result_ = " << std::endl << result_);
      // Fill SO(3) part
      // extract 3 bottom rows of inJacobian_
      inJacobian = inJacobian_.block (3, 0, 3, BlockIndex::cardinal (cols));
      // extract 3x3 bottom left part of outJacobian_
      MatrixBlocks <false, false> outJacobian (outJacobian_.block (3, 3, 3, 3));
      assert (qOut_.size () == 7);
      assert (f_qIn_.size () == 7);
      matrix3_t R_out
        (Eigen::Quaterniond (qOut_.tail <4> ()).toRotationMatrix ());
      matrix3_t R_f
        (Eigen::Quaterniond (f_qIn_.tail <4> ()).toRotationMatrix ());
      // \f$R_f^T R_{out}\f$
      matrix3_t R_f_T_R_out (R_f.transpose () * R_out);
      matrix3_t Jlog_R_f_T_R_out;
      vector3_t r;
      value_type theta;
      constraints::logSO3 (R_f_T_R_out, theta, r);
      constraints::JlogSO3 (theta, r, Jlog_R_f_T_R_out);
      outJacobian.lview (result_) = Jlog_R_f_T_R_out;
      hppDout (info, "result_ = " << std::endl << result_);
      inJacobian.lview (result_) = -Jlog_R_f_T_R_out * R_out.transpose () *
        R_f * Jf_.bottomRows <3> ();
      hppDout (info, "result_ = " << std::endl << result_);
    }

    template <> inline void JacobianVisitor::operator () <SE3 > (const SE3&)
    {
      using Eigen::MatrixBlocks;
      using Eigen::BlockIndex;
      typedef hpp::constraints::BlockIndex BlockIndex;
      assert (outJacobian_.nbRows () == 6);
      // extract 3 top rows of inJacobian_
      assert (qOut_.size () == 7);
      assert (f_qIn_.size () == 7);
      matrix3_t Rout (Eigen::Quaterniond (qOut_.tail <4> ()).
                      toRotationMatrix ());
      vector3_t pOut (qOut_.head <3> ());
      Transform3f Mout (Rout, pOut);
      matrix3_t Rf (Eigen::Quaterniond (f_qIn_.tail <4> ()).
                    toRotationMatrix ());
      vector3_t pf (f_qIn_.head <3> ());
      Transform3f Mf (Rf, pf);
      // \f$Mf^{-1} M_{out}\f$
      Transform3f Mf_inverse_Mout (Mf.inverse () * Mout);
      matrix6_t Jlog_Mf_inverse_Mout;
      constraints::JlogSE3 (Mf_inverse_Mout, Jlog_Mf_inverse_Mout);
      outJacobian_.lview (result_) = Jlog_Mf_inverse_Mout;
      matrix_t inJ (6, Jf_.cols ());
      inJ.topRows <3> () =
        Rout.transpose () * se3::skew (pOut - pf) * Rf * Jf_.bottomRows <3> ()
        - Rout.transpose () * Rf * Jf_.topRows <3> ();
      inJ.bottomRows <3> () = -Rout.transpose () * Rf * Jf_.bottomRows <3> ();
      inJacobian_.lview (result_) = Jlog_Mf_inverse_Mout * inJ;
    }

    template <typename LgT> void JacobianVisitor::operator () (const LgT&)
    {
      for (size_type i=0; i<outJacobian_.nbRows (); ++i) {
        outJacobian_.lview (result_) (i, i) = 1;
      }
      inJacobian_.lview (result_) = -Jf_;
    }

    HPP_PREDEF_CLASS (ImplicitFunction);
    typedef boost::shared_ptr <ImplicitFunction> ImplicitFunctionPtr_t;

    /// Function of the form f (q) = q2 - g (q1)
    ///
    /// where
    ///  \li q2 is a vector composed of a subset of configuration variables of
    ///      q,
    ///  \li q1 is the vector composed of the other configuration variables of
    ///      q,
    ///  g is a differentiable function with values in  a Lie group.
    class ImplicitFunction : public DifferentiableFunction
    {
    public:
      static ImplicitFunctionPtr_t create
      (const DevicePtr_t& robot, const DifferentiableFunctionPtr_t& function,
       const segments_t& inputConf, const segments_t& inputVelocity,
       const segments_t& outputConf, const segments_t& outputVelocity)
      {
	ImplicitFunction* ptr = new ImplicitFunction
	  (robot, function, inputConf, inputVelocity, outputConf,
           outputVelocity);
	return ImplicitFunctionPtr_t (ptr);
      }

    protected:
      ImplicitFunction (const DevicePtr_t& robot,
			const DifferentiableFunctionPtr_t& function,
			const segments_t& inputConf,
			const segments_t& inputVelocity,
                        const segments_t& outputConf,
			const segments_t& outputVelocity)
	: DifferentiableFunction (robot->configSize (), robot->numberDof (),
				  LiegroupSpace::Rn
                                  (function->outputSpace ()->nv ())),
	  robot_ (robot), inputToOutput_ (function),
          inputConfIntervals_ (inputConf),
	  inputDerivIntervals_ (inputVelocity),
          outputConfIntervals_ (outputConf),
	  outputDerivIntervals_ (outputVelocity), outJacobian_ (),
          inJacobian_ (), f_qIn_ (function->outputSpace ()),
          qOut_ (function->outputSpace ()), result_ (outputSpace ())
      {
	// Check input consistency
	// Each configuration variable is either input or output
	assert (function->inputSize () + function->outputSize () <=
		robot->configSize ());
	// Each velocity variable is either input or output
	assert (function->inputDerivativeSize () +
		function->outputDerivativeSize () <= robot->numberDof ());
	qIn_.resize (function->inputSize ());
	Jf_.resize (function->outputDerivativeSize (),
                    function->inputDerivativeSize ());
	size_type size = 0;
	// Sum of configuration output interval sizes equal function output size
	for (segments_t::const_iterator it = outputConf.begin ();
	     it != outputConf.end (); ++it) {
	  size += it->second;
	}
	assert (size == function->outputSize ());
	// Sum of velocity output interval sizes equal function output
	// derivative size
	size = 0;
	for (segments_t::const_iterator it = outputVelocity.begin ();
	     it != outputVelocity.end (); ++it) {
	  size += it->second;
	}
	assert (size == function->outputDerivativeSize ());
        computeJacobianBlocks ();
      }

      /// Compute q_{output} - f (q_{input})
      void impl_compute (LiegroupElement& result, vectorIn_t argument) const
      {
        hppDout (info, "argument=" << argument.transpose ());
        // Store q_{output} in result
	size_type index = 0;
	for (segments_t::const_iterator it = outputConfIntervals_.begin ();
	     it != outputConfIntervals_.end (); ++it) {
	  qOut_.vector ().segment (index, it->second) =
	    argument.segment (it->first, it->second);
	  index += it->second;
	}
        hppDout (info, "qOut_=" << qOut_);
        assert (index == qOut_.space ()->nq ());
	index = 0;
        // fill in q_{input}
	for (segments_t::const_iterator it = inputConfIntervals_.begin ();
	     it != inputConfIntervals_.end (); ++it) {
	  qIn_.segment (index, it->second) =
	    argument.segment (it->first, it->second);
	  index += it->second;
	}
        hppDout (info, "qIn_=" << qIn_);
        assert (index == qIn_.size ());
        // compute  f (q_{input}) -> output_
	inputToOutput_->value (f_qIn_, qIn_);
        hppDout (info, "f_qIn_=" << f_qIn_);
	result.vector () = qOut_ - f_qIn_;
        hppDout (info, "result=" << result);
      }

      void impl_jacobian (matrixOut_t jacobian, vectorIn_t arg) const
      {
	jacobian.setZero ();
	size_type row = 0;
        size_type iq = 0, iv = 0, nq, nv;
        std::size_t rank = 0;
        impl_compute (result_, arg);
        inputToOutput_->jacobian (Jf_, qIn_);
        hppDout (info, "Jf_=" << std::endl << Jf_);
        // Fill Jacobian by set of lines corresponding to the types of Lie group
        // that compose the outputspace of input to output function.
        segments_t outConfSegments (outputConfIntervals_);
        for (std::vector <LiegroupType>::const_iterator it =
               inputToOutput_->outputSpace ()-> liegroupTypes ().begin ();
             it != inputToOutput_->outputSpace ()-> liegroupTypes ().end ();
             ++it) {
          nq = inputToOutput_->outputSpace ()->nq (rank);
          nv = inputToOutput_->outputSpace ()->nv (rank);
          JacobianVisitor v (qOut_.vector ().segment (iq, nq),
                             f_qIn_.vector ().segment (iq, nq),
                             Jf_.middleRows (iv, nv), outJacobian_ [rank],
                             inJacobian_ [rank], jacobian.middleRows (iv, nv));
          boost::apply_visitor (v, *it);
          iq += nq;
          iv += nv;
          ++rank;
        }
      }

    private:
      void computeJacobianBlocks ()
      {
        segments_t remainingCols (outputDerivIntervals_);
        segments_t cols;
        size_type iv = 0, nv;
        std::size_t rank = 0;
        for (std::vector <LiegroupType>::const_iterator it =
               inputToOutput_->outputSpace ()->liegroupTypes ().begin ();
             it != inputToOutput_->outputSpace ()->liegroupTypes ().end ();
             ++it) {
          nv = inputToOutput_->outputSpace ()->nv (rank);
          cols = BlockIndex::split (remainingCols, nv);
          segments_t rows (1, std::make_pair (iv, nv));
          outJacobian_.push_back (Eigen::MatrixBlocks <false, false>
                                  (rows, cols));
          inJacobian_.push_back (Eigen::MatrixBlocks <false, false>
                                 (rows, inputDerivIntervals_));
          //
          iv += nv;
          ++rank;
        }
      }

      DevicePtr_t robot_;
      DifferentiableFunctionPtr_t inputToOutput_;
      segments_t inputConfIntervals_;
      segments_t inputDerivIntervals_;
      segments_t outputConfIntervals_;
      segments_t outputDerivIntervals_;
      std::vector <Eigen::MatrixBlocks <false, false> > outJacobian_;
      std::vector <Eigen::MatrixBlocks <false, false> > inJacobian_;
      mutable vector_t qIn_;
      mutable LiegroupElement f_qIn_, qOut_;
      mutable LiegroupElement result_;
      // Jacobian of explicit function
      mutable matrix_t Jf_;
    }; // class ImplicitFunction

  } // namespace core
} // namespace hpp

#endif // HPP_CORE_SRC_IMPLICIT_FUNCTION_HH
<commit_msg>Add getter to mapping from input to output in ImplicitFunction.<commit_after>// Copyright (c) 2015, 2016, 2017 LAAS-CNRS
// Authors: Florent Lamiraux
//
// This file is part of hpp-core.
// hpp-core 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, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-core 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 Lesser Public License for more details.  You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-core. If not, see <http://www.gnu.org/licenses/>.

#ifndef HPP_CORE_SRC_IMPLICIT_FUNCTION_HH
# define HPP_CORE_SRC_IMPLICIT_FUNCTION_HH

#include <Eigen/Geometry>
#include <hpp/pinocchio/device.hh>
#include <hpp/pinocchio/liegroup-space.hh>
#include <hpp/constraints/differentiable-function.hh>
#include <hpp/constraints/matrix-view.hh>
#include <hpp/constraints/tools.hh>
#include <hpp/core/comparison-type.hh>
#include <hpp/core/explicit-numerical-constraint.hh>

namespace hpp {
  namespace core {

    typedef hpp::pinocchio::liegroup::VectorSpaceOperation <3, false> R3;
    typedef hpp::pinocchio::liegroup::SpecialOrthogonalOperation <3> SO3;
    typedef hpp::pinocchio::liegroup::CartesianProductOperation <R3, SO3>
    R3xSO3;
    typedef se3::SpecialEuclideanOperation <3> SE3;
    typedef hpp::pinocchio::LiegroupType LiegroupType;

    struct JacobianVisitor : public boost::static_visitor <>
    {
      JacobianVisitor (vectorIn_t qOut, vectorIn_t f_qIn,
                       matrixIn_t Jf, const Eigen::MatrixBlocks <false, false>&
                       outJacobian, const Eigen::MatrixBlocks <false, false>&
                       inJacobian, matrixOut_t result) :
        qOut_ (qOut), f_qIn_ (f_qIn), Jf_ (Jf), outJacobian_ (outJacobian),
        inJacobian_ (inJacobian), result_ (result)
      {
      }

      template <typename LgT> void operator () (const LgT&);

      vectorIn_t qOut_, f_qIn_;
      matrixIn_t Jf_;
      const Eigen::MatrixBlocks <false, false>& outJacobian_;
      const Eigen::MatrixBlocks <false, false>& inJacobian_;
      matrixOut_t result_;
    }; // struct JacobianVisitor

    template <> inline void JacobianVisitor::operator () <R3xSO3 >
      (const R3xSO3&)
    {
      using Eigen::MatrixBlocks;
      using Eigen::BlockIndex;
      typedef hpp::constraints::BlockIndex BlockIndex;
      hppDout (info, "result_ = " << std::endl << result_);
      // Fill R^3 part
      assert (outJacobian_.nbRows () == 6);
      Eigen::MatrixBlocks <false, false> tmp (outJacobian_.block (0, 0, 3, 3));
      tmp.lview (result_) = matrix3_t::Identity ();
      hppDout (info, "result_ = " << std::endl << result_);
      // extract 3 top rows of inJacobian_
      segments_t cols (inJacobian_.cols ());
      MatrixBlocks <false, false> inJacobian
        (inJacobian_.block (0, 0, 3, BlockIndex::cardinal (cols)));
      inJacobian.lview (result_) = -Jf_.topRows <3> ();
      hppDout (info, "result_ = " << std::endl << result_);
      // Fill SO(3) part
      // extract 3 bottom rows of inJacobian_
      inJacobian = inJacobian_.block (3, 0, 3, BlockIndex::cardinal (cols));
      // extract 3x3 bottom left part of outJacobian_
      MatrixBlocks <false, false> outJacobian (outJacobian_.block (3, 3, 3, 3));
      assert (qOut_.size () == 7);
      assert (f_qIn_.size () == 7);
      matrix3_t R_out
        (Eigen::Quaterniond (qOut_.tail <4> ()).toRotationMatrix ());
      matrix3_t R_f
        (Eigen::Quaterniond (f_qIn_.tail <4> ()).toRotationMatrix ());
      // \f$R_f^T R_{out}\f$
      matrix3_t R_f_T_R_out (R_f.transpose () * R_out);
      matrix3_t Jlog_R_f_T_R_out;
      vector3_t r;
      value_type theta;
      constraints::logSO3 (R_f_T_R_out, theta, r);
      constraints::JlogSO3 (theta, r, Jlog_R_f_T_R_out);
      outJacobian.lview (result_) = Jlog_R_f_T_R_out;
      hppDout (info, "result_ = " << std::endl << result_);
      inJacobian.lview (result_) = -Jlog_R_f_T_R_out * R_out.transpose () *
        R_f * Jf_.bottomRows <3> ();
      hppDout (info, "result_ = " << std::endl << result_);
    }

    template <> inline void JacobianVisitor::operator () <SE3 > (const SE3&)
    {
      using Eigen::MatrixBlocks;
      using Eigen::BlockIndex;
      typedef hpp::constraints::BlockIndex BlockIndex;
      assert (outJacobian_.nbRows () == 6);
      // extract 3 top rows of inJacobian_
      assert (qOut_.size () == 7);
      assert (f_qIn_.size () == 7);
      matrix3_t Rout (Eigen::Quaterniond (qOut_.tail <4> ()).
                      toRotationMatrix ());
      vector3_t pOut (qOut_.head <3> ());
      Transform3f Mout (Rout, pOut);
      matrix3_t Rf (Eigen::Quaterniond (f_qIn_.tail <4> ()).
                    toRotationMatrix ());
      vector3_t pf (f_qIn_.head <3> ());
      Transform3f Mf (Rf, pf);
      // \f$Mf^{-1} M_{out}\f$
      Transform3f Mf_inverse_Mout (Mf.inverse () * Mout);
      matrix6_t Jlog_Mf_inverse_Mout;
      constraints::JlogSE3 (Mf_inverse_Mout, Jlog_Mf_inverse_Mout);
      outJacobian_.lview (result_) = Jlog_Mf_inverse_Mout;
      matrix_t inJ (6, Jf_.cols ());
      inJ.topRows <3> () =
        Rout.transpose () * se3::skew (pOut - pf) * Rf * Jf_.bottomRows <3> ()
        - Rout.transpose () * Rf * Jf_.topRows <3> ();
      inJ.bottomRows <3> () = -Rout.transpose () * Rf * Jf_.bottomRows <3> ();
      inJacobian_.lview (result_) = Jlog_Mf_inverse_Mout * inJ;
    }

    template <typename LgT> void JacobianVisitor::operator () (const LgT&)
    {
      for (size_type i=0; i<outJacobian_.nbRows (); ++i) {
        outJacobian_.lview (result_) (i, i) = 1;
      }
      inJacobian_.lview (result_) = -Jf_;
    }

    HPP_PREDEF_CLASS (ImplicitFunction);
    typedef boost::shared_ptr <ImplicitFunction> ImplicitFunctionPtr_t;

    /// Function of the form f (q) = q2 - g (q1)
    ///
    /// where
    ///  \li q2 is a vector composed of a subset of configuration variables of
    ///      q,
    ///  \li q1 is the vector composed of the other configuration variables of
    ///      q,
    ///  g is a differentiable function with values in  a Lie group.
    class ImplicitFunction : public DifferentiableFunction
    {
    public:
      static ImplicitFunctionPtr_t create
      (const DevicePtr_t& robot, const DifferentiableFunctionPtr_t& function,
       const segments_t& inputConf, const segments_t& inputVelocity,
       const segments_t& outputConf, const segments_t& outputVelocity)
      {
	ImplicitFunction* ptr = new ImplicitFunction
	  (robot, function, inputConf, inputVelocity, outputConf,
           outputVelocity);
	return ImplicitFunctionPtr_t (ptr);
      }

      /// Get function that maps input variables to output variables
      const DifferentiableFunctionPtr_t& inputToOutput () const
      {
        return inputToOutput_;
      }

    protected:
      ImplicitFunction (const DevicePtr_t& robot,
			const DifferentiableFunctionPtr_t& function,
			const segments_t& inputConf,
			const segments_t& inputVelocity,
                        const segments_t& outputConf,
			const segments_t& outputVelocity)
	: DifferentiableFunction (robot->configSize (), robot->numberDof (),
				  LiegroupSpace::Rn
                                  (function->outputSpace ()->nv ())),
	  robot_ (robot), inputToOutput_ (function),
          inputConfIntervals_ (inputConf),
	  inputDerivIntervals_ (inputVelocity),
          outputConfIntervals_ (outputConf),
	  outputDerivIntervals_ (outputVelocity), outJacobian_ (),
          inJacobian_ (), f_qIn_ (function->outputSpace ()),
          qOut_ (function->outputSpace ()), result_ (outputSpace ())
      {
	// Check input consistency
	// Each configuration variable is either input or output
	assert (function->inputSize () + function->outputSize () <=
		robot->configSize ());
	// Each velocity variable is either input or output
	assert (function->inputDerivativeSize () +
		function->outputDerivativeSize () <= robot->numberDof ());
	qIn_.resize (function->inputSize ());
	Jf_.resize (function->outputDerivativeSize (),
                    function->inputDerivativeSize ());
	size_type size = 0;
	// Sum of configuration output interval sizes equal function output size
	for (segments_t::const_iterator it = outputConf.begin ();
	     it != outputConf.end (); ++it) {
	  size += it->second;
	}
	assert (size == function->outputSize ());
	// Sum of velocity output interval sizes equal function output
	// derivative size
	size = 0;
	for (segments_t::const_iterator it = outputVelocity.begin ();
	     it != outputVelocity.end (); ++it) {
	  size += it->second;
	}
	assert (size == function->outputDerivativeSize ());
        computeJacobianBlocks ();
      }

      /// Compute q_{output} - f (q_{input})
      void impl_compute (LiegroupElement& result, vectorIn_t argument) const
      {
        hppDout (info, "argument=" << argument.transpose ());
        // Store q_{output} in result
	size_type index = 0;
	for (segments_t::const_iterator it = outputConfIntervals_.begin ();
	     it != outputConfIntervals_.end (); ++it) {
	  qOut_.vector ().segment (index, it->second) =
	    argument.segment (it->first, it->second);
	  index += it->second;
	}
        hppDout (info, "qOut_=" << qOut_);
        assert (index == qOut_.space ()->nq ());
	index = 0;
        // fill in q_{input}
	for (segments_t::const_iterator it = inputConfIntervals_.begin ();
	     it != inputConfIntervals_.end (); ++it) {
	  qIn_.segment (index, it->second) =
	    argument.segment (it->first, it->second);
	  index += it->second;
	}
        hppDout (info, "qIn_=" << qIn_);
        assert (index == qIn_.size ());
        // compute  f (q_{input}) -> output_
	inputToOutput_->value (f_qIn_, qIn_);
        hppDout (info, "f_qIn_=" << f_qIn_);
	result.vector () = qOut_ - f_qIn_;
        hppDout (info, "result=" << result);
      }

      void impl_jacobian (matrixOut_t jacobian, vectorIn_t arg) const
      {
	jacobian.setZero ();
	size_type row = 0;
        size_type iq = 0, iv = 0, nq, nv;
        std::size_t rank = 0;
        impl_compute (result_, arg);
        inputToOutput_->jacobian (Jf_, qIn_);
        hppDout (info, "Jf_=" << std::endl << Jf_);
        // Fill Jacobian by set of lines corresponding to the types of Lie group
        // that compose the outputspace of input to output function.
        segments_t outConfSegments (outputConfIntervals_);
        for (std::vector <LiegroupType>::const_iterator it =
               inputToOutput_->outputSpace ()-> liegroupTypes ().begin ();
             it != inputToOutput_->outputSpace ()-> liegroupTypes ().end ();
             ++it) {
          nq = inputToOutput_->outputSpace ()->nq (rank);
          nv = inputToOutput_->outputSpace ()->nv (rank);
          JacobianVisitor v (qOut_.vector ().segment (iq, nq),
                             f_qIn_.vector ().segment (iq, nq),
                             Jf_.middleRows (iv, nv), outJacobian_ [rank],
                             inJacobian_ [rank], jacobian.middleRows (iv, nv));
          boost::apply_visitor (v, *it);
          iq += nq;
          iv += nv;
          ++rank;
        }
      }

    private:
      void computeJacobianBlocks ()
      {
        segments_t remainingCols (outputDerivIntervals_);
        segments_t cols;
        size_type iv = 0, nv;
        std::size_t rank = 0;
        for (std::vector <LiegroupType>::const_iterator it =
               inputToOutput_->outputSpace ()->liegroupTypes ().begin ();
             it != inputToOutput_->outputSpace ()->liegroupTypes ().end ();
             ++it) {
          nv = inputToOutput_->outputSpace ()->nv (rank);
          cols = BlockIndex::split (remainingCols, nv);
          segments_t rows (1, std::make_pair (iv, nv));
          outJacobian_.push_back (Eigen::MatrixBlocks <false, false>
                                  (rows, cols));
          inJacobian_.push_back (Eigen::MatrixBlocks <false, false>
                                 (rows, inputDerivIntervals_));
          //
          iv += nv;
          ++rank;
        }
      }

      DevicePtr_t robot_;
      DifferentiableFunctionPtr_t inputToOutput_;
      segments_t inputConfIntervals_;
      segments_t inputDerivIntervals_;
      segments_t outputConfIntervals_;
      segments_t outputDerivIntervals_;
      std::vector <Eigen::MatrixBlocks <false, false> > outJacobian_;
      std::vector <Eigen::MatrixBlocks <false, false> > inJacobian_;
      mutable vector_t qIn_;
      mutable LiegroupElement f_qIn_, qOut_;
      mutable LiegroupElement result_;
      // Jacobian of explicit function
      mutable matrix_t Jf_;
    }; // class ImplicitFunction

  } // namespace core
} // namespace hpp

#endif // HPP_CORE_SRC_IMPLICIT_FUNCTION_HH
<|endoftext|>


<commit_before>/*****************************************************************************
 * Project: RooFit                                                           *
 * Package: RooFitCore                                                       *
 * @(#)root/roofitcore:$Id$
 * Authors:                                                                  *
 *   WV, Wouter Verkerke, UC Santa Barbara, verkerke@slac.stanford.edu       *
 *   DK, David Kirkby,    UC Irvine,         dkirkby@uci.edu                 *
 *                                                                           *
 * Copyright (c) 2000-2005, Regents of the University of California          *
 *                          and Stanford University. All rights reserved.    *
 *                                                                           *
 * Redistribution and use in source and binary forms,                        *
 * with or without modification, are permitted according to the terms        *
 * listed in LICENSE (http://roofit.sourceforge.net/license.txt)             *
 *****************************************************************************/

//////////////////////////////////////////////////////////////////////////////
//
// BEGIN_HTML
// RooGenFitStudy is an abstract base class for RooStudyManager modules
//
// END_HTML
//

#include "RooFit.h"
#include "Riostream.h"

#include "RooGenFitStudy.h"
#include "RooWorkspace.h"
#include "RooMsgService.h"
#include "RooDataSet.h"
#include "RooAbsPdf.h"
#include "RooRealVar.h"
#include "RooGlobalFunc.h"
#include "RooFitResult.h"

using namespace std ;

ClassImp(RooGenFitStudy)
  ;

//_____________________________________________________________________________
RooGenFitStudy::RooGenFitStudy(const char* name, const char* title) :
  RooAbsStudy(name?name:"RooGenFitStudy",title?title:"RooGenFitStudy"),
  _genPdf(0),
  _fitPdf(0),
  _genSpec(0),
  _nllVar(0),
  _ngenVar(0),
  _params(0),
  _initParams(0)
{
  // Constructor
}

//_____________________________________________________________________________
RooGenFitStudy::RooGenFitStudy(const RooGenFitStudy& other) :
  RooAbsStudy(other),
  _genPdfName(other._genPdfName),
  _genObsName(other._genObsName),
  _fitPdfName(other._fitPdfName),
  _fitObsName(other._fitObsName),
  _genPdf(0),
  _fitPdf(0),
  _genSpec(0),
  _nllVar(0),
  _ngenVar(0),
  _params(0),
  _initParams(0)
{
  // Copy constructor
  TIterator* giter = other._genOpts.MakeIterator() ;
  TObject* o ;
  while((o=giter->Next())) {
    _genOpts.Add(o->Clone()) ;
  }
  delete giter ;

  TIterator* fiter = other._fitOpts.MakeIterator() ;
  while((o=fiter->Next())) {
    _fitOpts.Add(o->Clone()) ;
  }
  delete fiter ;

}

//_____________________________________________________________________________
RooGenFitStudy::~RooGenFitStudy()
{
  if (_params) delete _params ;
}

//_____________________________________________________________________________
Bool_t RooGenFitStudy::attach(RooWorkspace& w)
{
  // Function called after insertion into workspace
  Bool_t ret = kFALSE ;

  RooAbsPdf* pdf = w.pdf(_genPdfName.c_str()) ;
  if (pdf) {
    _genPdf = pdf ;
  } else {
    coutE(InputArguments) << "RooGenFitStudy(" << GetName() << ") ERROR: generator p.d.f named " << _genPdfName << " not found in workspace " << w.GetName() << endl ;
    ret = kTRUE ;
  }

  _genObs.add(w.argSet(_genObsName.c_str())) ;
  if (_genObs.getSize()==0) {
    coutE(InputArguments) << "RooGenFitStudy(" << GetName() << ") ERROR: no generator observables defined" << endl ;
    ret = kTRUE ;
  }

  pdf = w.pdf(_fitPdfName.c_str()) ;
  if (pdf) {
    _fitPdf = pdf ;
  } else {
    coutE(InputArguments) << "RooGenFitStudy(" << GetName() << ") ERROR: fitting p.d.f named " << _fitPdfName << " not found in workspace " << w.GetName() << endl ;
    ret = kTRUE ;
  }

  _fitObs.add(w.argSet(_fitObsName.c_str())) ;
  if (_fitObs.getSize()==0) {
    coutE(InputArguments) << "RooGenFitStudy(" << GetName() << ") ERROR: no fitting observables defined" << endl ;
    ret = kTRUE ;
  }

  return ret ;
}

//_____________________________________________________________________________
void RooGenFitStudy::setGenConfig(const char* pdfName, const char* obsName, const RooCmdArg& arg1,const RooCmdArg& arg2,const RooCmdArg& arg3)
{
  _genPdfName = pdfName ;
  _genObsName = obsName ;
  _genOpts.Add(arg1.Clone()) ;
  _genOpts.Add(arg2.Clone()) ;
  _genOpts.Add(arg3.Clone()) ;
}

//_____________________________________________________________________________
void RooGenFitStudy::setFitConfig(const char* pdfName, const char* obsName, const RooCmdArg& arg1,const RooCmdArg& arg2,const RooCmdArg& arg3)
{
  _fitPdfName = pdfName ;
  _fitObsName = obsName ;
  _fitOpts.Add(arg1.Clone()) ;
  _fitOpts.Add(arg2.Clone()) ;
  _fitOpts.Add(arg3.Clone()) ;
}

//_____________________________________________________________________________
Bool_t RooGenFitStudy::initialize()
{
  // One-time initialization of study

  _nllVar = new RooRealVar("NLL","-log(Likelihood)",0) ;
  _ngenVar = new RooRealVar("ngen","number of generated events",0) ;

  _params = _fitPdf->getParameters(_genObs) ;
  _initParams = (RooArgSet*) _params->snapshot() ;
  _params->add(*_nllVar) ;
  _params->add(*_ngenVar) ;

  _genSpec = _genPdf->prepareMultiGen(_genObs,(RooCmdArg&)*_genOpts.At(0),(RooCmdArg&)*_genOpts.At(1),(RooCmdArg&)*_genOpts.At(2)) ;

  registerSummaryOutput(*_params) ;
  return kFALSE ;
}

//_____________________________________________________________________________
Bool_t RooGenFitStudy::execute()
{
  // Execute one study iteration
  *_params = *_initParams ;
  RooDataSet* data = _genPdf->generate(*_genSpec) ;
  RooFitResult* fr  = _fitPdf->fitTo(*data,RooFit::Save(kTRUE),(RooCmdArg&)*_fitOpts.At(0),(RooCmdArg&)*_fitOpts.At(1),(RooCmdArg&)*_fitOpts.At(2)) ;

  if (fr->status()==0) {
    _ngenVar->setVal(data->sumEntries()) ;
    _nllVar->setVal(fr->minNll()) ;
    storeSummaryOutput(*_params) ;
    storeDetailedOutput(*fr) ;
  }

  delete data ;
  return kFALSE ;
}

//_____________________________________________________________________________
Bool_t RooGenFitStudy::finalize()
{
  // Finalization of study
  delete _params ;
  delete _nllVar ;
  delete _ngenVar ;
  delete _initParams ;
  delete _genSpec ;
  _params = 0 ;
  _nllVar = 0 ;
  _ngenVar = 0 ;
  _initParams = 0 ;
  _genSpec = 0 ;

  return kFALSE ;
}

//_____________________________________________________________________________
void RooGenFitStudy::Print(Option_t */*options*/) const
{
}

<commit_msg><commit_after>/*****************************************************************************
 * Project: RooFit                                                           *
 * Package: RooFitCore                                                       *
 * @(#)root/roofitcore:$Id$
 * Authors:                                                                  *
 *   WV, Wouter Verkerke, UC Santa Barbara, verkerke@slac.stanford.edu       *
 *   DK, David Kirkby,    UC Irvine,         dkirkby@uci.edu                 *
 *                                                                           *
 * Copyright (c) 2000-2005, Regents of the University of California          *
 *                          and Stanford University. All rights reserved.    *
 *                                                                           *
 * Redistribution and use in source and binary forms,                        *
 * with or without modification, are permitted according to the terms        *
 * listed in LICENSE (http://roofit.sourceforge.net/license.txt)             *
 *****************************************************************************/

//////////////////////////////////////////////////////////////////////////////
//
// BEGIN_HTML
// RooGenFitStudy is an abstract base class for RooStudyManager modules
//
// END_HTML
//

#include "RooFit.h"
#include "Riostream.h"

#include "RooGenFitStudy.h"
#include "RooWorkspace.h"
#include "RooMsgService.h"
#include "RooDataSet.h"
#include "RooAbsPdf.h"
#include "RooRealVar.h"
#include "RooGlobalFunc.h"
#include "RooFitResult.h"

using namespace std ;

ClassImp(RooGenFitStudy)
  ;

//_____________________________________________________________________________
RooGenFitStudy::RooGenFitStudy(const char* name, const char* title) :
  RooAbsStudy(name?name:"RooGenFitStudy",title?title:"RooGenFitStudy"),
  _genPdf(0),
  _fitPdf(0),
  _genSpec(0),
  _nllVar(0),
  _ngenVar(0),
  _params(0),
  _initParams(0)
{
  // Constructor
}

//_____________________________________________________________________________
RooGenFitStudy::RooGenFitStudy(const RooGenFitStudy& other) :
  RooAbsStudy(other),
  _genPdfName(other._genPdfName),
  _genObsName(other._genObsName),
  _fitPdfName(other._fitPdfName),
  _fitObsName(other._fitObsName),
  _genPdf(0),
  _fitPdf(0),
  _genSpec(0),
  _nllVar(0),
  _ngenVar(0),
  _params(0),
  _initParams(0)
{
  // Copy constructor
  TIterator* giter = other._genOpts.MakeIterator() ;
  TObject* o ;
  while((o=giter->Next())) {
    _genOpts.Add(o->Clone()) ;
  }
  delete giter ;

  TIterator* fiter = other._fitOpts.MakeIterator() ;
  while((o=fiter->Next())) {
    _fitOpts.Add(o->Clone()) ;
  }
  delete fiter ;

}

//_____________________________________________________________________________
RooGenFitStudy::~RooGenFitStudy()
{
  if (_params) delete _params ;
}

//_____________________________________________________________________________
Bool_t RooGenFitStudy::attach(RooWorkspace& w)
{
  // Function called after insertion into workspace
  Bool_t ret = kFALSE ;

  RooAbsPdf* pdf = w.pdf(_genPdfName.c_str()) ;
  if (pdf) {
    _genPdf = pdf ;
  } else {
    coutE(InputArguments) << "RooGenFitStudy(" << GetName() << ") ERROR: generator p.d.f named " << _genPdfName << " not found in workspace " << w.GetName() << endl ;
    ret = kTRUE ;
  }

  _genObs.add(w.argSet(_genObsName.c_str())) ;
  if (_genObs.getSize()==0) {
    coutE(InputArguments) << "RooGenFitStudy(" << GetName() << ") ERROR: no generator observables defined" << endl ;
    ret = kTRUE ;
  }

  pdf = w.pdf(_fitPdfName.c_str()) ;
  if (pdf) {
    _fitPdf = pdf ;
  } else {
    coutE(InputArguments) << "RooGenFitStudy(" << GetName() << ") ERROR: fitting p.d.f named " << _fitPdfName << " not found in workspace " << w.GetName() << endl ;
    ret = kTRUE ;
  }

  _fitObs.add(w.argSet(_fitObsName.c_str())) ;
  if (_fitObs.getSize()==0) {
    coutE(InputArguments) << "RooGenFitStudy(" << GetName() << ") ERROR: no fitting observables defined" << endl ;
    ret = kTRUE ;
  }

  return ret ;
}

//_____________________________________________________________________________
void RooGenFitStudy::setGenConfig(const char* pdfName, const char* obsName, const RooCmdArg& arg1,const RooCmdArg& arg2,const RooCmdArg& arg3)
{
  _genPdfName = pdfName ;
  _genObsName = obsName ;
  _genOpts.Add(arg1.Clone()) ;
  _genOpts.Add(arg2.Clone()) ;
  _genOpts.Add(arg3.Clone()) ;
}

//_____________________________________________________________________________
void RooGenFitStudy::setFitConfig(const char* pdfName, const char* obsName, const RooCmdArg& arg1,const RooCmdArg& arg2,const RooCmdArg& arg3)
{
  _fitPdfName = pdfName ;
  _fitObsName = obsName ;
  _fitOpts.Add(arg1.Clone()) ;
  _fitOpts.Add(arg2.Clone()) ;
  _fitOpts.Add(arg3.Clone()) ;
}

//_____________________________________________________________________________
Bool_t RooGenFitStudy::initialize()
{
  // One-time initialization of study

  _nllVar = new RooRealVar("NLL","-log(Likelihood)",0) ;
  _ngenVar = new RooRealVar("ngen","number of generated events",0) ;

  _params = _fitPdf->getParameters(_genObs) ;
  _initParams = (RooArgSet*) _params->snapshot() ;
  _params->add(*_nllVar) ;
  _params->add(*_ngenVar) ;

  _genSpec = _genPdf->prepareMultiGen(_genObs,(RooCmdArg&)*_genOpts.At(0),(RooCmdArg&)*_genOpts.At(1),(RooCmdArg&)*_genOpts.At(2)) ;

  registerSummaryOutput(*_params) ;
  return kFALSE ;
}

//_____________________________________________________________________________
Bool_t RooGenFitStudy::execute()
{
  // Execute one study iteration
  *_params = *_initParams ;
  RooDataSet* data = _genPdf->generate(*_genSpec) ;
  RooFitResult* fr  = _fitPdf->fitTo(*data,RooFit::Save(kTRUE),(RooCmdArg&)*_fitOpts.At(0),(RooCmdArg&)*_fitOpts.At(1),(RooCmdArg&)*_fitOpts.At(2)) ;

  if (fr->status()==0) {
    _ngenVar->setVal(data->sumEntries()) ;
    _nllVar->setVal(fr->minNll()) ;
    storeSummaryOutput(*_params) ;
    storeDetailedOutput(*fr) ;
  }

  delete data ;
  return kFALSE ;
}

//_____________________________________________________________________________
Bool_t RooGenFitStudy::finalize()
{
  // Finalization of study
  delete _params ;
  delete _nllVar ;
  delete _ngenVar ;
  delete _initParams ;
  delete _genSpec ;
  _params = 0 ;
  _nllVar = 0 ;
  _ngenVar = 0 ;
  _initParams = 0 ;
  _genSpec = 0 ;

  return kFALSE ;
}

//_____________________________________________________________________________
void RooGenFitStudy::Print(Option_t* /*options*/) const
{
}

<|endoftext|>


<commit_before>#include <cstdio>
#include <functional>
#include <map>

// available types
enum class TypeEnum : int
{
    BOOL,
    INT,
    DOUBLE
};

// translate type enum to actual type
template<TypeEnum e> struct TypeOfTypeEnum;

template<> struct TypeOfTypeEnum<TypeEnum::BOOL>
{
    typedef bool type;
};

template<> struct TypeOfTypeEnum<TypeEnum::INT>
{
    typedef int type;
};

template<> struct TypeOfTypeEnum<TypeEnum::DOUBLE>
{
    typedef double type;
};

// Our variant type
struct Cont
{
    Cont(bool b)
        : type(TypeEnum::BOOL)
          , asBool(b)
    {}
    Cont(int i)
        : type(TypeEnum::INT)
          , asInt(i)
    {}
    Cont(double d)
        : type(TypeEnum::DOUBLE)
          , asDouble(d)
    {}

    TypeEnum type;
    union
    {
        bool asBool;
        int asInt;
        double asDouble;
    };
};

// read correct data based on type enum
template<TypeEnum e, typename T = typename TypeOfTypeEnum<e>::type>
 constexpr T const& GetMemberByType(Cont const& c);

template<>
constexpr bool const& GetMemberByType<TypeEnum::BOOL>(Cont const& c)
{
    return c.asBool;
}
template<>
constexpr int const& GetMemberByType<TypeEnum::INT>(Cont const& c)
{
    return c.asInt;
}
template<>
constexpr double const& GetMemberByType<TypeEnum::DOUBLE>(Cont const& c)
{
    return c.asDouble;
}

// utility template to map the correct template instantiation to the correct type enum type type type
template<template<TypeEnum, typename... Args> typename T, typename... MoreArgs>
std::map<TypeEnum, std::function<void(Cont const&, MoreArgs...)>> const& GenerateMap()
{
    static decltype(GenerateMap<T, MoreArgs...>()) rval {
        { TypeEnum::BOOL, &T<TypeEnum::BOOL>::template fn<MoreArgs...> },
        { TypeEnum::INT, &T<TypeEnum::INT>::template fn<MoreArgs...> },
        { TypeEnum::DOUBLE, &T<TypeEnum::DOUBLE>::template fn<MoreArgs...> }
    };
    return rval;
}

// utility to generate a dispatcher for a functor
template<template<typename T> typename FN>
struct DispatchOf
{
    template<TypeEnum e>
    struct type
    {
        template<typename... Args>
        static void fn(Cont const& c, Args&&... args)
        {
            return FN<typename TypeOfTypeEnum<e>::type>(args...)(GetMemberByType<e>(c));
        };
    };
};

// utility to actually call the potato
template<template<typename F> typename FN, typename... Args> void CallGenericFunctor(Cont const& c, Args&&... args)
{
    auto&& fn = GenerateMap<DispatchOf<FN>::template type, Args...>().at(c.type);
    return fn(c, args...);
}

// your processing functor
template<typename T>
struct Printer
{
    Printer(int& state)
        : myState(state)
    {}
    int& myState;
    void operator()(T const& t);
};

template<>
void Printer<bool>::operator()(bool const& t)
{
    printf("%d: bool: %s\n", myState++, t ? "true" : "false");
}

template<>
void Printer<int>::operator()(int const& t)
{
    printf("%d: int: %d\n", myState++, t);
}
template<>
void Printer<double>::operator()(double const& t)
{
    printf("%d: double: %lg\n", myState++, t);
}

int main(int argc, char* argv[])
{
    Cont conts[] = {
        true,
        42,
        3.14
    };
    int state = 1;
    for(size_t i = 0; i < 3; ++i)
    {
        CallGenericFunctor<Printer>(conts[i], state);
    }
}
<commit_msg>Update typedefTemplate.cpp<commit_after>#include <cstdio>
#include <functional>
#include <map>

// available types
enum class TypeEnum : int
{
    BOOL,
    INT,
    DOUBLE
};

// translate type enum to actual type
template<TypeEnum e> struct TypeOfTypeEnum;

template<> struct TypeOfTypeEnum<TypeEnum::BOOL>
{
    typedef bool type;
};

template<> struct TypeOfTypeEnum<TypeEnum::INT>
{
    typedef int type;
};

template<> struct TypeOfTypeEnum<TypeEnum::DOUBLE>
{
    typedef double type;
};

// Our variant type
struct Cont
{
    Cont(bool b)
        : type(TypeEnum::BOOL)
          , asBool(b)
    {}
    Cont(int i)
        : type(TypeEnum::INT)
          , asInt(i)
    {}
    Cont(double d)
        : type(TypeEnum::DOUBLE)
          , asDouble(d)
    {}

    TypeEnum type;
    union
    {
        bool asBool;
        int asInt;
        double asDouble;
    };
};

// read correct data based on type enum
template<TypeEnum e, typename T = typename TypeOfTypeEnum<e>::type>
 constexpr T const& GetMemberByType(Cont const& c);

template<>
constexpr bool const& GetMemberByType<TypeEnum::BOOL>(Cont const& c)
{
    return c.asBool;
}
template<>
constexpr int const& GetMemberByType<TypeEnum::INT>(Cont const& c)
{
    return c.asInt;
}
template<>
constexpr double const& GetMemberByType<TypeEnum::DOUBLE>(Cont const& c)
{
    return c.asDouble;
}

// utility template to map the correct template instantiation to the correct type enum type type type
template<template<TypeEnum, typename... Args> typename T, typename... MoreArgs>
std::map<TypeEnum, std::function<void(Cont const&, MoreArgs...)>> const& GenerateMap()
{
    static decltype(GenerateMap<T, MoreArgs...>()) rval {
        { TypeEnum::BOOL, &T<TypeEnum::BOOL>::template fn<MoreArgs...> },
        { TypeEnum::INT, &T<TypeEnum::INT>::template fn<MoreArgs...> },
        { TypeEnum::DOUBLE, &T<TypeEnum::DOUBLE>::template fn<MoreArgs...> }
    };
    return rval;
}

// utility to generate a dispatcher for a functor
template<template<typename T> typename FN>
struct DispatchOf
{
    template<TypeEnum e>
    struct type
    {
        template<typename... Args>
        static void fn(Cont const& c, Args&&... args)
        {
            return FN<typename TypeOfTypeEnum<e>::type>(args...)(GetMemberByType<e>(c));
        };
    };
};

// utility to actually call the potato
template<template<typename F> typename FN, typename... Args> void CallGenericFunctor(Cont const& c, Args&&... args)
{
    auto&& fn = GenerateMap<DispatchOf<FN>::template type, Args...>().at(c.type);
    return fn(c, args...);
}

// your processing functor
template<typename T>
struct Printer
{
    Printer(int& state)
        : myState(state)
    {}
    int& myState;
    void operator()(T const& t);
};

template<>
void Printer<bool>::operator()(bool const& t)
{
    printf("%d: bool: %s\n", myState++, t ? "true" : "false");
}

template<>
void Printer<int>::operator()(int const& t)
{
    printf("%d: int: %d\n", myState++, t);
}
template<>
void Printer<double>::operator()(double const& t)
{
    printf("%d: double: %lg\n", myState++, t);
}

// another processor
#include <vector>
template<typename T>
struct Squarer
{
    Squarer(std::vector<Cont>& stash)
        : myStash(stash)
    {}
    void operator()(T const&);
    std::vector<Cont>& myStash;
};

template<>
void Squarer<int>::operator()(int const& t)
{
    myStash.push_back(t * t);
}

template<>
void Squarer<double>::operator()(double const& t)
{
    myStash.push_back(t * t);
}

template<typename T>
void Squarer<T>::operator()(T const& t)
{
    myStash.push_back(t);
}

#include <algorithm>
int main(int argc, char* argv[])
{
    std::vector<Cont> conts {
        2,
        3,
        4,
        true,
        42,
        3.14
    };
    std::vector<Cont> squared;
    for(auto&& c : conts)
    {
        CallGenericFunctor<Squarer>(c, squared);
    }
    int lineNo = 1;
    auto p = [&lineNo](Cont const& c) {
        CallGenericFunctor<Printer>(c, lineNo);
    };
    std::for_each(squared.begin(), squared.end(), p);
}
<|endoftext|>


<commit_before>// Tests for the RooNaNPacker
// Authors: Stephan Hageboeck, CERN  04/2020
#include "RooNaNPacker.h"
#include "RooRealVar.h"
#include "RooGenericPdf.h"
#include "RooMinimizer.h"
#include "RooFitResult.h"
#include "RooDataSet.h"

#include "gtest/gtest.h"

#include <cmath>
#include <bitset>
#include <cstdint>

void dumpFloats(double val) {
  float tmp[2];
  std::memcpy(tmp, &val, sizeof(double));

  for (int i = 1; i >= 0; --i) {
    unsigned long long ull = 0ull;
    std::memcpy(&ull, &tmp[i], 4);
    std::bitset<32> bits(ull);
    std::cout << bits << " " << std::flush;
  }
  std::cout << std::endl;
}

TEST(RooNaNPacker, CanPackStuffIntoNaNs)
{
  static_assert((RooNaNPacker::magicTag & RooNaNPacker::magicTagMask) == RooNaNPacker::magicTag, "Bit mask wrong.");
  constexpr bool dump = false;

  // Create a NaN that has 1.337f as payload
  RooNaNPacker rnp;
  rnp.setPayload(1.337f);
  EXPECT_TRUE(std::isnan(rnp._payload));
  EXPECT_TRUE(rnp.isNaNWithPayload());
  EXPECT_FLOAT_EQ(rnp.getPayload(), 1.337f);
  if (dump) dumpFloats(rnp._payload);

  // Create a normal double
  rnp._payload = 1.337;
  EXPECT_FALSE(std::isnan(rnp._payload));
  EXPECT_FALSE(rnp.isNaNWithPayload());
  EXPECT_DOUBLE_EQ(rnp._payload, 1.337);
  EXPECT_FLOAT_EQ(rnp.getPayload(), 0.f);
  if (dump) dumpFloats(rnp._payload);

  // Create a normal double
  rnp._payload = 4.;
  EXPECT_FALSE(std::isnan(rnp._payload));
  EXPECT_FALSE(rnp.isNaNWithPayload());
  EXPECT_DOUBLE_EQ(rnp._payload, 4.);
  EXPECT_FLOAT_EQ(rnp.getPayload(), 0.f);
  if (dump) dumpFloats(rnp._payload);

  // Create a simple payload
  rnp.setPayload(0.);
  EXPECT_TRUE(std::isnan(rnp._payload));
  EXPECT_TRUE(rnp.isNaNWithPayload());
  EXPECT_FLOAT_EQ(rnp.getPayload(), 0.f);
  if (dump) dumpFloats(rnp._payload);

  // Create a simple payload
  rnp.setPayload(2.);
  EXPECT_TRUE(std::isnan(rnp._payload));
  EXPECT_TRUE(rnp.isNaNWithPayload());
  EXPECT_FLOAT_EQ(rnp.getPayload(), 2.f);
  if (dump) dumpFloats(rnp._payload);

  // Create a simple payload
  rnp.setPayload(4.);
  EXPECT_TRUE(std::isnan(rnp._payload));
  EXPECT_TRUE(rnp.isNaNWithPayload());
  EXPECT_FLOAT_EQ(rnp.getPayload(), 4.f);
  if (dump) dumpFloats(rnp._payload);

  // Create a NaN that doesn't have the magic tag,
  // so no information encoded
  rnp._payload = std::numeric_limits<double>::quiet_NaN();
  const float tmp = 1234.5f;
  std::memcpy(&rnp._payload, &tmp, sizeof(float));
  EXPECT_TRUE(std::isnan(rnp._payload));
  EXPECT_FALSE(rnp.isNaNWithPayload());
  EXPECT_FLOAT_EQ(rnp.getPayload(), 0.f);
  if (dump) dumpFloats(rnp._payload);

}

TEST(RooNaNPacker, FitSimpleLinear) {
  RooRealVar x("x", "x", -10, 10);
  RooRealVar a1("a1", "a1", 12., -5., 15.);
  RooGenericPdf pdf("pdf", "a1 + x", RooArgSet(x, a1));
  std::unique_ptr<RooDataSet> data(pdf.generate(x, 1000));
  std::unique_ptr<RooAbsReal> nll(pdf.createNLL(*data));

  ASSERT_FALSE(std::isnan(pdf.getVal(RooArgSet(x))));
  a1.setVal(-9.);
  ASSERT_TRUE(std::isnan(pdf.getVal(RooArgSet(x))));

  RooMinimizer minim(*nll);
  minim.migrad();
  minim.hesse();
  auto fitResult = minim.save();

  EXPECT_EQ(fitResult->status(), 0);
  EXPECT_NEAR(a1.getVal(), 12., a1.getError());
}

TEST(RooNaNPacker, FitParabola) {
  RooRealVar x("x", "x", -10, 10);
  RooRealVar a1("a1", "a1", 12., -10., 15.);
  RooRealVar a2("a2", "a2", 1.1, -5., 15.);
  RooGenericPdf pdf("pdf", "a1 + x + a2 *x*x", RooArgSet(x, a1, a2));
  std::unique_ptr<RooDataSet> data(pdf.generate(x, 10000));
  auto nll = pdf.createNLL(*data);

  RooArgSet params(a1, a2);
  RooArgSet evilValues;
  a1.setVal(-9.);
  a2.setVal(-1.);
  params.snapshot(evilValues);

  params = evilValues;
  auto fitResult1 = pdf.fitTo(*data, RooFit::Save());

  params = evilValues;

  auto fitResult2 = pdf.fitTo(*data, RooFit::Save());

  fitResult1->Print();
  fitResult2->Print();

  for (auto fitResult : std::initializer_list<RooFitResult*>{fitResult1, fitResult2}) {
    std::string config = (fitResult == fitResult1 ? "No error wall" : "Error wall");
    const auto& a1Final = static_cast<RooRealVar&>(fitResult->floatParsFinal()[0]);
    const auto& a2Final = static_cast<RooRealVar&>(fitResult->floatParsFinal()[1]);
    EXPECT_EQ(fitResult->status(), 0) << config;
    EXPECT_NEAR(a1Final.getVal(), 12., a1Final.getError()) << config;
    EXPECT_NEAR(a2Final.getVal(),  0., a2Final.getError()) << config;
  }

  EXPECT_LT(fitResult1->numInvalidNLL(), fitResult2->numInvalidNLL());
}

<commit_msg>[RF] Add tests for propagation of NaNs.<commit_after>// Tests for the RooNaNPacker
// Authors: Stephan Hageboeck, CERN  04/2020
#include "RooNaNPacker.h"
#include "RooRealVar.h"
#include "RooGenericPdf.h"
#include "RooMinimizer.h"
#include "RooFitResult.h"
#include "RooDataSet.h"
#include "RooAddPdf.h"
#include "RooRealSumPdf.h"
#include "RooRandom.h"

#include "gtest/gtest.h"

#include <cmath>
#include <bitset>
#include <cstdint>

void dumpFloats(double val) {
  float tmp[2];
  std::memcpy(tmp, &val, sizeof(double));

  for (int i = 1; i >= 0; --i) {
    unsigned long long ull = 0ull;
    std::memcpy(&ull, &tmp[i], 4);
    std::bitset<32> bits(ull);
    std::cout << bits << " " << std::flush;
  }
  std::cout << std::endl;
}

// Test if we can pack floats into NaNs, and recover them.
TEST(RooNaNPacker, CanPackStuffIntoNaNs)
{
  static_assert((RooNaNPacker::magicTag & RooNaNPacker::magicTagMask) == RooNaNPacker::magicTag, "Bit mask wrong.");
  constexpr bool dump = false;

  // Create a NaN that has 1.337f as payload
  RooNaNPacker rnp;
  rnp.setPayload(1.337f);
  EXPECT_TRUE(std::isnan(rnp._payload));
  EXPECT_TRUE(rnp.isNaNWithPayload());
  EXPECT_FLOAT_EQ(rnp.getPayload(), 1.337f);
  if (dump) dumpFloats(rnp._payload);

  // Create a normal double
  rnp._payload = 1.337;
  EXPECT_FALSE(std::isnan(rnp._payload));
  EXPECT_FALSE(rnp.isNaNWithPayload());
  EXPECT_DOUBLE_EQ(rnp._payload, 1.337);
  EXPECT_FLOAT_EQ(rnp.getPayload(), 0.f);
  if (dump) dumpFloats(rnp._payload);

  // Create a normal double
  rnp._payload = 4.;
  EXPECT_FALSE(std::isnan(rnp._payload));
  EXPECT_FALSE(rnp.isNaNWithPayload());
  EXPECT_DOUBLE_EQ(rnp._payload, 4.);
  EXPECT_FLOAT_EQ(rnp.getPayload(), 0.f);
  if (dump) dumpFloats(rnp._payload);

  // Create a simple payload
  rnp.setPayload(0.);
  EXPECT_TRUE(std::isnan(rnp._payload));
  EXPECT_TRUE(rnp.isNaNWithPayload());
  EXPECT_FLOAT_EQ(rnp.getPayload(), 0.f);
  if (dump) dumpFloats(rnp._payload);

  // Create a simple payload
  rnp.setPayload(2.);
  EXPECT_TRUE(std::isnan(rnp._payload));
  EXPECT_TRUE(rnp.isNaNWithPayload());
  EXPECT_FLOAT_EQ(rnp.getPayload(), 2.f);
  if (dump) dumpFloats(rnp._payload);

  // Create a simple payload
  rnp.setPayload(4.);
  EXPECT_TRUE(std::isnan(rnp._payload));
  EXPECT_TRUE(rnp.isNaNWithPayload());
  EXPECT_FLOAT_EQ(rnp.getPayload(), 4.f);
  if (dump) dumpFloats(rnp._payload);

  // Create a NaN that doesn't have the magic tag,
  // so no information encoded
  rnp._payload = std::numeric_limits<double>::quiet_NaN();
  const float tmp = 1234.5f;
  std::memcpy(&rnp._payload, &tmp, sizeof(float));
  EXPECT_TRUE(std::isnan(rnp._payload));
  EXPECT_FALSE(rnp.isNaNWithPayload());
  EXPECT_FLOAT_EQ(rnp.getPayload(), 0.f);
  if (dump) dumpFloats(rnp._payload);

}

/// Fit a simple linear function, that starts in the negative.
TEST(RooNaNPacker, FitSimpleLinear) {
  RooRealVar x("x", "x", -10, 10);
  RooRealVar a1("a1", "a1", 12., -5., 15.);
  RooGenericPdf pdf("pdf", "a1 + x", RooArgSet(x, a1));
  std::unique_ptr<RooDataSet> data(pdf.generate(x, 1000));
  std::unique_ptr<RooAbsReal> nll(pdf.createNLL(*data));

  ASSERT_FALSE(std::isnan(pdf.getVal(RooArgSet(x))));
  a1.setVal(-9.);
  ASSERT_TRUE(std::isnan(pdf.getVal(RooArgSet(x))));

  RooMinimizer minim(*nll);
  minim.setPrintLevel(-1);
  minim.setPrintEvalErrors(-1);
  minim.migrad();
  minim.hesse();
  auto fitResult = minim.save();

  EXPECT_EQ(fitResult->status(), 0);
  EXPECT_NEAR(a1.getVal(), 12., a1.getError());
}

/// Fit a parabola, where parameters are set up such that negative function values are obtained.
/// The minimiser needs to recover from that.
/// Test also that when recovery with NaN packing is switched off, the minimiser fails to recover.
TEST(RooNaNPacker, FitParabola) {
  constexpr bool verbose = false;

  RooRealVar x("x", "x", -10, 10);
  RooRealVar a1("a1", "a1", 12., -10., 20.);
  RooRealVar a2("a2", "a2", 1.1, -10., 20.);
  RooGenericPdf pdf("pdf", "a1 + x + a2 *x*x", RooArgSet(x, a1, a2));
  std::unique_ptr<RooDataSet> data(pdf.generate(x, 10000));
  auto nll = pdf.createNLL(*data);

  RooArgSet params(a1, a2);
  RooArgSet paramsInit;
  params.snapshot(paramsInit);
  RooArgSet evilValues;
  a1.setVal(-9.);
  a2.setVal(-1.);
  params.snapshot(evilValues);

  RooFitResult *fitResult1 = nullptr, *fitResult2 = nullptr;
  for (auto tryRecover : std::initializer_list<double>{0., 10.}) {
    params = evilValues;

    RooMinimizer::cleanup();
    RooMinimizer minim(*nll);
    minim.setRecoverFromNaNStrength(tryRecover);
    minim.setPrintLevel(-1);
    minim.setPrintEvalErrors(-1);
    minim.migrad();
    minim.hesse();
    minim.minos();
    auto fitResult = minim.save();
    (tryRecover != 0. ? fitResult1 : fitResult2) = fitResult;

    std::string config = (tryRecover != 0. ? "With recovery" : "Without recovery");
    const auto& a1Final = static_cast<RooRealVar&>(fitResult->floatParsFinal()[0]);
    const auto& a2Final = static_cast<RooRealVar&>(fitResult->floatParsFinal()[1]);

    if (tryRecover != 0.) {
      EXPECT_EQ(fitResult->status(), 0) << config;
      EXPECT_NEAR(a1Final.getVal(), static_cast<RooAbsReal&>(paramsInit["a1"]).getVal(), a1Final.getError()) << config;
      EXPECT_NEAR(a2Final.getVal(), static_cast<RooAbsReal&>(paramsInit["a2"]).getVal(), a2Final.getError()) << config;
    } else {
      EXPECT_LT(a1Final.getVal(), 0.);
      EXPECT_LT(a2Final.getVal(), 0.);
    }

    if (verbose) {
      std::cout << "Recovery strength:" << tryRecover << std::endl;
      fitResult->Print();
    }
  }

  EXPECT_LT(fitResult1->numInvalidNLL(), fitResult2->numInvalidNLL());
}

/// Make coefficients of RooAddPdf sum to more than 1. Fitter should recover from this.
TEST(RooNaNPacker, FitAddPdf_DegenerateCoeff) {
  constexpr bool verbose = false;
  RooRandom::randomGenerator()->SetSeed(100);

  RooRealVar x("x", "x", 0., 10);
  RooRealVar a1("a1", "a1", 0.4, -10., 10.);
  RooRealVar a2("a2", "a2", 0.4, -10., 10.);
  RooGenericPdf pdf1("gen1", "exp(-2.*x)", RooArgSet(x));
  RooGenericPdf pdf2("gen2", "TMath::Gaus(x, 3, 2)", RooArgSet(x));
  RooGenericPdf pdf3("gen3", "x*x*x+1", RooArgSet(x));
  RooAddPdf pdf("sum", "a1*gen1 + a2*gen2 + (1-a1-a2)*gen3", RooArgList(pdf1, pdf2, pdf3), RooArgList(a1, a2));
  std::unique_ptr<RooDataSet> data(pdf.generate(x, 2000));
  auto nll = pdf.createNLL(*data);

  RooArgSet params(a1, a2);
  RooArgSet paramsInit;
  params.snapshot(paramsInit);

  RooArgSet evilValues;
  a1.setVal(0.6);
  a2.setVal(0.7);
  params.snapshot(evilValues);

  params = evilValues;

  RooFitResult *fitResult1 = nullptr, *fitResult2 = nullptr;
  for (auto tryRecover : std::initializer_list<double>{0., 10.}) {
    params = evilValues;

    RooMinimizer::cleanup();
    RooMinimizer minim(*nll);
    minim.setRecoverFromNaNStrength(tryRecover);
    minim.setPrintLevel(-1);
    minim.setPrintEvalErrors(-1);
    minim.migrad();
    minim.hesse();
    auto fitResult = minim.save();
    (tryRecover != 0. ? fitResult1 : fitResult2) = fitResult;

    const auto& a1Final = static_cast<RooRealVar&>(fitResult->floatParsFinal()[0]);
    const auto& a2Final = static_cast<RooRealVar&>(fitResult->floatParsFinal()[1]);

    if (tryRecover != 0.) {
      EXPECT_EQ(fitResult->status(), 0) << "Recovery strength=" << tryRecover;
      EXPECT_NEAR(a1Final.getVal(), static_cast<RooAbsReal&>(paramsInit["a1"]).getVal(), a1Final.getError()) << "Recovery strength=" << tryRecover;
      EXPECT_NEAR(a2Final.getVal(), static_cast<RooAbsReal&>(paramsInit["a2"]).getVal(), a2Final.getError()) << "Recovery strength=" << tryRecover;
      EXPECT_NEAR(a1Final.getVal() + a2Final.getVal(), 0.8, 0.02) << "Check that coefficients sum to 1. " << "Recovery strength=" << tryRecover;
    } else {
      EXPECT_TRUE(a1Final.getVal() < 0. || a1Final.getVal() > 1. || a2Final.getVal() < 0. || a2Final.getVal() > 1.) << "Recovery strength=" << tryRecover;
    }

    if (verbose) {
      std::cout << "Recovery strength:" << tryRecover << "\n";
      fitResult->Print();
    }
  }

  EXPECT_LT(fitResult1->numInvalidNLL(), fitResult2->numInvalidNLL());
}

/// Make coefficients of RooRealSumPdf sum to more than 1. Fitter should recover from this.
TEST(RooNaNPacker, Interface_RooAbsPdf_fitTo_RooRealSumPdf_DegenerateCoeff) {
  constexpr bool verbose = false;
  RooRandom::randomGenerator()->SetSeed(100);

  RooRealVar x("x", "x", 0., 10);
  RooRealVar a1("a1", "a1", 0.3, -10., 10.);
  RooRealVar a2("a2", "a2", 0.4, -10., 10.);
  RooGenericPdf pdf1("gen1", "exp(-0.5*x)", RooArgSet(x));
  RooGenericPdf pdf2("gen2", "TMath::Gaus(x, 5, 0.7)", RooArgSet(x));
  RooGenericPdf pdf3("gen3", "TMath::Gaus(x, 8, 0.8)", RooArgSet(x));
  RooRealSumPdf pdf("sum", "a1*gen1 + a2*gen2 + (1-a1-a2)*gen3", RooArgList(pdf1, pdf2, pdf3), RooArgList(a1, a2));
  std::unique_ptr<RooDataSet> data(pdf.generate(x, 5000));

  RooArgSet params(a1, a2);
  RooArgSet paramsInit;
  params.snapshot(paramsInit);

  RooArgSet evilValues;
  a1.setVal(0.6);
  a2.setVal(0.7);
  params.snapshot(evilValues);

  params = evilValues;

  RooFitResult *fitResult1 = nullptr, *fitResult2 = nullptr;
  for (auto tryRecover : std::initializer_list<double>{0., 10.}) {
    params = evilValues;

    auto fitResult = pdf.fitTo(*data, RooFit::PrintLevel(-1), RooFit::PrintEvalErrors(-1), RooFit::Save(), RooFit::RecoverFromUndefinedRegions(tryRecover));
    (tryRecover != 0. ? fitResult1 : fitResult2) = fitResult;

    const auto& a1Final = static_cast<RooRealVar&>(fitResult->floatParsFinal()[0]);
    const auto& a2Final = static_cast<RooRealVar&>(fitResult->floatParsFinal()[1]);

    if (tryRecover != 0.) {
      EXPECT_EQ(fitResult->status(), 0) << "Recovery strength=" << tryRecover;
      EXPECT_NEAR(a1Final.getVal(), static_cast<RooAbsReal&>(paramsInit["a1"]).getVal(), 3.*a1Final.getError()) << "Recovery strength=" << tryRecover;
      EXPECT_NEAR(a2Final.getVal(), static_cast<RooAbsReal&>(paramsInit["a2"]).getVal(), 3.*a2Final.getError()) << "Recovery strength=" << tryRecover;
      EXPECT_GE(a1Final.getVal() + a2Final.getVal(), 0.) << "Check that coefficients are in [0, 1]. " << "Recovery strength=" << tryRecover;
      EXPECT_LE(a1Final.getVal() + a2Final.getVal(), 1.) << "Check that coefficients sum to [0, 1]. " << "Recovery strength=" << tryRecover;
    } else {
      EXPECT_TRUE(a1Final.getVal() < 0. || a1Final.getVal() > 1. || a2Final.getVal() < 0. || a2Final.getVal() > 1.) << "Recovery strength=" << tryRecover;
    }

    if (verbose) {
      std::cout << "Recovery strength:" << tryRecover << "\n";
      fitResult->Print();
    }
  }

  if (verbose) {
    fitResult1->Print();
    fitResult2->Print();
  }
  EXPECT_LT(fitResult1->numInvalidNLL(), fitResult2->numInvalidNLL());
}

<|endoftext|>