<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.
 *****************************************************************************/

/*
 * spiral_reference_line_smoother.cc
 */

#include "modules/planning/reference_line/spiral_reference_line_smoother.h"

#include <algorithm>
#include <iomanip>
#include <utility>

#include "IpIpoptApplication.hpp"
#include "IpSolveStatistics.hpp"

#include "modules/common/time/time.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/math/curve1d/quintic_spiral_path.h"
#include "modules/planning/reference_line/spiral_problem_interface.h"

namespace apollo {
namespace planning {

using apollo::common::time::Clock;

SpiralReferenceLineSmoother::SpiralReferenceLineSmoother(
    const double max_point_deviation)
    : default_max_point_deviation_(max_point_deviation) {
  CHECK(max_point_deviation >= 0.0);
}

bool SpiralReferenceLineSmoother::Smooth(
    const ReferenceLine& raw_reference_line,
    ReferenceLine* const smoothed_reference_line) {
  const double start_timestamp = Clock::NowInSeconds();
  std::vector<double> opt_x;
  std::vector<double> opt_y;
  std::vector<double> opt_theta;
  std::vector<double> opt_kappa;
  std::vector<double> opt_dkappa;
  std::vector<double> opt_s;

  if (anchor_points_.empty()) {
    const double piecewise_length = FLAGS_spiral_smoother_piecewise_length;
    const double length = raw_reference_line.Length();
    ADEBUG << "Length = " << length;
    uint32_t num_of_pieces =
        std::max(1u, static_cast<uint32_t>(length / piecewise_length));

    const double delta_s = length / num_of_pieces;
    double s = 0.0;

    std::vector<Eigen::Vector2d> raw_point2d;
    for (std::uint32_t i = 0; i <= num_of_pieces;
         ++i, s = std::fmin(s + delta_s, length)) {
      ReferencePoint rlp = raw_reference_line.GetReferencePoint(s);
      raw_point2d.emplace_back(rlp.x(), rlp.y());
    }

    Smooth(raw_point2d, &opt_theta, &opt_kappa, &opt_dkappa, &opt_s, &opt_x,
           &opt_y);
  } else {
    std::size_t start_index = 0;
    for (const auto& anchor_point : anchor_points_) {
      if (anchor_point.enforced) {
        start_index++;
      } else {
        break;
      }
    }

    std::vector<Eigen::Vector2d> raw_point2d;
    if (start_index == 0) {
      for (const auto& anchor_point : anchor_points_) {
        raw_point2d.emplace_back(anchor_point.path_point.x(),
                                 anchor_point.path_point.y());
      }
    } else {
      std::vector<double> overhead_s;
      for (std::size_t i = 0; i + 1 < start_index; ++i) {
        const auto& p0 = anchor_points_[i];
        const auto& p1 = anchor_points_[i + 1];
        overhead_s.push_back(p1.path_point.s() - p0.path_point.s());
      }

      std::vector<double> overhead_theta;
      std::vector<double> overhead_kappa;
      std::vector<double> overhead_dkappa;
      std::vector<double> overhead_x;
      std::vector<double> overhead_y;
      for (std::size_t i = 0; i < anchor_points_.size(); ++i) {
        const auto& p = anchor_points_[i];
        if (i + 1 < start_index) {
          overhead_theta.push_back(p.path_point.theta());
          overhead_kappa.push_back(p.path_point.kappa());
          overhead_dkappa.push_back(p.path_point.dkappa());
          overhead_x.push_back(p.path_point.x());
          overhead_y.push_back(p.path_point.y());
        } else {
          raw_point2d.emplace_back(p.path_point.x(), p.path_point.y());
        }
      }

      fixed_start_point_ = true;
      fixed_start_x_ = anchor_points_[start_index - 1].path_point.x();
      fixed_start_y_ = anchor_points_[start_index - 1].path_point.y();
      fixed_start_theta_ = common::math::NormalizeAngle(
          anchor_points_[start_index - 1].path_point.theta());
      fixed_start_kappa_ = anchor_points_[start_index - 1].path_point.kappa();
      fixed_start_dkappa_ = anchor_points_[start_index - 1].path_point.dkappa();

      Smooth(raw_point2d, &opt_theta, &opt_kappa, &opt_dkappa, &opt_s, &opt_x,
             &opt_y);

      opt_theta.insert(opt_theta.begin(), overhead_theta.begin(),
                       overhead_theta.end());
      opt_kappa.insert(opt_kappa.begin(), overhead_kappa.begin(),
                       overhead_kappa.end());
      opt_dkappa.insert(opt_dkappa.begin(), overhead_dkappa.begin(),
                        overhead_dkappa.end());
      opt_s.insert(opt_s.begin(), overhead_s.begin(), overhead_s.end());
      opt_x.insert(opt_x.begin(), overhead_x.begin(), overhead_x.end());
      opt_y.insert(opt_y.begin(), overhead_y.begin(), overhead_y.end());

      std::for_each(opt_x.begin(), opt_x.end(), [this](double& x){
        x += zero_x_;
      });

      std::for_each(opt_y.begin(), opt_y.end(), [this](double& y){
        y += zero_y_;
      });
    }
  }

  std::vector<common::PathPoint> smoothed_point2d =
      Interpolate(opt_theta, opt_kappa, opt_dkappa, opt_s, opt_x, opt_y,
                  FLAGS_spiral_reference_line_resolution);

  std::vector<ReferencePoint> ref_points;
  for (const auto& p : smoothed_point2d) {
    const double heading = p.theta();
    const double kappa = p.kappa();
    const double dkappa = p.dkappa();

    common::SLPoint ref_sl_point;
    if (!raw_reference_line.XYToSL({p.x(), p.y()}, &ref_sl_point)) {
      return false;
    }
    if (ref_sl_point.s() < 0 ||
        ref_sl_point.s() > raw_reference_line.Length()) {
      continue;
    }

    ReferencePoint rlp = raw_reference_line.GetReferencePoint(ref_sl_point.s());
    ref_points.emplace_back(
        ReferencePoint(hdmap::MapPathPoint(common::math::Vec2d(p.x(), p.y()),
                                           heading, rlp.lane_waypoints()),
                       kappa, dkappa, 0.0, 0.0));
  }

  ReferencePoint::RemoveDuplicates(&ref_points);
  if (ref_points.size() < 2) {
    AERROR << "Fail to generate smoothed reference line.";
    return false;
  }
  *smoothed_reference_line = ReferenceLine(ref_points);
  const double end_timestamp = Clock::NowInSeconds();
  ADEBUG << "Spiral reference line smoother time: "
         << (end_timestamp - start_timestamp) * 1000 << " ms.";
  return true;
}

bool SpiralReferenceLineSmoother::Smooth(std::vector<Eigen::Vector2d> point2d,
                                         std::vector<double>* ptr_theta,
                                         std::vector<double>* ptr_kappa,
                                         std::vector<double>* ptr_dkappa,
                                         std::vector<double>* ptr_s,
                                         std::vector<double>* ptr_x,
                                         std::vector<double>* ptr_y) const {
  CHECK_GT(point2d.size(), 1);

  SpiralProblemInterface* ptop = new SpiralProblemInterface(point2d);
  ptop->set_default_max_point_deviation(default_max_point_deviation_);
  if (fixed_start_point_) {
    ptop->set_start_point(fixed_start_x_, fixed_start_y_, fixed_start_theta_,
                          fixed_start_kappa_, fixed_start_dkappa_);
  }

  Ipopt::SmartPtr<Ipopt::TNLP> problem = ptop;

  // Create an instance of the IpoptApplication
  Ipopt::SmartPtr<Ipopt::IpoptApplication> app = IpoptApplicationFactory();

  app->Options()->SetStringValue("hessian_approximation", "limited-memory");
  //  app->Options()->SetStringValue("derivative_test", "first-order");
  //  app->Options()->SetNumericValue("derivative_test_perturbation", 1.0e-7);
  //  app->Options()->SetStringValue("derivative_test", "second-order");
  app->Options()->SetIntegerValue("print_level", 0);
  int num_iterations = FLAGS_spiral_smoother_num_iteration;
  app->Options()->SetIntegerValue("max_iter", num_iterations);
  app->Options()->SetIntegerValue("acceptable_iter", 5);
  app->Options()->SetNumericValue("tol", 1.0e-3);
  app->Options()->SetNumericValue("acceptable_tol", 1.0e-3);

  Ipopt::ApplicationReturnStatus status = app->Initialize();
  if (status != Ipopt::Solve_Succeeded) {
    ADEBUG << "*** Error during initialization!";
    return static_cast<int>(status);
  }

  status = app->OptimizeTNLP(problem);

  if (status == Ipopt::Solve_Succeeded ||
      status == Ipopt::Solved_To_Acceptable_Level) {
    // Retrieve some statistics about the solve
    Ipopt::Index iter_count = app->Statistics()->IterationCount();
    ADEBUG << "*** The problem solved in " << iter_count << " iterations!";

    Ipopt::Number final_obj = app->Statistics()->FinalObjective();
    ADEBUG << "*** The final value of the objective function is " << final_obj
           << '.';
  } else {
    ADEBUG << "Return status: " << int(status);
  }

  ptop->get_optimization_results(ptr_theta, ptr_kappa, ptr_dkappa, ptr_s, ptr_x,
                                 ptr_y);

  return status == Ipopt::Solve_Succeeded ||
         status == Ipopt::Solved_To_Acceptable_Level;
}

std::vector<common::PathPoint> SpiralReferenceLineSmoother::Interpolate(
    const std::vector<double>& theta, const std::vector<double>& kappa,
    const std::vector<double>& dkappa, const std::vector<double>& s,
    const std::vector<double>& x, const std::vector<double>& y,
    const double resolution) const {
  std::vector<common::PathPoint> smoothed_point2d;
  double start_s = 0.0;
  common::PathPoint first_point =
      to_path_point(x.front(), y.front(), start_s, theta.front(), kappa.front(),
                    dkappa.front());
  smoothed_point2d.push_back(first_point);

  for (std::size_t i = 0; i + 1 < theta.size(); ++i) {
    double start_x = x[i];
    double start_y = y[i];

    auto path_point_seg = Interpolate(
        start_x, start_y, start_s, theta[i], kappa[i], dkappa[i], theta[i + 1],
        kappa[i + 1], dkappa[i + 1], s[i], resolution);

    smoothed_point2d.insert(smoothed_point2d.end(), path_point_seg.begin(),
                            path_point_seg.end());

    start_s = smoothed_point2d.back().s();
  }
  return smoothed_point2d;
}

std::vector<common::PathPoint> SpiralReferenceLineSmoother::Interpolate(
    const double start_x, const double start_y, const double start_s,
    const double theta0, const double kappa0, const double dkappa0,
    const double theta1, const double kappa1, const double dkappa1,
    const double delta_s, const double resolution) const {
  std::vector<common::PathPoint> path_points;

  QuinticSpiralPath spiral_curve(theta0, kappa0, dkappa0, theta1, kappa1,
                                 dkappa1, delta_s);
  std::size_t num_of_points = std::ceil(delta_s / resolution) + 1;
  for (std::size_t i = 1; i <= num_of_points; ++i) {
    const double inter_s = delta_s / num_of_points * i;
    const double dx = spiral_curve.ComputeCartesianDeviationX<10>(inter_s);
    const double dy = spiral_curve.ComputeCartesianDeviationY<10>(inter_s);

    const double theta =
        common::math::NormalizeAngle(spiral_curve.Evaluate(0, inter_s));
    const double kappa = spiral_curve.Evaluate(1, inter_s);
    const double dkappa = spiral_curve.Evaluate(2, inter_s);

    auto path_point = to_path_point(start_x + dx, start_y + dy,
                                    start_s + inter_s, theta, kappa, dkappa);
    path_points.push_back(std::move(path_point));
  }
  return path_points;
}

common::PathPoint SpiralReferenceLineSmoother::to_path_point(
    const double x, const double y, const double s, const double theta,
    const double kappa, const double dkappa) const {
  common::PathPoint point;
  point.set_x(x);
  point.set_y(y);
  point.set_s(s);
  point.set_theta(theta);
  point.set_kappa(kappa);
  point.set_dkappa(dkappa);
  return point;
}

void SpiralReferenceLineSmoother::SetAnchorPoints(
    const std::vector<AnchorPoint>& anchor_points) {
  anchor_points_ = std::move(anchor_points);

  CHECK_GT(anchor_points_.size(), 1);
  zero_x_ = anchor_points_.front().path_point.x();
  zero_y_ = anchor_points_.front().path_point.y();

  std::for_each(anchor_points_.begin(), anchor_points_.end(),
      [this](AnchorPoint& p) {
        auto curr_x = p.path_point.x();
        auto curr_y = p.path_point.y();
        p.path_point.set_x(curr_x - zero_x_);
        p.path_point.set_y(curr_y - zero_y_);});
}

}  // namespace planning
}  // namespace apollo
<commit_msg>planning: further tuned parameters for spiral reference line smoother<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.
 *****************************************************************************/

