<commit_before>/******************************************************************************
 * Copyright 2018 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/fusion/common/information_filter.h"

namespace apollo {
namespace perception {
namespace fusion {

InformationFilter::InformationFilter()
    : BaseFilter("InformationFilter"), last_observation_init_(false) {}

bool InformationFilter::Init(const Eigen::VectorXd &global_states,
                             const Eigen::MatrixXd &global_uncertainty) {
  if (global_uncertainty.rows() != global_uncertainty.cols()) {
    return false;
  }

  states_num_ = static_cast<int>(global_uncertainty.rows());

  if (states_num_ <= 0) {
    return false;
  }

  if (states_num_ != global_states.rows()) {
    return false;
  }

  global_states_ = global_states;
  global_uncertainty_ = global_uncertainty;
  tmp_states_ = global_uncertainty_.inverse() * global_states_;

  transform_matrix_.setIdentity(states_num_, states_num_);
  last_to_cur_transform_matrix_.setIdentity(states_num_, states_num_);

  cur_observation_.setZero(states_num_, 1);
  cur_observation_uncertainty_.setIdentity(states_num_, states_num_);

  last_observation_.setZero(states_num_, 1);
  last_observation_uncertainty_.setIdentity(states_num_, states_num_);

  c_matrix_.setIdentity(states_num_, states_num_);
  env_uncertainty_.setZero(states_num_, states_num_);

  init_ = true;
  return true;
}

bool InformationFilter::SetLastObservation(
    const Eigen::VectorXd &last_observation,
    const Eigen::MatrixXd &last_observation_uncertainty,
    const Eigen::MatrixXd &last_to_cur_transform_matrix,
    const Eigen::MatrixXd &last_to_cur_env_uncertainty) {
  if (init_ == false) {
    return false;
  }
  if (last_observation.rows() != states_num_) {
    return false;
  }
  if (last_observation_uncertainty.rows() != states_num_) {
    return false;
  }
  if (last_observation_uncertainty.cols() != states_num_) {
    return false;
  }
  if (last_to_cur_transform_matrix.rows() != states_num_) {
    return false;
  }
  if (last_to_cur_transform_matrix.cols() != states_num_) {
    return false;
  }
  if (last_to_cur_env_uncertainty.rows() != states_num_) {
    return false;
  }
  if (last_to_cur_env_uncertainty.cols() != states_num_) {
    return false;
  }

  last_observation_ = last_observation;
  last_observation_uncertainty_ = last_observation_uncertainty;
  last_to_cur_transform_matrix_ = last_to_cur_transform_matrix;
  last_to_cur_env_uncertainty_ = last_to_cur_env_uncertainty;
  last_observation_init_ = true;
  return true;
}

bool InformationFilter::Predict(const Eigen::MatrixXd &transform_matrix,
                                const Eigen::MatrixXd &env_uncertainty) {
  if (init_ == false) {
    return false;
  }
  if (transform_matrix.rows() != states_num_) {
    return false;
  }
  if (transform_matrix.cols() != states_num_) {
    return false;
  }
  if (env_uncertainty.rows() != states_num_) {
    return false;
  }
  if (env_uncertainty.cols() != states_num_) {
    return false;
  }
  transform_matrix_ = transform_matrix;
  env_uncertainty_ = env_uncertainty;
  global_states_ = transform_matrix_ * global_states_;
  global_uncertainty_ =
      transform_matrix_ * global_uncertainty_ * transform_matrix_.transpose() +
      env_uncertainty_;
  return true;
}

bool InformationFilter::Correct(
    const Eigen::VectorXd &cur_observation,
    const Eigen::MatrixXd &cur_observation_uncertainty) {
  if (init_ == false) {
    return false;
  }
  if (cur_observation.rows() != states_num_) {
    return false;
  }
  if (cur_observation_uncertainty.rows() != states_num_) {
    return false;
  }
  if (cur_observation_uncertainty.cols() != states_num_) {
    return false;
  }
  cur_observation_ = cur_observation;
  cur_observation_uncertainty_ = cur_observation_uncertainty;
  global_uncertainty_ = global_uncertainty_.inverse();
  tmp_states_ = global_uncertainty_ * global_states_;
  cur_observation_uncertainty_ = cur_observation_uncertainty_.inverse();
  if (last_observation_init_) {
    last_observation_ = last_to_cur_transform_matrix_ * last_observation_;
    last_observation_uncertainty_ =
        last_to_cur_transform_matrix_ * last_observation_uncertainty_ *
            last_to_cur_transform_matrix_.transpose() +
        last_to_cur_env_uncertainty_;
    last_observation_uncertainty_ =
        c_matrix_ * last_observation_uncertainty_ * c_matrix_.transpose();
    global_uncertainty_ =
        c_matrix_.transpose() * global_uncertainty_ +
        (c_matrix_.transpose() * cur_observation_uncertainty_ * c_matrix_ -
         c_matrix_.transpose() * last_observation_uncertainty_.inverse() *
             c_matrix_);
    tmp_states_ +=
        (c_matrix_.transpose() * cur_observation_uncertainty_ *
             cur_observation_ -
         c_matrix_.transpose() * last_observation_uncertainty_.inverse() *
             last_observation_);
  } else {
    global_uncertainty_ +=
        c_matrix_.transpose() * cur_observation_uncertainty_ * c_matrix_;
    tmp_states_ +=
        c_matrix_.transpose() * cur_observation_uncertainty_ * cur_observation_;
  }

  global_states_ = global_uncertainty_.inverse() * tmp_states_;
  last_observation_init_ = false;
  return true;
}
bool InformationFilter::SetControlMatrix(
    const Eigen::MatrixXd &control_matrix) {
  if (init_ == false) {
    return false;
  }
  if (control_matrix.rows() != states_num_ ||
      control_matrix.cols() != states_num_) {
    return false;
  }
  c_matrix_ = control_matrix;
  return true;
}

}  // namespace fusion
}  // namespace perception
}  // namespace apollo
<commit_msg>Perception: fixed a bug in information matrix update step (#6190)<commit_after>/******************************************************************************
 * Copyright 2018 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/fusion/common/information_filter.h"

namespace apollo {
namespace perception {
namespace fusion {

InformationFilter::InformationFilter()
    : BaseFilter("InformationFilter"), last_observation_init_(false) {}

bool InformationFilter::Init(const Eigen::VectorXd &global_states,
                             const Eigen::MatrixXd &global_uncertainty) {
  if (global_uncertainty.rows() != global_uncertainty.cols()) {
    return false;
  }

  states_num_ = static_cast<int>(global_uncertainty.rows());

  if (states_num_ <= 0) {
    return false;
  }

  if (states_num_ != global_states.rows()) {
    return false;
  }

  global_states_ = global_states;
  global_uncertainty_ = global_uncertainty;
  tmp_states_ = global_uncertainty_.inverse() * global_states_;

  transform_matrix_.setIdentity(states_num_, states_num_);
  last_to_cur_transform_matrix_.setIdentity(states_num_, states_num_);

  cur_observation_.setZero(states_num_, 1);
  cur_observation_uncertainty_.setIdentity(states_num_, states_num_);

  last_observation_.setZero(states_num_, 1);
  last_observation_uncertainty_.setIdentity(states_num_, states_num_);

  c_matrix_.setIdentity(states_num_, states_num_);
  env_uncertainty_.setZero(states_num_, states_num_);

  init_ = true;
  return true;
}

bool InformationFilter::SetLastObservation(
    const Eigen::VectorXd &last_observation,
    const Eigen::MatrixXd &last_observation_uncertainty,
    const Eigen::MatrixXd &last_to_cur_transform_matrix,
    const Eigen::MatrixXd &last_to_cur_env_uncertainty) {
  if (init_ == false) {
    return false;
  }
  if (last_observation.rows() != states_num_) {
    return false;
  }
  if (last_observation_uncertainty.rows() != states_num_) {
    return false;
  }
  if (last_observation_uncertainty.cols() != states_num_) {
    return false;
  }
  if (last_to_cur_transform_matrix.rows() != states_num_) {
    return false;
  }
  if (last_to_cur_transform_matrix.cols() != states_num_) {
    return false;
  }
  if (last_to_cur_env_uncertainty.rows() != states_num_) {
    return false;
  }
  if (last_to_cur_env_uncertainty.cols() != states_num_) {
    return false;
  }

  last_observation_ = last_observation;
  last_observation_uncertainty_ = last_observation_uncertainty;
  last_to_cur_transform_matrix_ = last_to_cur_transform_matrix;
  last_to_cur_env_uncertainty_ = last_to_cur_env_uncertainty;
  last_observation_init_ = true;
  return true;
}

bool InformationFilter::Predict(const Eigen::MatrixXd &transform_matrix,
                                const Eigen::MatrixXd &env_uncertainty) {
  if (init_ == false) {
    return false;
  }
  if (transform_matrix.rows() != states_num_) {
    return false;
  }
  if (transform_matrix.cols() != states_num_) {
    return false;
  }
  if (env_uncertainty.rows() != states_num_) {
    return false;
  }
  if (env_uncertainty.cols() != states_num_) {
    return false;
  }
  transform_matrix_ = transform_matrix;
  env_uncertainty_ = env_uncertainty;
  global_states_ = transform_matrix_ * global_states_;
  global_uncertainty_ =
      transform_matrix_ * global_uncertainty_ * transform_matrix_.transpose() +
      env_uncertainty_;
  return true;
}

bool InformationFilter::Correct(
    const Eigen::VectorXd &cur_observation,
    const Eigen::MatrixXd &cur_observation_uncertainty) {
  if (init_ == false) {
    return false;
  }
  if (cur_observation.rows() != states_num_) {
    return false;
  }
  if (cur_observation_uncertainty.rows() != states_num_) {
    return false;
  }
  if (cur_observation_uncertainty.cols() != states_num_) {
    return false;
  }
  cur_observation_ = cur_observation;
  cur_observation_uncertainty_ = cur_observation_uncertainty;
  // global_uncertainty now stores information matrix
  global_uncertainty_ = global_uncertainty_.inverse();
  // tmp_states_ is information vector
  tmp_states_ = global_uncertainty_ * global_states_;
  // cur_observation_uncertainty_ is now the inverse of covariance matrix
  cur_observation_uncertainty_ = cur_observation_uncertainty_.inverse();
  if (last_observation_init_) {
    // propate to current time
    last_observation_ = last_to_cur_transform_matrix_ * last_observation_;
    last_observation_uncertainty_ =
        last_to_cur_transform_matrix_ * last_observation_uncertainty_ *
        last_to_cur_transform_matrix_.transpose() +
        last_to_cur_env_uncertainty_;
    last_observation_uncertainty_ =  // transform to measurement space
        c_matrix_ * last_observation_uncertainty_ * c_matrix_.transpose();
    global_uncertainty_ =  // update information matrix
        global_uncertainty_ +
        (c_matrix_.transpose() * cur_observation_uncertainty_ * c_matrix_ -
         c_matrix_.transpose() * last_observation_uncertainty_.inverse() *
             c_matrix_);
    tmp_states_ +=  // update information vector
        (c_matrix_.transpose() * cur_observation_uncertainty_ *
             cur_observation_ -
         c_matrix_.transpose() * last_observation_uncertainty_.inverse() *
             last_observation_);
  } else {
    global_uncertainty_ +=
        c_matrix_.transpose() * cur_observation_uncertainty_ * c_matrix_;
    tmp_states_ +=
        c_matrix_.transpose() * cur_observation_uncertainty_ * cur_observation_;
  }

  global_states_ = global_uncertainty_.inverse() * tmp_states_;
  last_observation_init_ = false;
  return true;
}
bool InformationFilter::SetControlMatrix(
    const Eigen::MatrixXd &control_matrix) {
  if (init_ == false) {
    return false;
  }
  if (control_matrix.rows() != states_num_ ||
      control_matrix.cols() != states_num_) {
    return false;
  }
  c_matrix_ = control_matrix;
  return true;
}

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


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

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

#include "vtkActor.h"
#include "vtkCellArray.h"
#include "vtkCellData.h"
#include "vtkHardwareSelector.h"
#include "vtkIdTypeArray.h"
#include "vtkObjectFactory.h"
#include "vtkPainterDeviceAdapter.h"
#include "vtkPointData.h"
#include "vtkPolyData.h"
#include "vtkRenderer.h"
#include "vtkRenderWindow.h"
#include "vtkTimerLog.h"
#include "vtkUnsignedIntArray.h"

vtkStandardNewMacro(vtkHardwareSelectionPolyDataPainter);
//-----------------------------------------------------------------------------
static inline int vtkHardwareSelectionPolyDataPainterGetTotalCells(vtkPolyData* pd,
  unsigned long typeflags)
{
  int total_cells = 0;
  total_cells += (typeflags & vtkPainter::VERTS)?
    pd->GetNumberOfVerts() : 0;
  total_cells += (typeflags & vtkPainter::LINES)?
    pd->GetNumberOfLines() : 0;
  total_cells += (typeflags & vtkPainter::POLYS)?
    pd->GetNumberOfPolys() : 0;
  total_cells += (typeflags & vtkPainter::STRIPS)?
    pd->GetNumberOfStrips() : 0;
  return total_cells;
}
//----------------------------------------------------------------------------
vtkHardwareSelectionPolyDataPainter::vtkHardwareSelectionPolyDataPainter()
{
  this->EnableSelection = 1;
  this->PointIdArrayName = NULL;
  this->CellIdArrayName = NULL;
  this->ProcessIdArrayName = NULL;
}

//----------------------------------------------------------------------------
vtkHardwareSelectionPolyDataPainter::~vtkHardwareSelectionPolyDataPainter()
{
  this->SetPointIdArrayName(NULL);
  this->SetCellIdArrayName(NULL);
  this->SetProcessIdArrayName(NULL);
}

//-----------------------------------------------------------------------------
void vtkHardwareSelectionPolyDataPainter::RenderInternal(
  vtkRenderer* renderer,
  vtkActor* vtkNotUsed(actor),
  unsigned long typeflags,
  bool vtkNotUsed(forceCompileOnly))
{
  if (typeflags == 0)
    {
    // No primitive to render.
    return;
    }

  vtkPainterDeviceAdapter* device =
    renderer->GetRenderWindow()->GetPainterDeviceAdapter();
  if (device == NULL)
    {
    vtkErrorMacro("Painter Device Adapter missing!");
    return;
    }

  vtkPolyData* pd = this->GetInputAsPolyData();
  this->TotalCells = vtkHardwareSelectionPolyDataPainterGetTotalCells(pd, typeflags);

  if (this->TotalCells == 0)
    {
    // skip empty polydatas.
    this->TimeToDraw = 0;
    return;
    }

  vtkHardwareSelector* selector = renderer->GetSelector();
  if (this->EnableSelection)
    {
    selector->BeginRenderProp();
    if (selector->GetFieldAssociation() == vtkDataObject::FIELD_ASSOCIATION_POINTS &&
      selector->GetCurrentPass() >= vtkHardwareSelector::ID_LOW24)
      {
      device->MakeVertexEmphasis(true);
      }
    }

  this->Timer->StartTimer();
  vtkIdType startCell = 0;

  if (typeflags & vtkPainter::VERTS)
    {
    this->DrawCells(VTK_POLY_VERTEX, pd->GetVerts(), startCell, renderer);
    }

  startCell += pd->GetNumberOfVerts();
  if (typeflags & vtkPainter::LINES)
    {
    this->DrawCells(VTK_POLY_LINE, pd->GetLines(), startCell, renderer);
    }

  startCell += pd->GetNumberOfLines();
  if (typeflags & vtkPainter::POLYS)
    {
    this->DrawCells(VTK_POLYGON, pd->GetPolys(), startCell, renderer);
    }

  startCell += pd->GetNumberOfPolys();
  if (typeflags & vtkPainter::STRIPS)
    {
    this->DrawCells(VTK_TRIANGLE_STRIP, pd->GetStrips(), startCell, renderer);
    }
  if (this->EnableSelection)
    {
    selector->EndRenderProp();
    if (selector->GetFieldAssociation() == vtkDataObject::FIELD_ASSOCIATION_POINTS &&
      selector->GetCurrentPass() >= vtkHardwareSelector::ID_LOW24)
      {
      device->MakeVertexEmphasis(false);
      }
    }

  this->Timer->StopTimer();
  this->TimeToDraw = this->Timer->GetElapsedTime();
}

//-----------------------------------------------------------------------------
void vtkHardwareSelectionPolyDataPainter::DrawCells(
  int mode, vtkCellArray *connectivity, vtkIdType startCellId,
  vtkRenderer *renderer)
{
  vtkPolyData* pd = this->GetInputAsPolyData();
  vtkPainterDeviceAdapter* device = renderer->GetRenderWindow()->
    GetPainterDeviceAdapter();

  vtkHardwareSelector* selector = renderer->GetSelector();
  int attributeMode = selector->GetFieldAssociation();
  if (attributeMode == vtkDataObject::FIELD_ASSOCIATION_POINTS &&
    selector->GetCurrentPass() >= vtkHardwareSelector::ID_LOW24 &&
    this->EnableSelection)
    {
    mode = VTK_POLY_VERTEX;
    }

  vtkPoints* p = pd->GetPoints();
  vtkIdType npts, *pts;
  vtkIdType cellId = startCellId;
  vtkUnsignedIntArray* procIdsArray = this->ProcessIdArrayName?
    vtkUnsignedIntArray::SafeDownCast(
      pd->GetPointData()->GetArray(this->ProcessIdArrayName)) : NULL;

  vtkIdTypeArray* pidArray = this->PointIdArrayName? vtkIdTypeArray::SafeDownCast(
    pd->GetPointData()->GetArray(this->PointIdArrayName)) : NULL;

  vtkIdTypeArray* cidArray = this->CellIdArrayName? vtkIdTypeArray::SafeDownCast(
    pd->GetCellData()->GetArray(this->CellIdArrayName)) : NULL;

  int pointtype = p->GetDataType();
  void* voidpoints = p->GetVoidPointer(0);
  int count = 0;

  // Note that cell attributes are overridden by point attributes.
  for (connectivity->InitTraversal(); connectivity->GetNextCell(npts, pts); count++)
    {
    device->BeginPrimitive(mode);
    if (attributeMode == vtkDataObject::FIELD_ASSOCIATION_CELLS &&
      this->EnableSelection)
      {
      selector->RenderAttributeId(
        cidArray? cidArray->GetValue(cellId) : cellId);
      }
    for (vtkIdType cellpointi = 0; cellpointi < npts; cellpointi++)
      {
      vtkIdType pointId = pts[cellpointi];
      if (attributeMode == vtkDataObject::FIELD_ASSOCIATION_POINTS &&
        this->EnableSelection)
        {
        selector->RenderAttributeId(
          pidArray? pidArray->GetValue(pointId) : pointId);
        }
      if (this->EnableSelection && procIdsArray &&
        selector->GetUseProcessIdFromData())
        {
        selector->RenderProcessId(procIdsArray->GetPointer(0)[pointId]);
        }
      device->SendAttribute(vtkPointData::NUM_ATTRIBUTES, 3,
        pointtype, voidpoints, 3*pointId);
      }
    device->EndPrimitive();
    cellId++;
    if (count == 10000)
      {
      count = 0;
      // report progress
      this->UpdateProgress(static_cast<double>(cellId - startCellId)/this->TotalCells);
      // Abort render.
      if (renderer->GetRenderWindow()->CheckAbortStatus())
        {
        return;
        }
      }
    }
}

//----------------------------------------------------------------------------
void vtkHardwareSelectionPolyDataPainter::PrintSelf(ostream& os, vtkIndent indent)
{
  this->Superclass::PrintSelf(os, indent);
  os << indent << "EnableSelection: " << this->EnableSelection << endl;
  os << indent << "CellIdArrayName: " <<
    (this->CellIdArrayName? this->CellIdArrayName : NULL) << endl;
  os << indent << "PointIdArrayName: " <<
    (this->PointIdArrayName? this->PointIdArrayName: NULL) << endl;
}
<commit_msg>Fix point selection by avoiding the selection to be spilled across block<commit_after>/*=========================================================================

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

#include "vtkActor.h"
#include "vtkCellArray.h"
#include "vtkCellData.h"
#include "vtkHardwareSelector.h"
#include "vtkIdTypeArray.h"
#include "vtkObjectFactory.h"
#include "vtkPainterDeviceAdapter.h"
#include "vtkPointData.h"
#include "vtkPolyData.h"
#include "vtkRenderer.h"
#include "vtkRenderWindow.h"
#include "vtkTimerLog.h"
#include "vtkUnsignedIntArray.h"

vtkStandardNewMacro(vtkHardwareSelectionPolyDataPainter);
//-----------------------------------------------------------------------------
static inline int vtkHardwareSelectionPolyDataPainterGetTotalCells(vtkPolyData* pd,
  unsigned long typeflags)
{
  int total_cells = 0;
  total_cells += (typeflags & vtkPainter::VERTS)?
    pd->GetNumberOfVerts() : 0;
  total_cells += (typeflags & vtkPainter::LINES)?
    pd->GetNumberOfLines() : 0;
  total_cells += (typeflags & vtkPainter::POLYS)?
    pd->GetNumberOfPolys() : 0;
  total_cells += (typeflags & vtkPainter::STRIPS)?
    pd->GetNumberOfStrips() : 0;
  return total_cells;
}
//----------------------------------------------------------------------------
vtkHardwareSelectionPolyDataPainter::vtkHardwareSelectionPolyDataPainter()
{
  this->EnableSelection = 1;
  this->PointIdArrayName = NULL;
  this->CellIdArrayName = NULL;
  this->ProcessIdArrayName = NULL;
}

//----------------------------------------------------------------------------
vtkHardwareSelectionPolyDataPainter::~vtkHardwareSelectionPolyDataPainter()
{
  this->SetPointIdArrayName(NULL);
  this->SetCellIdArrayName(NULL);
  this->SetProcessIdArrayName(NULL);
}

//-----------------------------------------------------------------------------
void vtkHardwareSelectionPolyDataPainter::RenderInternal(
  vtkRenderer* renderer,
  vtkActor* vtkNotUsed(actor),
  unsigned long typeflags,
  bool vtkNotUsed(forceCompileOnly))
{
  if (typeflags == 0)
    {
    // No primitive to render.
    return;
    }

  vtkPainterDeviceAdapter* device =
    renderer->GetRenderWindow()->GetPainterDeviceAdapter();
  if (device == NULL)
    {
    vtkErrorMacro("Painter Device Adapter missing!");
    return;
    }

  vtkPolyData* pd = this->GetInputAsPolyData();
  this->TotalCells = vtkHardwareSelectionPolyDataPainterGetTotalCells(pd, typeflags);

  if (this->TotalCells == 0)
    {
    // skip empty polydatas.
    this->TimeToDraw = 0;
    return;
    }

  vtkHardwareSelector* selector = renderer->GetSelector();
  if (this->EnableSelection)
    {
    selector->BeginRenderProp();
    // We emphasis the vertex size to make sure they will be properly detected.
    if (selector->GetFieldAssociation() == vtkDataObject::FIELD_ASSOCIATION_POINTS &&
      selector->GetCurrentPass() > vtkHardwareSelector::ACTOR_PASS)
      {
      device->MakeVertexEmphasis(true);
      }
    }

  this->Timer->StartTimer();
  vtkIdType startCell = 0;

  if (typeflags & vtkPainter::VERTS)
    {
    this->DrawCells(VTK_POLY_VERTEX, pd->GetVerts(), startCell, renderer);
    }

  startCell += pd->GetNumberOfVerts();
  if (typeflags & vtkPainter::LINES)
    {
    this->DrawCells(VTK_POLY_LINE, pd->GetLines(), startCell, renderer);
    }

  startCell += pd->GetNumberOfLines();
  if (typeflags & vtkPainter::POLYS)
    {
    this->DrawCells(VTK_POLYGON, pd->GetPolys(), startCell, renderer);
    }

  startCell += pd->GetNumberOfPolys();
  if (typeflags & vtkPainter::STRIPS)
    {
    this->DrawCells(VTK_TRIANGLE_STRIP, pd->GetStrips(), startCell, renderer);
    }
  if (this->EnableSelection)
    {
    selector->EndRenderProp();
    // We revert back our Vertex emphasis
    if (selector->GetFieldAssociation() == vtkDataObject::FIELD_ASSOCIATION_POINTS &&
      selector->GetCurrentPass() > vtkHardwareSelector::ACTOR_PASS)
      {
      device->MakeVertexEmphasis(false);
      }
    }

  this->Timer->StopTimer();
  this->TimeToDraw = this->Timer->GetElapsedTime();
}

//-----------------------------------------------------------------------------
void vtkHardwareSelectionPolyDataPainter::DrawCells(
  int mode, vtkCellArray *connectivity, vtkIdType startCellId,
  vtkRenderer *renderer)
{
  vtkPolyData* pd = this->GetInputAsPolyData();
  vtkPainterDeviceAdapter* device = renderer->GetRenderWindow()->
    GetPainterDeviceAdapter();

  vtkHardwareSelector* selector = renderer->GetSelector();
  int attributeMode = selector->GetFieldAssociation();
  // While looking at point selection we render only vertex so each pass
  // should fill the same pixels without risking of detecting vertex belonging
  // to other cells or block. BUT we do that after the ACTOR_PASS to make sure
  // we have the proper oclusion as we keep the Z-buffer arround. In that way
  // vertex that are hidden by some surface won't get selected.
  if (attributeMode == vtkDataObject::FIELD_ASSOCIATION_POINTS &&
    selector->GetCurrentPass() > vtkHardwareSelector::ACTOR_PASS &&
    this->EnableSelection)
    {
    mode = VTK_POLY_VERTEX;
    }

  vtkPoints* p = pd->GetPoints();
  vtkIdType npts, *pts;
  vtkIdType cellId = startCellId;
  vtkUnsignedIntArray* procIdsArray = this->ProcessIdArrayName?
    vtkUnsignedIntArray::SafeDownCast(
      pd->GetPointData()->GetArray(this->ProcessIdArrayName)) : NULL;

  vtkIdTypeArray* pidArray = this->PointIdArrayName? vtkIdTypeArray::SafeDownCast(
    pd->GetPointData()->GetArray(this->PointIdArrayName)) : NULL;

  vtkIdTypeArray* cidArray = this->CellIdArrayName? vtkIdTypeArray::SafeDownCast(
    pd->GetCellData()->GetArray(this->CellIdArrayName)) : NULL;

  int pointtype = p->GetDataType();
  void* voidpoints = p->GetVoidPointer(0);
  int count = 0;

  // Note that cell attributes are overridden by point attributes.
  for (connectivity->InitTraversal(); connectivity->GetNextCell(npts, pts); count++)
    {
    device->BeginPrimitive(mode);
    if (attributeMode == vtkDataObject::FIELD_ASSOCIATION_CELLS &&
      this->EnableSelection)
      {
      selector->RenderAttributeId(
        cidArray? cidArray->GetValue(cellId) : cellId);
      }
    for (vtkIdType cellpointi = 0; cellpointi < npts; cellpointi++)
      {
      vtkIdType pointId = pts[cellpointi];
      if (attributeMode == vtkDataObject::FIELD_ASSOCIATION_POINTS &&
        this->EnableSelection)
        {
        selector->RenderAttributeId(
          pidArray? pidArray->GetValue(pointId) : pointId);
        }
      if (this->EnableSelection && procIdsArray &&
        selector->GetUseProcessIdFromData())
        {
        selector->RenderProcessId(procIdsArray->GetPointer(0)[pointId]);
        }
      device->SendAttribute(vtkPointData::NUM_ATTRIBUTES, 3,
        pointtype, voidpoints, 3*pointId);
      }
    device->EndPrimitive();
    cellId++;
    if (count == 10000)
      {
      count = 0;
      // report progress
      this->UpdateProgress(static_cast<double>(cellId - startCellId)/this->TotalCells);
      // Abort render.
      if (renderer->GetRenderWindow()->CheckAbortStatus())
        {
        return;
        }
      }
    }
}

//----------------------------------------------------------------------------
void vtkHardwareSelectionPolyDataPainter::PrintSelf(ostream& os, vtkIndent indent)
{
  this->Superclass::PrintSelf(os, indent);
  os << indent << "EnableSelection: " << this->EnableSelection << endl;
  os << indent << "CellIdArrayName: " <<
    (this->CellIdArrayName? this->CellIdArrayName : NULL) << endl;
  os << indent << "PointIdArrayName: " <<
    (this->PointIdArrayName? this->PointIdArrayName: NULL) << endl;
}
<|endoftext|>


<commit_before>/** Copyright (C) 2017-2018 European Spallation Source */
//===----------------------------------------------------------------------===//
///
/// \file
///  Unit tests for MultiBladeEventBuilder using Google Test.
/// Here the various counters are tested.
///
/// Author: Carsten Søgaard, Niels Bohr Institute, University of Copenhagen
///
//===----------------------------------------------------------------------===//

