<commit_before>#ifndef VIENNAGRID_ALGORITHM_SPANNED_VOLUME_HPP
#define VIENNAGRID_ALGORITHM_SPANNED_VOLUME_HPP

/* =======================================================================
   Copyright (c) 2011-2012, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.

                            -----------------
                     ViennaGrid - The Vienna Grid Library
                            -----------------

   Authors:      Karl Rupp                           rupp@iue.tuwien.ac.at
                 Josef Weinbub                    weinbub@iue.tuwien.ac.at

   (A list of additional contributors can be found in the PDF manual)

   License:      MIT (X11), see file LICENSE in the base directory
======================================================================= */

//#include <math.h>
#include "viennagrid/forwards.h"
#include "viennagrid/algorithm/cross_prod.hpp"
#include "viennagrid/algorithm/norm.hpp"
#include "viennagrid/algorithm/inner_prod.hpp"
#include "viennagrid/traits/point.hpp"

/** @file spanned_volume.hpp
    @brief Computes the volume of n-simplices spanned by points
*/

namespace viennagrid
{

  namespace detail
  {
    template <typename PointType, long dim = traits::dimension<PointType>::value>
    struct signed_spanned_volume_impl;

    /** @brief Implementation of the volume spanned by two points in one dimension */
    template <typename PointType>
    struct signed_spanned_volume_impl<PointType, 1>
    {
      typedef typename traits::value_type<PointType>::type    value_type;

      static value_type apply(PointType const & p1,
                              PointType const & p2)
      {
        return p2[0] - p1[0];
      }
    };

    //in 2d:
    /** @brief Implementation of the volume of simplices spanned by points in two geometrical dimension */
    template <typename PointType>
    struct signed_spanned_volume_impl<PointType, 2>
    {
      typedef typename traits::value_type<PointType>::type    value_type;

      static value_type apply(PointType const & p1,
                              PointType const & p2)
      {
        //a line
        return sqrt(   (p2[0] - p1[0]) * (p2[0] - p1[0])
                     + (p2[1] - p1[1]) * (p2[1] - p1[1])  );
      }

      static value_type apply(PointType const & A,
                              PointType const & B,
                              PointType const & C)
      {
        //a triangle:
        return (  A[0] * (B[1] - C[1])
                    + B[0] * (C[1] - A[1])
                    + C[0] * (A[1] - B[1]) ) / 2.0;
      }

    };

    /** @brief Implementation of the volume of simplices spanned by points in three geometrical dimension */
    template <typename PointType>
    struct signed_spanned_volume_impl<PointType, 3>
    {
      typedef typename traits::value_type<PointType>::type    value_type;

      static value_type apply(PointType const & p1,
                              PointType const & p2)
      {
        //a line
        return sqrt(   (p2[0] - p1[0]) * (p2[0] - p1[0])
                     + (p2[1] - p1[1]) * (p2[1] - p1[1])
                     + (p2[2] - p1[2]) * (p2[2] - p1[2]) );
      }

      static value_type apply(PointType const & p1,
                              PointType const & p2,
                              PointType const & p3)
      {
        PointType v1 = p2 - p1;
        PointType v2 = p3 - p1;

        PointType v3 = cross_prod(v1, v2);

        return norm(v3) / 2.0;
      }

      static value_type apply(PointType const & p1,
                              PointType const & p2,
                              PointType const & p3,
                              PointType const & p4)
      {
        PointType v1 = p2 - p1;
        PointType v2 = p3 - p1;
        PointType v3 = p4 - p1;

        return (inner_prod(v1, cross_prod(v2, v3)) ) / 6.0;
      }

    };
  } //namespace detail

  //
  // Mixed coordinate systems:
  //
  /** @brief Dispatch facility for two points with possibly different coordinate systems */
  template<typename PointType1, typename PointType2, typename CSystem1, typename CSystem2>
  typename traits::value_type<PointType1>::type
  signed_spanned_volume_impl(PointType1 const & p1,
                      PointType2 const & p2,
                      CSystem1 const &,
                      CSystem2 const &)
  {
    typedef typename traits::value_type<PointType1>::type    value_type;
    typedef typename result_of::cartesian_point<PointType1>::type   CartesianPoint1;

    return detail::signed_spanned_volume_impl<CartesianPoint1>::apply(to_cartesian(p1), to_cartesian(p2));
  }

  /** @brief Dispatch facility for three points with possibly different coordinate systems */
  template<typename PointType1, typename PointType2, typename PointType3,
           typename CSystem1, typename CSystem2, typename CSystem3>
  typename traits::value_type<PointType1>::type
  signed_spanned_volume_impl(PointType1 const & p1,
                      PointType2 const & p2,
                      PointType3 const & p3,
                      CSystem1 const &,
                      CSystem2 const &,
                      CSystem3 const &)
  {
    typedef typename traits::value_type<PointType1>::type    value_type;
    typedef typename result_of::cartesian_point<PointType1>::type   CartesianPoint1;

    return detail::signed_spanned_volume_impl<CartesianPoint1>::apply(to_cartesian(p1), to_cartesian(p2), to_cartesian(p3));
  }

  /** @brief Dispatch facility for four points with possibly different coordinate systems */
  template<typename PointType1, typename PointType2, typename PointType3, typename PointType4,
           typename CSystem1, typename CSystem2, typename CSystem3, typename CSystem4>
  typename traits::value_type<PointType1>::type
  signed_spanned_volume_impl(PointType1 const & p1,
                      PointType2 const & p2,
                      PointType3 const & p3,
                      PointType4 const & p4,
                      CSystem1 const &,
                      CSystem2 const &,
                      CSystem3 const &,
                      CSystem4 const &)
  {
    typedef typename traits::value_type<PointType1>::type    value_type;
    typedef typename result_of::cartesian_point<PointType1>::type   CartesianPoint1;

    return detail::signed_spanned_volume_impl<CartesianPoint1>::apply(to_cartesian(p1), to_cartesian(p2), to_cartesian(p3), to_cartesian(p4));
  }

  //
  // All Cartesian:
  //
  /** @brief Dispatch facility for two points in Cartesian coordinates */
  template<typename PointType1, typename PointType2, long d>
  typename traits::value_type<PointType1>::type
  signed_spanned_volume_impl(PointType1 const & p1,
                      PointType2 const & p2,
                      cartesian_cs<d>,
                      cartesian_cs<d>)
  {
    return detail::signed_spanned_volume_impl<PointType1>::apply(p1, p2);
  }

  /** @brief Dispatch facility for three points in Cartesian coordinates */
  template <typename PointType1, typename PointType2, typename PointType3, long d>
  typename traits::value_type<PointType1>::type
  signed_spanned_volume_impl(PointType1 const & p1,
                      PointType2 const & p2,
                      PointType3 const & p3,
                      cartesian_cs<d>,
                      cartesian_cs<d>,
                      cartesian_cs<d>)
  {
    return detail::signed_spanned_volume_impl<PointType1>::apply(p1, p2, p3);
  }

  /** @brief Dispatch facility for four points in Cartesian coordinates */
  template <typename PointType1, typename PointType2, typename PointType3, typename PointType4, long d>
  typename traits::value_type<PointType1>::type
  signed_spanned_volume_impl(PointType1 const & p1,
                      PointType2 const & p2,
                      PointType3 const & p3,
                      PointType4 const & p4,
                      cartesian_cs<d>,
                      cartesian_cs<d>,
                      cartesian_cs<d>,
                      cartesian_cs<d>)
  {
    return detail::signed_spanned_volume_impl<PointType1>::apply(p1, p2, p3, p4);
  }

  //
  // public interface
  //
  /** @brief Returns the volume of the 1-simplex (line) spanned by the two points */
  template <typename PointType1, typename PointType2>
  typename traits::value_type<PointType1>::type
  signed_spanned_volume(PointType1 const & p1, PointType2 const & p2)
  {
    return signed_spanned_volume_impl(p1,
                               p2,
                               typename traits::coordinate_system<PointType1>::type(),
                               typename traits::coordinate_system<PointType2>::type());
  }

  /** @brief Returns the two-dimensional volume of the 2-simplex (triangle) spanned by the three points */
  template <typename PointType1, typename PointType2, typename PointType3>
  typename traits::value_type<PointType1>::type
  signed_spanned_volume(PointType1 const & p1, PointType2 const & p2, PointType3 const & p3)
  {
    return signed_spanned_volume_impl(p1,
                               p2,
                               p3,
                               typename traits::coordinate_system<PointType1>::type(),
                               typename traits::coordinate_system<PointType2>::type(),
                               typename traits::coordinate_system<PointType3>::type()
                              );

  }

  /** @brief Returns the three-dimensional volume of the 3-simplex (tetrahedron) spanned by the four points */
  template <typename PointType1, typename PointType2, typename PointType3, typename PointType4>
  typename traits::value_type<PointType1>::type
  signed_spanned_volume(PointType1 const & p1,
                  PointType2 const & p2,
                  PointType3 const & p3,
                  PointType4 const & p4)
  {
    return signed_signed_spanned_volume_impl(p1,
                               p2,
                               p3,
                               p4,
                               typename traits::coordinate_system<PointType1>::type(),
                               typename traits::coordinate_system<PointType2>::type(),
                               typename traits::coordinate_system<PointType3>::type(),
                               typename traits::coordinate_system<PointType4>::type()
                              );
  }

  /** @brief Returns the volume of the 1-simplex (line) spanned by the two points */
  template <typename PointType1, typename PointType2>
  typename traits::value_type<PointType1>::type
  spanned_volume(PointType1 const & p1, PointType2 const & p2)
  {
    return std::abs(signed_spanned_volume(p1, p2));
  }

  /** @brief Returns the two-dimensional volume of the 2-simplex (triangle) spanned by the three points */
  template <typename PointType1, typename PointType2, typename PointType3>
  typename traits::value_type<PointType1>::type
  spanned_volume(PointType1 const & p1, PointType2 const & p2, PointType3 const & p3)
  {
    return std::abs(signed_spanned_volume(p1, p2, p3));
  }