/*
 * spiral_reference_line_smoother.cc
 */

#include "modules/planning/reference_line/spiral_reference_line_smoother.h"

#include <algorithm>
#include <iomanip>
#include <utility>

#include "IpIpoptApplication.hpp"
#include "IpSolveStatistics.hpp"

#include "modules/common/time/time.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/math/curve1d/quintic_spiral_path.h"
#include "modules/planning/reference_line/spiral_problem_interface.h"

namespace apollo {
namespace planning {

using apollo::common::time::Clock;

SpiralReferenceLineSmoother::SpiralReferenceLineSmoother(
    const double max_point_deviation)
    : default_max_point_deviation_(max_point_deviation) {
  CHECK(max_point_deviation >= 0.0);
}

bool SpiralReferenceLineSmoother::Smooth(
    const ReferenceLine& raw_reference_line,
    ReferenceLine* const smoothed_reference_line) {
  const double start_timestamp = Clock::NowInSeconds();
  std::vector<double> opt_x;
  std::vector<double> opt_y;
  std::vector<double> opt_theta;
  std::vector<double> opt_kappa;
  std::vector<double> opt_dkappa;
  std::vector<double> opt_s;

  if (anchor_points_.empty()) {
    const double piecewise_length = FLAGS_spiral_smoother_piecewise_length;
    const double length = raw_reference_line.Length();
    ADEBUG << "Length = " << length;
    uint32_t num_of_pieces =
        std::max(1u, static_cast<uint32_t>(length / piecewise_length));

    const double delta_s = length / num_of_pieces;
    double s = 0.0;

    std::vector<Eigen::Vector2d> raw_point2d;
    for (std::uint32_t i = 0; i <= num_of_pieces;
         ++i, s = std::fmin(s + delta_s, length)) {
      ReferencePoint rlp = raw_reference_line.GetReferencePoint(s);
      raw_point2d.emplace_back(rlp.x(), rlp.y());
    }

    Smooth(raw_point2d, &opt_theta, &opt_kappa, &opt_dkappa, &opt_s, &opt_x,
           &opt_y);
  } else {
    std::size_t start_index = 0;
    for (const auto& anchor_point : anchor_points_) {
      if (anchor_point.enforced) {
        start_index++;
      } else {
        break;
      }
    }

    std::vector<Eigen::Vector2d> raw_point2d;
    if (start_index == 0) {
      for (const auto& anchor_point : anchor_points_) {
        raw_point2d.emplace_back(anchor_point.path_point.x(),
                                 anchor_point.path_point.y());
      }
    } else {
      std::vector<double> overhead_s;
      for (std::size_t i = 0; i + 1 < start_index; ++i) {
        const auto& p0 = anchor_points_[i];
        const auto& p1 = anchor_points_[i + 1];
        overhead_s.push_back(p1.path_point.s() - p0.path_point.s());
      }

      std::vector<double> overhead_theta;
      std::vector<double> overhead_kappa;
      std::vector<double> overhead_dkappa;
      std::vector<double> overhead_x;
      std::vector<double> overhead_y;
      for (std::size_t i = 0; i < anchor_points_.size(); ++i) {
        const auto& p = anchor_points_[i];
        if (i + 1 < start_index) {
          overhead_theta.push_back(p.path_point.theta());
          overhead_kappa.push_back(p.path_point.kappa());
          overhead_dkappa.push_back(p.path_point.dkappa());
          overhead_x.push_back(p.path_point.x());
          overhead_y.push_back(p.path_point.y());
        } else {
          raw_point2d.emplace_back(p.path_point.x(), p.path_point.y());
        }
      }

      fixed_start_point_ = true;
      fixed_start_x_ = anchor_points_[start_index - 1].path_point.x();
      fixed_start_y_ = anchor_points_[start_index - 1].path_point.y();
      fixed_start_theta_ = common::math::NormalizeAngle(
          anchor_points_[start_index - 1].path_point.theta());
      fixed_start_kappa_ = anchor_points_[start_index - 1].path_point.kappa();
      fixed_start_dkappa_ = anchor_points_[start_index - 1].path_point.dkappa();

      Smooth(raw_point2d, &opt_theta, &opt_kappa, &opt_dkappa, &opt_s, &opt_x,
             &opt_y);

      opt_theta.insert(opt_theta.begin(), overhead_theta.begin(),
                       overhead_theta.end());
      opt_kappa.insert(opt_kappa.begin(), overhead_kappa.begin(),
                       overhead_kappa.end());
      opt_dkappa.insert(opt_dkappa.begin(), overhead_dkappa.begin(),
                        overhead_dkappa.end());
      opt_s.insert(opt_s.begin(), overhead_s.begin(), overhead_s.end());
      opt_x.insert(opt_x.begin(), overhead_x.begin(), overhead_x.end());
      opt_y.insert(opt_y.begin(), overhead_y.begin(), overhead_y.end());

      std::for_each(opt_x.begin(), opt_x.end(), [this](double& x){
        x += zero_x_;
      });

      std::for_each(opt_y.begin(), opt_y.end(), [this](double& y){
        y += zero_y_;
      });
    }
  }

  std::vector<common::PathPoint> smoothed_point2d =
      Interpolate(opt_theta, opt_kappa, opt_dkappa, opt_s, opt_x, opt_y,
                  FLAGS_spiral_reference_line_resolution);

  std::vector<ReferencePoint> ref_points;
  for (const auto& p : smoothed_point2d) {
    const double heading = p.theta();
    const double kappa = p.kappa();
    const double dkappa = p.dkappa();

    common::SLPoint ref_sl_point;
    if (!raw_reference_line.XYToSL({p.x(), p.y()}, &ref_sl_point)) {
      return false;
    }
    if (ref_sl_point.s() < 0 ||
        ref_sl_point.s() > raw_reference_line.Length()) {
      continue;
    }

    ReferencePoint rlp = raw_reference_line.GetReferencePoint(ref_sl_point.s());
    ref_points.emplace_back(
        ReferencePoint(hdmap::MapPathPoint(common::math::Vec2d(p.x(), p.y()),
                                           heading, rlp.lane_waypoints()),
                       kappa, dkappa, 0.0, 0.0));
  }

  ReferencePoint::RemoveDuplicates(&ref_points);
  if (ref_points.size() < 2) {
    AERROR << "Fail to generate smoothed reference line.";
    return false;
  }
  *smoothed_reference_line = ReferenceLine(ref_points);
  const double end_timestamp = Clock::NowInSeconds();
  ADEBUG << "Spiral reference line smoother time: "
         << (end_timestamp - start_timestamp) * 1000 << " ms.";
  return true;
}

bool SpiralReferenceLineSmoother::Smooth(std::vector<Eigen::Vector2d> point2d,
                                         std::vector<double>* ptr_theta,
                                         std::vector<double>* ptr_kappa,
                                         std::vector<double>* ptr_dkappa,
                                         std::vector<double>* ptr_s,
                                         std::vector<double>* ptr_x,
                                         std::vector<double>* ptr_y) const {
  CHECK_GT(point2d.size(), 1);

  SpiralProblemInterface* ptop = new SpiralProblemInterface(point2d);
  ptop->set_default_max_point_deviation(default_max_point_deviation_);
  if (fixed_start_point_) {
    ptop->set_start_point(fixed_start_x_, fixed_start_y_, fixed_start_theta_,
                          fixed_start_kappa_, fixed_start_dkappa_);
  }

  Ipopt::SmartPtr<Ipopt::TNLP> problem = ptop;

  // Create an instance of the IpoptApplication
  Ipopt::SmartPtr<Ipopt::IpoptApplication> app = IpoptApplicationFactory();

  app->Options()->SetStringValue("hessian_approximation", "limited-memory");
  //  app->Options()->SetStringValue("derivative_test", "first-order");
  //  app->Options()->SetNumericValue("derivative_test_perturbation", 1.0e-7);
  //  app->Options()->SetStringValue("derivative_test", "second-order");
  app->Options()->SetIntegerValue("print_level", 0);
  int num_iterations = FLAGS_spiral_smoother_num_iteration;
  app->Options()->SetIntegerValue("max_iter", num_iterations);
  app->Options()->SetIntegerValue("acceptable_iter", 5);
  app->Options()->SetNumericValue("tol", 1.0e-4);
  app->Options()->SetNumericValue("acceptable_tol", 1.0e-5);

  Ipopt::ApplicationReturnStatus status = app->Initialize();
  if (status != Ipopt::Solve_Succeeded) {
    ADEBUG << "*** Error during initialization!";
    return static_cast<int>(status);
  }

  status = app->OptimizeTNLP(problem);

  if (status == Ipopt::Solve_Succeeded ||
      status == Ipopt::Solved_To_Acceptable_Level) {
    // Retrieve some statistics about the solve
    Ipopt::Index iter_count = app->Statistics()->IterationCount();
    ADEBUG << "*** The problem solved in " << iter_count << " iterations!";

    Ipopt::Number final_obj = app->Statistics()->FinalObjective();
    ADEBUG << "*** The final value of the objective function is " << final_obj
           << '.';
  } else {
    ADEBUG << "Return status: " << int(status);
  }

  ptop->get_optimization_results(ptr_theta, ptr_kappa, ptr_dkappa, ptr_s, ptr_x,
                                 ptr_y);

  return status == Ipopt::Solve_Succeeded ||
         status == Ipopt::Solved_To_Acceptable_Level;
}

std::vector<common::PathPoint> SpiralReferenceLineSmoother::Interpolate(
    const std::vector<double>& theta, const std::vector<double>& kappa,
    const std::vector<double>& dkappa, const std::vector<double>& s,
    const std::vector<double>& x, const std::vector<double>& y,
    const double resolution) const {
  std::vector<common::PathPoint> smoothed_point2d;
  double start_s = 0.0;
  common::PathPoint first_point =
      to_path_point(x.front(), y.front(), start_s, theta.front(), kappa.front(),
                    dkappa.front());
  smoothed_point2d.push_back(first_point);

  for (std::size_t i = 0; i + 1 < theta.size(); ++i) {
    double start_x = x[i];
    double start_y = y[i];

    auto path_point_seg = Interpolate(
        start_x, start_y, start_s, theta[i], kappa[i], dkappa[i], theta[i + 1],
        kappa[i + 1], dkappa[i + 1], s[i], resolution);

    smoothed_point2d.insert(smoothed_point2d.end(), path_point_seg.begin(),
                            path_point_seg.end());

    start_s = smoothed_point2d.back().s();
  }
  return smoothed_point2d;
}

std::vector<common::PathPoint> SpiralReferenceLineSmoother::Interpolate(
    const double start_x, const double start_y, const double start_s,
    const double theta0, const double kappa0, const double dkappa0,
    const double theta1, const double kappa1, const double dkappa1,
    const double delta_s, const double resolution) const {
  std::vector<common::PathPoint> path_points;

  QuinticSpiralPath spiral_curve(theta0, kappa0, dkappa0, theta1, kappa1,
                                 dkappa1, delta_s);
  std::size_t num_of_points = std::ceil(delta_s / resolution) + 1;
  for (std::size_t i = 1; i <= num_of_points; ++i) {
    const double inter_s = delta_s / num_of_points * i;
    const double dx = spiral_curve.ComputeCartesianDeviationX<10>(inter_s);
    const double dy = spiral_curve.ComputeCartesianDeviationY<10>(inter_s);

    const double theta =
        common::math::NormalizeAngle(spiral_curve.Evaluate(0, inter_s));
    const double kappa = spiral_curve.Evaluate(1, inter_s);
    const double dkappa = spiral_curve.Evaluate(2, inter_s);

    auto path_point = to_path_point(start_x + dx, start_y + dy,
                                    start_s + inter_s, theta, kappa, dkappa);
    path_points.push_back(std::move(path_point));
  }
  return path_points;
}

common::PathPoint SpiralReferenceLineSmoother::to_path_point(
    const double x, const double y, const double s, const double theta,
    const double kappa, const double dkappa) const {
  common::PathPoint point;
  point.set_x(x);
  point.set_y(y);
  point.set_s(s);
  point.set_theta(theta);
  point.set_kappa(kappa);
  point.set_dkappa(dkappa);
  return point;
}

void SpiralReferenceLineSmoother::SetAnchorPoints(
    const std::vector<AnchorPoint>& anchor_points) {
  anchor_points_ = std::move(anchor_points);

  CHECK_GT(anchor_points_.size(), 1);
  zero_x_ = anchor_points_.front().path_point.x();
  zero_y_ = anchor_points_.front().path_point.y();

  std::for_each(anchor_points_.begin(), anchor_points_.end(),
      [this](AnchorPoint& p) {
        auto curr_x = p.path_point.x();
        auto curr_y = p.path_point.y();
        p.path_point.set_x(curr_x - zero_x_);
        p.path_point.set_y(curr_y - zero_y_);});
}

}  // namespace planning
}  // namespace apollo
<|endoftext|>


