<commit_before>/*
 * Copyright 2016 The Cartographer Authors
 *
 * 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 "cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h"

#include <algorithm>
#include <array>
#include <cmath>
#include <map>
#include <memory>
#include <string>
#include <vector>

#include "Eigen/Core"
#include "cartographer/common/ceres_solver_options.h"
#include "cartographer/common/make_unique.h"
#include "cartographer/common/math.h"
#include "cartographer/common/time.h"
#include "cartographer/mapping_3d/acceleration_cost_function.h"
#include "cartographer/mapping_3d/ceres_pose.h"
#include "cartographer/mapping_3d/imu_integration.h"
#include "cartographer/mapping_3d/rotation_cost_function.h"
#include "cartographer/mapping_3d/rotation_parameterization.h"
#include "cartographer/mapping_3d/sparse_pose_graph/spa_cost_function.h"
#include "cartographer/transform/transform.h"
#include "ceres/ceres.h"
#include "ceres/jet.h"
#include "ceres/rotation.h"
#include "glog/logging.h"

namespace cartographer {
namespace mapping_3d {
namespace sparse_pose_graph {

OptimizationProblem::OptimizationProblem(
    const mapping::sparse_pose_graph::proto::OptimizationProblemOptions&
        options,
    FixZ fix_z)
    : options_(options), fix_z_(fix_z) {}

OptimizationProblem::~OptimizationProblem() {}

void OptimizationProblem::AddImuData(const int trajectory_id,
                                     const sensor::ImuData& imu_data) {
  CHECK_GE(trajectory_id, 0);
  imu_data_.resize(
      std::max(imu_data_.size(), static_cast<size_t>(trajectory_id) + 1));
  imu_data_[trajectory_id].push_back(imu_data);
}

void OptimizationProblem::AddFixedFramePoseData(
    const int trajectory_id,
    const sensor::FixedFramePoseData& fixed_frame_pose_data) {
  CHECK_GE(trajectory_id, 0);
  fixed_frame_pose_data_.resize(std::max(
      fixed_frame_pose_data_.size(), static_cast<size_t>(trajectory_id) + 1));
  fixed_frame_pose_data_[trajectory_id].Push(fixed_frame_pose_data.time,
                                             fixed_frame_pose_data.pose);
}

void OptimizationProblem::AddTrajectoryNode(
    const int trajectory_id, const common::Time time,
    const transform::Rigid3d& point_cloud_pose) {
  CHECK_GE(trajectory_id, 0);
  node_data_.resize(
      std::max(node_data_.size(), static_cast<size_t>(trajectory_id) + 1));
  node_data_[trajectory_id].push_back(NodeData{time, point_cloud_pose});
}

void OptimizationProblem::AddSubmap(const int trajectory_id,
                                    const transform::Rigid3d& submap_pose) {
  CHECK_GE(trajectory_id, 0);
  submap_data_.resize(
      std::max(submap_data_.size(), static_cast<size_t>(trajectory_id) + 1));
  submap_data_[trajectory_id].push_back(SubmapData{submap_pose});
}

void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) {
  options_.mutable_ceres_solver_options()->set_max_num_iterations(
      max_num_iterations);
}

void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
                                const std::set<int>& frozen_trajectories) {
  if (node_data_.empty()) {
    // Nothing to optimize.
    return;
  }

  ceres::Problem::Options problem_options;
  ceres::Problem problem(problem_options);

  const auto translation_parameterization =
      [this]() -> std::unique_ptr<ceres::LocalParameterization> {
    return fix_z_ == FixZ::kYes
               ? common::make_unique<ceres::SubsetParameterization>(
                     3, std::vector<int>{2})
               : nullptr;
  };

  // Set the starting point.
  CHECK(!submap_data_.empty());
  CHECK(!submap_data_[0].empty());
  // TODO(hrapp): Move ceres data into SubmapData.
  std::vector<std::deque<CeresPose>> C_submaps(submap_data_.size());
  std::vector<std::deque<CeresPose>> C_nodes(node_data_.size());
  for (size_t trajectory_id = 0; trajectory_id != submap_data_.size();
       ++trajectory_id) {
    const bool frozen = frozen_trajectories.count(trajectory_id);
    for (size_t submap_index = 0;
         submap_index != submap_data_[trajectory_id].size(); ++submap_index) {
      if (trajectory_id == 0 && submap_index == 0) {
        // Tie the first submap of the first trajectory to the origin.
        C_submaps[trajectory_id].emplace_back(
            transform::Rigid3d::Identity(), translation_parameterization(),
            common::make_unique<ceres::AutoDiffLocalParameterization<
                ConstantYawQuaternionPlus, 4, 2>>(),
            &problem);
        problem.SetParameterBlockConstant(
            C_submaps[trajectory_id].back().translation());
      } else {
        C_submaps[trajectory_id].emplace_back(
            submap_data_[trajectory_id][submap_index].pose,
            translation_parameterization(),
            common::make_unique<ceres::QuaternionParameterization>(), &problem);
      }
      if (frozen) {
        problem.SetParameterBlockConstant(
            C_submaps[trajectory_id].back().rotation());
        problem.SetParameterBlockConstant(
            C_submaps[trajectory_id].back().translation());
      }
    }
  }
  for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
       ++trajectory_id) {
    const bool frozen = frozen_trajectories.count(trajectory_id);
    for (size_t node_index = 0; node_index != node_data_[trajectory_id].size();
         ++node_index) {
      C_nodes[trajectory_id].emplace_back(
          node_data_[trajectory_id][node_index].point_cloud_pose,
          translation_parameterization(),
          common::make_unique<ceres::QuaternionParameterization>(), &problem);
      if (frozen) {
        problem.SetParameterBlockConstant(
            C_nodes[trajectory_id].back().rotation());
        problem.SetParameterBlockConstant(
            C_nodes[trajectory_id].back().translation());
      }
    }
  }

  // Add cost functions for intra- and inter-submap constraints.
  for (const Constraint& constraint : constraints) {
    problem.AddResidualBlock(
        new ceres::AutoDiffCostFunction<SpaCostFunction, 6, 4, 3, 4, 3>(
            new SpaCostFunction(constraint.pose)),
        // Only loop closure constraints should have a loss function.
        constraint.tag == Constraint::INTER_SUBMAP
            ? new ceres::HuberLoss(options_.huber_scale())
            : nullptr,
        C_submaps.at(constraint.submap_id.trajectory_id)
            .at(constraint.submap_id.submap_index)
            .rotation(),
        C_submaps.at(constraint.submap_id.trajectory_id)
            .at(constraint.submap_id.submap_index)
            .translation(),
        C_nodes.at(constraint.node_id.trajectory_id)
            .at(constraint.node_id.node_index)
            .rotation(),
        C_nodes.at(constraint.node_id.trajectory_id)
            .at(constraint.node_id.node_index)
            .translation());
  }

  // Add constraints based on IMU observations of angular velocities and
  // linear acceleration.
  trajectory_data_.resize(imu_data_.size());
  CHECK_GE(trajectory_data_.size(), node_data_.size());
  for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
       ++trajectory_id) {
    const auto& node_data = node_data_[trajectory_id];
    if (node_data.empty()) {
      // We skip empty trajectories which might not have any IMU data.
      continue;
    }
    TrajectoryData& trajectory_data = trajectory_data_.at(trajectory_id);
    problem.AddParameterBlock(trajectory_data.imu_calibration.data(), 4,
                              new ceres::QuaternionParameterization());
    const std::deque<sensor::ImuData>& imu_data = imu_data_.at(trajectory_id);
    CHECK(!imu_data.empty());

    // Skip IMU data before the first node of this trajectory.
    auto it = imu_data.cbegin();
    while ((it + 1) != imu_data.cend() && (it + 1)->time <= node_data[0].time) {
      ++it;
    }

    for (size_t node_index = 1; node_index < node_data.size(); ++node_index) {
      auto it2 = it;
      const IntegrateImuResult<double> result =
          IntegrateImu(imu_data, node_data[node_index - 1].time,
                       node_data[node_index].time, &it);
      if (node_index + 1 < node_data.size()) {
        const common::Time first_time = node_data[node_index - 1].time;
        const common::Time second_time = node_data[node_index].time;
        const common::Time third_time = node_data[node_index + 1].time;
        const common::Duration first_duration = second_time - first_time;
        const common::Duration second_duration = third_time - second_time;
        const common::Time first_center = first_time + first_duration / 2;
        const common::Time second_center = second_time + second_duration / 2;
        const IntegrateImuResult<double> result_to_first_center =
            IntegrateImu(imu_data, first_time, first_center, &it2);
        const IntegrateImuResult<double> result_center_to_center =
            IntegrateImu(imu_data, first_center, second_center, &it2);
        // 'delta_velocity' is the change in velocity from the point in time
        // halfway between the first and second poses to halfway between second
        // and third pose. It is computed from IMU data and still contains a
        // delta due to gravity. The orientation of this vector is in the IMU
        // frame at the second pose.
        const Eigen::Vector3d delta_velocity =
            (result.delta_rotation.inverse() *
             result_to_first_center.delta_rotation) *
            result_center_to_center.delta_velocity;
        problem.AddResidualBlock(
            new ceres::AutoDiffCostFunction<AccelerationCostFunction, 3, 4, 3,
                                            3, 3, 1, 4>(
                new AccelerationCostFunction(
                    options_.acceleration_weight(), delta_velocity,
                    common::ToSeconds(first_duration),
                    common::ToSeconds(second_duration))),
            nullptr, C_nodes[trajectory_id].at(node_index).rotation(),
            C_nodes[trajectory_id].at(node_index - 1).translation(),
            C_nodes[trajectory_id].at(node_index).translation(),
            C_nodes[trajectory_id].at(node_index + 1).translation(),
            &trajectory_data.gravity_constant,
            trajectory_data.imu_calibration.data());
      }
      problem.AddResidualBlock(
          new ceres::AutoDiffCostFunction<RotationCostFunction, 3, 4, 4, 4>(
              new RotationCostFunction(options_.rotation_weight(),
                                       result.delta_rotation)),
          nullptr, C_nodes[trajectory_id].at(node_index - 1).rotation(),
          C_nodes[trajectory_id].at(node_index).rotation(),
          trajectory_data.imu_calibration.data());
    }
  }

  // Solve.
  ceres::Solver::Summary summary;
  ceres::Solve(
      common::CreateCeresSolverOptions(options_.ceres_solver_options()),
      &problem, &summary);

  if (options_.log_solver_summary()) {
    LOG(INFO) << summary.FullReport();
    for (size_t trajectory_id = 0; trajectory_id != trajectory_data_.size();
         ++trajectory_id) {
      if (trajectory_id != 0) {
        LOG(INFO) << "Trajectory " << trajectory_id << ":";
      }
      LOG(INFO) << "Gravity was: "
                << trajectory_data_[trajectory_id].gravity_constant;
      const auto& imu_calibration =
          trajectory_data_[trajectory_id].imu_calibration;
      LOG(INFO) << "IMU correction was: "
                << common::RadToDeg(2. * std::acos(imu_calibration[0]))
                << " deg (" << imu_calibration[0] << ", " << imu_calibration[1]
                << ", " << imu_calibration[2] << ", " << imu_calibration[3]
                << ")";
    }
  }

  // Store the result.
  for (size_t trajectory_id = 0; trajectory_id != submap_data_.size();
       ++trajectory_id) {
    for (size_t submap_index = 0;
         submap_index != submap_data_[trajectory_id].size(); ++submap_index) {
      submap_data_[trajectory_id][submap_index].pose =
          C_submaps[trajectory_id][submap_index].ToRigid();
    }
  }
  for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
       ++trajectory_id) {
    for (size_t node_index = 0; node_index != node_data_[trajectory_id].size();
         ++node_index) {
      node_data_[trajectory_id][node_index].point_cloud_pose =
          C_nodes[trajectory_id][node_index].ToRigid();
    }
  }
}

const std::vector<std::vector<NodeData>>& OptimizationProblem::node_data()
    const {
  return node_data_;
}

const std::vector<std::vector<SubmapData>>& OptimizationProblem::submap_data()
    const {
  return submap_data_;
}

}  // namespace sparse_pose_graph
}  // namespace mapping_3d
}  // namespace cartographer
<commit_msg>Add fixed frame pose constraints to 3D problem. (#480)<commit_after>/*
 * Copyright 2016 The Cartographer Authors
 *
 * 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 "cartographer/mapping_3d/sparse_pose_graph/optimization_problem.h"

#include <algorithm>
#include <array>
#include <cmath>
#include <map>
#include <memory>
#include <string>
#include <vector>

#include "Eigen/Core"
#include "cartographer/common/ceres_solver_options.h"
#include "cartographer/common/make_unique.h"
#include "cartographer/common/math.h"
#include "cartographer/common/time.h"
#include "cartographer/mapping_3d/acceleration_cost_function.h"
#include "cartographer/mapping_3d/ceres_pose.h"
#include "cartographer/mapping_3d/imu_integration.h"
#include "cartographer/mapping_3d/rotation_cost_function.h"
#include "cartographer/mapping_3d/rotation_parameterization.h"
#include "cartographer/mapping_3d/sparse_pose_graph/spa_cost_function.h"
#include "cartographer/transform/transform.h"
#include "ceres/ceres.h"
#include "ceres/jet.h"
#include "ceres/rotation.h"
#include "glog/logging.h"

namespace cartographer {
namespace mapping_3d {
namespace sparse_pose_graph {

OptimizationProblem::OptimizationProblem(
    const mapping::sparse_pose_graph::proto::OptimizationProblemOptions&
        options,
    FixZ fix_z)
    : options_(options), fix_z_(fix_z) {}

OptimizationProblem::~OptimizationProblem() {}

void OptimizationProblem::AddImuData(const int trajectory_id,
                                     const sensor::ImuData& imu_data) {
  CHECK_GE(trajectory_id, 0);
  imu_data_.resize(
      std::max(imu_data_.size(), static_cast<size_t>(trajectory_id) + 1));
  imu_data_[trajectory_id].push_back(imu_data);
}

void OptimizationProblem::AddFixedFramePoseData(
    const int trajectory_id,
    const sensor::FixedFramePoseData& fixed_frame_pose_data) {
  CHECK_GE(trajectory_id, 0);
  fixed_frame_pose_data_.resize(std::max(
      fixed_frame_pose_data_.size(), static_cast<size_t>(trajectory_id) + 1));
  fixed_frame_pose_data_[trajectory_id].Push(fixed_frame_pose_data.time,
                                             fixed_frame_pose_data.pose);
}

void OptimizationProblem::AddTrajectoryNode(
    const int trajectory_id, const common::Time time,
    const transform::Rigid3d& point_cloud_pose) {
  CHECK_GE(trajectory_id, 0);
  node_data_.resize(
      std::max(node_data_.size(), static_cast<size_t>(trajectory_id) + 1));
  node_data_[trajectory_id].push_back(NodeData{time, point_cloud_pose});
}

void OptimizationProblem::AddSubmap(const int trajectory_id,
                                    const transform::Rigid3d& submap_pose) {
  CHECK_GE(trajectory_id, 0);
  submap_data_.resize(
      std::max(submap_data_.size(), static_cast<size_t>(trajectory_id) + 1));
  submap_data_[trajectory_id].push_back(SubmapData{submap_pose});
}

void OptimizationProblem::SetMaxNumIterations(const int32 max_num_iterations) {
  options_.mutable_ceres_solver_options()->set_max_num_iterations(
      max_num_iterations);
}

void OptimizationProblem::Solve(const std::vector<Constraint>& constraints,
                                const std::set<int>& frozen_trajectories) {
  if (node_data_.empty()) {
    // Nothing to optimize.
    return;
  }

  ceres::Problem::Options problem_options;
  ceres::Problem problem(problem_options);

  const auto translation_parameterization =
      [this]() -> std::unique_ptr<ceres::LocalParameterization> {
    return fix_z_ == FixZ::kYes
               ? common::make_unique<ceres::SubsetParameterization>(
                     3, std::vector<int>{2})
               : nullptr;
  };

  // Set the starting point.
  CHECK(!submap_data_.empty());
  CHECK(!submap_data_[0].empty());
  // TODO(hrapp): Move ceres data into SubmapData.
  std::vector<std::deque<CeresPose>> C_submaps(submap_data_.size());
  std::vector<std::deque<CeresPose>> C_nodes(node_data_.size());
  for (size_t trajectory_id = 0; trajectory_id != submap_data_.size();
       ++trajectory_id) {
    const bool frozen = frozen_trajectories.count(trajectory_id);
    for (size_t submap_index = 0;
         submap_index != submap_data_[trajectory_id].size(); ++submap_index) {
      if (trajectory_id == 0 && submap_index == 0) {
        // Tie the first submap of the first trajectory to the origin.
        C_submaps[trajectory_id].emplace_back(
            transform::Rigid3d::Identity(), translation_parameterization(),
            common::make_unique<ceres::AutoDiffLocalParameterization<
                ConstantYawQuaternionPlus, 4, 2>>(),
            &problem);
        problem.SetParameterBlockConstant(
            C_submaps[trajectory_id].back().translation());
      } else {
        C_submaps[trajectory_id].emplace_back(
            submap_data_[trajectory_id][submap_index].pose,
            translation_parameterization(),
            common::make_unique<ceres::QuaternionParameterization>(), &problem);
      }
      if (frozen) {
        problem.SetParameterBlockConstant(
            C_submaps[trajectory_id].back().rotation());
        problem.SetParameterBlockConstant(
            C_submaps[trajectory_id].back().translation());
      }
    }
  }
  for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
       ++trajectory_id) {
    const bool frozen = frozen_trajectories.count(trajectory_id);
    for (size_t node_index = 0; node_index != node_data_[trajectory_id].size();
         ++node_index) {
      C_nodes[trajectory_id].emplace_back(
          node_data_[trajectory_id][node_index].point_cloud_pose,
          translation_parameterization(),
          common::make_unique<ceres::QuaternionParameterization>(), &problem);
      if (frozen) {
        problem.SetParameterBlockConstant(
            C_nodes[trajectory_id].back().rotation());
        problem.SetParameterBlockConstant(
            C_nodes[trajectory_id].back().translation());
      }
    }
  }

  // Add cost functions for intra- and inter-submap constraints.
  for (const Constraint& constraint : constraints) {
    problem.AddResidualBlock(
        new ceres::AutoDiffCostFunction<SpaCostFunction, 6, 4, 3, 4, 3>(
            new SpaCostFunction(constraint.pose)),
        // Only loop closure constraints should have a loss function.
        constraint.tag == Constraint::INTER_SUBMAP
            ? new ceres::HuberLoss(options_.huber_scale())
            : nullptr,
        C_submaps.at(constraint.submap_id.trajectory_id)
            .at(constraint.submap_id.submap_index)
            .rotation(),
        C_submaps.at(constraint.submap_id.trajectory_id)
            .at(constraint.submap_id.submap_index)
            .translation(),
        C_nodes.at(constraint.node_id.trajectory_id)
            .at(constraint.node_id.node_index)
            .rotation(),
        C_nodes.at(constraint.node_id.trajectory_id)
            .at(constraint.node_id.node_index)
            .translation());
  }

  // Add constraints based on IMU observations of angular velocities and
  // linear acceleration.
  trajectory_data_.resize(imu_data_.size());
  CHECK_GE(trajectory_data_.size(), node_data_.size());
  for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
       ++trajectory_id) {
    const auto& node_data = node_data_[trajectory_id];
    if (node_data.empty()) {
      // We skip empty trajectories which might not have any IMU data.
      continue;
    }
    TrajectoryData& trajectory_data = trajectory_data_.at(trajectory_id);
    problem.AddParameterBlock(trajectory_data.imu_calibration.data(), 4,
                              new ceres::QuaternionParameterization());
    const std::deque<sensor::ImuData>& imu_data = imu_data_.at(trajectory_id);
    CHECK(!imu_data.empty());

    // Skip IMU data before the first node of this trajectory.
    auto it = imu_data.cbegin();
    while ((it + 1) != imu_data.cend() && (it + 1)->time <= node_data[0].time) {
      ++it;
    }

    for (size_t node_index = 1; node_index < node_data.size(); ++node_index) {
      auto it2 = it;
      const IntegrateImuResult<double> result =
          IntegrateImu(imu_data, node_data[node_index - 1].time,
                       node_data[node_index].time, &it);
      if (node_index + 1 < node_data.size()) {
        const common::Time first_time = node_data[node_index - 1].time;
        const common::Time second_time = node_data[node_index].time;
        const common::Time third_time = node_data[node_index + 1].time;
        const common::Duration first_duration = second_time - first_time;
        const common::Duration second_duration = third_time - second_time;
        const common::Time first_center = first_time + first_duration / 2;
        const common::Time second_center = second_time + second_duration / 2;
        const IntegrateImuResult<double> result_to_first_center =
            IntegrateImu(imu_data, first_time, first_center, &it2);
        const IntegrateImuResult<double> result_center_to_center =
            IntegrateImu(imu_data, first_center, second_center, &it2);
        // 'delta_velocity' is the change in velocity from the point in time
        // halfway between the first and second poses to halfway between second
        // and third pose. It is computed from IMU data and still contains a
        // delta due to gravity. The orientation of this vector is in the IMU
        // frame at the second pose.
        const Eigen::Vector3d delta_velocity =
            (result.delta_rotation.inverse() *
             result_to_first_center.delta_rotation) *
            result_center_to_center.delta_velocity;
        problem.AddResidualBlock(
            new ceres::AutoDiffCostFunction<AccelerationCostFunction, 3, 4, 3,
                                            3, 3, 1, 4>(
                new AccelerationCostFunction(
                    options_.acceleration_weight(), delta_velocity,
                    common::ToSeconds(first_duration),
                    common::ToSeconds(second_duration))),
            nullptr, C_nodes[trajectory_id].at(node_index).rotation(),
            C_nodes[trajectory_id].at(node_index - 1).translation(),
            C_nodes[trajectory_id].at(node_index).translation(),
            C_nodes[trajectory_id].at(node_index + 1).translation(),
            &trajectory_data.gravity_constant,
            trajectory_data.imu_calibration.data());
      }
      problem.AddResidualBlock(
          new ceres::AutoDiffCostFunction<RotationCostFunction, 3, 4, 4, 4>(
              new RotationCostFunction(options_.rotation_weight(),
                                       result.delta_rotation)),
          nullptr, C_nodes[trajectory_id].at(node_index - 1).rotation(),
          C_nodes[trajectory_id].at(node_index).rotation(),
          trajectory_data.imu_calibration.data());
    }
  }

  // Add fixed frame pose constraints.
  std::deque<CeresPose> C_fixed_frames;
  for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
       ++trajectory_id) {
    if (trajectory_id >= fixed_frame_pose_data_.size()) {
      break;
    }

    bool fixed_frame_pose_initialized = false;

    const auto& node_data = node_data_[trajectory_id];
    for (size_t node_index = 0; node_index < node_data.size(); ++node_index) {
      if (!fixed_frame_pose_data_.at(trajectory_id)
               .Has(node_data[node_index].time)) {
        continue;
      }

      const mapping::SparsePoseGraph::Constraint::Pose constraint_pose{
          fixed_frame_pose_data_.at(trajectory_id)
              .Lookup(node_data[node_index].time),
          options_.fixed_frame_pose_translation_weight(),
          options_.fixed_frame_pose_rotation_weight()};

      if (!fixed_frame_pose_initialized) {
        const transform::Rigid3d fixed_frame_pose_in_map =
            node_data[node_index].point_cloud_pose *
            constraint_pose.zbar_ij.inverse();
        C_fixed_frames.emplace_back(
            transform::Rigid3d(
                fixed_frame_pose_in_map.translation(),
                Eigen::AngleAxisd(
                    transform::GetYaw(fixed_frame_pose_in_map.rotation()),
                    Eigen::Vector3d::UnitZ())),
            nullptr,
            common::make_unique<ceres::AutoDiffLocalParameterization<
                YawOnlyQuaternionPlus, 4, 1>>(),
            &problem);
        fixed_frame_pose_initialized = true;
      }

      problem.AddResidualBlock(
          new ceres::AutoDiffCostFunction<SpaCostFunction, 6, 4, 3, 4, 3>(
              new SpaCostFunction(constraint_pose)),
          nullptr, C_fixed_frames.back().rotation(),
          C_fixed_frames.back().translation(),
          C_nodes.at(trajectory_id).at(node_index).rotation(),
          C_nodes.at(trajectory_id).at(node_index).translation());
    }
  }

  // Solve.
  ceres::Solver::Summary summary;
  ceres::Solve(
      common::CreateCeresSolverOptions(options_.ceres_solver_options()),
      &problem, &summary);

  if (options_.log_solver_summary()) {
    LOG(INFO) << summary.FullReport();
    for (size_t trajectory_id = 0; trajectory_id != trajectory_data_.size();
         ++trajectory_id) {
      if (trajectory_id != 0) {
        LOG(INFO) << "Trajectory " << trajectory_id << ":";
      }
      LOG(INFO) << "Gravity was: "
                << trajectory_data_[trajectory_id].gravity_constant;
      const auto& imu_calibration =
          trajectory_data_[trajectory_id].imu_calibration;
      LOG(INFO) << "IMU correction was: "
                << common::RadToDeg(2. * std::acos(imu_calibration[0]))
                << " deg (" << imu_calibration[0] << ", " << imu_calibration[1]
                << ", " << imu_calibration[2] << ", " << imu_calibration[3]
                << ")";
    }
  }

  // Store the result.
  for (size_t trajectory_id = 0; trajectory_id != submap_data_.size();
       ++trajectory_id) {
    for (size_t submap_index = 0;
         submap_index != submap_data_[trajectory_id].size(); ++submap_index) {
      submap_data_[trajectory_id][submap_index].pose =
          C_submaps[trajectory_id][submap_index].ToRigid();
    }
  }
  for (size_t trajectory_id = 0; trajectory_id != node_data_.size();
       ++trajectory_id) {
    for (size_t node_index = 0; node_index != node_data_[trajectory_id].size();
         ++node_index) {
      node_data_[trajectory_id][node_index].point_cloud_pose =
          C_nodes[trajectory_id][node_index].ToRigid();
    }
  }
}

const std::vector<std::vector<NodeData>>& OptimizationProblem::node_data()
    const {
  return node_data_;
}

const std::vector<std::vector<SubmapData>>& OptimizationProblem::submap_data()
    const {
  return submap_data_;
}

}  // namespace sparse_pose_graph
}  // namespace mapping_3d
}  // namespace cartographer
<|endoftext|>


<commit_before>#include <libport/unistd.h>
#include <fcntl.h>

#include <libport/assert.hh>
#include <libport/compiler.hh>
#include <libport/thread.hh>
#include <libport/unistd.h>

#include <liburbi/compatibility.hh>
#include <urbi/uconversion.hh>
#include <urbi/usyncclient.hh>

namespace urbi
{

  const USyncClient::options USyncClient::options::default_options =
    USyncClient::options();

  USyncClient::options::options()
    : timeout_(0)
    , mtag_(0)
    , mmod_(0)
  {}

  USyncClient::options&
  USyncClient::options::timeout(libport::utime_t usec)
  {
    timeout_ = usec;
    return *this;
  }

  USyncClient::options&
  USyncClient::options::tag(const char* tag, const char* mod)
  {
    mtag_ = tag;
    mmod_ = mod;
    return *this;
  }

  USyncClient::USyncClient(const std::string& host,
			   unsigned port,
			   size_t buflen,
			   bool server,
                           bool startCallbackThread,
			   unsigned semListenInc)
    : UClient(host, port, buflen, server, semListenInc)
    , sem_()
    , queueLock_()
    , message_(0)
    , syncLock_()
    , syncTag()
    , default_options_()
    , stopCallbackThread_(!startCallbackThread)
    , cbThread(0)
  {
    if (error())
      return;

    if (startCallbackThread)
      cbThread = libport::startThread(this, &USyncClient::callbackThread);
    if (!defaultClient)
      defaultClient = this;

    listenSem_++;
    callbackSem_++;
  }

  USyncClient::~USyncClient()
  {
    closeUClient();

    if (cbThread)
      joinCallbackThread_();
  }

  void USyncClient::callbackThread()
  {
    callbackSem_--;

    while (true)
    {
      sem_--;
      if (stopCallbackThread_)
      {
	// The call to stopCallbackThread is
	// waiting on stopCallbackSem_.
	stopCallbackSem_++;
	return;
      }
      queueLock_.lock();
      if (queue.empty())
      {
	// Only explanation: processEvents from another thread stole our
	// message.
	sem_++; // Give back the token we took without popping a message.
	queueLock_.unlock();
	continue;
      }
      UMessage *m = queue.front();
      queue.pop_front();
      queueLock_.unlock();
      UAbstractClient::notifyCallbacks(*m);
      delete m;
    }
  }

  void USyncClient::stopCallbackThread()
  {
    if (stopCallbackThread_)
      return;
    stopCallbackThread_ = true;
    sem_++;
    // Wait until the callback thread is actually stopped to avoid both
    // processEvents and the callbackThread running at the same time.
    stopCallbackSem_--;
  }

  bool USyncClient::processEvents(libport::utime_t timeout)
  {
    bool res = false;
    libport::utime_t startTime = libport::utime();
    while (timeout < 0 || libport::utime() - startTime <= timeout)
    {
      queueLock_.lock();
      if (queue.empty())
      {
	queueLock_.unlock();
	return res;
      }
      res = true;
      UMessage *m = queue.front();
      queue.pop_front();
      sem_--; // Will not block since queue was not empty.
      queueLock_.unlock();
      UAbstractClient::notifyCallbacks(*m);
      delete m;
    }
    return res;
  }

  int USyncClient::joinCallbackThread_()
  {
    stopCallbackThread();
    if (cbThread)
    {
      pthread_join(cbThread, 0);
      cbThread = 0;
    }
    return 0;
  }

  void
  USyncClient::notifyCallbacks(const UMessage& msg)
  {
    queueLock_.lock();
    // If waiting for a tag, pass it to the user.
    if (!syncTag.empty() && syncTag == msg.tag)
    {
      message_ = new UMessage(msg);
      syncTag.clear();
      syncLock_++;
    }
    else
    {
      queue.push_back(new UMessage(msg));
      sem_++;
    }
    queueLock_.unlock();
  }

  UMessage*
  USyncClient::waitForTag(const std::string& tag, libport::utime_t useconds)
  {
    syncTag = tag;
    queueLock_.unlock();
    // syncTag is reset by the other thread.
    UMessage *res = syncLock_.uget(useconds) ? message_ : 0;
    if (!res)
      std::cerr << "Timed out" << std::endl;
    else if (res->type == MESSAGE_ERROR)
      std::cerr << "Received error message: " << *res << std::endl;
    message_ = 0;
    return res;
  }

  namespace
  {
    /// Check that \a cp looks like "foo <" or "foo :".
    /// I venture this is an attempt to see if there is a "tag" or a
    /// channel.
    static
    bool
    has_tag(const char* cp)
    {
      while (*cp == ' ')
        ++cp;
      while (isalpha(*cp))
        ++cp;
      while (*cp == ' ')
        ++cp;
      return *cp == ':' || *cp == '<';
    }

    /// Return the concatenation of t1 and t2, make it unique
    /// if they are empty.
    static
    std::string
    make_tag(UAbstractClient& cl, const USyncClient::options& opt)
    {
      std::string res;
      if (opt.mtag_)
      {
        res = opt.mtag_;
        if (opt.mmod_)
          res += opt.mmod_;
      }
      else
        res = cl.fresh();
      return res;
    }
  }

  UMessage*
  USyncClient::syncGet_(const char* format, va_list& arg,
			const USyncClient::options& options)
  {
    const USyncClient::options& opt_used = getOptions(options);
    if (has_tag(format))
      return 0;
    sendBufferLock.lock();
    rc = vpack(format, arg);
    if (rc < 0)
    {
      sendBufferLock.unlock();
      return 0;
    }
    std::string tag = make_tag(*this, opt_used);
    effective_send(compatibility::evaluate_in_channel_open(tag));
    queueLock_.lock();
    rc = effective_send(sendBuffer);
    sendBuffer[0] = 0;
    sendBufferLock.unlock();
    effective_send(compatibility::evaluate_in_channel_close(tag));
    return waitForTag(opt_used.mtag_ ? opt_used.mtag_ : tag,
                      opt_used.timeout_);
  }

  UMessage*
  USyncClient::syncGet(const char* format, ...)
  {
    va_list arg;
    va_start(arg, format);
    UMessage* res = syncGet_(format, arg);
    va_end(arg);
    return res;
  }

  UMessage*
  USyncClient::syncGet(const std::string& msg)
  {
    // Yes, this is studid, as it will copy uselessly.  But that's the
    // only safe way to do it.  The interface should be redesigned,
    // without buffers actually.
    return syncGet("%s", msg.c_str());
  }

  UMessage*
  USyncClient::syncGet(libport::utime_t useconds,
                       const char* format, ...)
  {
    va_list arg;
    va_start(arg, format);
    UMessage* res = syncGet_(format, arg, options().timeout(useconds));
    va_end(arg);
    return res;
  }

  UMessage*
  USyncClient::syncGetTag(const char* format,
                          const char* mtag, const char* mmod, ...)
  {
    va_list arg;
    va_start(arg, mmod);
    UMessage* res = syncGet_(format, arg, options().tag(mtag, mmod));
    va_end(arg);
    return res;
  }

  UMessage*
  USyncClient::syncGetTag(libport::utime_t useconds,
                          const char* format,
                          const char* mtag,
                          const char* mmod, ...)
  {
    va_list arg;
    va_start(arg, mmod);
    UMessage* res = syncGet_(format, arg,
			     options().tag(mtag, mmod).timeout(useconds));
    va_end(arg);
    return res;
  }

  int
  USyncClient::syncGetImage(const char* camera,
			    void* buffer, size_t& buffersize,
			    int format, int transmitFormat,
			    size_t& width, size_t& height,
			    libport::utime_t useconds)
  {
    int f = format == IMAGE_JPEG  || transmitFormat == URBI_TRANSMIT_JPEG;
    //XXX required to ensure format change is applied
    send("%s.format = %d;\n"
         "noop;\n"
         "noop;\n", camera, f);
    UMessage *m = syncGet(useconds, "%s.val;\n", camera);
    if (!m)
      return 0;
    if (m->value->binary->type != BINARY_IMAGE)
    {
      delete m;
      return 0;
    }
    width = m->value->binary->image.width;
    height = m->value->binary->image.height;

    size_t osize = buffersize;
    if (f == 1  && format != IMAGE_JPEG)
    {
      //uncompress jpeg
      if (format == IMAGE_YCbCr)
	convertJPEGtoYCrCb((const byte*) m->value->binary->image.data,
			   m->value->binary->image.size, (byte*) buffer,
			   buffersize);
      else
	convertJPEGtoRGB((const byte*) m->value->binary->image.data,
			 m->value->binary->image.size, (byte*) buffer,
			 buffersize);
    }
    else if (format == IMAGE_RGB || format == IMAGE_PPM)
    {
      buffersize = std::min(m->value->binary->image.size,
			    static_cast<size_t> (buffersize));
      if (m->value->binary->image.imageFormat == IMAGE_YCbCr)
	convertYCrCbtoRGB((const byte*) m->value->binary->image.data,
			  buffersize, (byte*) buffer);
      else
	memcpy(buffer, m->value->binary->image.data, buffersize);

    }
    else
    {
      //jpeg jpeg, or ycrcb ycrcb
      buffersize = std::min(m->value->binary->image.size,
			    static_cast<size_t>(buffersize));
      memcpy(buffer, m->value->binary->image.data, buffersize);
    }
    if (format == IMAGE_PPM)
    {
      char p6h[20];
      sprintf(p6h, "P6\n%zu %zu\n255\n", width, height);
      size_t p6len = strlen(p6h);
      size_t mlen = osize > buffersize + p6len ? buffersize : osize - p6len;
      memmove((void *) (((long) buffer) + p6len), buffer, mlen);
      memcpy(buffer, p6h, p6len);
      buffersize += p6len;
    }
    delete m;
    return 1;
  }

  int
  USyncClient::syncGetNormalizedDevice(const char* device, double& val,
				       libport::utime_t useconds)
  {
    return getValue(syncGet(useconds, "%s.valn;\n", device), val);
  }

  int
  USyncClient::syncGetValue(const char* valName, UValue& val,
			    libport::utime_t useconds)
  {
    return syncGetValue(0, valName, val, useconds);
  }

  int
  USyncClient::syncGetValue(const char* tag, const char* valName, UValue& val,
			    libport::utime_t useconds)
  {
    return getValue(syncGetTag(useconds, "%s;\n", tag, 0, valName), val);
  }

  int
  USyncClient::syncGetDevice(const char* device, double& val,
			     libport::utime_t useconds)
  {
    return getValue(syncGet(useconds, "%s.val;\n", device), val);
  }

  int
  USyncClient::syncGetResult(const char* command, double& val,
			     libport::utime_t useconds)
  {
    return getValue(syncGet(useconds, "%s", command), val);
  }

  int
  USyncClient::syncGetDevice(const char* device, const char* access,
			     double& val, libport::utime_t useconds)
  {
    return getValue(syncGet(useconds, "%s.%s;", device, access), val);
  }

  int
  USyncClient::syncGetSound(const char* device, int duration, USound& sound,
			    libport::utime_t useconds)
  {
    send("syncgetsound = BIN 0;\n"
	 "loopsound: loop syncgetsound = syncgetsound +  %s.val,\n"
	 " {\n"
	 "   sleep(%d);\n"
	 "   stop loopsound;\n"
	 "   noop;\n"
	 "   noop;\n"
	 " };\n", device, duration);
    UMessage* m = syncGet(useconds, "%s", "syncgetsound;");
    if (!m)
      return 0;
    if (m->type != MESSAGE_DATA
	|| m->value->type != DATA_BINARY
	|| m->value->binary->type != BINARY_SOUND)
    {
      delete m;
      return 0;
    }
    convert(m->value->binary->sound, sound);
    delete m;
    return 1;
  }

  int
  USyncClient::syncSend(const void* buffer, size_t length)
  {
    if (rc != 0)
      return -1;
    sendBufferLock.lock();
    size_t sent = 0;
    while (sent < length)
    {
      int res = ::write(sd, (char*) buffer + sent, length - sent);
      if (res < 0)
      {
	rc = res;
	sendBufferLock.unlock();
	return res;
      }
      sent += res;
    }
    sendBufferLock.unlock();
    return 0;
  }

  void
  USyncClient::waitForKernelVersion(bool hasProcessingThread)
  {
    // Do not call kernelMajor() which precisely requires kernelMajor_
    // to be defined.
    while (kernelMajor_ < 0 && !error())
    {
      if (!hasProcessingThread)
        processEvents();
      usleep(100000);
    }
  }

  void
  USyncClient::setDefaultOptions(const USyncClient::options& opt)
  {
    default_options_ = opt;
  }

  const USyncClient::options&
  USyncClient::getOptions(const USyncClient::options& opt) const
  {
    return (&opt == &USyncClient::options::default_options) ?
      default_options_ : opt;
  }

} // namespace urbi
<commit_msg>usyncclient: simplifications.<commit_after>#include <libport/unistd.h>
#include <fcntl.h>

#include <libport/assert.hh>
#include <libport/compiler.hh>
#include <libport/thread.hh>
#include <libport/unistd.h>

#include <liburbi/compatibility.hh>
#include <urbi/uconversion.hh>
#include <urbi/usyncclient.hh>

namespace urbi
{

  const USyncClient::options USyncClient::options::default_options =
    USyncClient::options();

  USyncClient::options::options()
    : timeout_(0)
    , mtag_(0)
    , mmod_(0)
  {}

  USyncClient::options&
  USyncClient::options::timeout(libport::utime_t usec)
  {
    timeout_ = usec;
    return *this;
  }

  USyncClient::options&
  USyncClient::options::tag(const char* tag, const char* mod)
  {
    mtag_ = tag;
    mmod_ = mod;
    return *this;
  }

  USyncClient::USyncClient(const std::string& host,
			   unsigned port,
			   size_t buflen,
			   bool server,
                           bool startCallbackThread,
			   unsigned semListenInc)
    : UClient(host, port, buflen, server, semListenInc)
    , sem_()
    , queueLock_()
    , message_(0)
    , syncLock_()
    , syncTag()
    , default_options_()
    , stopCallbackThread_(!startCallbackThread)
    , cbThread(0)
  {
    if (error())
      return;

    if (startCallbackThread)
      cbThread = libport::startThread(this, &USyncClient::callbackThread);
    if (!defaultClient)
      defaultClient = this;

    listenSem_++;
    callbackSem_++;
  }

  USyncClient::~USyncClient()
  {
    closeUClient();

    if (cbThread)
      joinCallbackThread_();
  }

  void USyncClient::callbackThread()
  {
    callbackSem_--;

    while (true)
    {
      sem_--;
      if (stopCallbackThread_)
      {
	// The call to stopCallbackThread is
	// waiting on stopCallbackSem_.
	stopCallbackSem_++;
	return;
      }
      queueLock_.lock();
      if (queue.empty())
      {
	// Only explanation: processEvents from another thread stole our
	// message.
	sem_++; // Give back the token we took without popping a message.
	queueLock_.unlock();
	continue;
      }
      UMessage *m = queue.front();
      queue.pop_front();
      queueLock_.unlock();
      UAbstractClient::notifyCallbacks(*m);
      delete m;
    }
  }

  void USyncClient::stopCallbackThread()
  {
    if (stopCallbackThread_)
      return;
    stopCallbackThread_ = true;
    sem_++;
    // Wait until the callback thread is actually stopped to avoid both
    // processEvents and the callbackThread running at the same time.
    stopCallbackSem_--;
  }

  bool USyncClient::processEvents(libport::utime_t timeout)
  {
    bool res = false;
    libport::utime_t startTime = libport::utime();
    while (timeout < 0 || libport::utime() - startTime <= timeout)
    {
      queueLock_.lock();
      if (queue.empty())
      {
	queueLock_.unlock();
	return res;
      }
      res = true;
      UMessage *m = queue.front();
      queue.pop_front();
      sem_--; // Will not block since queue was not empty.
      queueLock_.unlock();
      UAbstractClient::notifyCallbacks(*m);
      delete m;
    }
    return res;
  }

  int USyncClient::joinCallbackThread_()
  {
    stopCallbackThread();
    if (cbThread)
    {
      pthread_join(cbThread, 0);
      cbThread = 0;
    }
    return 0;
  }

  void
  USyncClient::notifyCallbacks(const UMessage& msg)
  {
    queueLock_.lock();
    // If waiting for a tag, pass it to the user.
    if (!syncTag.empty() && syncTag == msg.tag)
    {
      message_ = new UMessage(msg);
      syncTag.clear();
      syncLock_++;
    }
    else
    {
      queue.push_back(new UMessage(msg));
      sem_++;
    }
    queueLock_.unlock();
  }

  UMessage*
  USyncClient::waitForTag(const std::string& tag, libport::utime_t useconds)
  {
    syncTag = tag;
    queueLock_.unlock();
    // syncTag is reset by the other thread.
    UMessage *res = syncLock_.uget(useconds) ? message_ : 0;
    if (!res)
      std::cerr << "Timed out" << std::endl;
    else if (res->type == MESSAGE_ERROR)
      std::cerr << "Received error message: " << *res << std::endl;
    message_ = 0;
    return res;
  }

  namespace
  {
    /// Check that \a cp looks like "foo <" or "foo :".
    /// I venture this is an attempt to see if there is a "tag" or a
    /// channel.
    static
    bool
    has_tag(const char* cp)
    {
      while (*cp == ' ')
        ++cp;
      while (isalpha(*cp))
        ++cp;
      while (*cp == ' ')
        ++cp;
      return *cp == ':' || *cp == '<';
    }

    /// Return the concatenation of t1 and t2, make it unique
    /// if they are empty.
    static
    std::string
    make_tag(UAbstractClient& cl, const USyncClient::options& opt)
    {
      std::string res;
      if (opt.mtag_)
      {
        res = opt.mtag_;
        if (opt.mmod_)
          res += opt.mmod_;
      }
      else
        res = cl.fresh();
      return res;
    }
  }

  UMessage*
  USyncClient::syncGet_(const char* format, va_list& arg,
			const USyncClient::options& options)
  {
    const USyncClient::options& opt_used = getOptions(options);
    if (has_tag(format))
      return 0;
    sendBufferLock.lock();
    rc = vpack(format, arg);
    if (rc < 0)
    {
      sendBufferLock.unlock();
      return 0;
    }
    std::string tag = make_tag(*this, opt_used);
    effective_send(compatibility::evaluate_in_channel_open(tag));
    queueLock_.lock();
    rc = effective_send(sendBuffer);
    sendBuffer[0] = 0;
    sendBufferLock.unlock();
    effective_send(compatibility::evaluate_in_channel_close(tag));
    return waitForTag(opt_used.mtag_ ? opt_used.mtag_ : tag,
                      opt_used.timeout_);
  }

  UMessage*
  USyncClient::syncGet(const char* format, ...)
  {
    va_list arg;
    va_start(arg, format);
    UMessage* res = syncGet_(format, arg);
    va_end(arg);
    return res;
  }

  UMessage*
  USyncClient::syncGet(const std::string& msg)
  {
    // Yes, this is studid, as it will copy uselessly.  But that's the
    // only safe way to do it.  The interface should be redesigned,
    // without buffers actually.
    return syncGet("%s", msg.c_str());
  }

  UMessage*
  USyncClient::syncGet(libport::utime_t useconds,
                       const char* format, ...)
  {
    va_list arg;
    va_start(arg, format);
    UMessage* res = syncGet_(format, arg, options().timeout(useconds));
    va_end(arg);
    return res;
  }

  UMessage*
  USyncClient::syncGetTag(const char* format,
                          const char* mtag, const char* mmod, ...)
  {
    va_list arg;
    va_start(arg, mmod);
    UMessage* res = syncGet_(format, arg, options().tag(mtag, mmod));
    va_end(arg);
    return res;
  }

  UMessage*
  USyncClient::syncGetTag(libport::utime_t useconds,
                          const char* format,
                          const char* mtag,
                          const char* mmod, ...)
  {
    va_list arg;
    va_start(arg, mmod);
    UMessage* res = syncGet_(format, arg,
			     options().tag(mtag, mmod).timeout(useconds));
    va_end(arg);
    return res;
  }

  int
  USyncClient::syncGetImage(const char* camera,
			    void* buffer, size_t& buffersize,
			    int format, int transmitFormat,
			    size_t& width, size_t& height,
			    libport::utime_t useconds)
  {
    int f = format == IMAGE_JPEG  || transmitFormat == URBI_TRANSMIT_JPEG;
    //XXX required to ensure format change is applied
    send("%s.format = %d;\n"
         "noop;\n"
         "noop;\n", camera, f);
    UMessage *m = syncGet(useconds, "%s.val", camera);
    if (!m)
      return 0;
    if (m->value->binary->type != BINARY_IMAGE)
    {
      delete m;
      return 0;
    }
    width = m->value->binary->image.width;
    height = m->value->binary->image.height;

    size_t osize = buffersize;
    if (f == 1  && format != IMAGE_JPEG)
    {
      //uncompress jpeg
      if (format == IMAGE_YCbCr)
	convertJPEGtoYCrCb((const byte*) m->value->binary->image.data,
			   m->value->binary->image.size, (byte*) buffer,
			   buffersize);
      else
	convertJPEGtoRGB((const byte*) m->value->binary->image.data,
			 m->value->binary->image.size, (byte*) buffer,
			 buffersize);
    }
    else if (format == IMAGE_RGB || format == IMAGE_PPM)
    {
      buffersize = std::min(m->value->binary->image.size,
			    static_cast<size_t> (buffersize));
      if (m->value->binary->image.imageFormat == IMAGE_YCbCr)
	convertYCrCbtoRGB((const byte*) m->value->binary->image.data,
			  buffersize, (byte*) buffer);
      else
	memcpy(buffer, m->value->binary->image.data, buffersize);

    }
    else
    {
      //jpeg jpeg, or ycrcb ycrcb
      buffersize = std::min(m->value->binary->image.size,
			    static_cast<size_t>(buffersize));
      memcpy(buffer, m->value->binary->image.data, buffersize);
    }
    if (format == IMAGE_PPM)
    {
      char p6h[20];
      sprintf(p6h, "P6\n%zu %zu\n255\n", width, height);
      size_t p6len = strlen(p6h);
      size_t mlen = osize > buffersize + p6len ? buffersize : osize - p6len;
      memmove((void *) (((long) buffer) + p6len), buffer, mlen);
      memcpy(buffer, p6h, p6len);
      buffersize += p6len;
    }
    delete m;
    return 1;
  }

  int
  USyncClient::syncGetNormalizedDevice(const char* device, double& val,
				       libport::utime_t useconds)
  {
    return getValue(syncGet(useconds, "%s.valn", device), val);
  }

  int
  USyncClient::syncGetValue(const char* valName, UValue& val,
			    libport::utime_t useconds)
  {
    return syncGetValue(0, valName, val, useconds);
  }

  int
  USyncClient::syncGetValue(const char* tag, const char* valName, UValue& val,
			    libport::utime_t useconds)
  {
    return getValue(syncGetTag(useconds, "%s", tag, 0, valName), val);
  }

  int
  USyncClient::syncGetDevice(const char* device, double& val,
			     libport::utime_t useconds)
  {
    return getValue(syncGet(useconds, "%s.val", device), val);
  }

  int
  USyncClient::syncGetResult(const char* command, double& val,
			     libport::utime_t useconds)
  {
    return getValue(syncGet(useconds, "%s", command), val);
  }

  int
  USyncClient::syncGetDevice(const char* device, const char* access,
			     double& val, libport::utime_t useconds)
  {
    return getValue(syncGet(useconds, "%s.%s", device, access), val);
  }

  int
  USyncClient::syncGetSound(const char* device, int duration, USound& sound,
			    libport::utime_t useconds)
  {
    send("syncgetsound = BIN 0;\n"
	 "loopsound: loop syncgetsound = syncgetsound +  %s.val,\n"
	 " {\n"
	 "   sleep(%d);\n"
	 "   stop loopsound;\n"
	 "   noop;\n"
	 "   noop;\n"
	 " };\n", device, duration);
    UMessage* m = syncGet(useconds, "%s", "syncgetsound;");
    if (!m)
      return 0;
    if (m->type != MESSAGE_DATA
	|| m->value->type != DATA_BINARY
	|| m->value->binary->type != BINARY_SOUND)
    {
      delete m;
      return 0;
    }
    convert(m->value->binary->sound, sound);
    delete m;
    return 1;
  }

  int
  USyncClient::syncSend(const void* buffer, size_t length)
  {
    if (rc != 0)
      return -1;
    sendBufferLock.lock();
    size_t sent = 0;
    while (sent < length)
    {
      int res = ::write(sd, (char*) buffer + sent, length - sent);
      if (res < 0)
      {
	rc = res;
	sendBufferLock.unlock();
	return res;
      }
      sent += res;
    }
    sendBufferLock.unlock();
    return 0;
  }

  void
  USyncClient::waitForKernelVersion(bool hasProcessingThread)
  {
    // Do not call kernelMajor() which precisely requires kernelMajor_
    // to be defined.
    while (kernelMajor_ < 0 && !error())
    {
      if (!hasProcessingThread)
        processEvents();
      usleep(100000);
    }
  }

  void
  USyncClient::setDefaultOptions(const USyncClient::options& opt)
  {
    default_options_ = opt;
  }

  const USyncClient::options&
  USyncClient::getOptions(const USyncClient::options& opt) const
  {
    return (&opt == &USyncClient::options::default_options) ?
      default_options_ : opt;
  }

} // namespace urbi
<|endoftext|>


<commit_before>/*******************************************************************************
 *
 * MIT License
 *
 * Copyright (c) 2017 Advanced Micro Devices, Inc.
 *
 * 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.
 *
 *******************************************************************************/