  /** @brief Returns the three-dimensional volume of the 3-simplex (tetrahedron) spanned by the four points */
  template <typename PointType1, typename PointType2, typename PointType3, typename PointType4>
  typename traits::value_type<PointType1>::type
  spanned_volume(PointType1 const & p1,
                  PointType2 const & p2,
                  PointType3 const & p3,
                  PointType4 const & p4)
  {
    return std::abs(signed_spanned_volume(p1, p2, p3, p4));
  }

}
#endif
<commit_msg>fixed typo<commit_after>#ifndef VIENNAGRID_ALGORITHM_SPANNED_VOLUME_HPP
#define VIENNAGRID_ALGORITHM_SPANNED_VOLUME_HPP

/* =======================================================================
   Copyright (c) 2011-2012, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.

                            -----------------
                     ViennaGrid - The Vienna Grid Library
                            -----------------

   Authors:      Karl Rupp                           rupp@iue.tuwien.ac.at
                 Josef Weinbub                    weinbub@iue.tuwien.ac.at

   (A list of additional contributors can be found in the PDF manual)

   License:      MIT (X11), see file LICENSE in the base directory
======================================================================= */

//#include <math.h>
#include "viennagrid/forwards.h"
#include "viennagrid/algorithm/cross_prod.hpp"
#include "viennagrid/algorithm/norm.hpp"
#include "viennagrid/algorithm/inner_prod.hpp"
#include "viennagrid/traits/point.hpp"

/** @file spanned_volume.hpp
    @brief Computes the volume of n-simplices spanned by points
*/

namespace viennagrid
{

  namespace detail
  {
    template <typename PointType, long dim = traits::dimension<PointType>::value>
    struct signed_spanned_volume_impl;

    /** @brief Implementation of the volume spanned by two points in one dimension */
    template <typename PointType>
    struct signed_spanned_volume_impl<PointType, 1>
    {
      typedef typename traits::value_type<PointType>::type    value_type;

      static value_type apply(PointType const & p1,
                              PointType const & p2)
      {
        return p2[0] - p1[0];
      }
    };

    //in 2d:
    /** @brief Implementation of the volume of simplices spanned by points in two geometrical dimension */
    template <typename PointType>
    struct signed_spanned_volume_impl<PointType, 2>
    {
      typedef typename traits::value_type<PointType>::type    value_type;

      static value_type apply(PointType const & p1,
                              PointType const & p2)
      {
        //a line
        return sqrt(   (p2[0] - p1[0]) * (p2[0] - p1[0])
                     + (p2[1] - p1[1]) * (p2[1] - p1[1])  );
      }

      static value_type apply(PointType const & A,
                              PointType const & B,
                              PointType const & C)
      {
        //a triangle:
        return (  A[0] * (B[1] - C[1])
                    + B[0] * (C[1] - A[1])
                    + C[0] * (A[1] - B[1]) ) / 2.0;
      }

    };

    /** @brief Implementation of the volume of simplices spanned by points in three geometrical dimension */
    template <typename PointType>
    struct signed_spanned_volume_impl<PointType, 3>
    {
      typedef typename traits::value_type<PointType>::type    value_type;

      static value_type apply(PointType const & p1,
                              PointType const & p2)
      {
        //a line
        return sqrt(   (p2[0] - p1[0]) * (p2[0] - p1[0])
                     + (p2[1] - p1[1]) * (p2[1] - p1[1])
                     + (p2[2] - p1[2]) * (p2[2] - p1[2]) );
      }

      static value_type apply(PointType const & p1,
                              PointType const & p2,
                              PointType const & p3)
      {
        PointType v1 = p2 - p1;
        PointType v2 = p3 - p1;

        PointType v3 = cross_prod(v1, v2);

        return norm(v3) / 2.0;
      }

      static value_type apply(PointType const & p1,
                              PointType const & p2,
                              PointType const & p3,
                              PointType const & p4)
      {
        PointType v1 = p2 - p1;
        PointType v2 = p3 - p1;
        PointType v3 = p4 - p1;

        return (inner_prod(v1, cross_prod(v2, v3)) ) / 6.0;
      }

    };
  } //namespace detail

  //
  // Mixed coordinate systems:
  //
  /** @brief Dispatch facility for two points with possibly different coordinate systems */
  template<typename PointType1, typename PointType2, typename CSystem1, typename CSystem2>
  typename traits::value_type<PointType1>::type
  signed_spanned_volume_impl(PointType1 const & p1,
                      PointType2 const & p2,
                      CSystem1 const &,
                      CSystem2 const &)
  {
    typedef typename traits::value_type<PointType1>::type    value_type;
    typedef typename result_of::cartesian_point<PointType1>::type   CartesianPoint1;

    return detail::signed_spanned_volume_impl<CartesianPoint1>::apply(to_cartesian(p1), to_cartesian(p2));
  }

  /** @brief Dispatch facility for three points with possibly different coordinate systems */
  template<typename PointType1, typename PointType2, typename PointType3,
           typename CSystem1, typename CSystem2, typename CSystem3>
  typename traits::value_type<PointType1>::type
  signed_spanned_volume_impl(PointType1 const & p1,
                      PointType2 const & p2,
                      PointType3 const & p3,
                      CSystem1 const &,
                      CSystem2 const &,
                      CSystem3 const &)
  {
    typedef typename traits::value_type<PointType1>::type    value_type;
    typedef typename result_of::cartesian_point<PointType1>::type   CartesianPoint1;

    return detail::signed_spanned_volume_impl<CartesianPoint1>::apply(to_cartesian(p1), to_cartesian(p2), to_cartesian(p3));
  }

  /** @brief Dispatch facility for four points with possibly different coordinate systems */
  template<typename PointType1, typename PointType2, typename PointType3, typename PointType4,
           typename CSystem1, typename CSystem2, typename CSystem3, typename CSystem4>
  typename traits::value_type<PointType1>::type
  signed_spanned_volume_impl(PointType1 const & p1,
                      PointType2 const & p2,
                      PointType3 const & p3,
                      PointType4 const & p4,
                      CSystem1 const &,
                      CSystem2 const &,
                      CSystem3 const &,
                      CSystem4 const &)
  {
    typedef typename traits::value_type<PointType1>::type    value_type;
    typedef typename result_of::cartesian_point<PointType1>::type   CartesianPoint1;

    return detail::signed_spanned_volume_impl<CartesianPoint1>::apply(to_cartesian(p1), to_cartesian(p2), to_cartesian(p3), to_cartesian(p4));
  }

  //
  // All Cartesian:
  //
  /** @brief Dispatch facility for two points in Cartesian coordinates */
  template<typename PointType1, typename PointType2, long d>
  typename traits::value_type<PointType1>::type
  signed_spanned_volume_impl(PointType1 const & p1,
                      PointType2 const & p2,
                      cartesian_cs<d>,
                      cartesian_cs<d>)
  {
    return detail::signed_spanned_volume_impl<PointType1>::apply(p1, p2);
  }

  /** @brief Dispatch facility for three points in Cartesian coordinates */
  template <typename PointType1, typename PointType2, typename PointType3, long d>
  typename traits::value_type<PointType1>::type
  signed_spanned_volume_impl(PointType1 const & p1,
                      PointType2 const & p2,
                      PointType3 const & p3,
                      cartesian_cs<d>,
                      cartesian_cs<d>,
                      cartesian_cs<d>)
  {
    return detail::signed_spanned_volume_impl<PointType1>::apply(p1, p2, p3);
  }

  /** @brief Dispatch facility for four points in Cartesian coordinates */
  template <typename PointType1, typename PointType2, typename PointType3, typename PointType4, long d>
  typename traits::value_type<PointType1>::type
  signed_spanned_volume_impl(PointType1 const & p1,
                      PointType2 const & p2,
                      PointType3 const & p3,
                      PointType4 const & p4,
                      cartesian_cs<d>,
                      cartesian_cs<d>,
                      cartesian_cs<d>,
                      cartesian_cs<d>)
  {
    return detail::signed_spanned_volume_impl<PointType1>::apply(p1, p2, p3, p4);
  }

  //
  // public interface
  //
  /** @brief Returns the volume of the 1-simplex (line) spanned by the two points */
  template <typename PointType1, typename PointType2>
  typename traits::value_type<PointType1>::type
  signed_spanned_volume(PointType1 const & p1, PointType2 const & p2)
  {
    return signed_spanned_volume_impl(p1,
                               p2,
                               typename traits::coordinate_system<PointType1>::type(),
                               typename traits::coordinate_system<PointType2>::type());
  }

  /** @brief Returns the two-dimensional volume of the 2-simplex (triangle) spanned by the three points */
  template <typename PointType1, typename PointType2, typename PointType3>
  typename traits::value_type<PointType1>::type
  signed_spanned_volume(PointType1 const & p1, PointType2 const & p2, PointType3 const & p3)
  {
    return signed_spanned_volume_impl(p1,
                               p2,
                               p3,
                               typename traits::coordinate_system<PointType1>::type(),
                               typename traits::coordinate_system<PointType2>::type(),
                               typename traits::coordinate_system<PointType3>::type()
                              );

  }

  /** @brief Returns the three-dimensional volume of the 3-simplex (tetrahedron) spanned by the four points */
  template <typename PointType1, typename PointType2, typename PointType3, typename PointType4>
  typename traits::value_type<PointType1>::type
  signed_spanned_volume(PointType1 const & p1,
                  PointType2 const & p2,
                  PointType3 const & p3,
                  PointType4 const & p4)
  {
    return signed_spanned_volume_impl(p1,
                               p2,
                               p3,
                               p4,
                               typename traits::coordinate_system<PointType1>::type(),
                               typename traits::coordinate_system<PointType2>::type(),
                               typename traits::coordinate_system<PointType3>::type(),
                               typename traits::coordinate_system<PointType4>::type()
                              );
  }