<commit_before>//
//  Exponent.cpp
//  Calculator
//
//  Created by Gavin Scheele on 3/27/14.
//  Copyright (c) 2014 Gavin Scheele. All rights reserved.
//

#include "Exponential.h"

Exponential::Exponential(Expression* base, Rational* exponent){
    this->type = "exponential";
    this->base = base;
    this->exponent = exponent;
    this->exde = new Integer(exponent->getDenominator());
    if (exde->getValue() != 1) {
    	//if the denominator of the exponent is not 1, make the base a root of the denominator, then setting the denominator equal to 1
    	Integer* baseAsInteger = (Integer *) base;
        base = new nthRoot(exde->getValue(), baseAsInteger->getValue(), 1);
        Integer* one = new Integer(1);
        exponent->setDenominator(one);
    }
    this->exnu = new Integer(exponent->getNumerator());
    if (canExponentiate()) {
    	exponentiate();
    }
}
Exponential::~Exponential(){

}

bool Exponential::canExponentiate() {
    if(base->type == "euler"){
        return false;

    }else if(base->type == "exponential"){
	Exponential* ex = (Exponential *) base;
	this->exponent->multiply(ex->getExponent());
	Integer* numSum = new Integer (1);
	ex->getExponent()->setNumerator(numSum);
        return false;
        // false is returned because the base itself would have already been exponentiated if it were possible

    }else if(base->type == "integer"){
        return true;

    }else if(base->type == "logarithm"){
        return false;

    }else if(base->type == "nthRoot"){
	nthRoot* nr = (nthRoot *) base;
	Rational* r = new Rational(this->exponent->getNumerator(), nr->getRoot()*this->exponent->getDenominator());
	//makes a new exponent, multiplying the denominator by the root, allowing the root to be simplified to one
	this->exponent = r;
	nr->setRoot(1);
	return false;

    }else if(base->type == "pi"){
        return false;

    }else if(base->type == "rational"){
        Rational* r = (Rational *) base;
        if (r->geteNumerator()->type == "integer" && r->geteDenominator()->type == "integer") {
          Exponential* nu = new Exponential(r->geteNumerator(), this->exponent);
          r->setNumerator(nu);
          Exponential* de = new Exponential(r->geteDenominator(), this->exponent);
          r->setDenominator(de);
        }

    }else{
        cout << "type not recognized" << endl;
    }
    return false;
}

void Exponential::exponentiate(){
	Integer* one = new Integer(1);
        Rational* oneRat = new Rational(1, 1);
	if (this->base->type == "rational") {
		Rational* ratBase = (Rational *) this->base;
		Exponential* numAsExponential = new Exponential ((ratBase->geteNumerator()), (this->exponent));  //no matching constructor for exponential
		Exponential* denAsExponential = new Exponential ((ratBase->geteDenominator()), (this->exponent));    //same error
		Rational* newRatBase = new Rational(numAsExponential, denAsExponential);
		this->base = newRatBase;
		this->exponent = oneRat;
	}
	else {

    if (this->exponent->getNumerator()==0) {

       this->exponent=oneRat;
       this->base=one;

    }
    bool toFlip = false;
    if (exnu->getValue()<0) {
	    exnu->setValue(exnu->getValue()*-1);
            toFlip = true;
            //handles negative exponents
    }
    Expression* constantBase = 0;
    if (base->type == "integer") {              //fixed the problem for integers but nothing else
        Integer *a = (Integer *)base;
        constantBase = new Integer(a->getValue());
    }

    while (exponent->getNumerator()>1)
    	{
        base->multiply(constantBase);
        exponent->setNumerator(exponent->geteNumerator()->subtract(one));
    }
    if (toFlip) {
        Integer* one = new Integer(1);
        Rational* mouse = new Rational(one, base);
    	base = mouse;
    }

	}

}