// FIXME TODO - nolonger valid , new data format

#include "mbcommon/MultiBladeEventBuilder.h"
#include "MultiBladeTestData.h"
#include "test/TestBase.h"

TEST(MBEventBuilder__Test, DataPointCounter) {

  // Instanciate the event-builder
  multiBladeEventBuilder p;

  // Test proper initialization.
  EXPECT_EQ(0U, p.getNumberOfEvents());

  // In the following: test proper incrementing.
  p.addDataPoint(0, 300, 0);
  EXPECT_EQ(1, p.getNumberOfDatapointsReceived());

  p.addDataPoint(1, 300, 1);
  EXPECT_EQ(2, p.getNumberOfDatapointsReceived());

  p.addDataPoint(2, 300, 2);
  EXPECT_EQ(3, p.getNumberOfDatapointsReceived());

  p.resetCounters();
  EXPECT_EQ(0, p.getNumberOfDatapointsReceived());
}

TEST(MBEventBuilder__Test, EventCounter) {

  // Test that events are counted correctly

  // Instanciate the event-builder
  multiBladeEventBuilder p;

  // Initialize the expected number of events counter
  uint nevents = 1;

  for (uint i = 0; i <= 15; i++) {

    // Read a subset of the test data
    uint begin = i * 5;
    uint end = begin + 5;
    std::vector<uint> datapoint(&data[begin], &data[end]);
    // Wire data-points
    EXPECT_EQ(datapoint[4], p.addDataPoint(datapoint[0], datapoint[2], datapoint[3]));
    // Strip data-points
    EXPECT_FALSE(p.addDataPoint(datapoint[1], datapoint[2], datapoint[3]));

    // Validation of event counter
    if (datapoint[4]) {
      EXPECT_EQ(nevents, p.getNumberOfEvents());
      // Increment validation iterator
      nevents++;
    }
  }

  // Provide a non-adjacent event, which is discarded
  for (uint i = 10; i < 15; i++) {

    // Remove a point (making it non adjacent)
    if (i != 12) {
      // Read a subset of the test data
      uint begin = i * 5;
      uint end = begin + 5;
      std::vector<uint> datapoint(&data[begin], &data[end]);
      // Wire data-points
      EXPECT_EQ(datapoint[4], p.addDataPoint(datapoint[0], datapoint[2], datapoint[3]));
      // Strip data-points
      EXPECT_FALSE(p.addDataPoint(datapoint[1], datapoint[2], datapoint[3]));
    }
  }

  // Validation of event counter at "end of run".
  // The counter was not incremented before, due to the non-adjacent point.
  p.lastPoint();
  EXPECT_EQ(nevents, p.getNumberOfEvents());
}