  /** @brief Returns the volume of the 1-simplex (line) spanned by the two points */
  template <typename PointType1, typename PointType2>
  typename traits::value_type<PointType1>::type
  spanned_volume(PointType1 const & p1, PointType2 const & p2)
  {
    return std::abs(signed_spanned_volume(p1, p2));
  }

  /** @brief Returns the two-dimensional volume of the 2-simplex (triangle) spanned by the three points */
  template <typename PointType1, typename PointType2, typename PointType3>
  typename traits::value_type<PointType1>::type
  spanned_volume(PointType1 const & p1, PointType2 const & p2, PointType3 const & p3)
  {
    return std::abs(signed_spanned_volume(p1, p2, p3));
  }

  /** @brief Returns the three-dimensional volume of the 3-simplex (tetrahedron) spanned by the four points */
  template <typename PointType1, typename PointType2, typename PointType3, typename PointType4>
  typename traits::value_type<PointType1>::type
  spanned_volume(PointType1 const & p1,
                  PointType2 const & p2,
                  PointType3 const & p3,
                  PointType4 const & p4)
  {
    return std::abs(signed_spanned_volume(p1, p2, p3, p4));
  }

}
#endif
<|endoftext|>


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

#include <fstream>
#include <iomanip>
#include "modules/localization/msf/common/io/velodyne_utility.h"
#include "modules/localization/msf/local_tool/map_creation/poses_interpolation/poses_interpolation.h"