Expression* Exponential::add(Expression* a){
    if(a->type == "euler"){

    }else if(a->type == "exponential"){
	Exponential* ex = (Exponential *) a;
	if ((ex->getBase()->type == this->base->type) && ((this->base->type == "euler") || (this->base->type == "pi"))) {
		if (ex->getExponent()==this->exponent) {
			Integer* two = new Integer(2);
			this->multiply(two);
		}
	}

    }else if(a->type == "integer"){

    }else if(a->type == "logarithm"){

    }else if(a->type == "nthRoot"){

    }else if(a->type == "pi"){

    }else if(a->type == "rational"){

    }else{
        cout << "type not recognized" << endl;
    }
    return this;
}
Expression* Exponential::subtract(Expression* a){
    if(a->type == "euler"){

    }else if(a->type == "exponential"){
	Exponential* ex = (Exponential *) a;
	if ((ex->getBase()->type == this->base->type) && ((this->base->type == "euler") || (this->base->type == "pi"))) {
		if (ex->getExponent()==this->exponent) {
			Integer* zero = new Integer(0);
			this->multiply(zero);
		}
	}
    }else if(a->type == "integer"){

    }else if(a->type == "logarithm"){

    }else if(a->type == "nthRoot"){

    }else if(a->type == "pi"){

    }else if(a->type == "rational"){

    }else{
        cout << "type not recognized" << endl;
    }
    return this;
}
Expression* Exponential::multiply(Expression* a){
	if(a->type == "euler"){
    		if (this->base->type == "euler") {
    			Rational* oneRat = new Rational(1, 1);
			this->exponent->add(oneRat);

    	}

    }else if(a->type == "exponential"){
		Exponential* ex = (Exponential *) a;
	//if (this->base == ex->getBase()) {
	//	this->exponent->add(ex->getExponent());
	//}
	if (ex->base->type == this->base->type) {
		if ((ex->base->type == "euler") || (ex->base->type == "pi")) {
			this->exponent->add(ex->exponent);
		}
	}

    }else if(a->type == "integer"){

    }else if(a->type == "logarithm"){

    }else if(a->type == "nthRoot"){

    }else if(a->type == "pi"){
    	if (this->base->type == "pi") {
    			Rational* oneRat = new Rational(1, 1);
			this->exponent->add(oneRat);
    	}

    }else if(a->type == "rational"){
	Rational* r = (Rational *) a;
	Expression* numToSet = r->geteNumerator();
	numToSet->multiply(this);
	r->setNumerator(numToSet);
	return r;

    }else{
        cout << "type not recognized" << endl;
    }
    return this;
}
Expression* Exponential::divide(Expression* a){
	if(a->type == "euler"){
		if (this->base->type == "euler") {
    				Rational* oneRat = new Rational(1, 1);
				this->exponent->subtract(oneRat);
    		}

    }else if(a->type == "exponential"){
	Exponential* ex = (Exponential *) a;
	//if (this->base == ex->getBase()) {
	//	this->exponent->subtract(ex->getExponent());
	//}
	if (ex->base->type == this->base->type) {
		if ((ex->base->type == "euler") || (ex->base->type == "pi")) {
			this->exponent->subtract(ex->exponent);
		}
	}

    }else if(a->type == "integer"){

    }else if(a->type == "logarithm"){

    }else if(a->type == "nthRoot"){

    }else if(a->type == "pi"){
    	if (this->base->type == "pi") {
    				Rational* oneRat = new Rational(1, 1);
				this->exponent->subtract(oneRat);
    		}

    }else if(a->type == "rational"){
	Rational* r = (Rational *) a;
	Expression* denToSet = r->geteDenominator();
	denToSet->multiply(this);
	r->setDenominator(denToSet);
	return r;

    }else{
        cout << "type not recognized" << endl;
    }
    return this;
}

Rational* Exponential::getExponent() {
    return exponent;
}

Expression* Exponential::getBase() {
    return base;
}

Integer* Exponential::getExnu() {
	return exnu;
}

Integer* Exponential::getExde() {
	return exde;
}

void Exponential::setExnu(Integer* n) {
	exnu = n;
}

void Exponential::setExde(Integer* n) {
	exde = n;
}

void Exponential::setExponent(Rational* e) {
    exponent = e;
}

void Exponential::setBase(Expression* e) {
    base = e;
}

string Exponential::toString() {
    stringstream str;
    if(exponent->getNumerator() == 1 && exponent->getDenominator() == 1){
        str << *base;
    }
    else if(exponent->getDenominator() == 1){
        str << *base << "^" << *exponent->geteNumerator();
    }else{
        str << *base << "^" << *exponent;
    }
    return str.str();
}

ostream& Exponential::print(std::ostream& output) const{
    Exponential *a = (Exponential *)this;
    output << a->toString();
    return output;
}

bool Exponential::canAdd(Expression* b){     //use "this" as comparison. Solver will call someExpression.canAdd(&someOtherExpression)

    if (this->type == b->type && this->type != "logarithm") {
        if (this->type == "nthRoot") {
        }
        return true;
    }else if((this->type == "integer" && b->type == "rational") || (this->type == "rational" && b->type == "integer")){
        return true;
    }else if(this->type == "multiple" && b->type == "multiple"){
        MultipleExpressions *t = (MultipleExpressions *)this;
        MultipleExpressions *m = (MultipleExpressions *)b;
        if ((t->meType == "as" && m->meType == "as") || (t->meType == "md" && m->meType == "md")) {
            return true;
        }
    }else if(this->type == "multiple" || b->type == "multiple") return true;
    return false;
}
bool Exponential::canSubtract(Expression* b){
    if (this->type == b->type) {
        return true;
    }else if((this->type == "integer" && b->type == "rational") || (this->type == "rational" && b->type == "integer")){
        return true;
    }else if(this->type == "multiple" && b->type == "multiple"){
        MultipleExpressions *t = (MultipleExpressions *)this;
        MultipleExpressions *m = (MultipleExpressions *)b;
        if ((t->meType == "as" && m->meType == "as") || (t->meType == "md" && m->meType == "md")) {
            return true;
        }
    }else if(this->type == "multiple" || b->type == "multiple") return true;
    return false;
}
bool Exponential::canMultiply(Expression* b){
    if (this->type == b->type) {
        return true;
    }
    else if (this->base->type == b->type) {
    	return true;
    }
    else if(this->type == "integer" && b->type == "rational") return true;
    else if(this->type == "rational" && b->type == "integer") return true;
    else if(this->type == "multiple" && b->type == "multiple"){
        MultipleExpressions *t = (MultipleExpressions *)this;
        MultipleExpressions *m = (MultipleExpressions *)b;
        if ((t->meType == "as" && m->meType == "as") || (t->meType == "md" && m->meType == "md")) {
            return true;
        }
    }else if(this->type == "multiple" || b->type == "multiple") return true;
    return false;

}
bool Exponential::canDivide(Expression* b){
    if (this->type == b->type) {
        return true;
    }
    else if (this->base->type == b->type) {
    	return true;
    }
    else if(this->type == "integer"){
        if( b->type == "rational") return true;
    }
    else if(this->type == "rational" && b->type == "integer") return true;
    else if(this->type == "multiple" && b->type == "multiple"){
        MultipleExpressions *t = (MultipleExpressions *)this;
        MultipleExpressions *m = (MultipleExpressions *)b;
        if ((t->meType == "as" && m->meType == "as") || (t->meType == "md" && m->meType == "md")) {
            return true;
        }
    }else if(this->type == "multiple" || b->type == "multiple") return true;
    return false;
}
<commit_msg>Fixed zero power case<commit_after>//
//  Exponent.cpp
//  Calculator
//
//  Created by Gavin Scheele on 3/27/14.
//  Copyright (c) 2014 Gavin Scheele. All rights reserved.
//

#include "Exponential.h"

Exponential::Exponential(Expression* base, Rational* exponent){
    this->type = "exponential";
    this->base = base;
    this->exponent = exponent;
    this->exde = new Integer(exponent->getDenominator());
    if (exde->getValue() != 1) {
    	//if the denominator of the exponent is not 1, make the base a root of the denominator, then setting the denominator equal to 1
    	Integer* baseAsInteger = (Integer *) base;
        base = new nthRoot(exde->getValue(), baseAsInteger->getValue(), 1);
        Integer* one = new Integer(1);
        exponent->setDenominator(one);
    }
    this->exnu = new Integer(exponent->getNumerator());
    if (canExponentiate()) {
    	exponentiate();
    }
}
Exponential::~Exponential(){

}

bool Exponential::canExponentiate() {
	this->exponent->getNumerator()
	if (this->exNu == 0) {
		return true;
	}
    if(base->type == "euler"){
        return false;

    }else if(base->type == "exponential"){
	Exponential* ex = (Exponential *) base;
	this->exponent->multiply(ex->getExponent());
	Integer* numSum = new Integer (1);
	ex->getExponent()->setNumerator(numSum);
        return false;
        // false is returned because the base itself would have already been exponentiated if it were possible

    }else if(base->type == "integer"){
        return true;

    }else if(base->type == "logarithm"){
        return false;

    }else if(base->type == "nthRoot"){
	nthRoot* nr = (nthRoot *) base;
	Rational* r = new Rational(this->exponent->getNumerator(), nr->getRoot()*this->exponent->getDenominator());
	//makes a new exponent, multiplying the denominator by the root, allowing the root to be simplified to one
	this->exponent = r;
	nr->setRoot(1);
	return false;

    }else if(base->type == "pi"){
        return false;

    }else if(base->type == "rational"){
        Rational* r = (Rational *) base;
        if (r->geteNumerator()->type == "integer" && r->geteDenominator()->type == "integer") {
          Exponential* nu = new Exponential(r->geteNumerator(), this->exponent);
          r->setNumerator(nu);
          Exponential* de = new Exponential(r->geteDenominator(), this->exponent);
          r->setDenominator(de);
        }

    }else{
        cout << "type not recognized" << endl;
    }
    return false;
}