TEST(MBEventBuilder__Test, ClusterCounters) {

  // Test the counters of number of points per event

  // Instaciate the event-counter and configure
  multiBladeEventBuilder p;
  p.setTimeWindow(config[0]);
  p.setNumberOfWireChannels(config[1]);
  p.setNumberOfStripChannels(config[2]);

  // Validation counters (similar to those in the event-builder).
  // Index 0 = 1 datapoint, index 1 = 2 datapoints, etc.
  std::array<uint64_t, 6> wireclusterpoints = {{0, 0, 0, 0, 0, 0}};
  std::array<uint64_t, 6> stripclusterpoints = {{0, 0, 0, 0, 0, 0}};

  // Counters for number of points per event
  uint wirepoints = 0;
  uint strippoints = 0;

  // Iterator for validation data
  // std::vector<double>::iterator valw = validation_weighted.begin();

  // Test the case when there are both wire and strip points
  for (uint i = 0; i <= 21; i++) {
    uint begin = i * 5;
    uint end = begin + 5;
    std::vector<uint> datapoint(&data[begin], &data[end]);
    // Wire data-points
    p.addDataPoint(datapoint[0], datapoint[2], datapoint[3]);

    // Strip data-points
    p.addDataPoint(datapoint[1], datapoint[2], datapoint[3]);

    if (!datapoint[4]) {

      wirepoints++;
      strippoints++;

    } else {

      // Increment the correct positions in the counters
      wireclusterpoints[wirepoints - 1]++;
      stripclusterpoints[strippoints - 1]++;

      // Check all indexes of the counters for each event.
      EXPECT_EQ(wireclusterpoints[0], p.get2DWireClusterCounter()[0]);
      EXPECT_EQ(wireclusterpoints[1], p.get2DWireClusterCounter()[1]);
      EXPECT_EQ(wireclusterpoints[2], p.get2DWireClusterCounter()[2]);
      EXPECT_EQ(wireclusterpoints[3], p.get2DWireClusterCounter()[3]);
      EXPECT_EQ(wireclusterpoints[4], p.get2DWireClusterCounter()[4]);
      EXPECT_EQ(wireclusterpoints[5], p.get2DWireClusterCounter()[5]);

      EXPECT_EQ(stripclusterpoints[0], p.get2DStripClusterCounter()[0]);
      EXPECT_EQ(stripclusterpoints[1], p.get2DStripClusterCounter()[1]);
      EXPECT_EQ(stripclusterpoints[2], p.get2DStripClusterCounter()[2]);
      EXPECT_EQ(stripclusterpoints[3], p.get2DStripClusterCounter()[3]);
      EXPECT_EQ(stripclusterpoints[4], p.get2DStripClusterCounter()[4]);
      EXPECT_EQ(stripclusterpoints[5], p.get2DStripClusterCounter()[5]);

      // Reset the points counters (1, since a point is provided when finishing
      // the cluster
      wirepoints = 1;
      strippoints = 1;
    }
  }

  // End the "run" and reset all counters
  p.lastPoint();
  p.resetCounters();

  // Reset counters
  wireclusterpoints = {{0, 0, 0, 0, 0, 0}};
  stripclusterpoints = {{0, 0, 0, 0, 0, 0}};

  // Reset the counters to "start of run"
  wirepoints = 0;
  strippoints = 0;

  // Test the case of only wire points
  for (uint i = 0; i <= 21; i++) {
    uint begin = i * 5;
    uint end = begin + 5;
    std::vector<uint> datapoint(&data[begin], &data[end]);
    // Wire data-points
    p.addDataPoint(datapoint[0], datapoint[2], datapoint[3]);

    if (!datapoint[4]) {

      wirepoints++;

    } else {

      // Incement at the correct index.
      wireclusterpoints[wirepoints - 1]++;

      // Test counter at all indexes
      EXPECT_EQ(wireclusterpoints[0], p.get1DWireClusterCounter()[0]);
      EXPECT_EQ(wireclusterpoints[1], p.get1DWireClusterCounter()[1]);
      EXPECT_EQ(wireclusterpoints[2], p.get1DWireClusterCounter()[2]);
      EXPECT_EQ(wireclusterpoints[3], p.get1DWireClusterCounter()[3]);
      EXPECT_EQ(wireclusterpoints[4], p.get1DWireClusterCounter()[4]);
      EXPECT_EQ(wireclusterpoints[5], p.get1DWireClusterCounter()[5]);

      EXPECT_EQ(0, p.get1DStripClusterCounter()[0]);
      EXPECT_EQ(0, p.get1DStripClusterCounter()[1]);
      EXPECT_EQ(0, p.get1DStripClusterCounter()[2]);
      EXPECT_EQ(0, p.get1DStripClusterCounter()[3]);
      EXPECT_EQ(0, p.get1DStripClusterCounter()[4]);
      EXPECT_EQ(0, p.get1DStripClusterCounter()[5]);

      wirepoints = 1;
    }
  }

  // "End of run" and reset all counters
  p.lastPoint();
  p.resetCounters();

  // Reset validation counters
  wireclusterpoints = {{0, 0, 0, 0, 0, 0}};
  stripclusterpoints = {{0, 0, 0, 0, 0, 0}};

  // Reset event counters
  wirepoints = 0;
  strippoints = 0;

  // Test the case of only strip points
  for (uint i = 0; i <= 21; i++) {
    uint begin = i * 5;
    uint end = begin + 5;
    std::vector<uint> datapoint(&data[begin], &data[end]);
    // Strip data-points
    p.addDataPoint(datapoint[1], datapoint[2], datapoint[3]);

    if (!datapoint[4]) {

      strippoints++;

      EXPECT_EQ(strippoints, p.getStripClusterSize());

    } else {

      // Incement at the correct index.
      stripclusterpoints[strippoints - 1]++;

      // Test counter at all indexes
      EXPECT_EQ(0, p.get1DWireClusterCounter()[0]);
      EXPECT_EQ(0, p.get1DWireClusterCounter()[1]);
      EXPECT_EQ(0, p.get1DWireClusterCounter()[2]);
      EXPECT_EQ(0, p.get1DWireClusterCounter()[3]);
      EXPECT_EQ(0, p.get1DWireClusterCounter()[4]);
      EXPECT_EQ(0, p.get1DWireClusterCounter()[5]);

      EXPECT_EQ(stripclusterpoints[0], p.get1DStripClusterCounter()[0]);
      EXPECT_EQ(stripclusterpoints[1], p.get1DStripClusterCounter()[1]);
      EXPECT_EQ(stripclusterpoints[2], p.get1DStripClusterCounter()[2]);
      EXPECT_EQ(stripclusterpoints[3], p.get1DStripClusterCounter()[3]);
      EXPECT_EQ(stripclusterpoints[4], p.get1DStripClusterCounter()[4]);
      EXPECT_EQ(stripclusterpoints[5], p.get1DStripClusterCounter()[5]);

      strippoints = 1;
    }
  }
}