namespace apollo {
namespace localization {
namespace msf {
PosesInterpolation::PosesInterpolation() {}

bool PosesInterpolation::Init(const std::string &input_poses_path,
                              const std::string &ref_timestamps_path,
                              const std::string &out_poses_path,
                              const std::string &extrinsic_path) {
  this->input_poses_path_ = input_poses_path;
  this->ref_timestamps_path_ = ref_timestamps_path;
  this->out_poses_path_ = out_poses_path;
  this->extrinsic_path_ = extrinsic_path;

  bool success = velodyne::LoadExtrinsic(extrinsic_path_, &velodyne_extrinsic_);
  if (!success) {
    std::cerr << "Load lidar extrinsic failed." << std::endl;
    return false;
  }

  return true;
}

void PosesInterpolation::DoInterpolation() {
  // Load input poses
  std::vector<Eigen::Vector3d> input_stds;
  velodyne::LoadPosesAndStds(input_poses_path_, &input_poses_, &input_stds,
                             &input_poses_timestamps_);

  // Load pcd timestamp
  LoadPCDTimestamp();

  // Interpolation
  PoseInterpolationByTime(input_poses_, input_poses_timestamps_,
                          ref_timestamps_, ref_ids_, &out_indexes_,
                          &out_timestamps_, &out_poses_);

  // Write pcd poses
  WritePCDPoses();
}

void PosesInterpolation::LoadPCDTimestamp() {
  FILE *file = fopen(ref_timestamps_path_.c_str(), "r");
  if (file) {
    unsigned int index;
    double timestamp;
    constexpr int kSize = 2;
    while (fscanf(file, "%u %lf\n", &index, &timestamp) == kSize) {
      ref_timestamps_.push_back(timestamp);
      ref_ids_.push_back(index);
    }
    fclose(file);
  } else {
    std::cerr << "Can't open file to read: " << ref_timestamps_path_;
  }
}

void PosesInterpolation::WritePCDPoses() {
  std::ofstream fout;
  fout.open(out_poses_path_.c_str(), std::ofstream::out);
  fout.setf(std::ios::fixed, std::ios::floatfield);
  fout.precision(6);

  if (fout.is_open()) {
    for (size_t i = 0; i < out_poses_.size(); i++) {
      double timestamp = out_timestamps_[i];

      Eigen::Affine3d pose_tem = out_poses_[i] * velodyne_extrinsic_;
      Eigen::Quaterniond quatd(pose_tem.linear());
      Eigen::Translation3d transd(pose_tem.translation());
      double qx = quatd.x();
      double qy = quatd.y();
      double qz = quatd.z();
      double qr = quatd.w();

      fout << out_indexes_[i] << " " << timestamp << " " << transd.x() << " "
           << transd.y() << " " << transd.z() << " " << qx << " " << qy << " "
           << qz << " " << qr << "\n";
    }
    fout.close();
  } else {
    std::cerr << "Can't open file to write: " << out_poses_path_ << std::endl;
  }
}  // namespace msf

void PosesInterpolation::PoseInterpolationByTime(
    const std::vector<Eigen::Affine3d> &in_poses,
    const std::vector<double> &in_timestamps,
    const std::vector<double> &ref_timestamps,
    const std::vector<unsigned int> &ref_indexes,
    std::vector<unsigned int> *out_indexes, std::vector<double> *out_timestamps,
    std::vector<Eigen::Affine3d> *out_poses) {
  out_indexes->clear();
  out_timestamps->clear();
  out_poses->clear();

  unsigned int index = 0;
  for (size_t i = 0; i < ref_timestamps.size(); i++) {
    double ref_timestamp = ref_timestamps[i];
    unsigned int ref_index = ref_indexes[i];

    while (index < in_timestamps.size() &&
           in_timestamps.at(index) < ref_timestamp) {
      ++index;
    }

    if (index < in_timestamps.size()) {
      if (index >= 1) {
        double cur_timestamp = in_timestamps[index];
        double pre_timestamp = in_timestamps[index - 1];
        assert(cur_timestamp != pre_timestamp);

        double t =
            (cur_timestamp - ref_timestamp) / (cur_timestamp - pre_timestamp);
        assert(t >= 0.0);
        assert(t <= 1.0);

        Eigen::Affine3d pre_pose = in_poses[index - 1];
        Eigen::Affine3d cur_pose = in_poses[index];
        Eigen::Quaterniond pre_quatd(pre_pose.linear());
        Eigen::Translation3d pre_transd(pre_pose.translation());
        Eigen::Quaterniond cur_quatd(cur_pose.linear());
        Eigen::Translation3d cur_transd(cur_pose.translation());

        Eigen::Quaterniond res_quatd = pre_quatd.slerp(1 - t, cur_quatd);

        Eigen::Translation3d re_transd;
        re_transd.x() = pre_transd.x() * t + cur_transd.x() * (1 - t);
        re_transd.y() = pre_transd.y() * t + cur_transd.y() * (1 - t);
        re_transd.z() = pre_transd.z() * t + cur_transd.z() * (1 - t);

        out_poses->push_back(re_transd * res_quatd);
        out_indexes->push_back(ref_index);
        out_timestamps->push_back(ref_timestamp);
      }
    } else {
      std::cerr << "[ERROR] No more poses. Exit now." << std::endl;
      break;
    }
    std::cout << "Frame_id: " << i << std::endl;
  }
}

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

#include <fstream>
#include <iomanip>
#include "modules/localization/msf/common/io/velodyne_utility.h"
#include "modules/localization/msf/local_tool/map_creation/poses_interpolation/poses_interpolation.h"

namespace apollo {
namespace localization {
namespace msf {
PosesInterpolation::PosesInterpolation() {}

bool PosesInterpolation::Init(const std::string &input_poses_path,
                              const std::string &ref_timestamps_path,
                              const std::string &out_poses_path,
                              const std::string &extrinsic_path) {
  this->input_poses_path_ = input_poses_path;
  this->ref_timestamps_path_ = ref_timestamps_path;
  this->out_poses_path_ = out_poses_path;
  this->extrinsic_path_ = extrinsic_path;

  bool success = velodyne::LoadExtrinsic(extrinsic_path_, &velodyne_extrinsic_);
  if (!success) {
    std::cerr << "Load lidar extrinsic failed." << std::endl;
    return false;
  }

  return true;
}

void PosesInterpolation::DoInterpolation() {
  // Load input poses
  std::vector<Eigen::Vector3d> input_stds;
  velodyne::LoadPosesAndStds(input_poses_path_, &input_poses_, &input_stds,
                             &input_poses_timestamps_);

  // Load pcd timestamp
  LoadPCDTimestamp();

  // Interpolation
  PoseInterpolationByTime(input_poses_, input_poses_timestamps_,
                          ref_timestamps_, ref_ids_, &out_indexes_,
                          &out_timestamps_, &out_poses_);

  // Write pcd poses
  WritePCDPoses();
}

void PosesInterpolation::LoadPCDTimestamp() {
  FILE *file = fopen(ref_timestamps_path_.c_str(), "r");
  if (file) {
    unsigned int index;
    double timestamp;
    constexpr int kSize = 2;
    while (fscanf(file, "%u %lf\n", &index, &timestamp) == kSize) {
      ref_timestamps_.push_back(timestamp);
      ref_ids_.push_back(index);
    }
    fclose(file);
  } else {
    std::cerr << "Can't open file to read: " << ref_timestamps_path_;
  }
}

void PosesInterpolation::WritePCDPoses() {
  std::ofstream fout;
  fout.open(out_poses_path_.c_str(), std::ofstream::out);
  fout.setf(std::ios::fixed, std::ios::floatfield);

  if (fout.is_open()) {
    for (size_t i = 0; i < out_poses_.size(); i++) {
      double timestamp = out_timestamps_[i];

      Eigen::Affine3d pose_tem = out_poses_[i] * velodyne_extrinsic_;
      Eigen::Quaterniond quatd(pose_tem.linear());
      Eigen::Translation3d transd(pose_tem.translation());
      double qx = quatd.x();
      double qy = quatd.y();
      double qz = quatd.z();
      double qr = quatd.w();

      fout.precision(6);
      fout << out_indexes_[i] << " " << timestamp << " " << transd.x() << " "
           << transd.y() << " " << transd.z() << " " << qx << " " << qy << " "
           << qz << " " << qr << "\n";
    }
    fout.close();
  } else {
    std::cerr << "Can't open file to write: " << out_poses_path_ << std::endl;
  }
}  // namespace msf

void PosesInterpolation::PoseInterpolationByTime(
    const std::vector<Eigen::Affine3d> &in_poses,
    const std::vector<double> &in_timestamps,
    const std::vector<double> &ref_timestamps,
    const std::vector<unsigned int> &ref_indexes,
    std::vector<unsigned int> *out_indexes, std::vector<double> *out_timestamps,
    std::vector<Eigen::Affine3d> *out_poses) {
  out_indexes->clear();
  out_timestamps->clear();
  out_poses->clear();

  unsigned int index = 0;
  for (size_t i = 0; i < ref_timestamps.size(); i++) {
    double ref_timestamp = ref_timestamps[i];
    unsigned int ref_index = ref_indexes[i];

    while (index < in_timestamps.size() &&
           in_timestamps.at(index) < ref_timestamp) {
      ++index;
    }

    if (index < in_timestamps.size()) {
      if (index >= 1) {
        double cur_timestamp = in_timestamps[index];
        double pre_timestamp = in_timestamps[index - 1];
        assert(cur_timestamp != pre_timestamp);

        double t =
            (cur_timestamp - ref_timestamp) / (cur_timestamp - pre_timestamp);
        assert(t >= 0.0);
        assert(t <= 1.0);

        Eigen::Affine3d pre_pose = in_poses[index - 1];
        Eigen::Affine3d cur_pose = in_poses[index];
        Eigen::Quaterniond pre_quatd(pre_pose.linear());
        Eigen::Translation3d pre_transd(pre_pose.translation());
        Eigen::Quaterniond cur_quatd(cur_pose.linear());
        Eigen::Translation3d cur_transd(cur_pose.translation());

        Eigen::Quaterniond res_quatd = pre_quatd.slerp(1 - t, cur_quatd);

        Eigen::Translation3d re_transd;
        re_transd.x() = pre_transd.x() * t + cur_transd.x() * (1 - t);
        re_transd.y() = pre_transd.y() * t + cur_transd.y() * (1 - t);
        re_transd.z() = pre_transd.z() * t + cur_transd.z() * (1 - t);

        out_poses->push_back(re_transd * res_quatd);
        out_indexes->push_back(ref_index);
        out_timestamps->push_back(ref_timestamp);
      }
    } else {
      std::cerr << "[ERROR] No more poses. Exit now." << std::endl;
      break;
    }
    std::cout << "Frame_id: " << i << std::endl;
  }
}

}  // namespace msf
}  // namespace localization
}  // namespace apollo
<|endoftext|>


<commit_before><commit_msg>coverity#982157 Unchecked return value<commit_after><|endoftext|>


<commit_before>/*************************************************************************
 *
 *  OpenOffice.org - a multi-platform office productivity suite
 *
 *  $RCSfile: component.cxx,v $
 *
 *  $Revision: 1.12 $
 *
 *  last change: $Author: rt $ $Date: 2005-09-08 09:25:07 $
 *
 *  The Contents of this file are made available subject to
 *  the terms of GNU Lesser General Public License Version 2.1.
 *
 *
 *    GNU Lesser General Public License Version 2.1
 *    =============================================
 *    Copyright 2005 by Sun Microsystems, Inc.
 *    901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *    This library is free software; you can redistribute it and/or
 *    modify it under the terms of the GNU Lesser General Public
 *    License version 2.1, as published by the Free Software Foundation.
 *
 *    This library is distributed in the hope that it will be useful,
 *    but WITHOUT ANY WARRANTY; without even the implied warranty of
 *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 *    Lesser General Public License for more details.
 *
 *    You should have received a copy of the GNU Lesser General Public
 *    License along with this library; if not, write to the Free Software
 *    Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 *    MA  02111-1307  USA
 *
 ************************************************************************/

#ifndef _RTL_STRING_HXX_
#include <rtl/string.hxx>
#endif
#ifndef _OSL_DIAGNOSE_H_
#include <osl/diagnose.h>
#endif
#ifndef _CPPUHELPER_COMPONENT_HXX_
#include <cppuhelper/component.hxx>
#endif
#ifndef _CPPUHELPER_QUERYINTERFACE_HXX_
#include <cppuhelper/queryinterface.hxx>
#endif
#ifndef _CPPUHELPER_TYPEPROVIDER_HXX_
#include <cppuhelper/typeprovider.hxx>
#endif
#include "com/sun/star/uno/RuntimeException.hpp"

using namespace osl;
using namespace rtl;
using namespace com::sun::star;
using namespace com::sun::star::uno;
using namespace com::sun::star::lang;

namespace cppu
{

//  ----------------------------------------------------
//  class OComponentHelper
//  ----------------------------------------------------

OComponentHelper::OComponentHelper( Mutex & rMutex ) SAL_THROW( () )
    : rBHelper( rMutex )
{
}
OComponentHelper::~OComponentHelper() SAL_THROW( (RuntimeException) )
{
}

Any OComponentHelper::queryInterface( Type const & rType ) throw (RuntimeException)
{
    return OWeakAggObject::queryInterface( rType );
}
Any OComponentHelper::queryAggregation( Type const & rType ) throw (RuntimeException)
{
    if (rType == ::getCppuType( (Reference< lang::XComponent > const *)0 ))
    {
        void * p = static_cast< lang::XComponent * >( this );
        return Any( &p, rType );
    }
    else if (rType == ::getCppuType( (Reference< lang::XTypeProvider > const *)0 ))
    {
        void * p = static_cast< lang::XTypeProvider * >( this );
        return Any( &p, rType );
    }
    return OWeakAggObject::queryAggregation( rType );
}
void OComponentHelper::acquire() throw ()
{
    OWeakAggObject::acquire();
}

void OComponentHelper::release() throw()
{
    Reference<XInterface > x( xDelegator );
    if (! x.is())
    {
        if (osl_decrementInterlockedCount( &m_refCount ) == 0)
        {
            if (! rBHelper.bDisposed)
            {
                Reference<XInterface > xHoldAlive( *this );
                // First dispose
                try
                {
                    dispose();
                }
                catch (::com::sun::star::uno::RuntimeException & exc)
                {
                    // release should not throw exceptions
#if OSL_DEBUG_LEVEL > 0
                    OString msg( OUStringToOString( exc.Message, RTL_TEXTENCODING_ASCII_US ) );
                    OSL_ENSURE( 0, msg.getStr() );
#endif
                }

                // only the alive ref holds the object
                OSL_ASSERT( m_refCount == 1 );
                // destroy the object if xHoldAlive decrement the refcount to 0
                return;
            }
        }
        // restore the reference count
        osl_incrementInterlockedCount( &m_refCount );
    }
    OWeakAggObject::release();
}

Sequence< Type > OComponentHelper::getTypes() throw (RuntimeException)
{
    static OTypeCollection * s_pTypes = 0;
    if (! s_pTypes)
    {
        MutexGuard aGuard( Mutex::getGlobalMutex() );
        if (! s_pTypes)
        {
            static OTypeCollection s_aTypes(
                ::getCppuType( (const Reference< lang::XComponent > *)0 ),
                ::getCppuType( (const Reference< lang::XTypeProvider > *)0 ),
                ::getCppuType( (const Reference< XAggregation > *)0 ),
                ::getCppuType( (const Reference< XWeak > *)0 ) );
            s_pTypes = &s_aTypes;
        }
    }
    return s_pTypes->getTypes();
}

// XComponent
void OComponentHelper::disposing()
{
}

// XComponent
void OComponentHelper::dispose()
    throw(::com::sun::star::uno::RuntimeException)
{
    // An frequently programming error is to release the last
    // reference to this object in the disposing message.
    // Make it rubust, hold a self Reference.
    Reference<XComponent > xSelf( this );

    // Guard dispose against multible threading
    // Remark: It is an error to call dispose more than once
    sal_Bool bDoDispose = sal_False;
    {
    MutexGuard aGuard( rBHelper.rMutex );
    if( !rBHelper.bDisposed && !rBHelper.bInDispose )
    {
        // only one call go into this section
        rBHelper.bInDispose = sal_True;
        bDoDispose = sal_True;
    }
    }

    // Do not hold the mutex because we are broadcasting
    if( bDoDispose )
    {
        // Create an event with this as sender
        try
        {
            try
            {
                Reference<XInterface > xSource(
                    Reference<XInterface >::query( (XComponent *)this ) );
                EventObject aEvt;
                aEvt.Source = xSource;
                // inform all listeners to release this object
                // The listener container are automaticly cleared
                rBHelper.aLC.disposeAndClear( aEvt );
                // notify subclasses to do their dispose
                disposing();
            }
            catch (...)
            {
                MutexGuard aGuard( rBHelper.rMutex );
                // bDispose and bInDisposing must be set in this order:
                rBHelper.bDisposed = sal_True;
                rBHelper.bInDispose = sal_False;
                throw;
            }
            MutexGuard aGuard( rBHelper.rMutex );
            // bDispose and bInDisposing must be set in this order:
            rBHelper.bDisposed = sal_True;
            rBHelper.bInDispose = sal_False;
        }
        catch (RuntimeException &)
        {
            throw;
        }
        catch (Exception & exc)
        {
            throw RuntimeException(
                OUString( RTL_CONSTASCII_USTRINGPARAM(
                              "unexpected UNO exception caught: ") ) +
                exc.Message, Reference< XInterface >() );
        }
    }
    else
    {
        // in a multithreaded environment, it can't be avoided,
        // that dispose is called twice.
        // However this condition is traced, because it MAY indicate an error.
        OSL_TRACE( "OComponentHelper::dispose() - dispose called twice" );
    }
}

// XComponent
void OComponentHelper::addEventListener(
    const Reference<XEventListener > & rxListener )
    throw(::com::sun::star::uno::RuntimeException)
{
    ClearableMutexGuard aGuard( rBHelper.rMutex );
    if (rBHelper.bDisposed || rBHelper.bInDispose)
    {
        aGuard.clear();
        Reference< XInterface > x( (XComponent *)this, UNO_QUERY );
        rxListener->disposing( EventObject( x ) );
    }
    else
    {
        rBHelper.addListener( ::getCppuType( &rxListener ) , rxListener );
    }
}

// XComponent
void OComponentHelper::removeEventListener(
    const Reference<XEventListener > & rxListener )
    throw(::com::sun::star::uno::RuntimeException)
{
    rBHelper.removeListener( ::getCppuType( &rxListener ) , rxListener );
}

}

<commit_msg>INTEGRATION: CWS warnings01 (1.12.4); FILE MERGED 2005/11/28 16:39:56 sb 1.12.4.1: #i53898# Made code warning-free.<commit_after>/*************************************************************************
 *
 *  OpenOffice.org - a multi-platform office productivity suite
 *
 *  $RCSfile: component.cxx,v $
 *
 *  $Revision: 1.13 $
 *
 *  last change: $Author: hr $ $Date: 2006-06-19 10:32:45 $
 *
 *  The Contents of this file are made available subject to
 *  the terms of GNU Lesser General Public License Version 2.1.
 *
 *
 *    GNU Lesser General Public License Version 2.1
 *    =============================================
 *    Copyright 2005 by Sun Microsystems, Inc.
 *    901 San Antonio Road, Palo Alto, CA 94303, USA
 *
 *    This library is free software; you can redistribute it and/or
 *    modify it under the terms of the GNU Lesser General Public
 *    License version 2.1, as published by the Free Software Foundation.
 *
 *    This library is distributed in the hope that it will be useful,
 *    but WITHOUT ANY WARRANTY; without even the implied warranty of
 *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 *    Lesser General Public License for more details.
 *
 *    You should have received a copy of the GNU Lesser General Public
 *    License along with this library; if not, write to the Free Software
 *    Foundation, Inc., 59 Temple Place, Suite 330, Boston,
 *    MA  02111-1307  USA
 *
 ************************************************************************/

#ifndef _RTL_STRING_HXX_
#include <rtl/string.hxx>
#endif
#ifndef _OSL_DIAGNOSE_H_
#include <osl/diagnose.h>
#endif
#ifndef _CPPUHELPER_COMPONENT_HXX_
#include <cppuhelper/component.hxx>
#endif
#ifndef _CPPUHELPER_QUERYINTERFACE_HXX_
#include <cppuhelper/queryinterface.hxx>
#endif
#ifndef _CPPUHELPER_TYPEPROVIDER_HXX_
#include <cppuhelper/typeprovider.hxx>
#endif
#include "com/sun/star/uno/RuntimeException.hpp"

using namespace osl;
using namespace rtl;
using namespace com::sun::star;
using namespace com::sun::star::uno;
using namespace com::sun::star::lang;

namespace cppu
{

//  ----------------------------------------------------
//  class OComponentHelper
//  ----------------------------------------------------

OComponentHelper::OComponentHelper( Mutex & rMutex ) SAL_THROW( () )
    : rBHelper( rMutex )
{
}
OComponentHelper::~OComponentHelper() SAL_THROW( (RuntimeException) )
{
}

Any OComponentHelper::queryInterface( Type const & rType ) throw (RuntimeException)
{
    return OWeakAggObject::queryInterface( rType );
}
Any OComponentHelper::queryAggregation( Type const & rType ) throw (RuntimeException)
{
    if (rType == ::getCppuType( (Reference< lang::XComponent > const *)0 ))
    {
        void * p = static_cast< lang::XComponent * >( this );
        return Any( &p, rType );
    }
    else if (rType == ::getCppuType( (Reference< lang::XTypeProvider > const *)0 ))
    {
        void * p = static_cast< lang::XTypeProvider * >( this );
        return Any( &p, rType );
    }
    return OWeakAggObject::queryAggregation( rType );
}
void OComponentHelper::acquire() throw ()
{
    OWeakAggObject::acquire();
}

void OComponentHelper::release() throw()
{
    Reference<XInterface > x( xDelegator );
    if (! x.is())
    {
        if (osl_decrementInterlockedCount( &m_refCount ) == 0)
        {
            if (! rBHelper.bDisposed)
            {
                Reference<XInterface > xHoldAlive( *this );
                // First dispose
                try
                {
                    dispose();
                }
                catch (::com::sun::star::uno::RuntimeException & exc)
                {
                    // release should not throw exceptions
#if OSL_DEBUG_LEVEL > 0
                    OString msg( OUStringToOString( exc.Message, RTL_TEXTENCODING_ASCII_US ) );
                    OSL_ENSURE( 0, msg.getStr() );
#else
                    (void) exc; // avoid warning about unused variable
#endif
                }

                // only the alive ref holds the object
                OSL_ASSERT( m_refCount == 1 );
                // destroy the object if xHoldAlive decrement the refcount to 0
                return;
            }
        }
        // restore the reference count
        osl_incrementInterlockedCount( &m_refCount );
    }
    OWeakAggObject::release();
}

Sequence< Type > OComponentHelper::getTypes() throw (RuntimeException)
{
    static OTypeCollection * s_pTypes = 0;
    if (! s_pTypes)
    {
        MutexGuard aGuard( Mutex::getGlobalMutex() );
        if (! s_pTypes)
        {
            static OTypeCollection s_aTypes(
                ::getCppuType( (const Reference< lang::XComponent > *)0 ),
                ::getCppuType( (const Reference< lang::XTypeProvider > *)0 ),
                ::getCppuType( (const Reference< XAggregation > *)0 ),
                ::getCppuType( (const Reference< XWeak > *)0 ) );
            s_pTypes = &s_aTypes;
        }
    }
    return s_pTypes->getTypes();
}

// XComponent
void OComponentHelper::disposing()
{
}

// XComponent
void OComponentHelper::dispose()
    throw(::com::sun::star::uno::RuntimeException)
{
    // An frequently programming error is to release the last
    // reference to this object in the disposing message.
    // Make it rubust, hold a self Reference.
    Reference<XComponent > xSelf( this );

    // Guard dispose against multible threading
    // Remark: It is an error to call dispose more than once
    sal_Bool bDoDispose = sal_False;
    {
    MutexGuard aGuard( rBHelper.rMutex );
    if( !rBHelper.bDisposed && !rBHelper.bInDispose )
    {
        // only one call go into this section
        rBHelper.bInDispose = sal_True;
        bDoDispose = sal_True;
    }
    }

    // Do not hold the mutex because we are broadcasting
    if( bDoDispose )
    {
        // Create an event with this as sender
        try
        {
            try
            {
                Reference<XInterface > xSource(
                    Reference<XInterface >::query( (XComponent *)this ) );
                EventObject aEvt;
                aEvt.Source = xSource;
                // inform all listeners to release this object
                // The listener container are automaticly cleared
                rBHelper.aLC.disposeAndClear( aEvt );
                // notify subclasses to do their dispose
                disposing();
            }
            catch (...)
            {
                MutexGuard aGuard( rBHelper.rMutex );
                // bDispose and bInDisposing must be set in this order:
                rBHelper.bDisposed = sal_True;
                rBHelper.bInDispose = sal_False;
                throw;
            }
            MutexGuard aGuard( rBHelper.rMutex );
            // bDispose and bInDisposing must be set in this order:
            rBHelper.bDisposed = sal_True;
            rBHelper.bInDispose = sal_False;
        }
        catch (RuntimeException &)
        {
            throw;
        }
        catch (Exception & exc)
        {
            throw RuntimeException(
                OUString( RTL_CONSTASCII_USTRINGPARAM(
                              "unexpected UNO exception caught: ") ) +
                exc.Message, Reference< XInterface >() );
        }
    }
    else
    {
        // in a multithreaded environment, it can't be avoided,
        // that dispose is called twice.
        // However this condition is traced, because it MAY indicate an error.
        OSL_TRACE( "OComponentHelper::dispose() - dispose called twice" );
    }
}

// XComponent
void OComponentHelper::addEventListener(
    const Reference<XEventListener > & rxListener )
    throw(::com::sun::star::uno::RuntimeException)
{
    ClearableMutexGuard aGuard( rBHelper.rMutex );
    if (rBHelper.bDisposed || rBHelper.bInDispose)
    {
        aGuard.clear();
        Reference< XInterface > x( (XComponent *)this, UNO_QUERY );
        rxListener->disposing( EventObject( x ) );
    }
    else
    {
        rBHelper.addListener( ::getCppuType( &rxListener ) , rxListener );
    }
}

// XComponent
void OComponentHelper::removeEventListener(
    const Reference<XEventListener > & rxListener )
    throw(::com::sun::star::uno::RuntimeException)
{
    rBHelper.removeListener( ::getCppuType( &rxListener ) , rxListener );
}

}

<|endoftext|>


<commit_before>/*
 * Copyright (c) 2017 Cryptonomex, Inc., and contributors.
 *
 * The MIT License
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */
#pragma once

#include <graphene/app/plugin.hpp>
#include <graphene/chain/database.hpp>

namespace graphene { namespace es_objects {
   using namespace chain;
   //using namespace graphene::db;
   //using boost::multi_index_container;
   //using namespace boost::multi_index;

//
// Plugins should #define their SPACE_ID's so plugins with
// conflicting SPACE_ID assignments can be compiled into the
// same binary (by simply re-assigning some of the conflicting #defined
// SPACE_ID's in a build script).
//
// Assignment of SPACE_ID's cannot be done at run-time because
// various template automagic depends on them being known at compile
// time.
//
#ifndef PROPOSAL_SPACE_ID
#define PROPOSAL_SPACE_ID 6
#endif

static size_t WriteCallback(void *contents, size_t size, size_t nmemb, void *userp)
{
   ((std::string*)userp)->append((char*)contents, size * nmemb);
   return size * nmemb;
}

namespace detail
{
    class es_objects_plugin_impl;
}

class es_objects_plugin : public graphene::app::plugin
{
   public:
      es_objects_plugin();
      virtual ~es_objects_plugin();