#include "fusionHost.hpp"
#include <miopen/stringutils.hpp>
//#include <miopen/batch_norm_activ.hpp>

using ptr_FusionPlanDesc = MIOPEN_MANAGE_PTR(miopenFusionPlanDescriptor_t,
                                             miopenDestroyFusionPlanDescriptor);
using ptr_FusionPlanArgs = MIOPEN_MANAGE_PTR(miopenOperatorArgs_t, miopenDestroyOperatorArgs);
using ptr_ActivationDesc = MIOPEN_MANAGE_PTR(miopenActivationDescriptor_t,
                                             miopenDestroyActivationDescriptor);

ptr_FusionPlanDesc GetManagedFusionPlanDesc(miopenTensorDescriptor_t inputDesc)
{
    miopenFusionPlanDescriptor_t fusePlanDesc;
    miopenCreateFusionPlan(&fusePlanDesc, miopenVerticalFusion, inputDesc);
    return ptr_FusionPlanDesc{fusePlanDesc};
}

ptr_FusionPlanArgs GetManageFusionPlanArgs()
{
    miopenOperatorArgs_t fusionArgs;
    miopenCreateOperatorArgs(&fusionArgs);
    return ptr_FusionPlanArgs{fusionArgs};
}

ptr_ActivationDesc GetManagedActivDesc()
{
    miopenActivationDescriptor_t activdesc;
    miopenCreateActivationDescriptor(&activdesc);
    return ptr_ActivationDesc{activdesc};
}
template <class T>
struct verify_inference_batchnorm_activ
{
    tensor<T> input;
    miopenTensorDescriptor_t inputDesc{};
    miopenActivationDescriptor_t activDesc{};
    miopenTensorDescriptor_t biasScaleTensor{};
    tensor<T> bnscale{};
    tensor<T> bnbias{};
    tensor<T> estMean{};
    tensor<T> estVariance{};
    miopenBatchNormMode_t bnmode;
    miopenFusionPlanDescriptor_t fusionplan;
    miopenFusionOpDescriptor_t bNormOp;
    miopenFusionOpDescriptor_t activOp;