void Exponential::exponentiate(){
	Integer* one = new Integer(1);
	if (this->exNu == 0) {
		return one;
	}
        Rational* oneRat = new Rational(1, 1);
	if (this->base->type == "rational") {
		Rational* ratBase = (Rational *) this->base;
		Exponential* numAsExponential = new Exponential ((ratBase->geteNumerator()), (this->exponent));  //no matching constructor for exponential
		Exponential* denAsExponential = new Exponential ((ratBase->geteDenominator()), (this->exponent));    //same error
		Rational* newRatBase = new Rational(numAsExponential, denAsExponential);
		this->base = newRatBase;
		this->exponent = oneRat;
	}
	else {

    if (this->exponent->getNumerator()==0) {

       this->exponent=oneRat;
       this->base=one;

    }
    bool toFlip = false;
    if (exnu->getValue()<0) {
	    exnu->setValue(exnu->getValue()*-1);
            toFlip = true;
            //handles negative exponents
    }
    Expression* constantBase = 0;
    if (base->type == "integer") {              //fixed the problem for integers but nothing else
        Integer *a = (Integer *)base;
        constantBase = new Integer(a->getValue());
    }

    while (exponent->getNumerator()>1)
    	{
        base->multiply(constantBase);
        exponent->setNumerator(exponent->geteNumerator()->subtract(one));
    }
    if (toFlip) {
        Integer* one = new Integer(1);
        Rational* mouse = new Rational(one, base);
    	base = mouse;
    }

	}

}

Expression* Exponential::add(Expression* a){
    if(a->type == "euler"){

    }else if(a->type == "exponential"){
	Exponential* ex = (Exponential *) a;
	if ((ex->getBase()->type == this->base->type) && ((this->base->type == "euler") || (this->base->type == "pi"))) {
		if (ex->getExponent()==this->exponent) {
			Integer* two = new Integer(2);
			this->multiply(two);
		}
	}

    }else if(a->type == "integer"){

    }else if(a->type == "logarithm"){

    }else if(a->type == "nthRoot"){

    }else if(a->type == "pi"){

    }else if(a->type == "rational"){

    }else{
        cout << "type not recognized" << endl;
    }
    return this;
}
Expression* Exponential::subtract(Expression* a){
    if(a->type == "euler"){

    }else if(a->type == "exponential"){
	Exponential* ex = (Exponential *) a;
	if ((ex->getBase()->type == this->base->type) && ((this->base->type == "euler") || (this->base->type == "pi"))) {
		if (ex->getExponent()==this->exponent) {
			Integer* zero = new Integer(0);
			this->multiply(zero);
		}
	}
    }else if(a->type == "integer"){

    }else if(a->type == "logarithm"){

    }else if(a->type == "nthRoot"){

    }else if(a->type == "pi"){

    }else if(a->type == "rational"){

    }else{
        cout << "type not recognized" << endl;
    }
    return this;
}
Expression* Exponential::multiply(Expression* a){
	if(a->type == "euler"){
    		if (this->base->type == "euler") {
    			Rational* oneRat = new Rational(1, 1);
			this->exponent->add(oneRat);

    	}

    }else if(a->type == "exponential"){
		Exponential* ex = (Exponential *) a;
	//if (this->base == ex->getBase()) {
	//	this->exponent->add(ex->getExponent());
	//}
	if (ex->base->type == this->base->type) {
		if ((ex->base->type == "euler") || (ex->base->type == "pi")) {
			this->exponent->add(ex->exponent);
		}
	}

    }else if(a->type == "integer"){

    }else if(a->type == "logarithm"){

    }else if(a->type == "nthRoot"){

    }else if(a->type == "pi"){
    	if (this->base->type == "pi") {
    			Rational* oneRat = new Rational(1, 1);
			this->exponent->add(oneRat);
    	}

    }else if(a->type == "rational"){
	Rational* r = (Rational *) a;
	Expression* numToSet = r->geteNumerator();
	numToSet->multiply(this);
	r->setNumerator(numToSet);
	return r;

    }else{
        cout << "type not recognized" << endl;
    }
    return this;
}
Expression* Exponential::divide(Expression* a){
	if(a->type == "euler"){
		if (this->base->type == "euler") {
    				Rational* oneRat = new Rational(1, 1);
				this->exponent->subtract(oneRat);
    		}

    }else if(a->type == "exponential"){
	Exponential* ex = (Exponential *) a;
	//if (this->base == ex->getBase()) {
	//	this->exponent->subtract(ex->getExponent());
	//}
	if (ex->base->type == this->base->type) {
		if ((ex->base->type == "euler") || (ex->base->type == "pi")) {
			this->exponent->subtract(ex->exponent);
		}
	}

    }else if(a->type == "integer"){

    }else if(a->type == "logarithm"){

    }else if(a->type == "nthRoot"){

    }else if(a->type == "pi"){
    	if (this->base->type == "pi") {
    				Rational* oneRat = new Rational(1, 1);
				this->exponent->subtract(oneRat);
    		}

    }else if(a->type == "rational"){
	Rational* r = (Rational *) a;
	Expression* denToSet = r->geteDenominator();
	denToSet->multiply(this);
	r->setDenominator(denToSet);
	return r;

    }else{
        cout << "type not recognized" << endl;
    }
    return this;
}

Rational* Exponential::getExponent() {
    return exponent;
}

Expression* Exponential::getBase() {
    return base;
}

Integer* Exponential::getExnu() {
	return exnu;
}

Integer* Exponential::getExde() {
	return exde;
}

void Exponential::setExnu(Integer* n) {
	exnu = n;
}

void Exponential::setExde(Integer* n) {
	exde = n;
}

void Exponential::setExponent(Rational* e) {
    exponent = e;
}

void Exponential::setBase(Expression* e) {
    base = e;
}

string Exponential::toString() {
    stringstream str;
    if(exponent->getNumerator() == 1 && exponent->getDenominator() == 1){
        str << *base;
    }
    else if(exponent->getDenominator() == 1){
        str << *base << "^" << *exponent->geteNumerator();
    }else{
        str << *base << "^" << *exponent;
    }
    return str.str();
}

ostream& Exponential::print(std::ostream& output) const{
    Exponential *a = (Exponential *)this;
    output << a->toString();
    return output;
}

bool Exponential::canAdd(Expression* b){     //use "this" as comparison. Solver will call someExpression.canAdd(&someOtherExpression)

    if (this->type == b->type && this->type != "logarithm") {
        if (this->type == "nthRoot") {
        }
        return true;
    }else if((this->type == "integer" && b->type == "rational") || (this->type == "rational" && b->type == "integer")){
        return true;
    }else if(this->type == "multiple" && b->type == "multiple"){
        MultipleExpressions *t = (MultipleExpressions *)this;
        MultipleExpressions *m = (MultipleExpressions *)b;
        if ((t->meType == "as" && m->meType == "as") || (t->meType == "md" && m->meType == "md")) {
            return true;
        }
    }else if(this->type == "multiple" || b->type == "multiple") return true;
    return false;
}
bool Exponential::canSubtract(Expression* b){
    if (this->type == b->type) {
        return true;
    }else if((this->type == "integer" && b->type == "rational") || (this->type == "rational" && b->type == "integer")){
        return true;
    }else if(this->type == "multiple" && b->type == "multiple"){
        MultipleExpressions *t = (MultipleExpressions *)this;
        MultipleExpressions *m = (MultipleExpressions *)b;
        if ((t->meType == "as" && m->meType == "as") || (t->meType == "md" && m->meType == "md")) {
            return true;
        }
    }else if(this->type == "multiple" || b->type == "multiple") return true;
    return false;
}
bool Exponential::canMultiply(Expression* b){
    if (this->type == b->type) {
        return true;
    }
    else if (this->base->type == b->type) {
    	return true;
    }
    else if(this->type == "integer" && b->type == "rational") return true;
    else if(this->type == "rational" && b->type == "integer") return true;
    else if(this->type == "multiple" && b->type == "multiple"){
        MultipleExpressions *t = (MultipleExpressions *)this;
        MultipleExpressions *m = (MultipleExpressions *)b;
        if ((t->meType == "as" && m->meType == "as") || (t->meType == "md" && m->meType == "md")) {
            return true;
        }
    }else if(this->type == "multiple" || b->type == "multiple") return true;
    return false;

}
bool Exponential::canDivide(Expression* b){
    if (this->type == b->type) {
        return true;
    }
    else if (this->base->type == b->type) {
    	return true;
    }
    else if(this->type == "integer"){
        if( b->type == "rational") return true;
    }
    else if(this->type == "rational" && b->type == "integer") return true;
    else if(this->type == "multiple" && b->type == "multiple"){
        MultipleExpressions *t = (MultipleExpressions *)this;
        MultipleExpressions *m = (MultipleExpressions *)b;
        if ((t->meType == "as" && m->meType == "as") || (t->meType == "md" && m->meType == "md")) {
            return true;
        }
    }else if(this->type == "multiple" || b->type == "multiple") return true;
    return false;
}
<|endoftext|>