TEST(MBEventBuilder__Test, NoDataRecieved) {

  multiBladeEventBuilder p;

  EXPECT_EQ(0, p.getNumberOfPositionRejected());

  p.lastPoint();

  EXPECT_DOUBLE_EQ(-1, p.getWirePosition());
  EXPECT_DOUBLE_EQ(-1, p.getStripPosition());

  EXPECT_EQ(1, p.getNumberOfPositionRejected());
}

int main(int argc, char **argv) {
  ::testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}
<commit_msg>minor edits II<commit_after>/** Copyright (C) 2017-2018 European Spallation Source */
//===----------------------------------------------------------------------===//
///
/// \file
///  Unit tests for MultiBladeEventBuilder using Google Test.
/// Here the various counters are tested.
///
/// Author: Carsten Søgaard, Niels Bohr Institute, University of Copenhagen
///
//===----------------------------------------------------------------------===//

/// \todo - nolonger valid - new data format

#include "mbcommon/MultiBladeEventBuilder.h"
#include "MultiBladeTestData.h"
#include "test/TestBase.h"

TEST(MBEventBuilder__Test, DataPointCounter) {

  // Instanciate the event-builder
  multiBladeEventBuilder p;

  // Test proper initialization.
  EXPECT_EQ(0U, p.getNumberOfEvents());

  // In the following: test proper incrementing.
  p.addDataPoint(0, 300, 0);
  EXPECT_EQ(1, p.getNumberOfDatapointsReceived());

  p.addDataPoint(1, 300, 1);
  EXPECT_EQ(2, p.getNumberOfDatapointsReceived());

  p.addDataPoint(2, 300, 2);
  EXPECT_EQ(3, p.getNumberOfDatapointsReceived());

  p.resetCounters();
  EXPECT_EQ(0, p.getNumberOfDatapointsReceived());
}

TEST(MBEventBuilder__Test, EventCounter) {

  // Test that events are counted correctly

  // Instanciate the event-builder
  multiBladeEventBuilder p;

  // Initialize the expected number of events counter
  uint nevents = 1;

  for (uint i = 0; i <= 15; i++) {

    // Read a subset of the test data
    uint begin = i * 5;
    uint end = begin + 5;
    std::vector<uint> datapoint(&data[begin], &data[end]);
    // Wire data-points
    EXPECT_EQ(datapoint[4], p.addDataPoint(datapoint[0], datapoint[2], datapoint[3]));
    // Strip data-points
    EXPECT_FALSE(p.addDataPoint(datapoint[1], datapoint[2], datapoint[3]));

    // Validation of event counter
    if (datapoint[4]) {
      EXPECT_EQ(nevents, p.getNumberOfEvents());
      // Increment validation iterator
      nevents++;
    }
  }

  // Provide a non-adjacent event, which is discarded
  for (uint i = 10; i < 15; i++) {

    // Remove a point (making it non adjacent)
    if (i != 12) {
      // Read a subset of the test data
      uint begin = i * 5;
      uint end = begin + 5;
      std::vector<uint> datapoint(&data[begin], &data[end]);
      // Wire data-points
      EXPECT_EQ(datapoint[4], p.addDataPoint(datapoint[0], datapoint[2], datapoint[3]));
      // Strip data-points
      EXPECT_FALSE(p.addDataPoint(datapoint[1], datapoint[2], datapoint[3]));
    }
  }

  // Validation of event counter at "end of run".
  // The counter was not incremented before, due to the non-adjacent point.
  p.lastPoint();
  EXPECT_EQ(nevents, p.getNumberOfEvents());
}

TEST(MBEventBuilder__Test, ClusterCounters) {

  // Test the counters of number of points per event

  // Instaciate the event-counter and configure
  multiBladeEventBuilder p;
  p.setTimeWindow(config[0]);
  p.setNumberOfWireChannels(config[1]);
  p.setNumberOfStripChannels(config[2]);

  // Validation counters (similar to those in the event-builder).
  // Index 0 = 1 datapoint, index 1 = 2 datapoints, etc.
  std::array<uint64_t, 6> wireclusterpoints = {{0, 0, 0, 0, 0, 0}};
  std::array<uint64_t, 6> stripclusterpoints = {{0, 0, 0, 0, 0, 0}};

  // Counters for number of points per event
  uint wirepoints = 0;
  uint strippoints = 0;

  // Iterator for validation data
  // std::vector<double>::iterator valw = validation_weighted.begin();

  // Test the case when there are both wire and strip points
  for (uint i = 0; i <= 21; i++) {
    uint begin = i * 5;
    uint end = begin + 5;
    std::vector<uint> datapoint(&data[begin], &data[end]);
    // Wire data-points
    p.addDataPoint(datapoint[0], datapoint[2], datapoint[3]);

    // Strip data-points
    p.addDataPoint(datapoint[1], datapoint[2], datapoint[3]);

    if (!datapoint[4]) {

      wirepoints++;
      strippoints++;

    } else {

      // Increment the correct positions in the counters
      wireclusterpoints[wirepoints - 1]++;
      stripclusterpoints[strippoints - 1]++;

      // Check all indexes of the counters for each event.
      EXPECT_EQ(wireclusterpoints[0], p.get2DWireClusterCounter()[0]);
      EXPECT_EQ(wireclusterpoints[1], p.get2DWireClusterCounter()[1]);
      EXPECT_EQ(wireclusterpoints[2], p.get2DWireClusterCounter()[2]);
      EXPECT_EQ(wireclusterpoints[3], p.get2DWireClusterCounter()[3]);
      EXPECT_EQ(wireclusterpoints[4], p.get2DWireClusterCounter()[4]);
      EXPECT_EQ(wireclusterpoints[5], p.get2DWireClusterCounter()[5]);

      EXPECT_EQ(stripclusterpoints[0], p.get2DStripClusterCounter()[0]);
      EXPECT_EQ(stripclusterpoints[1], p.get2DStripClusterCounter()[1]);
      EXPECT_EQ(stripclusterpoints[2], p.get2DStripClusterCounter()[2]);
      EXPECT_EQ(stripclusterpoints[3], p.get2DStripClusterCounter()[3]);
      EXPECT_EQ(stripclusterpoints[4], p.get2DStripClusterCounter()[4]);
      EXPECT_EQ(stripclusterpoints[5], p.get2DStripClusterCounter()[5]);

      // Reset the points counters (1, since a point is provided when finishing
      // the cluster
      wirepoints = 1;
      strippoints = 1;
    }
  }

  // End the "run" and reset all counters
  p.lastPoint();
  p.resetCounters();

  // Reset counters
  wireclusterpoints = {{0, 0, 0, 0, 0, 0}};
  stripclusterpoints = {{0, 0, 0, 0, 0, 0}};

  // Reset the counters to "start of run"
  wirepoints = 0;
  strippoints = 0;

  // Test the case of only wire points
  for (uint i = 0; i <= 21; i++) {
    uint begin = i * 5;
    uint end = begin + 5;
    std::vector<uint> datapoint(&data[begin], &data[end]);
    // Wire data-points
    p.addDataPoint(datapoint[0], datapoint[2], datapoint[3]);

    if (!datapoint[4]) {

      wirepoints++;

    } else {

      // Incement at the correct index.
      wireclusterpoints[wirepoints - 1]++;

      // Test counter at all indexes
      EXPECT_EQ(wireclusterpoints[0], p.get1DWireClusterCounter()[0]);
      EXPECT_EQ(wireclusterpoints[1], p.get1DWireClusterCounter()[1]);
      EXPECT_EQ(wireclusterpoints[2], p.get1DWireClusterCounter()[2]);
      EXPECT_EQ(wireclusterpoints[3], p.get1DWireClusterCounter()[3]);
      EXPECT_EQ(wireclusterpoints[4], p.get1DWireClusterCounter()[4]);
      EXPECT_EQ(wireclusterpoints[5], p.get1DWireClusterCounter()[5]);

      EXPECT_EQ(0, p.get1DStripClusterCounter()[0]);
      EXPECT_EQ(0, p.get1DStripClusterCounter()[1]);
      EXPECT_EQ(0, p.get1DStripClusterCounter()[2]);
      EXPECT_EQ(0, p.get1DStripClusterCounter()[3]);
      EXPECT_EQ(0, p.get1DStripClusterCounter()[4]);
      EXPECT_EQ(0, p.get1DStripClusterCounter()[5]);

      wirepoints = 1;
    }
  }

  // "End of run" and reset all counters
  p.lastPoint();
  p.resetCounters();

  // Reset validation counters
  wireclusterpoints = {{0, 0, 0, 0, 0, 0}};
  stripclusterpoints = {{0, 0, 0, 0, 0, 0}};

  // Reset event counters
  wirepoints = 0;
  strippoints = 0;

  // Test the case of only strip points
  for (uint i = 0; i <= 21; i++) {
    uint begin = i * 5;
    uint end = begin + 5;
    std::vector<uint> datapoint(&data[begin], &data[end]);
    // Strip data-points
    p.addDataPoint(datapoint[1], datapoint[2], datapoint[3]);

    if (!datapoint[4]) {

      strippoints++;

      EXPECT_EQ(strippoints, p.getStripClusterSize());

    } else {

      // Incement at the correct index.
      stripclusterpoints[strippoints - 1]++;

      // Test counter at all indexes
      EXPECT_EQ(0, p.get1DWireClusterCounter()[0]);
      EXPECT_EQ(0, p.get1DWireClusterCounter()[1]);
      EXPECT_EQ(0, p.get1DWireClusterCounter()[2]);
      EXPECT_EQ(0, p.get1DWireClusterCounter()[3]);
      EXPECT_EQ(0, p.get1DWireClusterCounter()[4]);
      EXPECT_EQ(0, p.get1DWireClusterCounter()[5]);

      EXPECT_EQ(stripclusterpoints[0], p.get1DStripClusterCounter()[0]);
      EXPECT_EQ(stripclusterpoints[1], p.get1DStripClusterCounter()[1]);
      EXPECT_EQ(stripclusterpoints[2], p.get1DStripClusterCounter()[2]);
      EXPECT_EQ(stripclusterpoints[3], p.get1DStripClusterCounter()[3]);
      EXPECT_EQ(stripclusterpoints[4], p.get1DStripClusterCounter()[4]);
      EXPECT_EQ(stripclusterpoints[5], p.get1DStripClusterCounter()[5]);

      strippoints = 1;
    }
  }
}

TEST(MBEventBuilder__Test, NoDataRecieved) {

  multiBladeEventBuilder p;

  EXPECT_EQ(0, p.getNumberOfPositionRejected());

  p.lastPoint();

  EXPECT_DOUBLE_EQ(-1, p.getWirePosition());
  EXPECT_DOUBLE_EQ(-1, p.getStripPosition());

  EXPECT_EQ(1, p.getNumberOfPositionRejected());
}