    double epsilon;

    verify_inference_batchnorm_activ(miopenFusionPlanDescriptor_t pfusionplan,
                                     tensor<T>& pinput,
                                     miopenActivationDescriptor_t pactivDesc,
                                     tensor<T>& pbnscale,
                                     tensor<T>& pbnbias,
                                     tensor<T>& pestMean,
                                     tensor<T>& pestVariance,
                                     miopenBatchNormMode_t pbnmode,
                                     miopenFusionOpDescriptor_t pbNormOp,
                                     miopenFusionOpDescriptor_t pactivOp)
    {
        input           = pinput;
        inputDesc       = &pinput.desc;
        activDesc       = pactivDesc;
        biasScaleTensor = &pbnscale.desc;
        bnscale         = pbnscale;
        bnbias          = pbnbias;
        estMean         = pestMean;
        estVariance     = pestVariance;
        bnmode          = pbnmode;
        fusionplan      = pfusionplan;
        bNormOp         = pbNormOp;
        activOp         = pactivOp;
        epsilon         = 1.0e-5;
    }

    tensor<T> cpu() const
    {
        auto bout     = input;
        std::fill(bout.begin(), bout.end(), 0.);
        auto aout = input;
        std::fill(aout.begin(), aout.end(), 0.);

        double activ_alpha, activ_beta, activ_gamma;
        miopenActivationMode_t activ_mode;
        miopenGetActivationDescriptor(
            activDesc, &activ_mode, &activ_alpha, &activ_beta, &activ_gamma);

        if(bnmode == miopenBNPerActivation)
        {
            batchNormPerActivHostInference(
                input, bout, bnscale, bnbias, epsilon, estMean, estVariance);
        }
        else
        {
            batchNormSpatialHostInference(
                input, bout, bnscale, bnbias, epsilon, estMean, estVariance);
        }

        activationHostInfer(
            activ_mode, activ_gamma, activ_beta, activ_alpha, bout.data, aout.data);

        return aout;
    }