<commit_before>//===----------------------------------------------------------------------===//
//
//                     The LLVM Compiler Infrastructure
//
// This file is dual licensed under the MIT and the University of Illinois Open
// Source Licenses. See LICENSE.TXT for details.
//
//===----------------------------------------------------------------------===//

// <algorithm>

// template<BidirectionalIterator Iter, StrictWeakOrder<auto, Iter::value_type> Compare>
//   requires ShuffleIterator<Iter>
//         && CopyConstructible<Compare>
//   void
//   inplace_merge(Iter first, Iter middle, Iter last, Compare comp);

#include <algorithm>
#include <functional>
#include <random>
#include <cassert>

#include "test_macros.h"

#if TEST_STD_VER >= 11
#include <memory>

struct indirect_less
{
    template <class P>
    bool operator()(const P& x, const P& y)
        {return *x < *y;}
};

struct S {
    S() : i_(0) {}
    S(int i) : i_(i) {}

    S(const S&  rhs) : i_(rhs.i_) {}
    S(      S&& rhs) : i_(rhs.i_) { rhs.i_ = -1; }

    S& operator =(const S&  rhs) { i_ = rhs.i_;              return *this; }
    S& operator =(      S&& rhs) { i_ = rhs.i_; rhs.i_ = -2; assert(this != &rhs); return *this; }
    S& operator =(int i)         { i_ = i;                   return *this; }

    bool operator  <(const S&  rhs) const { return i_ < rhs.i_; }
    bool operator  >(const S&  rhs) const { return i_ > rhs.i_; }
    bool operator ==(const S&  rhs) const { return i_ == rhs.i_; }
    bool operator ==(int i)         const { return i_ == i; }

    void set(int i) { i_ = i; }

    int i_;
    };

#endif  // TEST_STD_VER >= 11

#include "test_iterators.h"
#include "counting_predicates.hpp"

std::mt19937 randomness;

template <class Iter>
void
test_one(unsigned N, unsigned M)
{
    assert(M <= N);
    typedef typename std::iterator_traits<Iter>::value_type value_type;
    value_type* ia = new value_type[N];
    for (unsigned i = 0; i < N; ++i)
        ia[i] = i;
    std::shuffle(ia, ia+N, randomness);
    std::sort(ia, ia+M, std::greater<value_type>());
    std::sort(ia+M, ia+N, std::greater<value_type>());
    binary_counting_predicate<std::greater<value_type>, value_type, value_type> pred((std::greater<value_type>()));
    std::inplace_merge(Iter(ia), Iter(ia+M), Iter(ia+N), std::ref(pred));
    if(N > 0)
    {
        assert(ia[0] == static_cast<int>(N)-1);
        assert(ia[N-1] == 0);
        assert(std::is_sorted(ia, ia+N, std::greater<value_type>()));
        assert(pred.count() <= (N-1));
    }
    delete [] ia;
}

template <class Iter>
void
test(unsigned N)
{
    test_one<Iter>(N, 0);
    test_one<Iter>(N, N/4);
    test_one<Iter>(N, N/2);
    test_one<Iter>(N, 3*N/4);
    test_one<Iter>(N, N);
}

template <class Iter>
void
test()
{
    test_one<Iter>(0, 0);
    test_one<Iter>(1, 0);
    test_one<Iter>(1, 1);
    test_one<Iter>(2, 0);
    test_one<Iter>(2, 1);
    test_one<Iter>(2, 2);
    test_one<Iter>(3, 0);
    test_one<Iter>(3, 1);
    test_one<Iter>(3, 2);
    test_one<Iter>(3, 3);
    test<Iter>(4);
    test<Iter>(20);
    test<Iter>(100);
    test<Iter>(1000);
}

struct less_by_first {
  template <typename Pair>
  bool operator()(const Pair& lhs, const Pair& rhs) {
    return std::less<typename Pair::first_type>()(lhs.first, rhs.first);
  }
};

void test_PR31166 ()
{
    typedef std::pair<int, int> P;
    typedef std::vector<P> V;
    const V vec {{1, 0}, {2, 0}, {2, 1}, {2, 2}, {2, 3}};
    for ( int i = 0; i < 5; ++i ) {
        V res = vec;
        std::inplace_merge(res.begin(), res.begin() + i, res.end(), less_by_first());
        assert(res == vec);
    }
}

int main()
{
    test<bidirectional_iterator<int*> >();
    test<random_access_iterator<int*> >();
    test<int*>();

#if TEST_STD_VER >= 11
    test<bidirectional_iterator<S*> >();
    test<random_access_iterator<S*> >();
    test<S*>();

    {
    int N = 100;
    unsigned M = 50;
    std::unique_ptr<int>* ia = new std::unique_ptr<int>[N];
    for (int i = 0; i < N; ++i)
        ia[i].reset(new int(i));
    std::shuffle(ia, ia+N, randomness);
    std::sort(ia, ia+M, indirect_less());
    std::sort(ia+M, ia+N, indirect_less());
    std::inplace_merge(ia, ia+M, ia+N, indirect_less());
    if(N > 0)
    {
        assert(*ia[0] == 0);
        assert(*ia[N-1] == N-1);
        assert(std::is_sorted(ia, ia+N, indirect_less()));
    }
    delete [] ia;
    }
#endif  // TEST_STD_VER >= 11

    test_PR31166();
}
<commit_msg>Fix test for C++03<commit_after>//===----------------------------------------------------------------------===//
//
//                     The LLVM Compiler Infrastructure
//
// This file is dual licensed under the MIT and the University of Illinois Open
// Source Licenses. See LICENSE.TXT for details.
//
//===----------------------------------------------------------------------===//

// <algorithm>

// template<BidirectionalIterator Iter, StrictWeakOrder<auto, Iter::value_type> Compare>
//   requires ShuffleIterator<Iter>
//         && CopyConstructible<Compare>
//   void
//   inplace_merge(Iter first, Iter middle, Iter last, Compare comp);

#include <algorithm>
#include <functional>
#include <random>
#include <cassert>

#include "test_macros.h"

#if TEST_STD_VER >= 11
#include <memory>

struct indirect_less
{
    template <class P>
    bool operator()(const P& x, const P& y)
        {return *x < *y;}
};

struct S {
    S() : i_(0) {}
    S(int i) : i_(i) {}

    S(const S&  rhs) : i_(rhs.i_) {}
    S(      S&& rhs) : i_(rhs.i_) { rhs.i_ = -1; }

    S& operator =(const S&  rhs) { i_ = rhs.i_;              return *this; }
    S& operator =(      S&& rhs) { i_ = rhs.i_; rhs.i_ = -2; assert(this != &rhs); return *this; }
    S& operator =(int i)         { i_ = i;                   return *this; }

    bool operator  <(const S&  rhs) const { return i_ < rhs.i_; }
    bool operator  >(const S&  rhs) const { return i_ > rhs.i_; }
    bool operator ==(const S&  rhs) const { return i_ == rhs.i_; }
    bool operator ==(int i)         const { return i_ == i; }

    void set(int i) { i_ = i; }

    int i_;
    };

#endif  // TEST_STD_VER >= 11

#include "test_iterators.h"
#include "counting_predicates.hpp"

std::mt19937 randomness;

template <class Iter>
void
test_one(unsigned N, unsigned M)
{
    assert(M <= N);
    typedef typename std::iterator_traits<Iter>::value_type value_type;
    value_type* ia = new value_type[N];
    for (unsigned i = 0; i < N; ++i)
        ia[i] = i;
    std::shuffle(ia, ia+N, randomness);
    std::sort(ia, ia+M, std::greater<value_type>());
    std::sort(ia+M, ia+N, std::greater<value_type>());
    binary_counting_predicate<std::greater<value_type>, value_type, value_type> pred((std::greater<value_type>()));
    std::inplace_merge(Iter(ia), Iter(ia+M), Iter(ia+N), std::ref(pred));
    if(N > 0)
    {
        assert(ia[0] == static_cast<int>(N)-1);
        assert(ia[N-1] == 0);
        assert(std::is_sorted(ia, ia+N, std::greater<value_type>()));
        assert(pred.count() <= (N-1));
    }
    delete [] ia;
}

template <class Iter>
void
test(unsigned N)
{
    test_one<Iter>(N, 0);
    test_one<Iter>(N, N/4);
    test_one<Iter>(N, N/2);
    test_one<Iter>(N, 3*N/4);
    test_one<Iter>(N, N);
}

template <class Iter>
void
test()
{
    test_one<Iter>(0, 0);
    test_one<Iter>(1, 0);
    test_one<Iter>(1, 1);
    test_one<Iter>(2, 0);
    test_one<Iter>(2, 1);
    test_one<Iter>(2, 2);
    test_one<Iter>(3, 0);
    test_one<Iter>(3, 1);
    test_one<Iter>(3, 2);
    test_one<Iter>(3, 3);
    test<Iter>(4);
    test<Iter>(20);
    test<Iter>(100);
    test<Iter>(1000);
}

struct less_by_first {
  template <typename Pair>
  bool operator()(const Pair& lhs, const Pair& rhs) {
    return std::less<typename Pair::first_type>()(lhs.first, rhs.first);
  }
};

void test_PR31166 ()
{
    typedef std::pair<int, int> P;
    typedef std::vector<P> V;
    P vec[5] = {P(1, 0), P(2, 0), P(2, 1), P(2, 2), P(2, 3)};
    for ( int i = 0; i < 5; ++i ) {
        V res(vec, vec + 5);
        std::inplace_merge(res.begin(), res.begin() + i, res.end(), less_by_first());
        assert(res.size() == 5);
        assert(std::equal(res.begin(), res.end(), vec));
    }
}