int main(int argc, char **argv) {
  ::testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}
<|endoftext|>


<commit_before>#ifndef PLUGIN_H
#define PLUGIN_H

#include <functional>
#include <memory>
#include "wayfire/util.hpp"
#include "wayfire/bindings.hpp"

#include <wayfire/nonstd/wlroots.hpp>

class wayfire_config;
namespace wf
{
class output_t;

/**
 * Plugins can set their capabilities to indicate what kind of plugin they are.
 * At any point, only one plugin with a given capability can be active on its
 * output (although multiple plugins with the same capability can be loaded).
 */
enum plugin_capabilities_t
{
    /** The plugin provides view decorations */
    CAPABILITY_VIEW_DECORATOR    = 1 << 0,
    /** The plugin grabs input.
     * Required in order to use plugin_grab_interface_t::grab() */
    CAPABILITY_GRAB_INPUT        = 1 << 1,
    /** The plugin uses custom renderer */
    CAPABILITY_CUSTOM_RENDERER   = 1 << 2,
    /** The plugin manages the whole desktop, for ex. switches workspaces. */
    CAPABILITY_MANAGE_DESKTOP    = 1 << 3,
    /* Compound capabilities */

    /** The plugin manages the whole compositor state */
    CAPABILITY_MANAGE_COMPOSITOR = CAPABILITY_GRAB_INPUT |
        CAPABILITY_MANAGE_DESKTOP | CAPABILITY_CUSTOM_RENDERER,
};

/**
 * The plugin grab interface is what the plugins use to announce themselves to
 * the core and other plugins as active, and to request to grab input.
 */
struct plugin_grab_interface_t
{
  private:
    bool grabbed = false;

  public:
    /** The name of the plugin. Not important */
    std::string name;
    /** The plugin capabilities. A bitmask of the values specified above */
    uint32_t capabilities;
    /** The output the grab interface is on */
    wf::output_t*const output;

    plugin_grab_interface_t(wf::output_t *_output);

    /**
     * Grab the input on the output. Requires CAPABILITY_GRAB_INPUT.
     * On successful grab, core will reset keyboard/pointer/touch focus.
     *
     * @return True if input was successfully grabbed.
     */
    bool grab();
    /** @return If the grab interface is grabbed */
    bool is_grabbed();
    /** Ungrab input, if it is grabbed. */
    void ungrab();

    /**
     * When grabbed, core will redirect all input events to the grabbing plugin.
     * The grabbing plugin can subscribe to different input events by setting
     * the callbacks below.
     */
    struct
    {
        struct
        {
            std::function<void(wlr_event_pointer_axis*)> axis;
            std::function<void(uint32_t, uint32_t)> button; // button, state
            std::function<void(int32_t, int32_t)> motion; // x, y
            std::function<void(wlr_event_pointer_motion*)> relative_motion;
        } pointer;

        struct
        {
            std::function<void(uint32_t, uint32_t)> key; // button, state
            std::function<void(uint32_t, uint32_t)> mod; // modifier, state
        } keyboard;

        struct
        {
            std::function<void(int32_t, int32_t, int32_t)> down; // id, x, y
            std::function<void(int32_t)> up; // id
            std::function<void(int32_t, int32_t, int32_t)> motion; // id, x, y
        } touch;

        /**
         * Each plugin might be deactivated forcefully, for example when the
         * desktop is locked. Plugins MUST honor this signal and exit their
         * grabs/renderers immediately.
         *
         * Note that cancel() is emitted even when the plugin is just activated
         * without grabbing input.
         */
        std::function<void()> cancel;
    } callbacks;
};

using plugin_grab_interface_uptr = std::unique_ptr<plugin_grab_interface_t>;

class plugin_interface_t
{
  public:
    /**
     * The output this plugin is running on. Initialized by core.
     * Each output has its own set of plugin instances. This way, a plugin
     * rarely if ever needs to care about multi-monitor setups.
     */
    wf::output_t *output;

    /**
     * The grab interface of the plugin, initialized by core.
     */
    std::unique_ptr<plugin_grab_interface_t> grab_interface;

    /**
     * The init method is the entry of the plugin. In the init() method, the
     * plugin should register all bindings it provides, connect to signals, etc.
     */
    virtual void init() = 0;

    /**
     * The fini method is called when a plugin is unloaded. It should clean up
     * all global state it has set (for ex. signal callbacks, bindings, ...),
     * because the plugin will be freed after this.
     */
    virtual void fini();

    /**
     * A plugin can request that it is never unloaded, even if it is removed
     * from the config's plugin list.
     *
     * Note that unloading a plugin is sometimes unavoidable, for ex. when the
     * output the plugin is running on is destroyed. So non-unloadable plugins
     * should still provide proper fini() methods.
     */
    virtual bool is_unloadable()
    {
        return true;
    }

    virtual ~plugin_interface_t();

    /** Handle to the plugin's .so file, used by the plugin loader */
    void *handle = NULL;
};
}

/**
 * Each plugin must provide a function which instantiates the plugin's class
 * and returns the instance.
 *
 * This function must have the name newInstance() and should be declared with
 * extern "C" so that the loader can find it.
 */
using wayfire_plugin_load_func = wf::plugin_interface_t * (*)();

/** The version of Wayfire's API/ABI */
constexpr uint32_t WAYFIRE_API_ABI_VERSION = 2021'09'09;

/**
 * Each plugin must also provide a function which returns the Wayfire API/ABI
 * that it was compiled with.
 *
 * This function must have the name getWayfireVersion() and should be declared
 * with extern "C" so that the loader can find it.
 */
using wayfire_plugin_version_func = uint32_t (*)();

/**
 * A macro to declare the necessary functions, given the plugin class name
 */
#define DECLARE_WAYFIRE_PLUGIN(PluginClass) \
    extern "C" \
    { \
        wf::plugin_interface_t*newInstance() { return new PluginClass; } \
        uint32_t getWayfireVersion() { return WAYFIRE_API_ABI_VERSION; } \
    }

#endif
<commit_msg>api: bump version because of header changes<commit_after>#ifndef PLUGIN_H
#define PLUGIN_H

#include <functional>
#include <memory>
#include "wayfire/util.hpp"
#include "wayfire/bindings.hpp"

#include <wayfire/nonstd/wlroots.hpp>

class wayfire_config;
namespace wf
{
class output_t;

/**
 * Plugins can set their capabilities to indicate what kind of plugin they are.
 * At any point, only one plugin with a given capability can be active on its
 * output (although multiple plugins with the same capability can be loaded).
 */
enum plugin_capabilities_t
{
    /** The plugin provides view decorations */
    CAPABILITY_VIEW_DECORATOR    = 1 << 0,
    /** The plugin grabs input.
     * Required in order to use plugin_grab_interface_t::grab() */
    CAPABILITY_GRAB_INPUT        = 1 << 1,
    /** The plugin uses custom renderer */
    CAPABILITY_CUSTOM_RENDERER   = 1 << 2,
    /** The plugin manages the whole desktop, for ex. switches workspaces. */
    CAPABILITY_MANAGE_DESKTOP    = 1 << 3,
    /* Compound capabilities */

    /** The plugin manages the whole compositor state */
    CAPABILITY_MANAGE_COMPOSITOR = CAPABILITY_GRAB_INPUT |
        CAPABILITY_MANAGE_DESKTOP | CAPABILITY_CUSTOM_RENDERER,
};

/**
 * The plugin grab interface is what the plugins use to announce themselves to
 * the core and other plugins as active, and to request to grab input.
 */
struct plugin_grab_interface_t
{
  private:
    bool grabbed = false;

  public:
    /** The name of the plugin. Not important */
    std::string name;
    /** The plugin capabilities. A bitmask of the values specified above */
    uint32_t capabilities;
    /** The output the grab interface is on */
    wf::output_t*const output;

    plugin_grab_interface_t(wf::output_t *_output);

    /**
     * Grab the input on the output. Requires CAPABILITY_GRAB_INPUT.
     * On successful grab, core will reset keyboard/pointer/touch focus.
     *
     * @return True if input was successfully grabbed.
     */
    bool grab();
    /** @return If the grab interface is grabbed */
    bool is_grabbed();
    /** Ungrab input, if it is grabbed. */
    void ungrab();

    /**
     * When grabbed, core will redirect all input events to the grabbing plugin.
     * The grabbing plugin can subscribe to different input events by setting
     * the callbacks below.
     */
    struct
    {
        struct
        {
            std::function<void(wlr_event_pointer_axis*)> axis;
            std::function<void(uint32_t, uint32_t)> button; // button, state
            std::function<void(int32_t, int32_t)> motion; // x, y
            std::function<void(wlr_event_pointer_motion*)> relative_motion;
        } pointer;

        struct
        {
            std::function<void(uint32_t, uint32_t)> key; // button, state
            std::function<void(uint32_t, uint32_t)> mod; // modifier, state
        } keyboard;

        struct
        {
            std::function<void(int32_t, int32_t, int32_t)> down; // id, x, y
            std::function<void(int32_t)> up; // id
            std::function<void(int32_t, int32_t, int32_t)> motion; // id, x, y
        } touch;

        /**
         * Each plugin might be deactivated forcefully, for example when the
         * desktop is locked. Plugins MUST honor this signal and exit their
         * grabs/renderers immediately.
         *
         * Note that cancel() is emitted even when the plugin is just activated
         * without grabbing input.
         */
        std::function<void()> cancel;
    } callbacks;
};

using plugin_grab_interface_uptr = std::unique_ptr<plugin_grab_interface_t>;

class plugin_interface_t
{
  public:
    /**
     * The output this plugin is running on. Initialized by core.
     * Each output has its own set of plugin instances. This way, a plugin
     * rarely if ever needs to care about multi-monitor setups.
     */
    wf::output_t *output;

    /**
     * The grab interface of the plugin, initialized by core.
     */
    std::unique_ptr<plugin_grab_interface_t> grab_interface;

    /**
     * The init method is the entry of the plugin. In the init() method, the
     * plugin should register all bindings it provides, connect to signals, etc.
     */
    virtual void init() = 0;

    /**
     * The fini method is called when a plugin is unloaded. It should clean up
     * all global state it has set (for ex. signal callbacks, bindings, ...),
     * because the plugin will be freed after this.
     */
    virtual void fini();

    /**
     * A plugin can request that it is never unloaded, even if it is removed
     * from the config's plugin list.
     *
     * Note that unloading a plugin is sometimes unavoidable, for ex. when the
     * output the plugin is running on is destroyed. So non-unloadable plugins
     * should still provide proper fini() methods.
     */
    virtual bool is_unloadable()
    {
        return true;
    }

    virtual ~plugin_interface_t();

    /** Handle to the plugin's .so file, used by the plugin loader */
    void *handle = NULL;
};
}

/**
 * Each plugin must provide a function which instantiates the plugin's class
 * and returns the instance.
 *
 * This function must have the name newInstance() and should be declared with
 * extern "C" so that the loader can find it.
 */
using wayfire_plugin_load_func = wf::plugin_interface_t * (*)();

/** The version of Wayfire's API/ABI */
constexpr uint32_t WAYFIRE_API_ABI_VERSION = 2021'10'12;

/**
 * Each plugin must also provide a function which returns the Wayfire API/ABI
 * that it was compiled with.
 *
 * This function must have the name getWayfireVersion() and should be declared
 * with extern "C" so that the loader can find it.
 */
using wayfire_plugin_version_func = uint32_t (*)();

/**
 * A macro to declare the necessary functions, given the plugin class name
 */
#define DECLARE_WAYFIRE_PLUGIN(PluginClass) \
    extern "C" \
    { \
        wf::plugin_interface_t*newInstance() { return new PluginClass; } \
        uint32_t getWayfireVersion() { return WAYFIRE_API_ABI_VERSION; } \
    }

#endif
<|endoftext|>


<commit_before>/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2012, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the copyright holder(s) nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * $Id$
 *
 */

#include <sensor_msgs/PointCloud2.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/console/print.h>
#include <pcl/console/parse.h>
#include <pcl/console/time.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/filters/statistical_outlier_removal.h>

using namespace pcl;
using namespace pcl::io;
using namespace pcl::console;

std::string default_method = "radius";

int default_mean_k = 2;
double default_std_dev_mul = 0.0;
int default_negative = 1;