    tensor<T> gpu() const
    {
        auto&& handle = get_handle();
        auto baout    = input;
        std::fill(baout.begin(), baout.end(), 0.);
        auto in_dev          = handle.Write(input.data);
        auto out_dev         = handle.Write(baout.data);
        auto bnscale_dev     = handle.Write(bnscale.data);
        auto bnbias_dev      = handle.Write(bnbias.data);
        auto estMean_dev     = handle.Write(estMean.data);
        auto estVariance_dev = handle.Write(estVariance.data);

        double activ_alpha, activ_beta, activ_gamma;
        miopenActivationMode_t activ_mode;
        miopenGetActivationDescriptor(
            activDesc, &activ_mode, &activ_alpha, &activ_beta, &activ_gamma);

        double alpha = 1., beta = 0.;
        auto ptr_fusionargs = GetManageFusionPlanArgs();
        miopenSetOpArgsBatchNormInference(ptr_fusionargs.get(),
                                          bNormOp,
                                          &alpha,
                                          &beta,
                                          bnscale_dev.get(),
                                          bnbias_dev.get(),
                                          estMean_dev.get(),
                                          estVariance_dev.get(),
                                          epsilon);
        miopenSetOpArgsActivForward(
            ptr_fusionargs.get(), activOp, &alpha, &beta, activ_alpha, activ_beta, activ_gamma);
        miopenExecuteFusionPlan(&handle,
                                fusionplan,
                                inputDesc,
                                in_dev.get(),
                                inputDesc,
                                out_dev.get(),
                                ptr_fusionargs.get());
        baout.data = handle.Read<T>(out_dev, baout.data.size());
        return baout;
    }