      std::string plugin_name()const override;
      std::string plugin_description()const override;
      virtual void plugin_set_program_options(
         boost::program_options::options_description& cli,
         boost::program_options::options_description& cfg) override;
      virtual void plugin_initialize(const boost::program_options::variables_map& options) override;
      virtual void plugin_startup() override;

      friend class detail::es_objects_plugin_impl;
      std::unique_ptr<detail::es_objects_plugin_impl> my;
};

struct proposal_struct {
   proposal_id_type id;
   time_point_sec expiration_time;
   optional<time_point_sec> review_period_time;
   std::string proposed_transaction;
};

struct account_struct {
   account_id_type id;
   time_point_sec membership_expiration_date;
   account_id_type registrar;
   account_id_type referrer;
   account_id_type lifetime_referrer;
   uint16_t network_fee_percentage;
   uint16_t lifetime_referrer_fee_percentage;
   uint16_t referrer_rewards_percentage;
   string name;
   string owner;
   string active;
   account_id_type voting_account;
};
struct asset_struct {
   asset_id_type id;
   std::string symbol;
   account_id_type issuer;
};
struct balance_struct {
   balance_id_type id;
   address owner;
   asset_id_type asset_id;
   share_type amount;
};

} } //graphene::es_objects

FC_REFLECT( graphene::es_objects::proposal_struct, (id)(expiration_time)(review_period_time)(proposed_transaction) )
FC_REFLECT( graphene::es_objects::account_struct, (id)(membership_expiration_date)(registrar)(referrer)(lifetime_referrer)(network_fee_percentage)(lifetime_referrer_fee_percentage)(referrer_rewards_percentage)(name)(owner)(active)(voting_account) )
FC_REFLECT( graphene::es_objects::asset_struct, (id)(symbol)(issuer) )
FC_REFLECT( graphene::es_objects::balance_struct, (id)(owner)(asset_id)(amount) )

<commit_msg>remove non needed space_id from plugin<commit_after>/*
 * Copyright (c) 2017 Cryptonomex, Inc., and contributors.
 *
 * The MIT License
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */
#pragma once

#include <graphene/app/plugin.hpp>
#include <graphene/chain/database.hpp>

namespace graphene { namespace es_objects {

using namespace chain;

static size_t WriteCallback(void *contents, size_t size, size_t nmemb, void *userp)
{
   ((std::string*)userp)->append((char*)contents, size * nmemb);
   return size * nmemb;
}

namespace detail
{
    class es_objects_plugin_impl;
}

class es_objects_plugin : public graphene::app::plugin
{
   public:
      es_objects_plugin();
      virtual ~es_objects_plugin();