double default_radius = 0.0;
int default_min_pts = 0;

void
printHelp (int, char **argv)
{
  print_error ("Syntax is: %s input.pcd output.pcd <options>\n", argv[0]);
  print_info ("  where options are:\n");
  print_info ("                     -method X = the outlier removal method to be used (options: radius / statistical) (default: ");
  print_value ("%s", default_method.c_str ()); print_info (")\n");
  print_info ("                     -radius X = (RadiusOutlierRemoval) the sphere radius used for determining the k-nearest neighbors (default: ");
  print_value ("%d", default_min_pts); print_info (")\n");
  print_info ("                     -min_pts X = (RadiusOutlierRemoval) the minimum number of neighbors that a point needs to have in the given search radius in order to be considered an inlier (default: ");
  print_value ("%d", default_min_pts); print_info (")\n");
  print_info ("                     -mean_k X = (StatisticalOutlierRemoval only) the number of points to use for mean distance estimation (default: ");
  print_value ("%d", default_mean_k); print_info (")\n");
  print_info ("                     -std_dev_mul X = (StatisticalOutlierRemoval only) the standard deviation multiplier threshold (default: ");
  print_value ("%f", default_std_dev_mul); print_info (")\n");
  print_info ("                     -inliers X = (StatisticalOutlierRemoval only) decides whether the inliers should be returned (1), or the outliers (0). (default: ");
  print_value ("%d", default_negative); print_info (")\n");
}

bool
loadCloud (const std::string &filename, sensor_msgs::PointCloud2 &cloud)
{
  TicToc tt;
  print_highlight ("Loading "); print_value ("%s ", filename.c_str ());

  tt.tic ();
  if (loadPCDFile (filename, cloud) < 0)
    return (false);
  print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", cloud.width * cloud.height); print_info (" points]\n");
  print_info ("Available dimensions: "); print_value ("%s\n", pcl::getFieldsList (cloud).c_str ());

  return (true);
}

void
compute (const sensor_msgs::PointCloud2::ConstPtr &input, sensor_msgs::PointCloud2 &output,
         std::string method,
         int min_pts, double radius,
         int mean_k, double std_dev_mul, bool negative)
{

  PointCloud<PointXYZ>::Ptr xyz_cloud_pre (new pcl::PointCloud<PointXYZ> ()),
      xyz_cloud (new pcl::PointCloud<PointXYZ> ());
  fromROSMsg (*input, *xyz_cloud_pre);

  std::vector<int> index_vector;
  removeNaNFromPointCloud<PointXYZ> (*xyz_cloud_pre, *xyz_cloud, index_vector);

  TicToc tt;
  tt.tic ();
  PointCloud<PointXYZ>::Ptr xyz_cloud_filtered (new PointCloud<PointXYZ> ());
  if (method == "statistical")
  {
    StatisticalOutlierRemoval<PointXYZ> filter;
    filter.setInputCloud (xyz_cloud);
    filter.setMeanK (mean_k);
    filter.setStddevMulThresh (std_dev_mul);
    filter.setNegative (negative);
    PCL_INFO ("Computing filtered cloud with mean_k %d, std_dev_mul %f, inliers %d\n", filter.getMeanK (), filter.getStddevMulThresh (), filter.getNegative ());
    filter.filter (*xyz_cloud_filtered);
  }
  else if (method == "radius")
  {
    RadiusOutlierRemoval<PointXYZ> filter;
    filter.setInputCloud (xyz_cloud);
    filter.setRadiusSearch (radius);
    filter.setMinNeighborsInRadius (min_pts);
    PCL_INFO ("Computing filtered cloud with radius %f, min_pts %d\n", radius, min_pts);
    filter.filter (*xyz_cloud_filtered);
  }
  else
  {
    PCL_ERROR ("%s is not a valid filter name! Quitting!\n", method.c_str ());
    return;
  }

  print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", xyz_cloud_filtered->width * xyz_cloud_filtered->height); print_info (" points]\n");

  toROSMsg (*xyz_cloud_filtered, output);
}

void
saveCloud (const std::string &filename, const sensor_msgs::PointCloud2 &output)
{
  TicToc tt;
  tt.tic ();

  print_highlight ("Saving "); print_value ("%s ", filename.c_str ());

  pcl::io::savePCDFile (filename, output,  Eigen::Vector4f::Zero (),
                        Eigen::Quaternionf::Identity (), true);

  print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", output.width * output.height); print_info (" points]\n");
}

/* ---[ */
int
main (int argc, char** argv)
{
  print_info ("Statistical Outlier Removal filtering of a point cloud. For more information, use: %s -h\n", argv[0]);

  if (argc < 3)
  {
    printHelp (argc, argv);
    return (-1);
  }

  // Parse the command line arguments for .pcd files
  std::vector<int> p_file_indices;
  p_file_indices = parse_file_extension_argument (argc, argv, ".pcd");
  if (p_file_indices.size () != 2)
  {
    print_error ("Need one input PCD file and one output PCD file to continue.\n");
    return (-1);
  }

  // Command line parsing
  std::string method = default_method;
  int min_pts = default_min_pts;
  double radius = default_radius;
  int mean_k = default_mean_k;
  double std_dev_mul = default_std_dev_mul;
  int negative = default_negative;

  parse_argument (argc, argv, "-method", method);
  parse_argument (argc, argv, "-radius", radius);
  parse_argument (argc, argv, "-min_pts", min_pts);
  parse_argument (argc, argv, "-mean_k", mean_k);
  parse_argument (argc, argv, "-std_dev_mul", std_dev_mul);
  parse_argument (argc, argv, "-inliers", negative);

  // Load the first file
  sensor_msgs::PointCloud2::Ptr cloud (new sensor_msgs::PointCloud2);
  if (!loadCloud (argv[p_file_indices[0]], *cloud))
    return (-1);

  // Do the smoothing
  sensor_msgs::PointCloud2 output;
  compute (cloud, output, method, min_pts, radius, mean_k, std_dev_mul, negative);

  // Save into the second file
  saveCloud (argv[p_file_indices[1]], output);
}
<commit_msg>added keep_organized switch<commit_after>/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2012, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the copyright holder(s) nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * $Id$
 *
 */

#include <sensor_msgs/PointCloud2.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/console/print.h>
#include <pcl/console/parse.h>
#include <pcl/console/time.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/filters/statistical_outlier_removal.h>

using namespace pcl;
using namespace pcl::io;
using namespace pcl::console;

std::string default_method = "radius";

int default_mean_k = 2;
double default_std_dev_mul = 0.0;
int default_negative = 1;

double default_radius = 0.0;
int default_min_pts = 0;

void
printHelp (int, char **argv)
{
  print_error ("Syntax is: %s input.pcd output.pcd <options>\n", argv[0]);
  print_info ("  where options are:\n");
  print_info ("                     -method X = the outlier removal method to be used (options: radius / statistical) (default: ");
  print_value ("%s", default_method.c_str ()); print_info (")\n");
  print_info ("                     -radius X = (RadiusOutlierRemoval) the sphere radius used for determining the k-nearest neighbors (default: ");
  print_value ("%d", default_min_pts); print_info (")\n");
  print_info ("                     -min_pts X = (RadiusOutlierRemoval) the minimum number of neighbors that a point needs to have in the given search radius in order to be considered an inlier (default: ");
  print_value ("%d", default_min_pts); print_info (")\n");
  print_info ("                     -mean_k X = (StatisticalOutlierRemoval only) the number of points to use for mean distance estimation (default: ");
  print_value ("%d", default_mean_k); print_info (")\n");
  print_info ("                     -std_dev_mul X = (StatisticalOutlierRemoval only) the standard deviation multiplier threshold (default: ");
  print_value ("%f", default_std_dev_mul); print_info (")\n");
  print_info ("                     -inliers X = (StatisticalOutlierRemoval only) decides whether the inliers should be returned (1), or the outliers (0). (default: ");
  print_value ("%d", default_negative); print_info (")\n");
  print_info ("                     -keep_organized");
}

bool
loadCloud (const std::string &filename, sensor_msgs::PointCloud2 &cloud)
{
  TicToc tt;
  print_highlight ("Loading "); print_value ("%s ", filename.c_str ());

  tt.tic ();
  if (loadPCDFile (filename, cloud) < 0)
    return (false);
  print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", cloud.width * cloud.height); print_info (" points]\n");
  print_info ("Available dimensions: "); print_value ("%s\n", pcl::getFieldsList (cloud).c_str ());

  return (true);
}

void
compute (const sensor_msgs::PointCloud2::ConstPtr &input, sensor_msgs::PointCloud2 &output,
         std::string method,
         int min_pts, double radius,
         int mean_k, double std_dev_mul, bool negative, bool keep_organized)
{

  PointCloud<PointXYZ>::Ptr xyz_cloud_pre (new pcl::PointCloud<PointXYZ> ()),
      xyz_cloud (new pcl::PointCloud<PointXYZ> ());
  fromROSMsg (*input, *xyz_cloud_pre);

  //std::vector<int> index_vector;
  //removeNaNFromPointCloud<PointXYZ> (*xyz_cloud_pre, *xyz_cloud, index_vector);

  xyz_cloud = xyz_cloud_pre;

  TicToc tt;
  tt.tic ();
  PointCloud<PointXYZ>::Ptr xyz_cloud_filtered (new PointCloud<PointXYZ> ());
  if (method == "statistical")
  {
    StatisticalOutlierRemoval<PointXYZ> filter;
    filter.setInputCloud (xyz_cloud);
    filter.setMeanK (mean_k);
    filter.setStddevMulThresh (std_dev_mul);
    filter.setNegative (negative);
    filter.setKeepOrganized (keep_organized);
    PCL_INFO ("Computing filtered cloud with mean_k %d, std_dev_mul %f, inliers %d\n", filter.getMeanK (), filter.getStddevMulThresh (), filter.getNegative ());
    filter.filter (*xyz_cloud_filtered);
  }
  else if (method == "radius")
  {
    RadiusOutlierRemoval<PointXYZ> filter;
    filter.setInputCloud (xyz_cloud);
    filter.setRadiusSearch (radius);
    filter.setMinNeighborsInRadius (min_pts);
    filter.setKeepOrganized (keep_organized);
    PCL_INFO ("Computing filtered cloud with radius %f, min_pts %d\n", radius, min_pts);
    filter.filter (*xyz_cloud_filtered);
  }
  else
  {
    PCL_ERROR ("%s is not a valid filter name! Quitting!\n", method.c_str ());
    return;
  }

  print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", xyz_cloud_filtered->width * xyz_cloud_filtered->height); print_info (" points]\n");

  toROSMsg (*xyz_cloud_filtered, output);
}

void
saveCloud (const std::string &filename, const sensor_msgs::PointCloud2 &output)
{
  TicToc tt;
  tt.tic ();

  print_highlight ("Saving "); print_value ("%s ", filename.c_str ());

  pcl::io::savePCDFile (filename, output,  Eigen::Vector4f::Zero (),
                        Eigen::Quaternionf::Identity (), true);

  print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", output.width * output.height); print_info (" points]\n");
}

/* ---[ */
int
main (int argc, char** argv)
{
  print_info ("Statistical Outlier Removal filtering of a point cloud. For more information, use: %s -h\n", argv[0]);

  if (argc < 3)
  {
    printHelp (argc, argv);
    return (-1);
  }

  // Parse the command line arguments for .pcd files
  std::vector<int> p_file_indices;
  p_file_indices = parse_file_extension_argument (argc, argv, ".pcd");
  if (p_file_indices.size () != 2)
  {
    print_error ("Need one input PCD file and one output PCD file to continue.\n");
    return (-1);
  }

  // Command line parsing
  std::string method = default_method;
  int min_pts = default_min_pts;
  double radius = default_radius;
  int mean_k = default_mean_k;
  double std_dev_mul = default_std_dev_mul;
  int negative = default_negative;

  parse_argument (argc, argv, "-method", method);
  parse_argument (argc, argv, "-radius", radius);
  parse_argument (argc, argv, "-min_pts", min_pts);
  parse_argument (argc, argv, "-mean_k", mean_k);
  parse_argument (argc, argv, "-std_dev_mul", std_dev_mul);
  parse_argument (argc, argv, "-inliers", negative);
  bool keep_organized = find_switch (argc, argv, "-keep_organized");

  // Load the first file
  sensor_msgs::PointCloud2::Ptr cloud (new sensor_msgs::PointCloud2);
  if (!loadCloud (argv[p_file_indices[0]], *cloud))
    return (-1);

  // Do the smoothing
  sensor_msgs::PointCloud2 output;
  compute (cloud, output, method, min_pts, radius, mean_k, std_dev_mul, negative, keep_organized);

  // Save into the second file
  saveCloud (argv[p_file_indices[1]], output);
}
<|endoftext|>