    void fail(float = 0) const { std::cerr << "BatchNorm+Activation Inference:" << std::endl; }
};

static std::string transform_mode(std::string s)
{
    return miopen::RemovePrefix(miopen::ToUpper(s), "MIOPENACTIVATION");
}

template <class T>
struct na_fusion_driver : test_driver
{
    tensor<T> input;
    tensor<T> scale;
    tensor<T> shift;
    tensor<T> estMean;
    tensor<T> estVariance;
    ptr_ActivationDesc ptr_activdesc = nullptr;

    miopenActivationMode_t activ_mode = miopenActivationRELU;
    std::string amode;
    miopenBatchNormMode_t bnmode{};
    int batchnormMode = 0;

    unsigned long max_value = miopen_type<T>{} == miopenHalf ? 5 : 17;
    double alpha = 0., beta = 0., gamma = 0.;

    na_fusion_driver()
    {
        add(input, "input", get_input_tensor());
        add(alpha, "alpha", generate_data({/*1.,*/ 0.5}));
        add(beta, "beta", generate_data({/*0.,*/ 0.5}));
        add(gamma, "gamma", generate_data({/*1.,*/ 0.5}));
        add(amode,
            "amode",
            generate_data(
                {"MIOPENACTIVATIONRELU", "MIOPENACTIVATIONLOGISTIC", "MIOPENACTIVATIONABS"}));
        add(batchnormMode, "batch-norm-mode", generate_data({0, 1}));
    }