      std::string plugin_name()const override;
      std::string plugin_description()const override;
      virtual void plugin_set_program_options(
         boost::program_options::options_description& cli,
         boost::program_options::options_description& cfg) override;
      virtual void plugin_initialize(const boost::program_options::variables_map& options) override;
      virtual void plugin_startup() override;

      friend class detail::es_objects_plugin_impl;
      std::unique_ptr<detail::es_objects_plugin_impl> my;
};

struct proposal_struct {
   proposal_id_type id;
   time_point_sec expiration_time;
   optional<time_point_sec> review_period_time;
   std::string proposed_transaction;
};

struct account_struct {
   account_id_type id;
   time_point_sec membership_expiration_date;
   account_id_type registrar;
   account_id_type referrer;
   account_id_type lifetime_referrer;
   uint16_t network_fee_percentage;
   uint16_t lifetime_referrer_fee_percentage;
   uint16_t referrer_rewards_percentage;
   string name;
   string owner;
   string active;
   account_id_type voting_account;
};
struct asset_struct {
   asset_id_type id;
   std::string symbol;
   account_id_type issuer;
};
struct balance_struct {
   balance_id_type id;
   address owner;
   asset_id_type asset_id;
   share_type amount;
};

} } //graphene::es_objects

FC_REFLECT( graphene::es_objects::proposal_struct, (id)(expiration_time)(review_period_time)(proposed_transaction) )
FC_REFLECT( graphene::es_objects::account_struct, (id)(membership_expiration_date)(registrar)(referrer)(lifetime_referrer)(network_fee_percentage)(lifetime_referrer_fee_percentage)(referrer_rewards_percentage)(name)(owner)(active)(voting_account) )
FC_REFLECT( graphene::es_objects::asset_struct, (id)(symbol)(issuer) )
FC_REFLECT( graphene::es_objects::balance_struct, (id)(owner)(asset_id)(amount) )

<|endoftext|>


<commit_before>#ifndef ALEPH_TOPOLOGY_MESH_HH__
#define ALEPH_TOPOLOGY_MESH_HH__

#include <cassert>

#include <algorithm>
#include <iterator>
#include <memory>
#include <vector>

namespace aleph
{

namespace topology
{

/**
  @class Mesh
  @brief Half-edge mesh data structure

  This data structure is capable of representing two-dimensional piecewise
  linear manifolds. In order to speed up standard queries, this class uses
  a standard half-edge data structure.
*/

template <class Position = float, class Data = float> class Mesh
{
public:
  struct Face;
  struct HalfEdge;
  struct Vertex;

  using Index           = std::size_t;

  using FacePointer     = std::shared_ptr<Face>;
  using HalfEdgePointer = std::shared_ptr<HalfEdge>;
  using VertexPointer   = std::shared_ptr<Vertex>;

  struct HalfEdge
  {
    FacePointer   face;
    VertexPointer vertex;

    HalfEdgePointer next; // Next half-edge (counter-clockwise)
    HalfEdgePointer prev; // Previous half-edge
    HalfEdgePointer pair; // Opposite half-edge

    VertexPointer source() const noexcept
    {
      return pair->vertex;
    }

    VertexPointer target() const noexcept
    {
      return vertex;
    }
  };

  struct Face
  {
    HalfEdgePointer edge;
  };

  struct Vertex
  {
    Position x = Position();
    Position y = Position();
    Position z = Position();
    Data     d = Data();

    HalfEdgePointer edge;
  };

  // Mesh attributes ---------------------------------------------------

  std::size_t vertices() const noexcept
  {
    return _vertices.size();
  }

  std::size_t faces() const noexcept
  {
    // TODO: Not yet implemented
    return 0;
  }

  // Mesh modification -------------------------------------------------

  /** Adds a new vertex to the mesh */
  void addVertex( Position x, Position y, Position z, Data d = Data() )
  {
    Vertex v;

    v.x = x;
    v.y = y;
    v.z = z;
    v.d = d;

    _vertices.push_back( std::make_shared<Vertex>( v ) );
  }

  /**
    Adds a new face to the mesh. This function expects a range of vertex IDs
    that make up the face. The vertices of the face need to sorted correctly
    in order for the orientation to be consistent.
  */

  template <class InputIterator> void addFace( InputIterator begin, InputIterator end )
  {
    FacePointer face = std::make_shared<Face>();

    // Stores all half-edges created (or found) by this function in the
    // order in which they belong to the face.
    std::vector<HalfEdgePointer> edges;
    edges.reserve( std::distance( begin, end ) );

    for( InputIterator it = begin; it != end; ++it )
    {
      auto curr = it;
      auto next = std::next( it );

      if( next == end )
        next = begin;

      auto source = _vertices.at( *curr );   // Edge source vertex
      auto target = _vertices.at( *next );   // Edge target vertex
      auto edge   = getEdge( *curr, *next ); // Edge

      if( !edge )
      {
        edge      = std::make_shared<HalfEdge>();
        auto pair = std::make_shared<HalfEdge>();

        edge->face   = face;
        edge->pair   = pair;
        edge->vertex = target;
        pair->vertex = source;
        pair->pair   = edge;

        if( !source->edge )
          source->edge = edge;

        if( !target->edge )
          target->edge = pair;
      }
      else
      {
        assert( !edge->face );
        edge->face = face;
      }

      edges.push_back( edge );
    }

    // Set 'next' and 'prev' pointers correctly ------------------------
    //
    // We first traverse all edges that bound the current face. Here, it
    // should be possible to traverse the face directly, so we require a
    // proper pointer in both directions.

    for( auto itEdge = edges.begin(); itEdge != edges.end(); ++itEdge )
    {
      auto curr = itEdge;
      auto prev = std::prev( curr );
      auto next = std::next( curr );

      if( curr == edges.begin() )
        prev = std::prev( edges.end() );

      if( next == edges.end() )
        next = edges.begin();

      auto&& edge = *itEdge;

      assert( !edge->next );
      assert( !edge->prev );

      edge->next = *next;
      edge->prev = *prev;
    }
  }

  // Mesh queries ------------------------------------------------------

  std::vector<VertexPointer> getLowerNeighbours( const Vertex& v )
  {
    auto neighbours = this->getNeighbours( v );
    auto d          = v.d;

    neighbours.erase( std::remove_if( neighbours.begin(), neighbours.end(),
                                      [&d] ( const VertexPointer& neighbour )
                                      {
                                        return neighbour->d >= d;
                                      } ),
                      neighbours.end() );

    return neighbours;
  }

  std::vector<VertexPointer> getHigherNeighbours( const Vertex& v )
  {
    auto neighbours = this->getNeighbours( v );
    auto d          = v.d;

    neighbours.erase( std::remove_if( neighbours.begin(), neighbours.end(),
                                      [&d] ( const VertexPointer& neighbour )
                                      {
                                        return neighbour->d <= d;
                                      } ),
                      neighbours.end() );

    return neighbours;
  }

private:

  /** Gets all vertices that are adjacent to a given vertex */
  std::vector<VertexPointer> getNeighbours( const Vertex& v )
  {
    std::vector<VertexPointer> neighbours;

    auto edge = v.edge;
    do
    {
      if( edge )
        neighbours.push_back( edge->target );
      else
        break;

      edge = edge->pair->next;
    }
    while( edge != v.edge );

    return neighbours;
  }

  /** Gets all edges that are incident on a given vertex. */
  std::vector<HalfEdgePointer> getEdges( const Vertex& v )
  {
    std::vector<HalfEdgePointer> edges;

    auto edge = v.edge;
    do
    {
      if( edge )
        edges.push_back( edge );
      else
        break;

      edge = edge->pair->next;
    }
    while( edge != v.edge );

    return edges;
  }

  /**
    Check whether a given (directed) edge already exists. If so,
    a pointer to the edge is being returned.
  */

  HalfEdgePointer getEdge( Index u, Index v )
  {
    auto source = _vertices.at(u);           // Edge source vertex
    auto target = _vertices.at(v);           // Edge target vertex
    auto edges  = this->getEdges( *source ); // Incident edges

    auto itEdge = std::find_if( edges.begin(), edges.end(),
                                [&source, &target] ( const HalfEdgePointer& edge )
                                {
                                  return edge->source() == source && edge->target() == target;
                                } );

    if( itEdge != edges.end() )
      return *itEdge;
    else
      return nullptr;
  }

  /**
    Stores all vertex pointers. This is sufficient to store the
    complete mesh.
  */

  std::vector<VertexPointer> _vertices;
};

} // namespace topology

} // namespace aleph

#endif
<commit_msg>Ensuring const-correctness of mesh operations<commit_after>#ifndef ALEPH_TOPOLOGY_MESH_HH__
#define ALEPH_TOPOLOGY_MESH_HH__

#include <cassert>

#include <algorithm>
#include <iterator>
#include <memory>
#include <vector>

namespace aleph
{

namespace topology
{

/**
  @class Mesh
  @brief Half-edge mesh data structure

  This data structure is capable of representing two-dimensional piecewise
  linear manifolds. In order to speed up standard queries, this class uses
  a standard half-edge data structure.
*/

template <class Position = float, class Data = float> class Mesh
{
public:
  struct Face;
  struct HalfEdge;
  struct Vertex;

  using Index           = std::size_t;

  using FacePointer     = std::shared_ptr<Face>;
  using HalfEdgePointer = std::shared_ptr<HalfEdge>;
  using VertexPointer   = std::shared_ptr<Vertex>;

  struct HalfEdge
  {
    FacePointer   face;
    VertexPointer vertex;

    HalfEdgePointer next; // Next half-edge (counter-clockwise)
    HalfEdgePointer prev; // Previous half-edge
    HalfEdgePointer pair; // Opposite half-edge

    VertexPointer source() const noexcept
    {
      return pair->vertex;
    }

    VertexPointer target() const noexcept
    {
      return vertex;
    }
  };

  struct Face
  {
    HalfEdgePointer edge;
  };

  struct Vertex
  {
    Position x = Position();
    Position y = Position();
    Position z = Position();
    Data     d = Data();

    HalfEdgePointer edge;
  };

  // Mesh attributes ---------------------------------------------------

  std::size_t vertices() const noexcept
  {
    return _vertices.size();
  }

  std::size_t faces() const noexcept
  {
    // TODO: Not yet implemented
    return 0;
  }

  // Mesh modification -------------------------------------------------

  /** Adds a new vertex to the mesh */
  void addVertex( Position x, Position y, Position z, Data d = Data() )
  {
    Vertex v;

    v.x = x;
    v.y = y;
    v.z = z;
    v.d = d;

    _vertices.push_back( std::make_shared<Vertex>( v ) );
  }

  /**
    Adds a new face to the mesh. This function expects a range of vertex IDs
    that make up the face. The vertices of the face need to sorted correctly
    in order for the orientation to be consistent.
  */

  template <class InputIterator> void addFace( InputIterator begin, InputIterator end )
  {
    FacePointer face = std::make_shared<Face>();

    // Stores all half-edges created (or found) by this function in the
    // order in which they belong to the face.
    std::vector<HalfEdgePointer> edges;
    edges.reserve( std::distance( begin, end ) );

    for( InputIterator it = begin; it != end; ++it )
    {
      auto curr = it;
      auto next = std::next( it );

      if( next == end )
        next = begin;

      auto source = _vertices.at( *curr );   // Edge source vertex
      auto target = _vertices.at( *next );   // Edge target vertex
      auto edge   = getEdge( *curr, *next ); // Edge

      if( !edge )
      {
        edge      = std::make_shared<HalfEdge>();
        auto pair = std::make_shared<HalfEdge>();

        edge->face   = face;
        edge->pair   = pair;
        edge->vertex = target;
        pair->vertex = source;
        pair->pair   = edge;

        if( !source->edge )
          source->edge = edge;

        if( !target->edge )
          target->edge = pair;
      }
      else
      {
        assert( !edge->face );
        edge->face = face;
      }

      edges.push_back( edge );
    }

    // Set 'next' and 'prev' pointers correctly ------------------------
    //
    // We first traverse all edges that bound the current face. Here, it
    // should be possible to traverse the face directly, so we require a
    // proper pointer in both directions.

    for( auto itEdge = edges.begin(); itEdge != edges.end(); ++itEdge )
    {
      auto curr = itEdge;
      auto prev = std::prev( curr );
      auto next = std::next( curr );

      if( curr == edges.begin() )
        prev = std::prev( edges.end() );

      if( next == edges.end() )
        next = edges.begin();

      auto&& edge = *itEdge;

      assert( !edge->next );
      assert( !edge->prev );

      edge->next = *next;
      edge->prev = *prev;
    }
  }

  // Mesh queries ------------------------------------------------------

  std::vector<VertexPointer> getLowerNeighbours( const Vertex& v ) const noexcept
  {
    auto neighbours = this->getNeighbours( v );
    auto d          = v.d;

    neighbours.erase( std::remove_if( neighbours.begin(), neighbours.end(),
                                      [&d] ( const VertexPointer& neighbour )
                                      {
                                        return neighbour->d >= d;
                                      } ),
                      neighbours.end() );

    return neighbours;
  }

  std::vector<VertexPointer> getHigherNeighbours( const Vertex& v ) const noexcept
  {
    auto neighbours = this->getNeighbours( v );
    auto d          = v.d;

    neighbours.erase( std::remove_if( neighbours.begin(), neighbours.end(),
                                      [&d] ( const VertexPointer& neighbour )
                                      {
                                        return neighbour->d <= d;
                                      } ),
                      neighbours.end() );

    return neighbours;
  }

  VertexPointer vertex( std::size_t id ) const
  {
    return _vertices.at( id );
  }

private:

  /** Gets all vertices that are adjacent to a given vertex */
  std::vector<VertexPointer> getNeighbours( const Vertex& v ) const noexcept
  {
    std::vector<VertexPointer> neighbours;

    auto edge = v.edge;
    do
    {
      if( edge )
        neighbours.push_back( edge->target() );
      else
        break;

      edge = edge->pair->next;
    }
    while( edge != v.edge );

    return neighbours;
  }

  /** Gets all edges that are incident on a given vertex. */
  std::vector<HalfEdgePointer> getEdges( const Vertex& v ) const noexcept
  {
    std::vector<HalfEdgePointer> edges;

    auto edge = v.edge;
    do
    {
      if( edge )
        edges.push_back( edge );
      else
        break;

      edge = edge->pair->next;
    }
    while( edge != v.edge );

    return edges;
  }

  /**
    Check whether a given (directed) edge already exists. If so,
    a pointer to the edge is being returned.
  */

  HalfEdgePointer getEdge( Index u, Index v ) const noexcept
  {
    auto source = _vertices.at(u);           // Edge source vertex
    auto target = _vertices.at(v);           // Edge target vertex
    auto edges  = this->getEdges( *source ); // Incident edges

    auto itEdge = std::find_if( edges.begin(), edges.end(),
                                [&source, &target] ( const HalfEdgePointer& edge )
                                {
                                  return edge->source() == source && edge->target() == target;
                                } );

    if( itEdge != edges.end() )
      return *itEdge;
    else
      return nullptr;
  }

  /**
    Stores all vertex pointers. This is sufficient to store the
    complete mesh.
  */

  std::vector<VertexPointer> _vertices;
};

} // namespace topology

} // namespace aleph

#endif
<|endoftext|>


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

#include "sitkImageConvert.h"
#include "itkHolderCommand.h"

namespace itk
{

namespace simple
{

/** \brief A utility method to help convert between itk image types efficiently.
 *
 */
template< typename TPixelType, unsigned int ImageDimension >
SITKCommon_HIDDEN
typename itk::Image< itk::Vector< TPixelType, ImageDimension >, ImageDimension>::Pointer
GetImageFromVectorImage( itk::VectorImage< TPixelType, ImageDimension > *img, bool transferOwnership )
{
  typedef itk::Image< itk::Vector< TPixelType, ImageDimension >, ImageDimension> ImageType;
  typedef itk::VectorImage< TPixelType, ImageDimension > VectorImageType;

  // check number of element compatibility
  if ( img->GetNumberOfComponentsPerPixel() != VectorImageType::ImageDimension )
    {
    sitkExceptionMacro("Expected number of elements in vector image to be the same as the dimension!");
    }

  size_t numberOfElements = img->GetBufferedRegion().GetNumberOfPixels();
  typename ImageType::PixelType* buffer = reinterpret_cast<typename ImageType::PixelType*>( img->GetPixelContainer()->GetBufferPointer() );

  typename ImageType::Pointer out = ImageType::New();

  if (img->GetPixelContainer()->GetContainerManageMemory() && transferOwnership )
    {
    out->GetPixelContainer()->SetImportPointer(buffer, numberOfElements, true );
    img->GetPixelContainer()->ContainerManageMemoryOff();
    }
  else
    {
    auto holder = itk::HolderCommand<typename VectorImageType::PixelContainer::Pointer>::New();
    holder->Set( img->GetPixelContainer() );

    // Set the image's pixel container to import the pointer provided.
    out->GetPixelContainer()->SetImportPointer(buffer, numberOfElements, false );
    out->AddObserver( itk::DeleteEvent(), holder);
    }

  out->CopyInformation( img );
  out->SetRegions( img->GetBufferedRegion() );

  return out;

}

template< class TPixelType, unsigned int NImageDimension, unsigned int NLength >
SITKCommon_HIDDEN
typename itk::VectorImage< TPixelType, NImageDimension >::Pointer
GetVectorImageFromImage( itk::Image< itk::Vector< TPixelType, NLength >, NImageDimension> *img, bool transferOwnership )
{
  typedef itk::VectorImage< TPixelType, NImageDimension > VectorImageType;
  typedef itk::Image< itk::Vector< TPixelType, NLength >, NImageDimension> ImageType;

  size_t numberOfElements = img->GetBufferedRegion().GetNumberOfPixels();
  typename VectorImageType::InternalPixelType* buffer = reinterpret_cast<typename VectorImageType::InternalPixelType*>( img->GetPixelContainer()->GetBufferPointer() );

  // Unlike an image of Vectors a VectorImage's container is a
  // container of TPixelType, whos size is the image's number of
  // pixels * number of pixels per component
  numberOfElements *= NImageDimension;

  typename VectorImageType::Pointer out = VectorImageType::New();

  if (img->GetPixelContainer()->GetContainerManageMemory() && transferOwnership)
    {
    out->GetPixelContainer()->SetImportPointer(buffer, numberOfElements, true );
    img->GetPixelContainer()->ContainerManageMemoryOff();
    }
  else
    {
    auto holder = itk::HolderCommand<typename ImageType::PixelContainer::Pointer>::New();
    holder->Set( typename ImageType::PixelContainer::Pointer(img->GetPixelContainer()) );

    // Set the image's pixel container to import the pointer provided.
    out->GetPixelContainer()->SetImportPointer(buffer, numberOfElements, false );
    out->AddObserver( itk::DeleteEvent(), holder);

    }

  out->CopyInformation( img );
  out->SetRegions( img->GetBufferedRegion() );

  return out;
}

template< class TPixelType, unsigned int NImageDimension, unsigned int NLength >
SITKCommon_HIDDEN
typename itk::VectorImage< TPixelType, NImageDimension >::Pointer
GetVectorImageFromImage( itk::Image< itk::CovariantVector< TPixelType, NLength>, NImageDimension> *img, bool transferOwnership )
{
  typedef itk::VectorImage< TPixelType, NImageDimension > VectorImageType;
  typedef itk::Image< itk::CovariantVector< TPixelType, NLength >, NImageDimension> ImageType;

  size_t numberOfElements = img->GetBufferedRegion().GetNumberOfPixels();
  typename VectorImageType::InternalPixelType* buffer = reinterpret_cast<typename VectorImageType::InternalPixelType*>( img->GetPixelContainer()->GetBufferPointer() );

  // Unlike an image of Vectors a VectorImage's container is a
  // container of TPixelType, whos size is the image's number of
  // pixels * number of pixels per component
  numberOfElements *= NLength;

  typename VectorImageType::Pointer out = VectorImageType::New();

  if (img->GetPixelContainer()->GetContainerManageMemory() && transferOwnership)
    {
    out->GetPixelContainer()->SetImportPointer(buffer, numberOfElements, true );
    img->GetPixelContainer()->ContainerManageMemoryOff();
    }
  else
    {
    auto holder = itk::HolderCommand<typename ImageType::PixelContainer::Pointer>::New();
    holder->Set( typename ImageType::PixelContainer::Pointer(img->GetPixelContainer()) );

    // Set the image's pixel container to import the pointer provided.
    out->GetPixelContainer()->SetImportPointer(buffer, numberOfElements, false );
    out->AddObserver( itk::DeleteEvent(), holder);

    }

  out->CopyInformation( img );
  out->SetRegions( img->GetBufferedRegion() );

  return out;
}

}
}

#endif // sitkImageConvert_hxx
<commit_msg>Fix size buffer size in GetVectorImageFromImage<commit_after>/*=========================================================================
*
*  Copyright NumFOCUS
*
*  Licensed under the Apache License, Version 2.0 (the "License");
*  you may not use this file except in compliance with the License.
*  You may obtain a copy of the License at
*
*         http://www.apache.org/licenses/LICENSE-2.0.txt
*
*  Unless required by applicable law or agreed to in writing, software
*  distributed under the License is distributed on an "AS IS" BASIS,
*  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
*  See the License for the specific language governing permissions and
*  limitations under the License.
*
*=========================================================================*/
#ifndef sitkImageConvert_hxx
#define sitkImageConvert_hxx

#include "sitkImageConvert.h"
#include "itkHolderCommand.h"

namespace itk
{

namespace simple
{

/** \brief A utility method to help convert between itk image types efficiently.
 *
 */
template< typename TPixelType, unsigned int ImageDimension >
SITKCommon_HIDDEN
typename itk::Image< itk::Vector< TPixelType, ImageDimension >, ImageDimension>::Pointer
GetImageFromVectorImage( itk::VectorImage< TPixelType, ImageDimension > *img, bool transferOwnership )
{
  typedef itk::Image< itk::Vector< TPixelType, ImageDimension >, ImageDimension> ImageType;
  typedef itk::VectorImage< TPixelType, ImageDimension > VectorImageType;

  // check number of element compatibility
  if ( img->GetNumberOfComponentsPerPixel() != VectorImageType::ImageDimension )
    {
    sitkExceptionMacro("Expected number of elements in vector image to be the same as the dimension!");
    }

  size_t numberOfElements = img->GetBufferedRegion().GetNumberOfPixels();
  typename ImageType::PixelType* buffer = reinterpret_cast<typename ImageType::PixelType*>( img->GetPixelContainer()->GetBufferPointer() );

  typename ImageType::Pointer out = ImageType::New();

  if (img->GetPixelContainer()->GetContainerManageMemory() && transferOwnership )
    {
    out->GetPixelContainer()->SetImportPointer(buffer, numberOfElements, true );
    img->GetPixelContainer()->ContainerManageMemoryOff();
    }
  else
    {
    auto holder = itk::HolderCommand<typename VectorImageType::PixelContainer::Pointer>::New();
    holder->Set( img->GetPixelContainer() );

    // Set the image's pixel container to import the pointer provided.
    out->GetPixelContainer()->SetImportPointer(buffer, numberOfElements, false );
    out->AddObserver( itk::DeleteEvent(), holder);
    }

  out->CopyInformation( img );
  out->SetRegions( img->GetBufferedRegion() );

  return out;

}

template< class TPixelType, unsigned int NImageDimension, unsigned int NLength >
SITKCommon_HIDDEN
typename itk::VectorImage< TPixelType, NImageDimension >::Pointer
GetVectorImageFromImage( itk::Image< itk::Vector< TPixelType, NLength >, NImageDimension> *img, bool transferOwnership )
{
  typedef itk::VectorImage< TPixelType, NImageDimension > VectorImageType;
  typedef itk::Image< itk::Vector< TPixelType, NLength >, NImageDimension> ImageType;

  size_t numberOfElements = img->GetBufferedRegion().GetNumberOfPixels();
  typename VectorImageType::InternalPixelType* buffer = reinterpret_cast<typename VectorImageType::InternalPixelType*>( img->GetPixelContainer()->GetBufferPointer() );

  // Unlike an image of Vectors a VectorImage's container is a
  // container of TPixelType, whos size is the image's number of
  // pixels * number of pixels per component
  numberOfElements *= NLength;

  typename VectorImageType::Pointer out = VectorImageType::New();

  if (img->GetPixelContainer()->GetContainerManageMemory() && transferOwnership)
    {
    out->GetPixelContainer()->SetImportPointer(buffer, numberOfElements, true );
    img->GetPixelContainer()->ContainerManageMemoryOff();
    }
  else
    {
    auto holder = itk::HolderCommand<typename ImageType::PixelContainer::Pointer>::New();
    holder->Set( typename ImageType::PixelContainer::Pointer(img->GetPixelContainer()) );

    // Set the image's pixel container to import the pointer provided.
    out->GetPixelContainer()->SetImportPointer(buffer, numberOfElements, false );
    out->AddObserver( itk::DeleteEvent(), holder);

    }

  out->CopyInformation( img );
  out->SetRegions( img->GetBufferedRegion() );

  assert(out->GetNumberOfComponentsPerPixel() == NLength);

  return out;
}

template< class TPixelType, unsigned int NImageDimension, unsigned int NLength >
SITKCommon_HIDDEN
typename itk::VectorImage< TPixelType, NImageDimension >::Pointer
GetVectorImageFromImage( itk::Image< itk::CovariantVector< TPixelType, NLength>, NImageDimension> *img, bool transferOwnership )
{
  typedef itk::VectorImage< TPixelType, NImageDimension > VectorImageType;
  typedef itk::Image< itk::CovariantVector< TPixelType, NLength >, NImageDimension> ImageType;

  size_t numberOfElements = img->GetBufferedRegion().GetNumberOfPixels();
  typename VectorImageType::InternalPixelType* buffer = reinterpret_cast<typename VectorImageType::InternalPixelType*>( img->GetPixelContainer()->GetBufferPointer() );

  // Unlike an image of Vectors a VectorImage's container is a
  // container of TPixelType, whos size is the image's number of
  // pixels * number of pixels per component
  numberOfElements *= NLength;

  typename VectorImageType::Pointer out = VectorImageType::New();

  if (img->GetPixelContainer()->GetContainerManageMemory() && transferOwnership)
    {
    out->GetPixelContainer()->SetImportPointer(buffer, numberOfElements, true );
    img->GetPixelContainer()->ContainerManageMemoryOff();
    }
  else
    {
    auto holder = itk::HolderCommand<typename ImageType::PixelContainer::Pointer>::New();
    holder->Set( typename ImageType::PixelContainer::Pointer(img->GetPixelContainer()) );

    // Set the image's pixel container to import the pointer provided.
    out->GetPixelContainer()->SetImportPointer(buffer, numberOfElements, false );
    out->AddObserver( itk::DeleteEvent(), holder);

    }

  out->CopyInformation( img );
  out->SetRegions( img->GetBufferedRegion() );

  assert(out->GetNumberOfComponentsPerPixel() == NLength);

  return out;
}

}
}

#endif // sitkImageConvert_hxx
<|endoftext|>