int main()
{
    test<bidirectional_iterator<int*> >();
    test<random_access_iterator<int*> >();
    test<int*>();

#if TEST_STD_VER >= 11
    test<bidirectional_iterator<S*> >();
    test<random_access_iterator<S*> >();
    test<S*>();

    {
    int N = 100;
    unsigned M = 50;
    std::unique_ptr<int>* ia = new std::unique_ptr<int>[N];
    for (int i = 0; i < N; ++i)
        ia[i].reset(new int(i));
    std::shuffle(ia, ia+N, randomness);
    std::sort(ia, ia+M, indirect_less());
    std::sort(ia+M, ia+N, indirect_less());
    std::inplace_merge(ia, ia+M, ia+N, indirect_less());
    if(N > 0)
    {
        assert(*ia[0] == 0);
        assert(*ia[N-1] == N-1);
        assert(std::is_sorted(ia, ia+N, indirect_less()));
    }
    delete [] ia;
    }
#endif  // TEST_STD_VER >= 11

    test_PR31166();
}
<|endoftext|>


<commit_before>/*M///////////////////////////////////////////////////////////////////////////////////////
//
//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
//  By downloading, copying, installing or using the software you agree to this license.
//  If you do not agree to this license, do not download, install,
//  copy or use the software.
//
//
//                           License Agreement
//                For Open Source Computer Vision Library
//
// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
// Copyright (C) 2009, Willow Garage Inc., all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
//   * Redistribution's of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//
//   * Redistribution's in binary form must reproduce the above copyright notice,
//     this list of conditions and the following disclaimer in the documentation
//     and/or other GpuMaterials provided with the distribution.
//
//   * The name of the copyright holders may not 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 bpied warranties, including, but not limited to, the bpied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation 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.
//
//M*/

#include "precomp.hpp"

using namespace std;
using namespace cv;
using namespace cv::gpu;

#if !defined (HAVE_CUDA) || defined (CUDA_DISABLER)

cv::gpu::PyrLKOpticalFlow::PyrLKOpticalFlow() { throw_nogpu(); }
void cv::gpu::PyrLKOpticalFlow::sparse(const GpuMat&, const GpuMat&, const GpuMat&, GpuMat&, GpuMat&, GpuMat*) { throw_nogpu(); }
void cv::gpu::PyrLKOpticalFlow::dense(const GpuMat&, const GpuMat&, GpuMat&, GpuMat&, GpuMat*) { throw_nogpu(); }
void cv::gpu::PyrLKOpticalFlow::releaseMemory() {}

#else /* !defined (HAVE_CUDA) */

namespace cv { namespace gpu { namespace device
{
    namespace pyrlk
    {
        void loadConstants(int2 winSize, int iters);

        void lkSparse1_gpu(PtrStepSzf I, PtrStepSzf J, const float2* prevPts, float2* nextPts, uchar* status, float* err, int ptcount,
            int level, dim3 block, dim3 patch, cudaStream_t stream = 0);
        void lkSparse4_gpu(PtrStepSz<float4> I, PtrStepSz<float4> J, const float2* prevPts, float2* nextPts, uchar* status, float* err, int ptcount,
            int level, dim3 block, dim3 patch, cudaStream_t stream = 0);

        void lkDense_gpu(PtrStepSzb I, PtrStepSzf J, PtrStepSzf u, PtrStepSzf v, PtrStepSzf prevU, PtrStepSzf prevV,
                         PtrStepSzf err, int2 winSize, cudaStream_t stream = 0);
    }
}}}

cv::gpu::PyrLKOpticalFlow::PyrLKOpticalFlow()
{
    winSize = Size(21, 21);
    maxLevel = 3;
    iters = 30;
    useInitialFlow = false;
    isDeviceArch11_ = !DeviceInfo().supports(FEATURE_SET_COMPUTE_12);
}

namespace
{
    void calcPatchSize(cv::Size winSize, dim3& block, dim3& patch, bool isDeviceArch11)
    {
        if (winSize.width > 32 && winSize.width > 2 * winSize.height)
        {
            block.x = isDeviceArch11 ? 16 : 32;
            block.y = 8;
        }
        else
        {
            block.x = 16;
            block.y = isDeviceArch11 ? 8 : 16;
        }

        patch.x = (winSize.width  + block.x - 1) / block.x;
        patch.y = (winSize.height + block.y - 1) / block.y;

        block.z = patch.z = 1;
    }
}

void cv::gpu::PyrLKOpticalFlow::sparse(const GpuMat& prevImg, const GpuMat& nextImg, const GpuMat& prevPts, GpuMat& nextPts, GpuMat& status, GpuMat* err)
{
    using namespace cv::gpu::device::pyrlk;

    if (prevPts.empty())
    {
        nextPts.release();
        status.release();
        if (err) err->release();
        return;
    }

    dim3 block, patch;
    calcPatchSize(winSize, block, patch, isDeviceArch11_);

    CV_Assert(prevImg.type() == CV_8UC1 || prevImg.type() == CV_8UC3 || prevImg.type() == CV_8UC4);
    CV_Assert(prevImg.size() == nextImg.size() && prevImg.type() == nextImg.type());
    CV_Assert(maxLevel >= 0);
    CV_Assert(winSize.width > 2 && winSize.height > 2);
    CV_Assert(patch.x > 0 && patch.x < 6 && patch.y > 0 && patch.y < 6);
    CV_Assert(prevPts.rows == 1 && prevPts.type() == CV_32FC2);

    if (useInitialFlow)
        CV_Assert(nextPts.size() == prevPts.size() && nextPts.type() == CV_32FC2);
    else
        ensureSizeIsEnough(1, prevPts.cols, prevPts.type(), nextPts);

    GpuMat temp1 = (useInitialFlow ? nextPts : prevPts).reshape(1);
    GpuMat temp2 = nextPts.reshape(1);
    multiply(temp1, Scalar::all(1.0 / (1 << maxLevel) / 2.0), temp2);

    ensureSizeIsEnough(1, prevPts.cols, CV_8UC1, status);
    status.setTo(Scalar::all(1));

    if (err)
        ensureSizeIsEnough(1, prevPts.cols, CV_32FC1, *err);

    // build the image pyramids.

    prevPyr_.resize(maxLevel + 1);
    nextPyr_.resize(maxLevel + 1);

    int cn = prevImg.channels();

    if (cn == 1 || cn == 4)
    {
        prevImg.convertTo(prevPyr_[0], CV_32F);
        nextImg.convertTo(nextPyr_[0], CV_32F);
    }
    else
    {
        cvtColor(prevImg, buf_, COLOR_BGR2BGRA);
        buf_.convertTo(prevPyr_[0], CV_32F);

        cvtColor(nextImg, buf_, COLOR_BGR2BGRA);
        buf_.convertTo(nextPyr_[0], CV_32F);
    }

    for (int level = 1; level <= maxLevel; ++level)
    {
        pyrDown(prevPyr_[level - 1], prevPyr_[level]);
        pyrDown(nextPyr_[level - 1], nextPyr_[level]);
    }

    loadConstants(make_int2(winSize.width, winSize.height), iters);

    for (int level = maxLevel; level >= 0; level--)
    {
        if (cn == 1)
        {
            lkSparse1_gpu(prevPyr_[level], nextPyr_[level],
                prevPts.ptr<float2>(), nextPts.ptr<float2>(), status.ptr(), level == 0 && err ? err->ptr<float>() : 0, prevPts.cols,
                level, block, patch);
        }
        else
        {
            lkSparse4_gpu(prevPyr_[level], nextPyr_[level],
                prevPts.ptr<float2>(), nextPts.ptr<float2>(), status.ptr(), level == 0 && err ? err->ptr<float>() : 0, prevPts.cols,
                level, block, patch);
        }
    }
}

void cv::gpu::PyrLKOpticalFlow::dense(const GpuMat& prevImg, const GpuMat& nextImg, GpuMat& u, GpuMat& v, GpuMat* err)
{
    using namespace cv::gpu::device::pyrlk;

    CV_Assert(prevImg.type() == CV_8UC1);
    CV_Assert(prevImg.size() == nextImg.size() && prevImg.type() == nextImg.type());
    CV_Assert(maxLevel >= 0);
    CV_Assert(winSize.width > 2 && winSize.height > 2);

    if (err)
        err->create(prevImg.size(), CV_32FC1);

    // build the image pyramids.

    prevPyr_.resize(maxLevel + 1);
    nextPyr_.resize(maxLevel + 1);

    prevPyr_[0] = prevImg;
    nextImg.convertTo(nextPyr_[0], CV_32F);

    for (int level = 1; level <= maxLevel; ++level)
    {
        pyrDown(prevPyr_[level - 1], prevPyr_[level]);
        pyrDown(nextPyr_[level - 1], nextPyr_[level]);
    }

    uPyr_.resize(2);
    vPyr_.resize(2);

    ensureSizeIsEnough(prevImg.size(), CV_32FC1, uPyr_[0]);
    ensureSizeIsEnough(prevImg.size(), CV_32FC1, vPyr_[0]);
    ensureSizeIsEnough(prevImg.size(), CV_32FC1, uPyr_[1]);
    ensureSizeIsEnough(prevImg.size(), CV_32FC1, vPyr_[1]);
    uPyr_[1].setTo(Scalar::all(0));
    vPyr_[1].setTo(Scalar::all(0));

    int2 winSize2i = make_int2(winSize.width, winSize.height);
    loadConstants(winSize2i, iters);

    PtrStepSzf derr = err ? *err : PtrStepSzf();

    int idx = 0;

    for (int level = maxLevel; level >= 0; level--)
    {
        int idx2 = (idx + 1) & 1;

        lkDense_gpu(prevPyr_[level], nextPyr_[level], uPyr_[idx], vPyr_[idx], uPyr_[idx2], vPyr_[idx2],
            level == 0 ? derr : PtrStepSzf(), winSize2i);

        if (level > 0)
            idx = idx2;
    }

    uPyr_[idx].copyTo(u);
    vPyr_[idx].copyTo(v);
}