    void run()
    {
        amode = transform_mode(amode);

        if(amode == "PASSTHRU")
            activ_mode = miopenActivationPASTHRU;
        else if(amode == "LOGISTIC")
            activ_mode = miopenActivationLOGISTIC;
        else if(amode == "TANH")
            activ_mode = miopenActivationTANH;
        else if(amode == "RELU")
            activ_mode = miopenActivationRELU;
        else if(amode == "SOFTRELU")
            activ_mode = miopenActivationSOFTRELU;
        else if(amode == "ABS")
            activ_mode = miopenActivationABS;
        else if(amode == "POWER")
            activ_mode = miopenActivationPOWER;
        else if(amode == "CLIPPEDRELU")
            activ_mode = miopenActivationCLIPPEDRELU;
        else if(amode == "LEAKYRELU")
            activ_mode = miopenActivationLEAKYRELU;
        else if(amode == "ELU")
            activ_mode = miopenActivationELU;

        int input_c, input_h, input_w;
        std::tie(std::ignore, input_c, input_h, input_w) = miopen::tien<4>(input.desc.GetLengths());
        ptr_activdesc = GetManagedActivDesc();
        miopenSetActivationDescriptor(ptr_activdesc.get(), activ_mode, alpha, beta, gamma);

        if(batchnormMode == 1)
        {
            bnmode = miopenBNSpatial;
        }
        else if(batchnormMode == 0)
        {
            bnmode = miopenBNPerActivation;
        }

        std::size_t ssn, ssc, ssh, ssw;
        auto derivedBnDesc = miopen::TensorDescriptor{};
        miopen::DeriveBNTensorDescriptor(derivedBnDesc, input.desc, bnmode);
        std::tie(ssn, ssc, ssh, ssw) = miopen::tien<4>(derivedBnDesc.GetLengths());

        if(input.desc.GetType() == miopenFloat)
        {
            scale       = tensor<T>{ssn, ssc, ssh, ssw}.generate(rand_gen{});
            shift       = tensor<T>{ssn, ssc, ssh, ssw}.generate(rand_gen{});
            estMean     = tensor<T>{ssn, ssc, ssh, ssw}.generate(rand_gen{});
            estVariance = tensor<T>{ssn, ssc, ssh, ssw}.generate(rand_gen{});
        }
        else
        {
            scale       = tensor<T>{ssn, ssc, ssh, ssw};
            shift       = tensor<T>{ssn, ssc, ssh, ssw};
            estMean     = tensor<T>{ssn, ssc, ssh, ssw};
            estVariance = tensor<T>{ssn, ssc, ssh, ssw};

            srand(0);
            for(int i = 0; i < scale.desc.GetElementSize(); i++)
            {
                scale[i]       = (((rand() % 2) == 1) ? -1 : 1) * 1e-4 * T(rand() % 100);
                shift[i]       = (((rand() % 2) == 1) ? -1 : 1) * 1e-4 * T(rand() % 100);
                estMean[i]     = (((rand() % 2) == 1) ? -1 : 1) * 1e-4 * T(rand() % 100);
                estVariance[i] = std::fabs((((rand() % 2) == 1) ? -1 : 1) * 1e-2 * T(rand() % 100));
            }
            for(int i = 0; i < input.desc.GetElementSize(); i++)
            {
                input[i] = (((rand() % 2) == 1) ? -1 : 1) * T(rand() % 100);
            }
        }

        auto&& handle = get_handle();

        miopenFusionOpDescriptor_t bNormOp = nullptr;
        miopenFusionOpDescriptor_t activOp = nullptr;

        auto ptr_fusionplan = GetManagedFusionPlanDesc(&input.desc);

        miopenCreateOpBatchNormInference(ptr_fusionplan.get(), &bNormOp, bnmode, &scale.desc);
        miopenCreateOpActivationForward(ptr_fusionplan.get(), &activOp, activ_mode);

        miopenStatus_t miopenError = miopenCompileFusionPlan(&handle, ptr_fusionplan.get());
        if(miopenError != miopenStatusSuccess)
        {
            std::cerr << "BatchNorm+Activation Inference plan not supported." << std::endl;
        }
        else
        {
            verify(verify_inference_batchnorm_activ<T>{
                            ptr_fusionplan.get(), input, ptr_activdesc.get(),
                            scale, shift, estMean, estVariance, bnmode, bNormOp, activOp});
        }

    }
};

int main(int argc, const char* argv[]) { test_drive<na_fusion_driver>(argc, argv); }
<commit_msg>Formatting.<commit_after>/*******************************************************************************
 *
 * MIT License
 *
 * Copyright (c) 2017 Advanced Micro Devices, Inc.
 *
 * 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.
 *
 *******************************************************************************/
#include "fusionHost.hpp"
#include <miopen/stringutils.hpp>
//#include <miopen/batch_norm_activ.hpp>

using ptr_FusionPlanDesc = MIOPEN_MANAGE_PTR(miopenFusionPlanDescriptor_t,
                                             miopenDestroyFusionPlanDescriptor);
using ptr_FusionPlanArgs = MIOPEN_MANAGE_PTR(miopenOperatorArgs_t, miopenDestroyOperatorArgs);
using ptr_ActivationDesc = MIOPEN_MANAGE_PTR(miopenActivationDescriptor_t,
                                             miopenDestroyActivationDescriptor);

ptr_FusionPlanDesc GetManagedFusionPlanDesc(miopenTensorDescriptor_t inputDesc)
{
    miopenFusionPlanDescriptor_t fusePlanDesc;
    miopenCreateFusionPlan(&fusePlanDesc, miopenVerticalFusion, inputDesc);
    return ptr_FusionPlanDesc{fusePlanDesc};
}

ptr_FusionPlanArgs GetManageFusionPlanArgs()
{
    miopenOperatorArgs_t fusionArgs;
    miopenCreateOperatorArgs(&fusionArgs);
    return ptr_FusionPlanArgs{fusionArgs};
}

ptr_ActivationDesc GetManagedActivDesc()
{
    miopenActivationDescriptor_t activdesc;
    miopenCreateActivationDescriptor(&activdesc);
    return ptr_ActivationDesc{activdesc};
}
template <class T>
struct verify_inference_batchnorm_activ
{
    tensor<T> input;
    miopenTensorDescriptor_t inputDesc{};
    miopenActivationDescriptor_t activDesc{};
    miopenTensorDescriptor_t biasScaleTensor{};
    tensor<T> bnscale{};
    tensor<T> bnbias{};
    tensor<T> estMean{};
    tensor<T> estVariance{};
    miopenBatchNormMode_t bnmode;
    miopenFusionPlanDescriptor_t fusionplan;
    miopenFusionOpDescriptor_t bNormOp;
    miopenFusionOpDescriptor_t activOp;

    double epsilon;

    verify_inference_batchnorm_activ(miopenFusionPlanDescriptor_t pfusionplan,
                                     tensor<T>& pinput,
                                     miopenActivationDescriptor_t pactivDesc,
                                     tensor<T>& pbnscale,
                                     tensor<T>& pbnbias,
                                     tensor<T>& pestMean,
                                     tensor<T>& pestVariance,
                                     miopenBatchNormMode_t pbnmode,
                                     miopenFusionOpDescriptor_t pbNormOp,
                                     miopenFusionOpDescriptor_t pactivOp)
    {
        input           = pinput;
        inputDesc       = &pinput.desc;
        activDesc       = pactivDesc;
        biasScaleTensor = &pbnscale.desc;
        bnscale         = pbnscale;
        bnbias          = pbnbias;
        estMean         = pestMean;
        estVariance     = pestVariance;
        bnmode          = pbnmode;
        fusionplan      = pfusionplan;
        bNormOp         = pbNormOp;
        activOp         = pactivOp;
        epsilon         = 1.0e-5;
    }

    tensor<T> cpu() const
    {
        auto bout = input;
        std::fill(bout.begin(), bout.end(), 0.);
        auto aout = input;
        std::fill(aout.begin(), aout.end(), 0.);

        double activ_alpha, activ_beta, activ_gamma;
        miopenActivationMode_t activ_mode;
        miopenGetActivationDescriptor(
            activDesc, &activ_mode, &activ_alpha, &activ_beta, &activ_gamma);

        if(bnmode == miopenBNPerActivation)
        {
            batchNormPerActivHostInference(
                input, bout, bnscale, bnbias, epsilon, estMean, estVariance);
        }
        else
        {
            batchNormSpatialHostInference(
                input, bout, bnscale, bnbias, epsilon, estMean, estVariance);
        }

        activationHostInfer(activ_mode, activ_gamma, activ_beta, activ_alpha, bout.data, aout.data);

        return aout;
    }