<commit_before>/*
 *  Copyright (c) 2013 The WebRTC project authors. All Rights Reserved.
 *
 *  Use of this source code is governed by a BSD-style license
 *  that can be found in the LICENSE file in the root of the source
 *  tree. An additional intellectual property rights grant can be found
 *  in the file PATENTS.  All contributing project authors may
 *  be found in the AUTHORS file in the root of the source tree.
 */

#include <memory>

#include "webrtc/modules/desktop_capture/mouse_cursor_monitor.h"

#include <X11/extensions/Xfixes.h>
#include <X11/Xlib.h>
#include <X11/Xutil.h>

#include "webrtc/modules/desktop_capture/desktop_capture_options.h"
#include "webrtc/modules/desktop_capture/desktop_frame.h"
#include "webrtc/modules/desktop_capture/mouse_cursor.h"
#include "webrtc/modules/desktop_capture/x11/x_error_trap.h"
#include "webrtc/system_wrappers/include/logging.h"

namespace {

// WindowCapturer returns window IDs of X11 windows with WM_STATE attribute.
// These windows may not be immediate children of the root window, because
// window managers may re-parent them to add decorations. However,
// XQueryPointer() expects to be passed children of the root. This function
// searches up the list of the windows to find the root child that corresponds
// to |window|.
Window GetTopLevelWindow(Display* display, Window window) {
  while (true) {
    // If the window is in WithdrawnState then look at all of its children.
    ::Window root, parent;
    ::Window *children;
    unsigned int num_children;
    if (!XQueryTree(display, window, &root, &parent, &children,
                    &num_children)) {
      LOG(LS_ERROR) << "Failed to query for child windows although window"
                    << "does not have a valid WM_STATE.";
      return None;
    }
    if (children)
      XFree(children);

    if (parent == root)
      break;

    window = parent;
  }

  return window;
}

}  // namespace

namespace webrtc {

class MouseCursorMonitorX11 : public MouseCursorMonitor,
                              public SharedXDisplay::XEventHandler {
 public:
  MouseCursorMonitorX11(const DesktopCaptureOptions& options, Window window);
  ~MouseCursorMonitorX11() override;

  void Init(Callback* callback, Mode mode) override;
  void Capture() override;

 private:
  // SharedXDisplay::XEventHandler interface.
  bool HandleXEvent(const XEvent& event) override;

  Display* display() { return x_display_->display(); }

  // Captures current cursor shape and stores it in |cursor_shape_|.
  void CaptureCursor();

  rtc::scoped_refptr<SharedXDisplay> x_display_;
  Callback* callback_;
  Mode mode_;
  Window window_;

  bool have_xfixes_;
  int xfixes_event_base_;
  int xfixes_error_base_;

  std::unique_ptr<MouseCursor> cursor_shape_;
};

MouseCursorMonitorX11::MouseCursorMonitorX11(
    const DesktopCaptureOptions& options,
    Window window)
    : x_display_(options.x_display()),
      callback_(NULL),
      mode_(SHAPE_AND_POSITION),
      window_(window),
      have_xfixes_(false),
      xfixes_event_base_(-1),
      xfixes_error_base_(-1) {}

MouseCursorMonitorX11::~MouseCursorMonitorX11() {
  if (have_xfixes_) {
    x_display_->RemoveEventHandler(xfixes_event_base_ + XFixesCursorNotify,
                                   this);
  }
}

void MouseCursorMonitorX11::Init(Callback* callback, Mode mode) {
  // Init can be called only once per instance of MouseCursorMonitor.
  assert(!callback_);
  assert(callback);

  callback_ = callback;
  mode_ = mode;

  have_xfixes_ =
      XFixesQueryExtension(display(), &xfixes_event_base_, &xfixes_error_base_);

  if (have_xfixes_) {
    // Register for changes to the cursor shape.
    XFixesSelectCursorInput(display(), window_, XFixesDisplayCursorNotifyMask);
    x_display_->AddEventHandler(xfixes_event_base_ + XFixesCursorNotify, this);

    CaptureCursor();
  } else {
    LOG(LS_INFO) << "X server does not support XFixes.";
  }
}

void MouseCursorMonitorX11::Capture() {
  assert(callback_);

  // Process X11 events in case XFixes has sent cursor notification.
  x_display_->ProcessPendingXEvents();

  // cursor_shape_| is set only if we were notified of a cursor shape change.
  if (cursor_shape_.get())
    callback_->OnMouseCursor(cursor_shape_.release());

  // Get cursor position if necessary.
  if (mode_ == SHAPE_AND_POSITION) {
    int root_x;
    int root_y;
    int win_x;
    int win_y;
    Window root_window;
    Window child_window;
    unsigned int mask;

    XErrorTrap error_trap(display());
    Bool result = XQueryPointer(display(), window_, &root_window, &child_window,
                                &root_x, &root_y, &win_x, &win_y, &mask);
    CursorState state;
    if (!result || error_trap.GetLastErrorAndDisable() != 0) {
      state = OUTSIDE;
    } else {
      // In screen mode (window_ == root_window) the mouse is always inside.
      // XQueryPointer() sets |child_window| to None if the cursor is outside
      // |window_|.
      state =
          (window_ == root_window || child_window != None) ? INSIDE : OUTSIDE;
    }

    callback_->OnMouseCursorPosition(state,
                                     webrtc::DesktopVector(win_x, win_y));
  }
}

bool MouseCursorMonitorX11::HandleXEvent(const XEvent& event) {
  if (have_xfixes_ && event.type == xfixes_event_base_ + XFixesCursorNotify) {
    const XFixesCursorNotifyEvent* cursor_event =
        reinterpret_cast<const XFixesCursorNotifyEvent*>(&event);
    if (cursor_event->subtype == XFixesDisplayCursorNotify) {
      CaptureCursor();
    }
    // Return false, even if the event has been handled, because there might be
    // other listeners for cursor notifications.
  }
  return false;
}

void MouseCursorMonitorX11::CaptureCursor() {
  assert(have_xfixes_);

  XFixesCursorImage* img;
  {
    XErrorTrap error_trap(display());
    img = XFixesGetCursorImage(display());
    if (!img || error_trap.GetLastErrorAndDisable() != 0)
       return;
   }

   std::unique_ptr<DesktopFrame> image(
       new BasicDesktopFrame(DesktopSize(img->width, img->height)));

  // Xlib stores 32-bit data in longs, even if longs are 64-bits long.
  unsigned long* src = img->pixels;
  uint32_t* dst = reinterpret_cast<uint32_t*>(image->data());
  uint32_t* dst_end = dst + (img->width * img->height);
  while (dst < dst_end) {
    *dst++ = static_cast<uint32_t>(*src++);
  }

  DesktopVector hotspot(std::min(img->width, img->xhot),
                        std::min(img->height, img->yhot));

  XFree(img);

  cursor_shape_.reset(new MouseCursor(image.release(), hotspot));
}

// static
MouseCursorMonitor* MouseCursorMonitor::CreateForWindow(
    const DesktopCaptureOptions& options, WindowId window) {
  if (!options.x_display())
    return NULL;
  window = GetTopLevelWindow(options.x_display()->display(), window);
  if (window == None)
    return NULL;
  return new MouseCursorMonitorX11(options, window);
}

MouseCursorMonitor* MouseCursorMonitor::CreateForScreen(
    const DesktopCaptureOptions& options,
    ScreenId screen) {
  if (!options.x_display())
    return NULL;
  return new MouseCursorMonitorX11(
      options, DefaultRootWindow(options.x_display()->display()));
}

}  // namespace webrtc
<commit_msg>Use a default mouse cursor if XFixes is not supported.<commit_after>/*
 *  Copyright (c) 2013 The WebRTC project authors. All Rights Reserved.
 *
 *  Use of this source code is governed by a BSD-style license
 *  that can be found in the LICENSE file in the root of the source
 *  tree. An additional intellectual property rights grant can be found
 *  in the file PATENTS.  All contributing project authors may
 *  be found in the AUTHORS file in the root of the source tree.
 */

#include <memory>

#include "webrtc/modules/desktop_capture/mouse_cursor_monitor.h"

#include <X11/extensions/Xfixes.h>
#include <X11/Xlib.h>
#include <X11/Xutil.h>

#include "webrtc/modules/desktop_capture/desktop_capture_options.h"
#include "webrtc/modules/desktop_capture/desktop_frame.h"
#include "webrtc/modules/desktop_capture/mouse_cursor.h"
#include "webrtc/modules/desktop_capture/x11/x_error_trap.h"
#include "webrtc/system_wrappers/include/logging.h"

namespace {

// WindowCapturer returns window IDs of X11 windows with WM_STATE attribute.
// These windows may not be immediate children of the root window, because
// window managers may re-parent them to add decorations. However,
// XQueryPointer() expects to be passed children of the root. This function
// searches up the list of the windows to find the root child that corresponds
// to |window|.
Window GetTopLevelWindow(Display* display, Window window) {
  while (true) {
    // If the window is in WithdrawnState then look at all of its children.
    ::Window root, parent;
    ::Window *children;
    unsigned int num_children;
    if (!XQueryTree(display, window, &root, &parent, &children,
                    &num_children)) {
      LOG(LS_ERROR) << "Failed to query for child windows although window"
                    << "does not have a valid WM_STATE.";
      return None;
    }
    if (children)
      XFree(children);

    if (parent == root)
      break;

    window = parent;
  }

  return window;
}

}  // namespace