void cv::gpu::PyrLKOpticalFlow::releaseMemory()
{
    prevPyr_.clear();
    nextPyr_.clear();

    buf_.release();

    uPyr_[0].release();
    vPyr_[0].release();

    uPyr_[1].release();
    vPyr_[1].release();
}

#endif /* !defined (HAVE_CUDA) */
<commit_msg>Revert "Merge pull request #141 from jet47/gpu-pyrlk-fix" because this bug appears only in 2.4<commit_after>/*M///////////////////////////////////////////////////////////////////////////////////////
//
//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
//  By downloading, copying, installing or using the software you agree to this license.
//  If you do not agree to this license, do not download, install,
//  copy or use the software.
//
//
//                           License Agreement
//                For Open Source Computer Vision Library
//
// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
// Copyright (C) 2009, Willow Garage Inc., all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
//   * Redistribution's of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//
//   * Redistribution's in binary form must reproduce the above copyright notice,
//     this list of conditions and the following disclaimer in the documentation
//     and/or other GpuMaterials provided with the distribution.
//
//   * The name of the copyright holders may not 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 bpied warranties, including, but not limited to, the bpied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation 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.
//
//M*/

#include "precomp.hpp"

using namespace std;
using namespace cv;
using namespace cv::gpu;

#if !defined (HAVE_CUDA) || defined (CUDA_DISABLER)

cv::gpu::PyrLKOpticalFlow::PyrLKOpticalFlow() { throw_nogpu(); }
void cv::gpu::PyrLKOpticalFlow::sparse(const GpuMat&, const GpuMat&, const GpuMat&, GpuMat&, GpuMat&, GpuMat*) { throw_nogpu(); }
void cv::gpu::PyrLKOpticalFlow::dense(const GpuMat&, const GpuMat&, GpuMat&, GpuMat&, GpuMat*) { throw_nogpu(); }
void cv::gpu::PyrLKOpticalFlow::releaseMemory() {}

#else /* !defined (HAVE_CUDA) */

namespace cv { namespace gpu { namespace device
{
    namespace pyrlk
    {
        void loadConstants(int2 winSize, int iters);

        void lkSparse1_gpu(PtrStepSzf I, PtrStepSzf J, const float2* prevPts, float2* nextPts, uchar* status, float* err, int ptcount,
            int level, dim3 block, dim3 patch, cudaStream_t stream = 0);
        void lkSparse4_gpu(PtrStepSz<float4> I, PtrStepSz<float4> J, const float2* prevPts, float2* nextPts, uchar* status, float* err, int ptcount,
            int level, dim3 block, dim3 patch, cudaStream_t stream = 0);

        void lkDense_gpu(PtrStepSzb I, PtrStepSzf J, PtrStepSzf u, PtrStepSzf v, PtrStepSzf prevU, PtrStepSzf prevV,
                         PtrStepSzf err, int2 winSize, cudaStream_t stream = 0);
    }
}}}

cv::gpu::PyrLKOpticalFlow::PyrLKOpticalFlow()
{
    winSize = Size(21, 21);
    maxLevel = 3;
    iters = 30;
    useInitialFlow = false;
    isDeviceArch11_ = !DeviceInfo().supports(FEATURE_SET_COMPUTE_12);
}

namespace
{
    void calcPatchSize(cv::Size winSize, dim3& block, dim3& patch, bool isDeviceArch11)
    {
        if (winSize.width > 32 && winSize.width > 2 * winSize.height)
        {
            block.x = isDeviceArch11 ? 16 : 32;
            block.y = 8;
        }
        else
        {
            block.x = 16;
            block.y = isDeviceArch11 ? 8 : 16;
        }

        patch.x = (winSize.width  + block.x - 1) / block.x;
        patch.y = (winSize.height + block.y - 1) / block.y;

        block.z = patch.z = 1;
    }
}

void cv::gpu::PyrLKOpticalFlow::sparse(const GpuMat& prevImg, const GpuMat& nextImg, const GpuMat& prevPts, GpuMat& nextPts, GpuMat& status, GpuMat* err)
{
    using namespace cv::gpu::device::pyrlk;

    if (prevPts.empty())
    {
        nextPts.release();
        status.release();
        if (err) err->release();
        return;
    }

    dim3 block, patch;
    calcPatchSize(winSize, block, patch, isDeviceArch11_);

    CV_Assert(prevImg.type() == CV_8UC1 || prevImg.type() == CV_8UC3 || prevImg.type() == CV_8UC4);
    CV_Assert(prevImg.size() == nextImg.size() && prevImg.type() == nextImg.type());
    CV_Assert(maxLevel >= 0);
    CV_Assert(winSize.width > 2 && winSize.height > 2);
    CV_Assert(patch.x > 0 && patch.x < 6 && patch.y > 0 && patch.y < 6);
    CV_Assert(prevPts.rows == 1 && prevPts.type() == CV_32FC2);

    if (useInitialFlow)
        CV_Assert(nextPts.size() == prevPts.size() && nextPts.type() == CV_32FC2);
    else
        ensureSizeIsEnough(1, prevPts.cols, prevPts.type(), nextPts);

    GpuMat temp1 = (useInitialFlow ? nextPts : prevPts).reshape(1);
    GpuMat temp2 = nextPts.reshape(1);
    multiply(temp1, Scalar::all(1.0 / (1 << maxLevel) / 2.0), temp2);

    ensureSizeIsEnough(1, prevPts.cols, CV_8UC1, status);
    status.setTo(Scalar::all(1));

    if (err)
        ensureSizeIsEnough(1, prevPts.cols, CV_32FC1, *err);

    // build the image pyramids.

    prevPyr_.resize(maxLevel + 1);
    nextPyr_.resize(maxLevel + 1);

    int cn = prevImg.channels();

    if (cn == 1 || cn == 4)
    {
        prevImg.convertTo(prevPyr_[0], CV_32F);
        nextImg.convertTo(nextPyr_[0], CV_32F);
    }
    else
    {
        cvtColor(prevImg, buf_, COLOR_BGR2BGRA);
        buf_.convertTo(prevPyr_[0], CV_32F);

        cvtColor(nextImg, buf_, COLOR_BGR2BGRA);
        buf_.convertTo(nextPyr_[0], CV_32F);
    }

    for (int level = 1; level <= maxLevel; ++level)
    {
        pyrDown(prevPyr_[level - 1], prevPyr_[level]);
        pyrDown(nextPyr_[level - 1], nextPyr_[level]);
    }

    loadConstants(make_int2(winSize.width, winSize.height), iters);

    for (int level = maxLevel; level >= 0; level--)
    {
        if (cn == 1)
        {
            lkSparse1_gpu(prevPyr_[level], nextPyr_[level],
                prevPts.ptr<float2>(), nextPts.ptr<float2>(), status.ptr(), level == 0 && err ? err->ptr<float>() : 0, prevPts.cols,
                level, block, patch);
        }
        else
        {
            lkSparse4_gpu(prevPyr_[level], nextPyr_[level],
                prevPts.ptr<float2>(), nextPts.ptr<float2>(), status.ptr(), level == 0 && err ? err->ptr<float>() : 0, prevPts.cols,
                level, block, patch);
        }
    }
}

void cv::gpu::PyrLKOpticalFlow::dense(const GpuMat& prevImg, const GpuMat& nextImg, GpuMat& u, GpuMat& v, GpuMat* err)
{
    using namespace cv::gpu::device::pyrlk;

    CV_Assert(prevImg.type() == CV_8UC1);
    CV_Assert(prevImg.size() == nextImg.size() && prevImg.type() == nextImg.type());
    CV_Assert(maxLevel >= 0);
    CV_Assert(winSize.width > 2 && winSize.height > 2);

    if (err)
        err->create(prevImg.size(), CV_32FC1);

    // build the image pyramids.

    prevPyr_.resize(maxLevel + 1);
    nextPyr_.resize(maxLevel + 1);

    prevPyr_[0] = prevImg;
    nextImg.convertTo(nextPyr_[0], CV_32F);

    for (int level = 1; level <= maxLevel; ++level)
    {
        pyrDown(prevPyr_[level - 1], prevPyr_[level]);
        pyrDown(nextPyr_[level - 1], nextPyr_[level]);
    }

    ensureSizeIsEnough(prevImg.size(), CV_32FC1, uPyr_[0]);
    ensureSizeIsEnough(prevImg.size(), CV_32FC1, vPyr_[0]);
    ensureSizeIsEnough(prevImg.size(), CV_32FC1, uPyr_[1]);
    ensureSizeIsEnough(prevImg.size(), CV_32FC1, vPyr_[1]);
    uPyr_[1].setTo(Scalar::all(0));
    vPyr_[1].setTo(Scalar::all(0));

    int2 winSize2i = make_int2(winSize.width, winSize.height);
    loadConstants(winSize2i, iters);

    PtrStepSzf derr = err ? *err : PtrStepSzf();

    int idx = 0;

    for (int level = maxLevel; level >= 0; level--)
    {
        int idx2 = (idx + 1) & 1;

        lkDense_gpu(prevPyr_[level], nextPyr_[level], uPyr_[idx], vPyr_[idx], uPyr_[idx2], vPyr_[idx2],
            level == 0 ? derr : PtrStepSzf(), winSize2i);

        if (level > 0)
            idx = idx2;
    }

    uPyr_[idx].copyTo(u);
    vPyr_[idx].copyTo(v);
}

void cv::gpu::PyrLKOpticalFlow::releaseMemory()
{
    prevPyr_.clear();
    nextPyr_.clear();

    buf_.release();

    uPyr_[0].release();
    vPyr_[0].release();

    uPyr_[1].release();
    vPyr_[1].release();
}

#endif /* !defined (HAVE_CUDA) */
<|endoftext|>