    tensor<T> gpu() const
    {
        auto&& handle = get_handle();
        auto baout    = input;
        std::fill(baout.begin(), baout.end(), 0.);
        auto in_dev          = handle.Write(input.data);
        auto out_dev         = handle.Write(baout.data);
        auto bnscale_dev     = handle.Write(bnscale.data);
        auto bnbias_dev      = handle.Write(bnbias.data);
        auto estMean_dev     = handle.Write(estMean.data);
        auto estVariance_dev = handle.Write(estVariance.data);

        double activ_alpha, activ_beta, activ_gamma;
        miopenActivationMode_t activ_mode;
        miopenGetActivationDescriptor(
            activDesc, &activ_mode, &activ_alpha, &activ_beta, &activ_gamma);

        double alpha = 1., beta = 0.;
        auto ptr_fusionargs = GetManageFusionPlanArgs();
        miopenSetOpArgsBatchNormInference(ptr_fusionargs.get(),
                                          bNormOp,
                                          &alpha,
                                          &beta,
                                          bnscale_dev.get(),
                                          bnbias_dev.get(),
                                          estMean_dev.get(),
                                          estVariance_dev.get(),
                                          epsilon);
        miopenSetOpArgsActivForward(
            ptr_fusionargs.get(), activOp, &alpha, &beta, activ_alpha, activ_beta, activ_gamma);
        miopenExecuteFusionPlan(&handle,
                                fusionplan,
                                inputDesc,
                                in_dev.get(),
                                inputDesc,
                                out_dev.get(),
                                ptr_fusionargs.get());
        baout.data = handle.Read<T>(out_dev, baout.data.size());
        return baout;
    }

    void fail(float = 0) const { std::cerr << "BatchNorm+Activation Inference:" << std::endl; }
};

static std::string transform_mode(std::string s)
{
    return miopen::RemovePrefix(miopen::ToUpper(s), "MIOPENACTIVATION");
}

template <class T>
struct na_fusion_driver : test_driver
{
    tensor<T> input;
    tensor<T> scale;
    tensor<T> shift;
    tensor<T> estMean;
    tensor<T> estVariance;
    ptr_ActivationDesc ptr_activdesc = nullptr;

    miopenActivationMode_t activ_mode = miopenActivationRELU;
    std::string amode;
    miopenBatchNormMode_t bnmode{};
    int batchnormMode = 0;

    unsigned long max_value = miopen_type<T>{} == miopenHalf ? 5 : 17;
    double alpha = 0., beta = 0., gamma = 0.;

    na_fusion_driver()
    {
        add(input, "input", get_input_tensor());
        add(alpha, "alpha", generate_data({/*1.,*/ 0.5}));
        add(beta, "beta", generate_data({/*0.,*/ 0.5}));
        add(gamma, "gamma", generate_data({/*1.,*/ 0.5}));
        add(amode,
            "amode",
            generate_data(
                {"MIOPENACTIVATIONRELU", "MIOPENACTIVATIONLOGISTIC", "MIOPENACTIVATIONABS"}));
        add(batchnormMode, "batch-norm-mode", generate_data({0, 1}));
    }

    void run()
    {
        amode = transform_mode(amode);

        if(amode == "PASSTHRU")
            activ_mode = miopenActivationPASTHRU;
        else if(amode == "LOGISTIC")
            activ_mode = miopenActivationLOGISTIC;
        else if(amode == "TANH")
            activ_mode = miopenActivationTANH;
        else if(amode == "RELU")
            activ_mode = miopenActivationRELU;
        else if(amode == "SOFTRELU")
            activ_mode = miopenActivationSOFTRELU;
        else if(amode == "ABS")
            activ_mode = miopenActivationABS;
        else if(amode == "POWER")
            activ_mode = miopenActivationPOWER;
        else if(amode == "CLIPPEDRELU")
            activ_mode = miopenActivationCLIPPEDRELU;
        else if(amode == "LEAKYRELU")
            activ_mode = miopenActivationLEAKYRELU;
        else if(amode == "ELU")
            activ_mode = miopenActivationELU;

        int input_c, input_h, input_w;
        std::tie(std::ignore, input_c, input_h, input_w) = miopen::tien<4>(input.desc.GetLengths());
        ptr_activdesc = GetManagedActivDesc();
        miopenSetActivationDescriptor(ptr_activdesc.get(), activ_mode, alpha, beta, gamma);

        if(batchnormMode == 1)
        {
            bnmode = miopenBNSpatial;
        }
        else if(batchnormMode == 0)
        {
            bnmode = miopenBNPerActivation;
        }

        std::size_t ssn, ssc, ssh, ssw;
        auto derivedBnDesc = miopen::TensorDescriptor{};
        miopen::DeriveBNTensorDescriptor(derivedBnDesc, input.desc, bnmode);
        std::tie(ssn, ssc, ssh, ssw) = miopen::tien<4>(derivedBnDesc.GetLengths());

        if(input.desc.GetType() == miopenFloat)
        {
            scale       = tensor<T>{ssn, ssc, ssh, ssw}.generate(rand_gen{});
            shift       = tensor<T>{ssn, ssc, ssh, ssw}.generate(rand_gen{});
            estMean     = tensor<T>{ssn, ssc, ssh, ssw}.generate(rand_gen{});
            estVariance = tensor<T>{ssn, ssc, ssh, ssw}.generate(rand_gen{});
        }
        else
        {
            scale       = tensor<T>{ssn, ssc, ssh, ssw};
            shift       = tensor<T>{ssn, ssc, ssh, ssw};
            estMean     = tensor<T>{ssn, ssc, ssh, ssw};
            estVariance = tensor<T>{ssn, ssc, ssh, ssw};

            srand(0);
            for(int i = 0; i < scale.desc.GetElementSize(); i++)
            {
                scale[i]       = (((rand() % 2) == 1) ? -1 : 1) * 1e-4 * T(rand() % 100);
                shift[i]       = (((rand() % 2) == 1) ? -1 : 1) * 1e-4 * T(rand() % 100);
                estMean[i]     = (((rand() % 2) == 1) ? -1 : 1) * 1e-4 * T(rand() % 100);
                estVariance[i] = std::fabs((((rand() % 2) == 1) ? -1 : 1) * 1e-2 * T(rand() % 100));
            }
            for(int i = 0; i < input.desc.GetElementSize(); i++)
            {
                input[i] = (((rand() % 2) == 1) ? -1 : 1) * T(rand() % 100);
            }
        }

        auto&& handle = get_handle();

        miopenFusionOpDescriptor_t bNormOp = nullptr;
        miopenFusionOpDescriptor_t activOp = nullptr;

        auto ptr_fusionplan = GetManagedFusionPlanDesc(&input.desc);

        miopenCreateOpBatchNormInference(ptr_fusionplan.get(), &bNormOp, bnmode, &scale.desc);
        miopenCreateOpActivationForward(ptr_fusionplan.get(), &activOp, activ_mode);

        miopenStatus_t miopenError = miopenCompileFusionPlan(&handle, ptr_fusionplan.get());
        if(miopenError != miopenStatusSuccess)
        {
            std::cerr << "BatchNorm+Activation Inference plan not supported." << std::endl;
        }
        else
        {
            verify(verify_inference_batchnorm_activ<T>{ptr_fusionplan.get(),
                                                       input,
                                                       ptr_activdesc.get(),
                                                       scale,
                                                       shift,
                                                       estMean,
                                                       estVariance,
                                                       bnmode,
                                                       bNormOp,
                                                       activOp});
        }
    }
};

int main(int argc, const char* argv[]) { test_drive<na_fusion_driver>(argc, argv); }
<|endoftext|>


<commit_before>/*************************************************************************
 *
 *  $RCSfile: datasourcehandling.hxx,v $
 *
 *  $Revision: 1.3 $
 *
 *  last change: $Author: vg $ $Date: 2003-06-02 08:04:05 $
 *
 *  The Contents of this file are made available subject to the terms of
 *  either of the following licenses
 *
 *         - GNU Lesser General Public License Version 2.1
 *         - Sun Industry Standards Source License Version 1.1
 *
 *  Sun Microsystems Inc., October, 2000
 *
 *  GNU Lesser General Public License Version 2.1
 *  =============================================
 *  Copyright 2000 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
 *
 *
 *  Sun Industry Standards Source License Version 1.1
 *  =================================================
 *  The contents of this file are subject to the Sun Industry Standards
 *  Source License Version 1.1 (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.openoffice.org/license.html.
 *
 *  Software provided under this License is provided on an "AS IS" basis,
 *  WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESS OR IMPLIED, INCLUDING,
 *  WITHOUT LIMITATION, WARRANTIES THAT THE SOFTWARE IS FREE OF DEFECTS,
 *  MERCHANTABLE, FIT FOR A PARTICULAR PURPOSE, OR NON-INFRINGING.
 *  See the License for the specific provisions governing your rights and
 *  obligations concerning the Software.
 *
 *  The Initial Developer of the Original Code is: Sun Microsystems, Inc..
 *
 *  Copyright: 2000 by Sun Microsystems, Inc.
 *
 *  All Rights Reserved.
 *
 *  Contributor(s): _______________________________________
 *
 *
 ************************************************************************/

#ifndef EXTENSIONS_ABP_DATASOURCEHANDLING_HXX
#define EXTENSIONS_ABP_DATASOURCEHANDLING_HXX

#ifndef _COM_SUN_STAR_UNO_REFERENCE_HXX_
#include <com/sun/star/uno/Reference.hxx>
#endif
#ifndef EXTENSIONS_ABP_ABPTYPES_HXX
#include "abptypes.hxx"
#endif

//========================================================================
namespace com { namespace sun { namespace star {
    namespace lang {
        class XMultiServiceFactory;
    }
    namespace beans {
        class XPropertySet;
    }
} } }

class Window;

//.........................................................................
namespace abp
{
//.........................................................................

    //=====================================================================
    //= ODataSourceContext
    //=====================================================================
    struct ODataSourceContextImpl;
    class ODataSource;
    /// a non-UNO wrapper for the data source context
    class ODataSourceContext
    {
    private:
        ODataSourceContextImpl*     m_pImpl;

    public:
        ODataSourceContext(
            const ::com::sun::star::uno::Reference< ::com::sun::star::lang::XMultiServiceFactory >& _rxORB
        );

        /// retrieves the names of all data sources
        void    getDataSourceNames( StringBag& _rNames ) const SAL_THROW (( ));

        /// disambiguates the given name by appending auccessive numbers
        ::rtl::OUString& disambiguate(::rtl::OUString& _rDataSourceName);

        /// creates a new MORK data source
        ODataSource createNewMORK( const ::rtl::OUString& _rName ) SAL_THROW (( ));

        /// creates a new Evolution data source
        ODataSource createNewEvolution( const ::rtl::OUString& _rName ) SAL_THROW (( ));

        /// creates a new LDAP data source
        ODataSource createNewLDAP( const ::rtl::OUString& _rName ) SAL_THROW (( ));