namespace webrtc {

class MouseCursorMonitorX11 : public MouseCursorMonitor,
                              public SharedXDisplay::XEventHandler {
 public:
  MouseCursorMonitorX11(const DesktopCaptureOptions& options, Window window);
  ~MouseCursorMonitorX11() override;

  void Init(Callback* callback, Mode mode) override;
  void Capture() override;

 private:
  // SharedXDisplay::XEventHandler interface.
  bool HandleXEvent(const XEvent& event) override;

  Display* display() { return x_display_->display(); }

  // Captures current cursor shape and stores it in |cursor_shape_|.
  void CaptureCursor();

  rtc::scoped_refptr<SharedXDisplay> x_display_;
  Callback* callback_;
  Mode mode_;
  Window window_;

  bool have_xfixes_;
  int xfixes_event_base_;
  int xfixes_error_base_;

  std::unique_ptr<MouseCursor> cursor_shape_;
};

MouseCursorMonitorX11::MouseCursorMonitorX11(
    const DesktopCaptureOptions& options,
    Window window)
    : x_display_(options.x_display()),
      callback_(NULL),
      mode_(SHAPE_AND_POSITION),
      window_(window),
      have_xfixes_(false),
      xfixes_event_base_(-1),
      xfixes_error_base_(-1) {
  // Set a default initial cursor shape in case XFixes is not present.
  const int kSize = 5;
  std::unique_ptr<DesktopFrame> default_cursor(
      new BasicDesktopFrame(DesktopSize(kSize, kSize)));
  const uint8_t pixels[kSize * kSize] = {
    0x00, 0x00, 0x00, 0x00, 0x00,
    0x00, 0xff, 0xff, 0xff, 0x00,
    0x00, 0xff, 0xff, 0xff, 0x00,
    0x00, 0xff, 0xff, 0xff, 0x00,
    0x00, 0x00, 0x00, 0x00, 0x00
  };
  uint8_t* ptr = default_cursor->data();
  for (int y = 0; y < kSize; ++y) {
    for (int x = 0; x < kSize; ++x) {
      *ptr++ = pixels[kSize * y + x];
      *ptr++ = pixels[kSize * y + x];
      *ptr++ = pixels[kSize * y + x];
      *ptr++ = 0xff;
    }
  }
  DesktopVector hotspot(2, 2);
  cursor_shape_.reset(new MouseCursor(default_cursor.release(), hotspot));
}

MouseCursorMonitorX11::~MouseCursorMonitorX11() {
  if (have_xfixes_) {
    x_display_->RemoveEventHandler(xfixes_event_base_ + XFixesCursorNotify,
                                   this);
  }
}

void MouseCursorMonitorX11::Init(Callback* callback, Mode mode) {
  // Init can be called only once per instance of MouseCursorMonitor.
  assert(!callback_);
  assert(callback);

  callback_ = callback;
  mode_ = mode;

  have_xfixes_ =
      XFixesQueryExtension(display(), &xfixes_event_base_, &xfixes_error_base_);

  if (have_xfixes_) {
    // Register for changes to the cursor shape.
    XFixesSelectCursorInput(display(), window_, XFixesDisplayCursorNotifyMask);
    x_display_->AddEventHandler(xfixes_event_base_ + XFixesCursorNotify, this);

    CaptureCursor();
  } else {
    LOG(LS_INFO) << "X server does not support XFixes.";
  }
}

void MouseCursorMonitorX11::Capture() {
  assert(callback_);

  // Process X11 events in case XFixes has sent cursor notification.
  x_display_->ProcessPendingXEvents();

  // cursor_shape_| is set only if we were notified of a cursor shape change.
  if (cursor_shape_.get())
    callback_->OnMouseCursor(cursor_shape_.release());

  // Get cursor position if necessary.
  if (mode_ == SHAPE_AND_POSITION) {
    int root_x;
    int root_y;
    int win_x;
    int win_y;
    Window root_window;
    Window child_window;
    unsigned int mask;

    XErrorTrap error_trap(display());
    Bool result = XQueryPointer(display(), window_, &root_window, &child_window,
                                &root_x, &root_y, &win_x, &win_y, &mask);
    CursorState state;
    if (!result || error_trap.GetLastErrorAndDisable() != 0) {
      state = OUTSIDE;
    } else {
      // In screen mode (window_ == root_window) the mouse is always inside.
      // XQueryPointer() sets |child_window| to None if the cursor is outside
      // |window_|.
      state =
          (window_ == root_window || child_window != None) ? INSIDE : OUTSIDE;
    }

    callback_->OnMouseCursorPosition(state,
                                     webrtc::DesktopVector(win_x, win_y));
  }
}

bool MouseCursorMonitorX11::HandleXEvent(const XEvent& event) {
  if (have_xfixes_ && event.type == xfixes_event_base_ + XFixesCursorNotify) {
    const XFixesCursorNotifyEvent* cursor_event =
        reinterpret_cast<const XFixesCursorNotifyEvent*>(&event);
    if (cursor_event->subtype == XFixesDisplayCursorNotify) {
      CaptureCursor();
    }
    // Return false, even if the event has been handled, because there might be
    // other listeners for cursor notifications.
  }
  return false;
}

void MouseCursorMonitorX11::CaptureCursor() {
  assert(have_xfixes_);

  XFixesCursorImage* img;
  {
    XErrorTrap error_trap(display());
    img = XFixesGetCursorImage(display());
    if (!img || error_trap.GetLastErrorAndDisable() != 0)
       return;
   }

   std::unique_ptr<DesktopFrame> image(
       new BasicDesktopFrame(DesktopSize(img->width, img->height)));

  // Xlib stores 32-bit data in longs, even if longs are 64-bits long.
  unsigned long* src = img->pixels;
  uint32_t* dst = reinterpret_cast<uint32_t*>(image->data());
  uint32_t* dst_end = dst + (img->width * img->height);
  while (dst < dst_end) {
    *dst++ = static_cast<uint32_t>(*src++);
  }

  DesktopVector hotspot(std::min(img->width, img->xhot),
                        std::min(img->height, img->yhot));

  XFree(img);

  cursor_shape_.reset(new MouseCursor(image.release(), hotspot));
}

// static
MouseCursorMonitor* MouseCursorMonitor::CreateForWindow(
    const DesktopCaptureOptions& options, WindowId window) {
  if (!options.x_display())
    return NULL;
  window = GetTopLevelWindow(options.x_display()->display(), window);
  if (window == None)
    return NULL;
  return new MouseCursorMonitorX11(options, window);
}

MouseCursorMonitor* MouseCursorMonitor::CreateForScreen(
    const DesktopCaptureOptions& options,
    ScreenId screen) {
  if (!options.x_display())
    return NULL;
  return new MouseCursorMonitorX11(
      options, DefaultRootWindow(options.x_display()->display()));
}

}  // namespace webrtc
<|endoftext|>


<commit_before>/*
 * Copyright (c) 2010 by Cristian Maglie <c.maglie@bug.st>
 * Copyright (c) 2014 by Paul Stoffregen <paul@pjrc.com> (Transaction API)
 * Copyright (c) 2014 by Matthijs Kooijman <matthijs@stdin.nl> (SPISettings AVR)
 * Copyright (c) 2014 by Andrew J. Kroll <xxxajk@gmail.com> (atomicity fixes)
 * SPI Master library for arduino.
 *
 * This file is free software; you can redistribute it and/or modify
 * it under the terms of either the GNU General Public License version 2
 * or the GNU Lesser General Public License version 2.1, both as
 * published by the Free Software Foundation.
 */

#include "SPI.h"

#if defined(__PIC32_PPS__)
SPIClass SPI(_DSPI0_BASE, _DSPI0_MISO_PIN, _DSPI0_MOSI_PIN, _DSPI0_MISO_IN, _DSPI0_MOSI_OUT);
#else
SPIClass SPI(_DSPI0_BASE);
#endif

#if defined(__PIC32_PPS__)
SPIClass::SPIClass(uint32_t base, int pinMI, int pinMO, ppsFunctionType ppsMI, ppsFunctionType ppsMO) {
    pspi = (p32_spi *)base;
    pinMISO = pinMI;
    pinMOSI = pinMO;
    ppsMISO = ppsMI;
    ppsMOSI = ppsMO;
}
#else
SPIClass::SPIClass(uint32_t base) {
    pspi = (p32_spi *)base;
}
#endif

void SPIClass::begin() {
    uint32_t sreg = disableInterrupts();

    if (!initialized) {
#if defined(__PIC32_PPS__)
        pinMode(pinMISO, INPUT);
        mapPps(pinMISO, ppsMISO);
        pinMode(pinMOSI, OUTPUT);
        mapPps(pinMOSI, ppsMOSI);
#endif
        pspi->sxCon.set = (1<<_SPICON_MSTEN) | (1<<_SPICON_ON);
        setClockDivider(SPI_CLOCK_DIV128);
        setDataMode(SPI_MODE0);
    }

    initialized++; // reference count
    restoreInterrupts(sreg);
}

void SPIClass::end() {
    uint32_t sreg = disableInterrupts();

    // Decrease the reference counter
    if (initialized) {
        initialized--;
//        mapPps(pinMISO, 0);
//        mapPps(pinMOSI, 0);
    }

    // If there are no more references disable SPI
    if (!initialized) {
        pspi->sxCon.clr = 1<<_SPICON_ON;
        interruptMode = 0;
#ifdef SPI_TRANSACTION_MISMATCH_LED
        inTransactionFlag = 0;
#endif
    }

    restoreInterrupts(sreg);
}

void SPIClass::usingInterrupt(uint8_t interruptNumber) {
}

void SPIClass::notUsingInterrupt(uint8_t interruptNumber) {
}
<commit_msg>Added option to select default DSPI port for use with SPI library<commit_after>/*
 * Copyright (c) 2010 by Cristian Maglie <c.maglie@bug.st>
 * Copyright (c) 2014 by Paul Stoffregen <paul@pjrc.com> (Transaction API)
 * Copyright (c) 2014 by Matthijs Kooijman <matthijs@stdin.nl> (SPISettings AVR)
 * Copyright (c) 2014 by Andrew J. Kroll <xxxajk@gmail.com> (atomicity fixes)
 * SPI Master library for arduino.
 *
 * This file is free software; you can redistribute it and/or modify
 * it under the terms of either the GNU General Public License version 2
 * or the GNU Lesser General Public License version 2.1, both as
 * published by the Free Software Foundation.
 */

#include "SPI.h"

#ifndef _SPI_PORT
    #define _SPI_PORT 0
#endif

#if (_SPI_PORT == 0)
    #if defined(__PIC32_PPS__)
        SPIClass SPI(_DSPI0_BASE, _DSPI0_MISO_PIN, _DSPI0_MOSI_PIN, _DSPI0_MISO_IN, _DSPI0_MOSI_OUT);
    #else
        SPIClass SPI(_DSPI0_BASE);
    #endif
#elif (_SPI_PORT == 1)
    #if defined(__PIC32_PPS__)
        SPIClass SPI(_DSPI1_BASE, _DSPI1_MISO_PIN, _DSPI1_MOSI_PIN, _DSPI1_MISO_IN, _DSPI1_MOSI_OUT);
    #else
        SPIClass SPI(_DSPI1_BASE);
    #endif
#elif (_SPI_PORT == 2)
    #if defined(__PIC32_PPS__)
        SPIClass SPI(_DSPI2_BASE, _DSPI2_MISO_PIN, _DSPI2_MOSI_PIN, _DSPI2_MISO_IN, _DSPI2_MOSI_OUT);
    #else
        SPIClass SPI(_DSPI2_BASE);
    #endif
#elif (_SPI_PORT == 3)
    #if defined(__PIC32_PPS__)
        SPIClass SPI(_DSPI3_BASE, _DSPI3_MISO_PIN, _DSPI3_MOSI_PIN, _DSPI3_MISO_IN, _DSPI3_MOSI_OUT);
    #else
        SPIClass SPI(_DSPI3_BASE);
    #endif
#elif (_SPI_PORT == 4)
    #if defined(__PIC32_PPS__)
        SPIClass SPI(_DSPI4_BASE, _DSPI4_MISO_PIN, _DSPI4_MOSI_PIN, _DSPI4_MISO_IN, _DSPI4_MOSI_OUT);
    #else
        SPIClass SPI(_DSPI4_BASE);
    #endif
#elif (_SPI_PORT == 5)
    #if defined(__PIC32_PPS__)
        SPIClass SPI(_DSPI5_BASE, _DSPI5_MISO_PIN, _DSPI5_MOSI_PIN, _DSPI5_MISO_IN, _DSPI5_MOSI_OUT);
    #else
        SPIClass SPI(_DSPI5_BASE);
    #endif
#else
    #if defined(__PIC32_PPS__)
        SPIClass SPI(_DSPI0_BASE, _DSPI0_MISO_PIN, _DSPI0_MOSI_PIN, _DSPI0_MISO_IN, _DSPI0_MOSI_OUT);
    #else
        SPIClass SPI(_DSPI0_BASE);
    #endif
#endif

#if defined(__PIC32_PPS__)
SPIClass::SPIClass(uint32_t base, int pinMI, int pinMO, ppsFunctionType ppsMI, ppsFunctionType ppsMO) {
    pspi = (p32_spi *)base;
    pinMISO = pinMI;
    pinMOSI = pinMO;
    ppsMISO = ppsMI;
    ppsMOSI = ppsMO;
}
#else
SPIClass::SPIClass(uint32_t base) {
    pspi = (p32_spi *)base;
}
#endif

void SPIClass::begin() {
    uint32_t sreg = disableInterrupts();

    if (!initialized) {
#if defined(__PIC32_PPS__)
        pinMode(pinMISO, INPUT);
        mapPps(pinMISO, ppsMISO);
        pinMode(pinMOSI, OUTPUT);
        mapPps(pinMOSI, ppsMOSI);
#endif
        pspi->sxCon.set = (1<<_SPICON_MSTEN) | (1<<_SPICON_ON);
        setClockDivider(SPI_CLOCK_DIV128);
        setDataMode(SPI_MODE0);
    }

    initialized++; // reference count
    restoreInterrupts(sreg);
}

void SPIClass::end() {
    uint32_t sreg = disableInterrupts();

    // Decrease the reference counter
    if (initialized) {
        initialized--;
//        mapPps(pinMISO, 0);
//        mapPps(pinMOSI, 0);
    }

    // If there are no more references disable SPI
    if (!initialized) {
        pspi->sxCon.clr = 1<<_SPICON_ON;
        interruptMode = 0;
#ifdef SPI_TRANSACTION_MISMATCH_LED
        inTransactionFlag = 0;
#endif
    }

    restoreInterrupts(sreg);
}

void SPIClass::usingInterrupt(uint8_t interruptNumber) {
}

void SPIClass::notUsingInterrupt(uint8_t interruptNumber) {
}
<|endoftext|>