        /// creates a new Outlook data source
        ODataSource createNewOutlook( const ::rtl::OUString& _rName ) SAL_THROW (( ));

        /// creates a new Outlook express data source
        ODataSource createNewOE( const ::rtl::OUString& _rName ) SAL_THROW (( ));

        /// creates a new dBase data source
        ODataSource createNewDBase( const ::rtl::OUString& _rName ) SAL_THROW (( ));
    };

    //=====================================================================
    //= ODataSource
    //=====================================================================
    struct ODataSourceImpl;
    struct PackageAccessControl;
    /** a non-UNO wrapper for a data source

This class allows to access data sources without the need to compile the respective file with
        exception handling enabled (hopefully :).

In addition to wrapping an UNO data source, an instance of this class can handle at most
        one valid connection, as obtained from the data source.

    */
    class ODataSource
    {
    private:
        ODataSourceImpl*    m_pImpl;

    public:
        // ----------------------------------------------------------------
        // - ctor/dtor/assignment
        // ----------------------------------------------------------------
        /** ctor
            @param _rxORB
                the service factory to use to access the UNO objects
            @param _rName
                the name of the data source the object should represent
        */
        ODataSource(
            const ::com::sun::star::uno::Reference< ::com::sun::star::lang::XMultiServiceFactory >& _rxORB,
            const ::rtl::OUString& _rName
        );

        /// constructs an object which is initially invalid
        ODataSource(
            const ::com::sun::star::uno::Reference< ::com::sun::star::lang::XMultiServiceFactory >& _rxORB
        );

        /// copy ctor
        ODataSource( const ODataSource& _rSource );

        /// dtor
        ~ODataSource( );

        /// assignment
        ODataSource& operator=( const ODataSource& _rSource );

        // ----------------------------------------------------------------
        /// checks whether or not the object represents a valid data source
        sal_Bool    isValid() const SAL_THROW (( ));

        // ----------------------------------------------------------------
        /// removes the data source represented by the object from the data source context
        void        remove() SAL_THROW (( ));
            // TODO: put this into the context class

        /// returns the name of the data source
        ::rtl::OUString
                    getName() const SAL_THROW (( ));

        /// renames the data source
        sal_Bool    rename( const ::rtl::OUString& _rName ) SAL_THROW (( ));
            // TODO: put this into the context class

        // ----------------------------------------------------------------
        // - connection handling
        // ----------------------------------------------------------------
        /** connects to the data source represented by this object
            @param _pMessageParent
                the window to use as parent for any error messages. If this is <NULL/>, no messages are displayed
                at all.
            @see isConnected
        */
        sal_Bool    connect( Window* _pMessageParent ) SAL_THROW (( ));

        /// returns <TRUE/> if the object has a valid connection, obtained from it's data source
        sal_Bool    isConnected( ) const SAL_THROW (( ));

        /// disconnects from the data source (i.e. disposes the UNO connection hold internally)
        void        disconnect( ) SAL_THROW (( ));

        // ----------------------------------------------------------------
        /** retrieves the tables names from the connection

to be called when <method>isConnection</method> returns <TRUE/> only

        */
        const StringBag&    getTableNames() const SAL_THROW (( ));

        // ----------------------------------------------------------------
        /** set a new data source.

Available to selected clients only

        */
        void        setDataSource(
            const ::com::sun::star::uno::Reference< ::com::sun::star::beans::XPropertySet >& _rxDS,
            PackageAccessControl
        );

    protected:
        /// a unsafe version of getName()
        ::rtl::OUString
                    implGetName() const SAL_THROW (( ::com::sun::star::uno::Exception ));

    private:
        ODataSource( ); // never implemented
    };

//.........................................................................
}   // namespace abp
//.........................................................................

#endif // EXTENSIONS_ABP_DATASOURCEHANDLING_HXX

<commit_msg>INTEGRATION: CWS insight01 (1.3.148); FILE MERGED 2004/04/16 12:07:04 oj 1.3.148.2: correct alignment and default 2004/03/05 07:38:43 oj 1.3.148.1: #111090# changes for the new prop dialogs<commit_after>/*************************************************************************
 *
 *  $RCSfile: datasourcehandling.hxx,v $
 *
 *  $Revision: 1.4 $
 *
 *  last change: $Author: hr $ $Date: 2004-08-02 17:36:51 $
 *
 *  The Contents of this file are made available subject to the terms of
 *  either of the following licenses
 *
 *         - GNU Lesser General Public License Version 2.1
 *         - Sun Industry Standards Source License Version 1.1
 *
 *  Sun Microsystems Inc., October, 2000
 *
 *  GNU Lesser General Public License Version 2.1
 *  =============================================
 *  Copyright 2000 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
 *
 *
 *  Sun Industry Standards Source License Version 1.1
 *  =================================================
 *  The contents of this file are subject to the Sun Industry Standards
 *  Source License Version 1.1 (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.openoffice.org/license.html.
 *
 *  Software provided under this License is provided on an "AS IS" basis,
 *  WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESS OR IMPLIED, INCLUDING,
 *  WITHOUT LIMITATION, WARRANTIES THAT THE SOFTWARE IS FREE OF DEFECTS,
 *  MERCHANTABLE, FIT FOR A PARTICULAR PURPOSE, OR NON-INFRINGING.
 *  See the License for the specific provisions governing your rights and
 *  obligations concerning the Software.
 *
 *  The Initial Developer of the Original Code is: Sun Microsystems, Inc..
 *
 *  Copyright: 2000 by Sun Microsystems, Inc.
 *
 *  All Rights Reserved.
 *
 *  Contributor(s): _______________________________________
 *
 *
 ************************************************************************/

#ifndef EXTENSIONS_ABP_DATASOURCEHANDLING_HXX
#define EXTENSIONS_ABP_DATASOURCEHANDLING_HXX

#ifndef _COM_SUN_STAR_UNO_REFERENCE_HXX_
#include <com/sun/star/uno/Reference.hxx>
#endif
#ifndef EXTENSIONS_ABP_ABPTYPES_HXX
#include "abptypes.hxx"
#endif

//========================================================================
namespace com { namespace sun { namespace star {
    namespace lang {
        class XMultiServiceFactory;
    }
    namespace beans {
        class XPropertySet;
    }
} } }

class Window;

//.........................................................................
namespace abp
{
//.........................................................................

    //=====================================================================
    //= ODataSourceContext
    //=====================================================================
    struct ODataSourceContextImpl;
    class ODataSource;
    /// a non-UNO wrapper for the data source context
    class ODataSourceContext
    {
    private:
        ODataSourceContextImpl*     m_pImpl;

    public:
        ODataSourceContext(
            const ::com::sun::star::uno::Reference< ::com::sun::star::lang::XMultiServiceFactory >& _rxORB
        );

        /// retrieves the names of all data sources
        void    getDataSourceNames( StringBag& _rNames ) const SAL_THROW (( ));

        /// disambiguates the given name by appending auccessive numbers
        ::rtl::OUString& disambiguate(::rtl::OUString& _rDataSourceName);

        /// creates a new MORK data source
        ODataSource createNewMORK( const ::rtl::OUString& _rName ) SAL_THROW (( ));

        /// creates a new Evolution data source
        ODataSource createNewEvolution( const ::rtl::OUString& _rName ) SAL_THROW (( ));

        /// creates a new LDAP data source
        ODataSource createNewLDAP( const ::rtl::OUString& _rName ) SAL_THROW (( ));

        /// creates a new Outlook data source
        ODataSource createNewOutlook( const ::rtl::OUString& _rName ) SAL_THROW (( ));

        /// creates a new Outlook express data source
        ODataSource createNewOE( const ::rtl::OUString& _rName ) SAL_THROW (( ));

        /// creates a new dBase data source
        ODataSource createNewDBase( const ::rtl::OUString& _rName ) SAL_THROW (( ));
    };

    //=====================================================================
    //= ODataSource
    //=====================================================================
    struct ODataSourceImpl;
    struct PackageAccessControl;
    /** a non-UNO wrapper for a data source

This class allows to access data sources without the need to compile the respective file with
        exception handling enabled (hopefully :).

In addition to wrapping an UNO data source, an instance of this class can handle at most
        one valid connection, as obtained from the data source.

    */
    class ODataSource
    {
    private:
        ODataSourceImpl*    m_pImpl;

    public:
        // ----------------------------------------------------------------
        // - ctor/dtor/assignment
        // ----------------------------------------------------------------
        /** ctor
            @param _rxORB
                the service factory to use to access the UNO objects
            @param _rName
                the name of the data source the object should represent
        */
        ODataSource(
            const ::com::sun::star::uno::Reference< ::com::sun::star::lang::XMultiServiceFactory >& _rxORB,
            const ::rtl::OUString& _rName
        );

        /// constructs an object which is initially invalid
        ODataSource(
            const ::com::sun::star::uno::Reference< ::com::sun::star::lang::XMultiServiceFactory >& _rxORB
        );

        /// copy ctor
        ODataSource( const ODataSource& _rSource );

        /// dtor
        ~ODataSource( );

        /// assignment
        ODataSource& operator=( const ODataSource& _rSource );

        // ----------------------------------------------------------------
        /// checks whether or not the object represents a valid data source
        sal_Bool    isValid() const SAL_THROW (( ));

        // ----------------------------------------------------------------
        /// removes the data source represented by the object from the data source context
        void        remove() SAL_THROW (( ));
            // TODO: put this into the context class

        /// returns the name of the data source
        ::rtl::OUString
                    getName() const SAL_THROW (( ));

        /// renames the data source
        sal_Bool    rename( const ::rtl::OUString& _rName ) SAL_THROW (( ));
            // TODO: put this into the context class

        // ----------------------------------------------------------------
        // - connection handling
        // ----------------------------------------------------------------
        /** connects to the data source represented by this object
            @param _pMessageParent
                the window to use as parent for any error messages. If this is <NULL/>, no messages are displayed
                at all.
            @see isConnected
        */
        sal_Bool    connect( Window* _pMessageParent ) SAL_THROW (( ));

        /// returns <TRUE/> if the object has a valid connection, obtained from it's data source
        sal_Bool    isConnected( ) const SAL_THROW (( ));

        /// disconnects from the data source (i.e. disposes the UNO connection hold internally)
        void        disconnect( ) SAL_THROW (( ));

        /// stores the database file
        void        store() SAL_THROW (( ));

        /// register the data source under the given name in the configuration
        void        registerDataSource( const ::rtl::OUString& _sRegisteredDataSourceName )  SAL_THROW (( ));

        // ----------------------------------------------------------------
        /** retrieves the tables names from the connection

to be called when <method>isConnection</method> returns <TRUE/> only

        */
        const StringBag&    getTableNames() const SAL_THROW (( ));

        /// return the intern data source object
        ::com::sun::star::uno::Reference< ::com::sun::star::beans::XPropertySet > getDataSource() const SAL_THROW (( ));

        // ----------------------------------------------------------------
        /** set a new data source.

Available to selected clients only

        */
        void        setDataSource(
            const ::com::sun::star::uno::Reference< ::com::sun::star::beans::XPropertySet >& _rxDS
            ,const ::rtl::OUString& _sName
            ,PackageAccessControl
        );

    private:
        ODataSource( ); // never implemented
    };

//.........................................................................
}   // namespace abp
//.........................................................................

#endif // EXTENSIONS_ABP_DATASOURCEHANDLING_HXX

<|endoftext|>