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

#include <cmath>

#include "modules/common/adapters/adapter_gflags.h"
#include "modules/common/adapters/adapter_manager.h"
#include "modules/common/log.h"
#include "modules/guardian/common/guardian_gflags.h"
#include "ros/include/ros/ros.h"

namespace apollo {
namespace guardian {

using apollo::canbus::Chassis;
using apollo::common::ErrorCode;
using apollo::common::Status;
using apollo::common::adapter::AdapterManager;
using apollo::control::ControlCommand;
using apollo::guardian::GuardianCommand;
using apollo::monitor::SystemStatus;

std::string Guardian::Name() const { return FLAGS_module_name; }

Status Guardian::Init() {
  AdapterManager::Init(FLAGS_adapter_config_filename);
  CHECK(AdapterManager::GetChassis()) << "Chassis is not initialized.";
  AdapterManager::AddChassisCallback(&Guardian::OnChassis, this);
  CHECK(AdapterManager::GetSystemStatus())
      << "SystemStatus is not initialized.";
  AdapterManager::AddSystemStatusCallback(&Guardian::OnSystemStatus, this);
  CHECK(AdapterManager::GetControlCommand()) << "Control is not initialized.";
  AdapterManager::AddControlCommandCallback(&Guardian::OnControl, this);
  return Status::OK();
}

Status Guardian::Start() {
  const double duration = 1.0 / FLAGS_guardian_cmd_freq;
  timer_ = AdapterManager::CreateTimer(ros::Duration(duration),
                                       &Guardian::OnTimer, this);

  return Status::OK();
}

void Guardian::Stop() { timer_.stop(); }

void Guardian::OnTimer(const ros::TimerEvent&) {
  ADEBUG << "Timer is triggered: publish Guardian result";
  std::lock_guard<std::mutex> lock(mutex_);
  if (system_status_.has_safety_mode_trigger_time() && FLAGS_guardian_enabled) {
    ADEBUG << "Safety mode triggerd, enable safty mode";
    TriggerSafetyMode();
  } else {
    ADEBUG << "Safety mode not triggerd, bypass control command";
    ByPassControlCommand();
  }
  AdapterManager::FillGuardianHeader(FLAGS_node_name, &guardian_cmd_);
  AdapterManager::PublishGuardian(guardian_cmd_);
}

void Guardian::OnChassis(const Chassis& message) {
  ADEBUG << "Received chassis data: run chassis callback.";
  std::lock_guard<std::mutex> lock(mutex_);
  chassis_.CopyFrom(message);
}

void Guardian::OnSystemStatus(const SystemStatus& message) {
  ADEBUG << "Received monitor data: run monitor callback.";
  std::lock_guard<std::mutex> lock(mutex_);
  system_status_.CopyFrom(message);
}

void Guardian::OnControl(const ControlCommand& message) {
  ADEBUG << "Received control data: run control command callback.";
  std::lock_guard<std::mutex> lock(mutex_);
  control_cmd_.CopyFrom(message);
}

void Guardian::ByPassControlCommand() {
  std::lock_guard<std::mutex> lock(mutex_);
  guardian_cmd_.mutable_control_command()->CopyFrom(control_cmd_);
}

void Guardian::TriggerSafetyMode() {
  ADEBUG << "Received chassis data: run chassis callback.";
  std::lock_guard<std::mutex> lock(mutex_);
  bool sensor_malfunction = false, obstacle_detected = false;
  if (!chassis_.surround().sonar_enabled() ||
      chassis_.surround().sonar_fault()) {
    AINFO
        << "Ultrasonic sensor not enabled for faulted, will do emergency stop!";
    sensor_malfunction = true;
  } else {
    // TODO(QiL) : Load for config
    for (int i = 0; i < chassis_.surround().sonar_range_size(); ++i) {
      if ((chassis_.surround().sonar_range(i) > 0.0 &&
           chassis_.surround().sonar_range(i) < 2.5) ||
          chassis_.surround().sonar_range(i) > 30) {
        AINFO << "Object detected or ultrasonic sensor fault output, will do "
                 "emergency stop!";
        obstacle_detected = true;
      }
    }
  }

  guardian_cmd_.mutable_control_command()->set_throttle(0.0);
  guardian_cmd_.mutable_control_command()->set_steering_target(0.0);
  guardian_cmd_.mutable_control_command()->set_steering_rate(0.0);
  guardian_cmd_.mutable_control_command()->set_is_in_safe_mode(true);

  if (system_status_.require_emergency_stop() || sensor_malfunction ||
      obstacle_detected) {
    AINFO << "Emergency stop triggered! with system status from monitor as : "
          << system_status_.require_emergency_stop();
    guardian_cmd_.mutable_control_command()->set_brake(
        FLAGS_guardian_cmd_emergency_stop_percentage);
  } else {
    AINFO << "Soft stop triggered! with system status from monitor as : "
          << system_status_.require_emergency_stop();
    guardian_cmd_.mutable_control_command()->set_brake(
        FLAGS_guardian_cmd_soft_stop_percentage);
  }
}

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

#include <cmath>

#include "modules/common/adapters/adapter_gflags.h"
#include "modules/common/adapters/adapter_manager.h"
#include "modules/common/log.h"
#include "modules/guardian/common/guardian_gflags.h"
#include "ros/include/ros/ros.h"

namespace apollo {
namespace guardian {

using apollo::canbus::Chassis;
using apollo::common::ErrorCode;
using apollo::common::Status;
using apollo::common::adapter::AdapterManager;
using apollo::control::ControlCommand;
using apollo::guardian::GuardianCommand;
using apollo::monitor::SystemStatus;

std::string Guardian::Name() const { return FLAGS_module_name; }

Status Guardian::Init() {
  AdapterManager::Init(FLAGS_adapter_config_filename);
  CHECK(AdapterManager::GetChassis()) << "Chassis is not initialized.";
  AdapterManager::AddChassisCallback(&Guardian::OnChassis, this);
  CHECK(AdapterManager::GetSystemStatus())
      << "SystemStatus is not initialized.";
  AdapterManager::AddSystemStatusCallback(&Guardian::OnSystemStatus, this);
  CHECK(AdapterManager::GetControlCommand()) << "Control is not initialized.";
  AdapterManager::AddControlCommandCallback(&Guardian::OnControl, this);
  return Status::OK();
}

Status Guardian::Start() {
  const double duration = 1.0 / FLAGS_guardian_cmd_freq;
  timer_ = AdapterManager::CreateTimer(ros::Duration(duration),
                                       &Guardian::OnTimer, this);

  return Status::OK();
}

void Guardian::Stop() { timer_.stop(); }

void Guardian::OnTimer(const ros::TimerEvent&) {
  ADEBUG << "Timer is triggered: publish Guardian result";
  bool safety_mode_triggered = false;
  {
    std::lock_guard<std::mutex> lock(mutex_);
    if (system_status_.has_safety_mode_trigger_time()) {
      safety_mode_triggered = true;
    }
  }

  if (safety_mode_triggered && FLAGS_guardian_enabled) {
    ADEBUG << "Safety mode triggerd, enable safty mode";
    TriggerSafetyMode();
  } else {
    ADEBUG << "Safety mode not triggerd, bypass control command";
    ByPassControlCommand();
  }

  AdapterManager::FillGuardianHeader(FLAGS_node_name, &guardian_cmd_);
  AdapterManager::PublishGuardian(guardian_cmd_);
}

void Guardian::OnChassis(const Chassis& message) {
  ADEBUG << "Received chassis data: run chassis callback.";
  std::lock_guard<std::mutex> lock(mutex_);
  chassis_.CopyFrom(message);
}

void Guardian::OnSystemStatus(const SystemStatus& message) {
  ADEBUG << "Received monitor data: run monitor callback.";
  std::lock_guard<std::mutex> lock(mutex_);
  system_status_.CopyFrom(message);
}

void Guardian::OnControl(const ControlCommand& message) {
  ADEBUG << "Received control data: run control command callback.";
  std::lock_guard<std::mutex> lock(mutex_);
  control_cmd_.CopyFrom(message);
}

void Guardian::ByPassControlCommand() {
  std::lock_guard<std::mutex> lock(mutex_);
  guardian_cmd_.mutable_control_command()->CopyFrom(control_cmd_);
}

void Guardian::TriggerSafetyMode() {
  ADEBUG << "Received chassis data: run chassis callback.";
  std::lock_guard<std::mutex> lock(mutex_);
  bool sensor_malfunction = false, obstacle_detected = false;
  if (!chassis_.surround().sonar_enabled() ||
      chassis_.surround().sonar_fault()) {
    AINFO << "Ultrasonic sensor not enabled for faulted, will do emergency "
             "stop!";
    sensor_malfunction = true;
  } else {
    // TODO(QiL) : Load for config
    for (int i = 0; i < chassis_.surround().sonar_range_size(); ++i) {
      if ((chassis_.surround().sonar_range(i) > 0.0 &&
           chassis_.surround().sonar_range(i) < 2.5) ||
          chassis_.surround().sonar_range(i) > 30) {
        AINFO << "Object detected or ultrasonic sensor fault output, will do "
                 "emergency stop!";
        obstacle_detected = true;
      }
    }
  }

  guardian_cmd_.mutable_control_command()->set_throttle(0.0);
  guardian_cmd_.mutable_control_command()->set_steering_target(0.0);
  guardian_cmd_.mutable_control_command()->set_steering_rate(0.0);
  guardian_cmd_.mutable_control_command()->set_is_in_safe_mode(true);

  if (system_status_.require_emergency_stop() || sensor_malfunction ||
      obstacle_detected) {
    AINFO << "Emergency stop triggered! with system status from monitor as : "
          << system_status_.require_emergency_stop();
    guardian_cmd_.mutable_control_command()->set_brake(
        FLAGS_guardian_cmd_emergency_stop_percentage);
  } else {
    AINFO << "Soft stop triggered! with system status from monitor as : "
          << system_status_.require_emergency_stop();
    guardian_cmd_.mutable_control_command()->set_brake(
        FLAGS_guardian_cmd_soft_stop_percentage);
  }
}

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


<commit_before>#include "stdafx.h"
#include "CppUnitTest.h"

#include <algorithm>
#include <string>
#include <vector>

#include "..\algorithms-and-data-structures\trie.h"

using namespace Microsoft::VisualStudio::CppUnitTestFramework;

#include <iostream>

namespace tests
{
    TEST_CLASS(TrieTest) {
public:
    TEST_METHOD(TrieSingleInsertion) {
        Trie trie;
        insert(trie, "a");
        Assert::IsTrue(contains(trie, "a"));
        Assert::IsFalse(contains(trie, "b"));
    }
    TEST_METHOD(TrieHeavyInsertion) {
        Trie trie;
        std::vector<std::string> strings = { "", "a", "b", "aa", "ab", "bb", "aaa", "aab", "abb", "bbb" };
        std::vector<std::string> inserted;
        std::vector<std::string> not_inserted;
        // Use 3/4 of the permutations.
        const short insert_before_skipping = 3;
        short counter = 0;
        for (auto string : strings) {
            bool got_larger_permutation = false;
            do {
                got_larger_permutation = std::next_permutation(string.begin(), string.end());
                if (counter == insert_before_skipping) {
                    counter = 0; // Reset the counter, do not add this permutation.
                    not_inserted.push_back(string);
                }
                else {
                    insert(trie, string);
                    inserted.push_back(string);

                    std::cout << "Inserted " << string << ".\n";

                    ++counter;
                }
            } while (got_larger_permutation);
        }
        // Verify all of them.
        for (auto string : inserted) Assert::IsTrue(contains(trie, string));
        for (auto string : not_inserted) Assert::IsFalse(contains(trie, string));
    }
    };
}
<commit_msg>Removed unwanted cout usage in a test.<commit_after>#include "stdafx.h"
#include "CppUnitTest.h"

#include <algorithm>
#include <string>
#include <vector>

#include "..\algorithms-and-data-structures\trie.h"

using namespace Microsoft::VisualStudio::CppUnitTestFramework;

#include <iostream>

namespace tests
{
    TEST_CLASS(TrieTest) {
public:
    TEST_METHOD(TrieSingleInsertion) {
        Trie trie;
        insert(trie, "a");
        Assert::IsTrue(contains(trie, "a"));
        Assert::IsFalse(contains(trie, "b"));
    }
    TEST_METHOD(TrieHeavyInsertion) {
        Trie trie;
        std::vector<std::string> strings = { "", "a", "b", "aa", "ab", "bb", "aaa", "aab", "abb", "bbb" };
        std::vector<std::string> inserted;
        std::vector<std::string> not_inserted;
        // Use 3/4 of the permutations.
        const short insert_before_skipping = 3;
        short counter = 0;
        for (auto string : strings) {
            bool got_larger_permutation = false;
            do {
                got_larger_permutation = std::next_permutation(string.begin(), string.end());
                if (counter == insert_before_skipping) {
                    counter = 0; // Reset the counter, do not add this permutation.
                    not_inserted.push_back(string);
                }
                else {
                    insert(trie, string);
                    inserted.push_back(string);
                    ++counter;
                }
            } while (got_larger_permutation);
        }
        // Verify all of them.
        for (auto string : inserted) Assert::IsTrue(contains(trie, string));
        for (auto string : not_inserted) Assert::IsFalse(contains(trie, string));
    }
    };
}
<|endoftext|>


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

#include "modules/planning/planning.h"

#include <algorithm>

#include "google/protobuf/repeated_field.h"
#include "modules/common/adapters/adapter_manager.h"
#include "modules/common/time/time.h"
#include "modules/common/vehicle_state/vehicle_state.h"
#include "modules/common/vehicle_state/vehicle_state.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/planner/em/em_planner.h"
#include "modules/planning/planner/rtk/rtk_replay_planner.h"
#include "modules/planning/trajectory_stitcher/trajectory_stitcher.h"

namespace apollo {
namespace planning {

using apollo::common::ErrorCode;
using apollo::common::Status;
using apollo::common::TrajectoryPoint;
using apollo::common::VehicleState;
using apollo::common::adapter::AdapterManager;
using apollo::common::time::Clock;

std::string Planning::Name() const { return "planning"; }

void Planning::RegisterPlanners() {
  planner_factory_.Register(
      PlanningConfig::RTK, []() -> Planner* { return new RTKReplayPlanner(); });
  planner_factory_.Register(PlanningConfig::EM,
                            []() -> Planner* { return new EMPlanner(); });
}

const Frame* Planning::GetFrame() const { return frame_.get(); }
const hdmap::PncMap* Planning::GetPncMap() const { return pnc_map_.get(); }

bool Planning::InitFrame(const uint32_t sequence_num) {
  frame_.reset(new Frame(sequence_num));
  if (AdapterManager::GetRoutingResponse()->Empty()) {
    AERROR << "Routing is empty";
    return false;
  }
  common::TrajectoryPoint point;
  frame_->SetVehicleInitPose(VehicleState::instance()->pose());
  frame_->SetRoutingResponse(
      AdapterManager::GetRoutingResponse()->GetLatestObserved());
  if (FLAGS_enable_prediction && !AdapterManager::GetPrediction()->Empty()) {
    const auto& prediction =
        AdapterManager::GetPrediction()->GetLatestObserved();
    frame_->SetPrediction(prediction);
    ADEBUG << "Get prediction";
  }

  if (!frame_->Init()) {
    AERROR << "failed to init frame";
    return false;
  }
  frame_->RecordInputDebug();
  return true;
}

Status Planning::Init() {
  pnc_map_.reset(new hdmap::PncMap(FLAGS_map_filename));
  Frame::SetMap(pnc_map_.get());

  if (!apollo::common::util::GetProtoFromFile(FLAGS_planning_config_file,
                                              &config_)) {
    AERROR << "failed to load planning config file "
           << FLAGS_planning_config_file;
    return Status(
        ErrorCode::PLANNING_ERROR,
        "failed to load planning config file: " + FLAGS_planning_config_file);
  }

  if (!AdapterManager::Initialized()) {
    AdapterManager::Init(FLAGS_adapter_config_path);
  }
  if (AdapterManager::GetLocalization() == nullptr) {
    std::string error_msg("Localization is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }
  if (AdapterManager::GetChassis() == nullptr) {
    std::string error_msg("Chassis is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }
  if (AdapterManager::GetRoutingResponse() == nullptr) {
    std::string error_msg("RoutingResponse is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }
  // TODO(all) temporarily use the offline routing data.
  if (!AdapterManager::GetRoutingResponse()->HasReceived()) {
    if (!AdapterManager::GetRoutingResponse()->FeedFile(
            FLAGS_offline_routing_file)) {
      auto error_msg = common::util::StrCat(
          "Failed to load offline routing file ", FLAGS_offline_routing_file);
      AERROR << error_msg;
      return Status(ErrorCode::PLANNING_ERROR, error_msg);
    } else {
      AWARN << "Using offline routing file " << FLAGS_offline_routing_file;
    }
  }
  if (AdapterManager::GetPrediction() == nullptr) {
    std::string error_msg("Prediction is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }

  RegisterPlanners();
  planner_ = planner_factory_.CreateObject(config_.planner_type());
  if (!planner_) {
    return Status(
        ErrorCode::PLANNING_ERROR,
        "planning is not initialized with config : " + config_.DebugString());
  }

  return planner_->Init(config_);
}

Status Planning::Start() {
  static ros::Rate loop_rate(FLAGS_planning_loop_rate);
  while (ros::ok()) {
    RunOnce();
    FrameHistory::instance()->Add(frame_->sequence_num(), std::move(frame_));
    ros::spinOnce();
    loop_rate.sleep();
  }
  return Status::OK();
}

void Planning::RunOnce() {
  AdapterManager::Observe();
  if (AdapterManager::GetLocalization()->Empty()) {
    AERROR << "Localization is not available; skip the planning cycle";
    return;
  }

  if (AdapterManager::GetChassis()->Empty()) {
    AERROR << "Chassis is not available; skip the planning cycle";
    return;
  }
  if (AdapterManager::GetRoutingResponse()->Empty()) {
    AERROR << "RoutingResponse is not available; skip the planning cycle";
    return;
  }

  // FIXME(all): enable prediction check when perception and prediction is
  // ready.
  // if (FLAGS_enable_prediction && AdapterManager::GetPrediction()->Empty()) {
  //   AERROR << "Prediction is not available; skip the planning cycle";
  //   return;
  // }

  AINFO << "Start planning ...";
  const double start_timestamp = apollo::common::time::ToSecond(Clock::Now());

  // localization
  const auto& localization =
      AdapterManager::GetLocalization()->GetLatestObserved();
  ADEBUG << "Get localization:" << localization.DebugString();

  // chassis
  const auto& chassis = AdapterManager::GetChassis()->GetLatestObserved();
  ADEBUG << "Get chassis:" << chassis.DebugString();

  common::VehicleState::instance()->Update(localization, chassis);

  const double planning_cycle_time = 1.0 / FLAGS_planning_loop_rate;

  const uint32_t frame_num = AdapterManager::GetPlanning()->GetSeqNum() + 1;
  if (!InitFrame(frame_num)) {
    AERROR << "Init frame failed";
    return;
  }

  bool is_auto_mode = chassis.driving_mode() == chassis.COMPLETE_AUTO_DRIVE;
  bool res_planning = Plan(is_auto_mode, start_timestamp, planning_cycle_time);

  const double end_timestamp = apollo::common::time::ToSecond(Clock::Now());
  const double time_diff_ms = (end_timestamp - start_timestamp) * 1000;
  auto trajectory_pb = frame_->MutableADCTrajectory();
  trajectory_pb->mutable_latency_stats()->set_total_time_ms(time_diff_ms);
  ADEBUG << "Planning latency: "
         << trajectory_pb->latency_stats().DebugString();

  if (res_planning) {
    AdapterManager::FillPlanningHeader("planning", trajectory_pb);
    trajectory_pb->mutable_header()->set_timestamp_sec(start_timestamp);
    // TODO(all): integrate reverse gear
    trajectory_pb->set_gear(canbus::Chassis::GEAR_DRIVE);
    AdapterManager::PublishPlanning(*trajectory_pb);
    ADEBUG << "Planning succeeded:" << trajectory_pb->header().DebugString();
  } else {
    AERROR << "Planning failed";
  }
}

void Planning::Stop() {}

bool Planning::Plan(const bool is_on_auto_mode, const double current_time_stamp,
                    const double planning_cycle_time) {
  const auto& stitching_trajectory =
      TrajectoryStitcher::ComputeStitchingTrajectory(
          is_on_auto_mode, current_time_stamp, planning_cycle_time,
          last_publishable_trajectory_);

  frame_->SetPlanningStartPoint(stitching_trajectory.back());

  auto trajectory_pb = frame_->MutableADCTrajectory();
  if (FLAGS_enable_record_debug) {
    trajectory_pb->mutable_debug()
        ->mutable_planning_data()
        ->mutable_init_point()
        ->CopyFrom(stitching_trajectory.back());
  }

  frame_->AlignPredictionTime(current_time_stamp);

  PublishableTrajectory publishable_trajectory;
  auto status = planner_->Plan(stitching_trajectory.back(), frame_.get(),
                               &publishable_trajectory);

  if (status != Status::OK()) {
    AERROR << "planner failed to make a driving plan";
    last_publishable_trajectory_.Clear();
    return false;
  }

  publishable_trajectory.PrependTrajectoryPoints(
      stitching_trajectory.begin(), stitching_trajectory.end() - 1);

  publishable_trajectory.set_header_time(current_time_stamp);

  publishable_trajectory.PopulateTrajectoryProtobuf(trajectory_pb);
  trajectory_pb->set_is_replan(stitching_trajectory.size() == 1);

  // update last publishable trajectory;
  last_publishable_trajectory_ = std::move(publishable_trajectory);

  return true;
}

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

#include "modules/planning/planning.h"

#include <algorithm>

#include "google/protobuf/repeated_field.h"
#include "modules/common/adapters/adapter_manager.h"
#include "modules/common/time/time.h"
#include "modules/common/vehicle_state/vehicle_state.h"
#include "modules/common/vehicle_state/vehicle_state.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/planner/em/em_planner.h"
#include "modules/planning/planner/rtk/rtk_replay_planner.h"
#include "modules/planning/trajectory_stitcher/trajectory_stitcher.h"

namespace apollo {
namespace planning {

using apollo::common::ErrorCode;
using apollo::common::Status;
using apollo::common::TrajectoryPoint;
using apollo::common::VehicleState;
using apollo::common::adapter::AdapterManager;
using apollo::common::time::Clock;

std::string Planning::Name() const { return "planning"; }

void Planning::RegisterPlanners() {
  planner_factory_.Register(
      PlanningConfig::RTK, []() -> Planner* { return new RTKReplayPlanner(); });
  planner_factory_.Register(PlanningConfig::EM,
                            []() -> Planner* { return new EMPlanner(); });
}

const Frame* Planning::GetFrame() const { return frame_.get(); }
const hdmap::PncMap* Planning::GetPncMap() const { return pnc_map_.get(); }

bool Planning::InitFrame(const uint32_t sequence_num) {
  frame_.reset(new Frame(sequence_num));
  if (AdapterManager::GetRoutingResponse()->Empty()) {
    AERROR << "Routing is empty";
    return false;
  }
  common::TrajectoryPoint point;
  frame_->SetVehicleInitPose(VehicleState::instance()->pose());
  frame_->SetRoutingResponse(
      AdapterManager::GetRoutingResponse()->GetLatestObserved());
  if (FLAGS_enable_prediction && !AdapterManager::GetPrediction()->Empty()) {
    const auto& prediction =
        AdapterManager::GetPrediction()->GetLatestObserved();
    frame_->SetPrediction(prediction);
    ADEBUG << "Get prediction";
  }

  if (!frame_->Init()) {
    AERROR << "failed to init frame";
    return false;
  }
  frame_->RecordInputDebug();
  return true;
}

Status Planning::Init() {
  pnc_map_.reset(new hdmap::PncMap(FLAGS_map_filename));
  Frame::SetMap(pnc_map_.get());

  if (!apollo::common::util::GetProtoFromFile(FLAGS_planning_config_file,
                                              &config_)) {
    AERROR << "failed to load planning config file "
           << FLAGS_planning_config_file;
    return Status(
        ErrorCode::PLANNING_ERROR,
        "failed to load planning config file: " + FLAGS_planning_config_file);
  }

  if (!AdapterManager::Initialized()) {
    AdapterManager::Init(FLAGS_adapter_config_path);
  }
  if (AdapterManager::GetLocalization() == nullptr) {
    std::string error_msg("Localization is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }
  if (AdapterManager::GetChassis() == nullptr) {
    std::string error_msg("Chassis is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }
  if (AdapterManager::GetRoutingResponse() == nullptr) {
    std::string error_msg("RoutingResponse is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }
  // TODO(all) temporarily use the offline routing data.
  if (!AdapterManager::GetRoutingResponse()->HasReceived()) {
    if (!AdapterManager::GetRoutingResponse()->FeedFile(
            FLAGS_offline_routing_file)) {
      auto error_msg = common::util::StrCat(
          "Failed to load offline routing file ", FLAGS_offline_routing_file);
      AERROR << error_msg;
      return Status(ErrorCode::PLANNING_ERROR, error_msg);
    } else {
      AWARN << "Using offline routing file " << FLAGS_offline_routing_file;
    }
  }
  if (AdapterManager::GetPrediction() == nullptr) {
    std::string error_msg("Prediction is not registered");
    AERROR << error_msg;
    return Status(ErrorCode::PLANNING_ERROR, error_msg);
  }

  RegisterPlanners();
  planner_ = planner_factory_.CreateObject(config_.planner_type());
  if (!planner_) {
    return Status(
        ErrorCode::PLANNING_ERROR,
        "planning is not initialized with config : " + config_.DebugString());
  }

  return planner_->Init(config_);
}

Status Planning::Start() {
  static ros::Rate loop_rate(FLAGS_planning_loop_rate);
  while (ros::ok()) {
    RunOnce();
    if (frame_) {
      auto seq_num = frame_->sequence_num();
      FrameHistory::instance()->Add(seq_num, std::move(frame_));
    }
    ros::spinOnce();
    loop_rate.sleep();
  }
  return Status::OK();
}

void Planning::RunOnce() {
  AdapterManager::Observe();
  if (AdapterManager::GetLocalization()->Empty()) {
    AERROR << "Localization is not available; skip the planning cycle";
    return;
  }

  if (AdapterManager::GetChassis()->Empty()) {
    AERROR << "Chassis is not available; skip the planning cycle";
    return;
  }
  if (AdapterManager::GetRoutingResponse()->Empty()) {
    AERROR << "RoutingResponse is not available; skip the planning cycle";
    return;
  }

  // FIXME(all): enable prediction check when perception and prediction is
  // ready.
  // if (FLAGS_enable_prediction && AdapterManager::GetPrediction()->Empty()) {
  //   AERROR << "Prediction is not available; skip the planning cycle";
  //   return;
  // }

  AINFO << "Start planning ...";
  const double start_timestamp = apollo::common::time::ToSecond(Clock::Now());

  // localization
  const auto& localization =
      AdapterManager::GetLocalization()->GetLatestObserved();
  ADEBUG << "Get localization:" << localization.DebugString();

  // chassis
  const auto& chassis = AdapterManager::GetChassis()->GetLatestObserved();
  ADEBUG << "Get chassis:" << chassis.DebugString();

  common::VehicleState::instance()->Update(localization, chassis);

  const double planning_cycle_time = 1.0 / FLAGS_planning_loop_rate;

  const uint32_t frame_num = AdapterManager::GetPlanning()->GetSeqNum() + 1;
  if (!InitFrame(frame_num)) {
    AERROR << "Init frame failed";
    return;
  }

  bool is_auto_mode = chassis.driving_mode() == chassis.COMPLETE_AUTO_DRIVE;
  bool res_planning = Plan(is_auto_mode, start_timestamp, planning_cycle_time);

  const double end_timestamp = apollo::common::time::ToSecond(Clock::Now());
  const double time_diff_ms = (end_timestamp - start_timestamp) * 1000;
  auto trajectory_pb = frame_->MutableADCTrajectory();
  trajectory_pb->mutable_latency_stats()->set_total_time_ms(time_diff_ms);
  ADEBUG << "Planning latency: "
         << trajectory_pb->latency_stats().DebugString();

  if (res_planning) {
    AdapterManager::FillPlanningHeader("planning", trajectory_pb);
    trajectory_pb->mutable_header()->set_timestamp_sec(start_timestamp);
    // TODO(all): integrate reverse gear
    trajectory_pb->set_gear(canbus::Chassis::GEAR_DRIVE);
    AdapterManager::PublishPlanning(*trajectory_pb);
    ADEBUG << "Planning succeeded:" << trajectory_pb->header().DebugString();
  } else {
    AERROR << "Planning failed";
  }
}

void Planning::Stop() {}

bool Planning::Plan(const bool is_on_auto_mode, const double current_time_stamp,
                    const double planning_cycle_time) {
  const auto& stitching_trajectory =
      TrajectoryStitcher::ComputeStitchingTrajectory(
          is_on_auto_mode, current_time_stamp, planning_cycle_time,
          last_publishable_trajectory_);

  frame_->SetPlanningStartPoint(stitching_trajectory.back());

  auto trajectory_pb = frame_->MutableADCTrajectory();
  if (FLAGS_enable_record_debug) {
    trajectory_pb->mutable_debug()
        ->mutable_planning_data()
        ->mutable_init_point()
        ->CopyFrom(stitching_trajectory.back());
  }

  frame_->AlignPredictionTime(current_time_stamp);

  PublishableTrajectory publishable_trajectory;
  auto status = planner_->Plan(stitching_trajectory.back(), frame_.get(),
                               &publishable_trajectory);

  if (status != Status::OK()) {
    AERROR << "planner failed to make a driving plan";
    last_publishable_trajectory_.Clear();
    return false;
  }

  publishable_trajectory.PrependTrajectoryPoints(
      stitching_trajectory.begin(), stitching_trajectory.end() - 1);

  publishable_trajectory.set_header_time(current_time_stamp);

  publishable_trajectory.PopulateTrajectoryProtobuf(trajectory_pb);
  trajectory_pb->set_is_replan(stitching_trajectory.size() == 1);

  // update last publishable trajectory;
  last_publishable_trajectory_ = std::move(publishable_trajectory);

  return true;
}

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


<commit_before>// $Id$

/***********************************************************************
Moses - factored phrase-based language decoder
Copyright (c) 2006 University of Edinburgh
All rights reserved.

Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:

    * Redistributions of source code must retain the above copyright notice,
			this list of conditions and the following disclaimer.
    * Redistributions in binary form must reproduce the above copyright notice,
			this list of conditions and the following disclaimer in the documentation
			and/or other materials provided with the distribution.
    * Neither the name of the University of Edinburgh nor the names of its contributors
			may be used to endorse or promote products derived from this software
			without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS
BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
POSSIBILITY OF SUCH DAMAGE.
***********************************************************************/

// example file on how to use moses library

#ifdef WIN32
// Include Visual Leak Detector
//#include <vld.h>
#endif

#include <exception>
#include <fstream>
#include "Main.h"
#include "FactorCollection.h"
#include "Manager.h"
#include "Phrase.h"
#include "Util.h"
#include "Timer.h"
#include "IOWrapper.h"
#include "Sentence.h"
#include "ConfusionNet.h"
#include "WordLattice.h"
#include "TreeInput.h"
#include "TranslationAnalysis.h"
#include "mbr.h"
#include "ThreadPool.h"
#include "ChartManager.h"
#include "ChartHypothesis.h"
#include "ChartTrellisPath.h"
#include "ChartTrellisPathList.h"
#include "Incremental/Manager.h"

#include "util/usage.hh"

using namespace std;
using namespace Moses;
using namespace MosesChartCmd;

/**
  * Translates a sentence.
 **/
class TranslationTask : public Task
{
public:
  TranslationTask(InputType *source, IOWrapper &ioWrapper)
    : m_source(source)
    , m_ioWrapper(ioWrapper)
  {}

  ~TranslationTask() {
    delete m_source;
  }

  void Run() {
    const StaticData &staticData = StaticData::Instance();
    const TranslationSystem &system = staticData.GetTranslationSystem(TranslationSystem::DEFAULT);
    const size_t lineNumber = m_source->GetTranslationId();

    VERBOSE(2,"\nTRANSLATING(" << lineNumber << "): " << *m_source);

    if ((*m_source).GetSize() == 0) return;

    if (staticData.GetSearchAlgorithm() == ChartIncremental) {
      Incremental::Manager manager(*m_source, system);
      manager.ProcessSentence();
      if (m_ioWrapper.ExposeSingleBest()) {
        m_ioWrapper.ExposeSingleBest()->Write(lineNumber, manager.String() + '\n');
      }
      return;
    }

    ChartManager manager(*m_source, &system);
    manager.ProcessSentence();

    CHECK(!staticData.UseMBR());

    // 1-best
    const ChartHypothesis *bestHypo = manager.GetBestHypothesis();
    m_ioWrapper.OutputBestHypo(bestHypo, lineNumber);
    IFVERBOSE(2) {
      PrintUserTime("Best Hypothesis Generation Time:");
    }

    if (staticData.IsDetailedTranslationReportingEnabled()) {
      const Sentence &sentence = dynamic_cast<const Sentence &>(*m_source);
      m_ioWrapper.OutputDetailedTranslationReport(bestHypo, sentence, lineNumber);
    }

    // n-best
    size_t nBestSize = staticData.GetNBestSize();
    if (nBestSize > 0) {
      VERBOSE(2,"WRITING " << nBestSize << " TRANSLATION ALTERNATIVES TO " << staticData.GetNBestFilePath() << endl);
      ChartTrellisPathList nBestList;
      manager.CalcNBest(nBestSize, nBestList,staticData.GetDistinctNBest());
      m_ioWrapper.OutputNBestList(nBestList, bestHypo, &system, lineNumber);
      IFVERBOSE(2) {
        PrintUserTime("N-Best Hypotheses Generation Time:");
      }
    }

    if (staticData.GetOutputSearchGraph()) {
      std::ostringstream out;
      manager.GetSearchGraph(lineNumber, out);
      OutputCollector *oc = m_ioWrapper.GetSearchGraphOutputCollector();
      CHECK(oc);
      oc->Write(lineNumber, out.str());
    }

    IFVERBOSE(2) {
      PrintUserTime("Sentence Decoding Time:");
    }
    manager.CalcDecoderStatistics();
  }

private:
  // Non-copyable: copy constructor and assignment operator not implemented.
  TranslationTask(const TranslationTask &);
  TranslationTask &operator=(const TranslationTask &);

  InputType *m_source;
  IOWrapper &m_ioWrapper;
};

bool ReadInput(IOWrapper &ioWrapper, InputTypeEnum inputType, InputType*& source)
{
  delete source;
  switch(inputType) {
  case SentenceInput:
    source = ioWrapper.GetInput(new Sentence);
    break;
  case ConfusionNetworkInput:
    source = ioWrapper.GetInput(new ConfusionNet);
    break;
  case WordLatticeInput:
    source = ioWrapper.GetInput(new WordLattice);
    break;
  case TreeInputType:
    source = ioWrapper.GetInput(new TreeInput);
    break;
  default:
    TRACE_ERR("Unknown input type: " << inputType << "\n");
  }
  return (source ? true : false);
}
static void PrintFeatureWeight(const FeatureFunction* ff)
{
  size_t numScoreComps = ff->GetNumScoreComponents();
  if (numScoreComps != ScoreProducer::unlimited) {
    vector<float> values = StaticData::Instance().GetAllWeights().GetScoresForProducer(ff);
    for (size_t i = 0; i < numScoreComps; ++i)
      cout << ff->GetScoreProducerDescription() <<  " "
           << ff->GetScoreProducerWeightShortName() << " "
           << values[i] << endl;
  }
  else {
  	if (ff->GetSparseProducerWeight() == 1)
  		cout << ff->GetScoreProducerDescription() << " " <<
  		ff->GetScoreProducerWeightShortName() << " sparse" << endl;
  	else
  		cout << ff->GetScoreProducerDescription() << " " <<
  		ff->GetScoreProducerWeightShortName() << " " << ff->GetSparseProducerWeight() << endl;
  }
}

static void ShowWeights()
{
  cout.precision(6);
  const StaticData& staticData = StaticData::Instance();
  const TranslationSystem& system = staticData.GetTranslationSystem(TranslationSystem::DEFAULT);
  const vector<const StatelessFeatureFunction*>& slf =system.GetStatelessFeatureFunctions();
  const vector<const StatefulFeatureFunction*>& sff = system.GetStatefulFeatureFunctions();
  for (size_t i = 0; i < sff.size(); ++i) {
    PrintFeatureWeight(sff[i]);
  }
  for (size_t i = 0; i < slf.size(); ++i) {
    if (slf[i]->GetScoreProducerWeightShortName() != "u") {
  	  PrintFeatureWeight(slf[i]);
    }
  }
}

int main(int argc, char* argv[])
{
  try {
    IFVERBOSE(1) {
      TRACE_ERR("command: ");
      for(int i=0; i<argc; ++i) TRACE_ERR(argv[i]<<" ");
      TRACE_ERR(endl);
    }

    IOWrapper::FixPrecision(cout);
    IOWrapper::FixPrecision(cerr);

    // load data structures
    Parameter parameter;
    if (!parameter.LoadParam(argc, argv)) {
      return EXIT_FAILURE;
    }

    const StaticData &staticData = StaticData::Instance();
    if (!StaticData::LoadDataStatic(&parameter, argv[0]))
      return EXIT_FAILURE;

    if (parameter.isParamSpecified("show-weights")) {
      ShowWeights();
      exit(0);
    }

    CHECK(staticData.IsChart());

    // set up read/writing class
    IOWrapper *ioWrapper = GetIOWrapper(staticData);

    // check on weights
    const ScoreComponentCollection& weights = staticData.GetAllWeights();
    IFVERBOSE(2) {
      TRACE_ERR("The global weight vector looks like this: ");
      TRACE_ERR(weights);
      TRACE_ERR("\n");
    }

    if (ioWrapper == NULL)
      return EXIT_FAILURE;

#ifdef WITH_THREADS
    ThreadPool pool(staticData.ThreadCount());
#endif

    // read each sentence & decode
    InputType *source=0;
    while(ReadInput(*ioWrapper,staticData.GetInputType(),source)) {
      IFVERBOSE(1)
      ResetUserTime();
      TranslationTask *task = new TranslationTask(source, *ioWrapper);
      source = NULL;  // task will delete source
#ifdef WITH_THREADS
      pool.Submit(task);  // pool will delete task
#else
      task->Run();
      delete task;
#endif
    }

#ifdef WITH_THREADS
    pool.Stop(true);  // flush remaining jobs
#endif

    delete ioWrapper;

    IFVERBOSE(1)
    PrintUserTime("End.");

  } catch (const std::exception &e) {
    std::cerr << "Exception: " << e.what() << std::endl;
    return EXIT_FAILURE;
  }

  IFVERBOSE(1) util::PrintUsage(std::cerr);

#ifdef HACK_EXIT
  //This avoids that detructors are called (it can take a long time)
  exit(EXIT_SUCCESS);
#else
  return EXIT_SUCCESS;
#endif
}

IOWrapper *GetIOWrapper(const StaticData &staticData)
{
  IOWrapper *ioWrapper;
  const std::vector<FactorType> &inputFactorOrder = staticData.GetInputFactorOrder()
      ,&outputFactorOrder = staticData.GetOutputFactorOrder();
  FactorMask inputFactorUsed(inputFactorOrder);

  // io
  if (staticData.GetParam("input-file").size() == 1) {
    VERBOSE(2,"IO from File" << endl);
    string filePath = staticData.GetParam("input-file")[0];

    ioWrapper = new IOWrapper(inputFactorOrder, outputFactorOrder, inputFactorUsed
                              , staticData.GetNBestSize()
                              , staticData.GetNBestFilePath()
                              , filePath);
  } else {
    VERBOSE(1,"IO from STDOUT/STDIN" << endl);
    ioWrapper = new IOWrapper(inputFactorOrder, outputFactorOrder, inputFactorUsed
                              , staticData.GetNBestSize()
                              , staticData.GetNBestFilePath());
  }
  ioWrapper->ResetTranslationId();

  IFVERBOSE(1)
  PrintUserTime("Created input-output object");

  return ioWrapper;
}
<commit_msg>fix order in show-weights<commit_after>// $Id$

/***********************************************************************
Moses - factored phrase-based language decoder
Copyright (c) 2006 University of Edinburgh
All rights reserved.

Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:

    * Redistributions of source code must retain the above copyright notice,
			this list of conditions and the following disclaimer.
    * Redistributions in binary form must reproduce the above copyright notice,
			this list of conditions and the following disclaimer in the documentation
			and/or other materials provided with the distribution.
    * Neither the name of the University of Edinburgh nor the names of its contributors
			may be used to endorse or promote products derived from this software
			without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS
BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
POSSIBILITY OF SUCH DAMAGE.
***********************************************************************/

// example file on how to use moses library

#ifdef WIN32
// Include Visual Leak Detector
//#include <vld.h>
#endif

#include <exception>
#include <fstream>
#include "Main.h"
#include "DummyScoreProducers.h"
#include "FactorCollection.h"
#include "Manager.h"
#include "Phrase.h"
#include "Util.h"
#include "Timer.h"
#include "IOWrapper.h"
#include "Sentence.h"
#include "ConfusionNet.h"
#include "WordLattice.h"
#include "TreeInput.h"
#include "TranslationAnalysis.h"
#include "mbr.h"
#include "ThreadPool.h"
#include "ChartManager.h"
#include "ChartHypothesis.h"
#include "ChartTrellisPath.h"
#include "ChartTrellisPathList.h"
#include "Incremental/Manager.h"

#include "util/usage.hh"

using namespace std;
using namespace Moses;
using namespace MosesChartCmd;

/**
  * Translates a sentence.
 **/
class TranslationTask : public Task
{
public:
  TranslationTask(InputType *source, IOWrapper &ioWrapper)
    : m_source(source)
    , m_ioWrapper(ioWrapper)
  {}

  ~TranslationTask() {
    delete m_source;
  }

  void Run() {
    const StaticData &staticData = StaticData::Instance();
    const TranslationSystem &system = staticData.GetTranslationSystem(TranslationSystem::DEFAULT);
    const size_t lineNumber = m_source->GetTranslationId();

    VERBOSE(2,"\nTRANSLATING(" << lineNumber << "): " << *m_source);

    if ((*m_source).GetSize() == 0) return;

    if (staticData.GetSearchAlgorithm() == ChartIncremental) {
      Incremental::Manager manager(*m_source, system);
      manager.ProcessSentence();
      if (m_ioWrapper.ExposeSingleBest()) {
        m_ioWrapper.ExposeSingleBest()->Write(lineNumber, manager.String() + '\n');
      }
      return;
    }

    ChartManager manager(*m_source, &system);
    manager.ProcessSentence();

    CHECK(!staticData.UseMBR());

    // 1-best
    const ChartHypothesis *bestHypo = manager.GetBestHypothesis();
    m_ioWrapper.OutputBestHypo(bestHypo, lineNumber);
    IFVERBOSE(2) {
      PrintUserTime("Best Hypothesis Generation Time:");
    }

    if (staticData.IsDetailedTranslationReportingEnabled()) {
      const Sentence &sentence = dynamic_cast<const Sentence &>(*m_source);
      m_ioWrapper.OutputDetailedTranslationReport(bestHypo, sentence, lineNumber);
    }

    // n-best
    size_t nBestSize = staticData.GetNBestSize();
    if (nBestSize > 0) {
      VERBOSE(2,"WRITING " << nBestSize << " TRANSLATION ALTERNATIVES TO " << staticData.GetNBestFilePath() << endl);
      ChartTrellisPathList nBestList;
      manager.CalcNBest(nBestSize, nBestList,staticData.GetDistinctNBest());
      m_ioWrapper.OutputNBestList(nBestList, bestHypo, &system, lineNumber);
      IFVERBOSE(2) {
        PrintUserTime("N-Best Hypotheses Generation Time:");
      }
    }

    if (staticData.GetOutputSearchGraph()) {
      std::ostringstream out;
      manager.GetSearchGraph(lineNumber, out);
      OutputCollector *oc = m_ioWrapper.GetSearchGraphOutputCollector();
      CHECK(oc);
      oc->Write(lineNumber, out.str());
    }

    IFVERBOSE(2) {
      PrintUserTime("Sentence Decoding Time:");
    }
    manager.CalcDecoderStatistics();
  }

private:
  // Non-copyable: copy constructor and assignment operator not implemented.
  TranslationTask(const TranslationTask &);
  TranslationTask &operator=(const TranslationTask &);

  InputType *m_source;
  IOWrapper &m_ioWrapper;
};

bool ReadInput(IOWrapper &ioWrapper, InputTypeEnum inputType, InputType*& source)
{
  delete source;
  switch(inputType) {
  case SentenceInput:
    source = ioWrapper.GetInput(new Sentence);
    break;
  case ConfusionNetworkInput:
    source = ioWrapper.GetInput(new ConfusionNet);
    break;
  case WordLatticeInput:
    source = ioWrapper.GetInput(new WordLattice);
    break;
  case TreeInputType:
    source = ioWrapper.GetInput(new TreeInput);
    break;
  default:
    TRACE_ERR("Unknown input type: " << inputType << "\n");
  }
  return (source ? true : false);
}
static void PrintFeatureWeight(const FeatureFunction* ff)
{
  size_t numScoreComps = ff->GetNumScoreComponents();
  if (numScoreComps != ScoreProducer::unlimited) {
    vector<float> values = StaticData::Instance().GetAllWeights().GetScoresForProducer(ff);
    for (size_t i = 0; i < numScoreComps; ++i)
      cout << ff->GetScoreProducerDescription() <<  " "
           << ff->GetScoreProducerWeightShortName() << " "
           << values[i] << endl;
  }
  else {
  	if (ff->GetSparseProducerWeight() == 1)
  		cout << ff->GetScoreProducerDescription() << " " <<
  		ff->GetScoreProducerWeightShortName() << " sparse" << endl;
  	else
  		cout << ff->GetScoreProducerDescription() << " " <<
  		ff->GetScoreProducerWeightShortName() << " " << ff->GetSparseProducerWeight() << endl;
  }
}

static void ShowWeights()
{
  cout.precision(6);
  const StaticData& staticData = StaticData::Instance();
  const TranslationSystem& system = staticData.GetTranslationSystem(TranslationSystem::DEFAULT);
  //This has to match the order in the nbest list

  //LMs
  const LMList& lml = system.GetLanguageModels();
  LMList::const_iterator lmi = lml.begin();
  for (; lmi != lml.end(); ++lmi) {
    PrintFeatureWeight(*lmi);
  }

  //sparse stateful ffs
  const vector<const StatefulFeatureFunction*>& sff = system.GetStatefulFeatureFunctions();
  for( size_t i=0; i<sff.size(); i++ ) {
    if (sff[i]->GetNumScoreComponents() == ScoreProducer::unlimited) {
      PrintFeatureWeight(sff[i]);
    }
  }

  // translation components - phrase dicts
  const vector<PhraseDictionaryFeature*>& pds = system.GetPhraseDictionaries();
  for( size_t i=0; i<pds.size(); i++ ) {
    PrintFeatureWeight(pds[i]);
  }

  //word penalty
  PrintFeatureWeight(system.GetWordPenaltyProducer());

  //generation dicts
  const vector<GenerationDictionary*>& gds = system.GetGenerationDictionaries();
  for( size_t i=0; i<gds.size(); i++ ) {
    PrintFeatureWeight(gds[i]);
  }

  //sparse stateless ffs
  const vector<const StatelessFeatureFunction*>& slf = system.GetStatelessFeatureFunctions();
  for( size_t i=0; i<slf.size(); i++ ) {
    if (slf[i]->GetNumScoreComponents() == ScoreProducer::unlimited) {
      PrintFeatureWeight(slf[i]);
    }
  }

}

int main(int argc, char* argv[])
{
  try {
    IFVERBOSE(1) {
      TRACE_ERR("command: ");
      for(int i=0; i<argc; ++i) TRACE_ERR(argv[i]<<" ");
      TRACE_ERR(endl);
    }

    IOWrapper::FixPrecision(cout);
    IOWrapper::FixPrecision(cerr);

    // load data structures
    Parameter parameter;
    if (!parameter.LoadParam(argc, argv)) {
      return EXIT_FAILURE;
    }

    const StaticData &staticData = StaticData::Instance();
    if (!StaticData::LoadDataStatic(&parameter, argv[0]))
      return EXIT_FAILURE;

    if (parameter.isParamSpecified("show-weights")) {
      ShowWeights();
      exit(0);
    }

    CHECK(staticData.IsChart());

    // set up read/writing class
    IOWrapper *ioWrapper = GetIOWrapper(staticData);

    // check on weights
    const ScoreComponentCollection& weights = staticData.GetAllWeights();
    IFVERBOSE(2) {
      TRACE_ERR("The global weight vector looks like this: ");
      TRACE_ERR(weights);
      TRACE_ERR("\n");
    }

    if (ioWrapper == NULL)
      return EXIT_FAILURE;

#ifdef WITH_THREADS
    ThreadPool pool(staticData.ThreadCount());
#endif

    // read each sentence & decode
    InputType *source=0;
    while(ReadInput(*ioWrapper,staticData.GetInputType(),source)) {
      IFVERBOSE(1)
      ResetUserTime();
      TranslationTask *task = new TranslationTask(source, *ioWrapper);
      source = NULL;  // task will delete source
#ifdef WITH_THREADS
      pool.Submit(task);  // pool will delete task
#else
      task->Run();
      delete task;
#endif
    }

#ifdef WITH_THREADS
    pool.Stop(true);  // flush remaining jobs
#endif

    delete ioWrapper;

    IFVERBOSE(1)
    PrintUserTime("End.");

  } catch (const std::exception &e) {
    std::cerr << "Exception: " << e.what() << std::endl;
    return EXIT_FAILURE;
  }

  IFVERBOSE(1) util::PrintUsage(std::cerr);

#ifdef HACK_EXIT
  //This avoids that detructors are called (it can take a long time)
  exit(EXIT_SUCCESS);
#else
  return EXIT_SUCCESS;
#endif
}

IOWrapper *GetIOWrapper(const StaticData &staticData)
{
  IOWrapper *ioWrapper;
  const std::vector<FactorType> &inputFactorOrder = staticData.GetInputFactorOrder()
      ,&outputFactorOrder = staticData.GetOutputFactorOrder();
  FactorMask inputFactorUsed(inputFactorOrder);

  // io
  if (staticData.GetParam("input-file").size() == 1) {
    VERBOSE(2,"IO from File" << endl);
    string filePath = staticData.GetParam("input-file")[0];

    ioWrapper = new IOWrapper(inputFactorOrder, outputFactorOrder, inputFactorUsed
                              , staticData.GetNBestSize()
                              , staticData.GetNBestFilePath()
                              , filePath);
  } else {
    VERBOSE(1,"IO from STDOUT/STDIN" << endl);
    ioWrapper = new IOWrapper(inputFactorOrder, outputFactorOrder, inputFactorUsed
                              , staticData.GetNBestSize()
                              , staticData.GetNBestFilePath());
  }
  ioWrapper->ResetTranslationId();

  IFVERBOSE(1)
  PrintUserTime("Created input-output object");

  return ioWrapper;
}
<|endoftext|>


<commit_before>#include "tiles.hpp"
#include "../utils/utils.hpp"
#include "../utils/safeops.hpp"
#include <cerrno>
#include <cstdio>
#include <ctime>
#include <limits>

bool Tiles::hashvecinit = false;
unsigned long Tiles::hashvec[Ntiles][Ntiles];

Tiles::Tiles() {
	initops();
	if (!hashvecinit)
		inithashvec();
}

Tiles::Tiles(FILE *in) {
	readruml(in);
	initops();
	if (!hashvecinit)
		inithashvec();
}

void Tiles::readruml(FILE *in) {
	unsigned int w, h;

	if (fscanf(in, " %u %u", &w, &h) != 2)
		fatalx(errno, "Failed to read width and height");

	if (!safe::can_mul(w, h))
		fatal("The tiles board is too big");

	if (w != Width && h != HEIGHT)
		fatal("Width and height instance/compiler option mismatch");

	if (fscanf(in, " starting positions for each tile:") != 0)
		fatalx(errno, "Failed to read the starting position label");

	for (Tile t = 0; t < Ntiles; t++) {
		unsigned int p;
		int r = fscanf(in, " %u", &p);
		if (r != 1)
			fatalx(errno, "Failed to read the starting positions: r=%d", r);
		init[p] = t;
	}

	if (fscanf(in, " goal positions:") != 0)
		fatalx(errno, "Failed to read the goal position label");

	for (Tile t = 0; t < Ntiles; t++) {
		unsigned int p;
		if (fscanf(in, " %u", &p) != 1)
			fatalx(errno, "Failed to read the goal position");
		goalpos[t] = p;
	}
}

void Tiles::initops() {
	for (unsigned int i = 0; i < Ntiles; i++) {
		ops[i].n = 0;
		if (i >= Width)
			ops[i].mvs[ops[i].n++] = i - Width;
		if (i % Width > 0)
			ops[i].mvs[ops[i].n++] = i - 1;
		if (i % Width < Width - 1)
			ops[i].mvs[ops[i].n++] = i + 1;
		if (i < Ntiles - Width)
			ops[i].mvs[ops[i].n++] = i + Width;
	}
}

void Tiles::inithashvec() {
	hashvecinit = true;
	Rand r(time(NULL));
	for (int i = 0; i < Ntiles; i++) {
	for (int j = 0; j < Ntiles; j++)
		hashvec[i][j] = r.bits();
	}
}

void Tiles::dumptiles(FILE *out, Tile ts[]) {
	for (unsigned int i = 0; i < Ntiles; i++) {
		if (i > 0 && i % Width == 0)
			fprintf(out, "\n");
		else if (i > 0)
			fprintf(out, "\t");
		fprintf(out, "%2d", ts[i]);
	}
	fprintf(out, "\n");
}<commit_msg>tiles: better read error messages.<commit_after>#include "tiles.hpp"
#include "../utils/utils.hpp"
#include "../utils/safeops.hpp"
#include <cerrno>
#include <cstdio>
#include <ctime>
#include <limits>

bool Tiles::hashvecinit = false;
unsigned long Tiles::hashvec[Ntiles][Ntiles];

Tiles::Tiles() {
	initops();
	if (!hashvecinit)
		inithashvec();
}

Tiles::Tiles(FILE *in) {
	readruml(in);
	initops();
	if (!hashvecinit)
		inithashvec();
}

void Tiles::readruml(FILE *in) {
	unsigned int w, h;

	if (fscanf(in, " %u %u", &w, &h) != 2)
		fatal("Failed to read width and height");

	if (!safe::can_mul(w, h))
		fatal("The tiles board is too big");

	if (w != Width && h != HEIGHT)
		fatal("Width and height instance/compiler option mismatch");

	if (fscanf(in, " starting positions for each tile:") != 0)
		fatal("Failed to read the starting position label");

	for (Tile t = 0; t < Ntiles; t++) {
		unsigned int p;
		int r = fscanf(in, " %u", &p);
		if (r != 1)
			fatal("Failed to read the starting position for tile %d", t);
		init[p] = t;
	}

	if (fscanf(in, " goal positions:") != 0)
		fatal("Failed to read the goal position label");

	for (Tile t = 0; t < Ntiles; t++) {
		unsigned int p;
		if (fscanf(in, " %u", &p) != 1)
			fatal("Failed to read the goal position");
		goalpos[t] = p;
	}
}

void Tiles::initops() {
	for (unsigned int i = 0; i < Ntiles; i++) {
		ops[i].n = 0;
		if (i >= Width)
			ops[i].mvs[ops[i].n++] = i - Width;
		if (i % Width > 0)
			ops[i].mvs[ops[i].n++] = i - 1;
		if (i % Width < Width - 1)
			ops[i].mvs[ops[i].n++] = i + 1;
		if (i < Ntiles - Width)
			ops[i].mvs[ops[i].n++] = i + Width;
	}
}

void Tiles::inithashvec() {
	hashvecinit = true;
	Rand r(time(NULL));
	for (int i = 0; i < Ntiles; i++) {
	for (int j = 0; j < Ntiles; j++)
		hashvec[i][j] = r.bits();
	}
}

void Tiles::dumptiles(FILE *out, Tile ts[]) {
	for (unsigned int i = 0; i < Ntiles; i++) {
		if (i > 0 && i % Width == 0)
			fprintf(out, "\n");
		else if (i > 0)
			fprintf(out, "\t");
		fprintf(out, "%2d", ts[i]);
	}
	fprintf(out, "\n");
}<|endoftext|>


<commit_before>//
//  ORForest.cpp
//
//  Created on: 17 Mar 2015
//      Author: Guotai Wang
//
// Copyright (c) 2014-2016 University College London, United Kingdom. All rights reserved.
// http://cmictig.cs.ucl.ac.uk
//
// Distributed under the BSD-3 licence. Please see the file licence.txt
//

#include "ORForest.h"
#include "Node.h"
#include <iostream>
#include <ctime>

template<typename T>
RandomForest::ORForest<T>::ORForest()
{
	treeNumber=20;
	maxDepth=10;
	leastNsample=10;
    trainData=nullptr;;
    testData=nullptr;
    testDataRatio=0.3;
    balanceType=SingleParameterBoostrap;
    samplingType=DownSamplingMajority;
    onlineUpdate=true;
}

template<typename T>
RandomForest::ORForest<T>::~ORForest()
{
}

template<typename T>
void RandomForest::ORForest<T>::Init(int Ntree,int treeDepth, int leastNsampleForSplit)
{
	treeNumber=Ntree;
	maxDepth=treeDepth;
	leastNsample=leastNsampleForSplit;
}

template<typename T>
void RandomForest::ORForest<T>::Clear()
{
    if(trees.size()>0)
    {
        trees.resize(0);
    }
    if(trainData)
    {
        trainData.reset();
        trainData=nullptr;;
    }
    if(testData)
    {
        testData.reset();
        trainData=nullptr;
    }
}

template<typename T>
void RandomForest::ORForest<T>::SetBalanceType(BalanceType type)
{
    balanceType=type;
}

template<typename T>
RandomForest::BalanceType RandomForest::ORForest<T>::GetBalanceType() const
{
    return balanceType;
}

template<typename T>
void RandomForest::ORForest<T>::SetSamplingType(RandomForest::SamplingType type)
{
    samplingType=type;
}

template<typename T>
RandomForest::SamplingType RandomForest::ORForest<T>::GetSamplingType() const
{
    return samplingType;
}

template<typename T>
void RandomForest::ORForest<T>::DisableOnlineUpdate()
{
    onlineUpdate=false;
}

template<typename T>
void RandomForest::ORForest<T>::GetTrainAndTestData(const std::shared_ptr<std::vector<std::shared_ptr<std::vector<T> > > > i_trainData)
{
    int i_addNs=i_trainData->size();
    int posNadd=0, negNadd=0;
    for(int i=0;i<i_addNs;i++)
    {
        T tempLabel=i_trainData->at(i)->back();
        if(tempLabel==1)
        {
            posNadd++;
        }
        else
        {
            negNadd++;
        }
    }
    if(trainData==nullptr && (posNadd==0 || negNadd==0))
    {
        printf("addNs %d, posNadd %d, negNadd %d, the class number of training data shoule be 2!\n", i_addNs, posNadd, negNadd);
        return;
    }

    int addNTest=i_addNs*testDataRatio;
    int addNTrain=i_addNs-addNTest;

    // select train and test data randomly
    std::vector<bool> TestDataMask;
    for(int i=0;i<i_addNs;i++) TestDataMask.push_back(false);

    int add_nTest=0;
    while(add_nTest<addNTest)
    {
        double randf=static_cast<double>(rand())/RAND_MAX;
        int tempIdx=i_addNs*randf;
        if(TestDataMask[tempIdx]==false)
        {
            TestDataMask[tempIdx]=true;
            add_nTest++;
        }
    }

    std::shared_ptr<std::vector<std::shared_ptr<std::vector<T> > > > addTrainData(new std::vector<std::shared_ptr<std::vector<T> > >);
    std::shared_ptr<std::vector<std::shared_ptr<std::vector<T> > > > addTestData(new std::vector<std::shared_ptr<std::vector<T> > >);
    addTrainData->reserve(addNTrain);
    addTestData->reserve(addNTest);
    for(int i=0;i<i_addNs;i++)
    {
        if(TestDataMask[i])
        {
            addTestData->push_back(i_trainData->at(i));
        }
        else{
            addTrainData->push_back(i_trainData->at(i));
        }
    }

    std::shared_ptr<std::vector<std::shared_ptr<std::vector<T> > > > newTrainData(new std::vector<std::shared_ptr<std::vector<T> > >);
    std::shared_ptr<std::vector<std::shared_ptr<std::vector<T> > > > newTestData(new std::vector<std::shared_ptr<std::vector<T> > >);
    if(trainData==nullptr)
    {
        newTrainData=addTrainData;
        newTestData=addTestData;
    }
    else{
        newTrainData->insert(newTrainData->end(), trainData->begin(), trainData->end());
        newTrainData->insert(newTrainData->end(),addTrainData->begin(), addTrainData->end());

        newTestData->insert(newTestData->end(), testData->begin(), testData->end());
        newTestData->insert(newTestData->end(), addTestData->begin(), addTestData->end());
    }
    trainData=newTrainData;
    testData=newTestData;
 }

template<typename T>
void RandomForest::ORForest<T>::Train(const T *i_trainData, int i_Ns, int i_Nfp1)
{
    std::shared_ptr<std::vector<std::shared_ptr<std::vector<T> > > > tempTrainData(new std::vector<std::shared_ptr<std::vector<T> > >);
    tempTrainData->resize(i_Ns);
    for(int i=0;i<i_Ns;i++)
    {
        std::shared_ptr<std::vector<T> > tempSample(new std::vector<T>);
        tempSample->resize(i_Nfp1);
        for(int j=0;j<i_Nfp1;j++)
        {
            tempSample->at(j)=*(i_trainData+i*i_Nfp1+j);
        }
        tempTrainData->at(i)=tempSample;
    }
    Train(tempTrainData);
}
template<typename T>
void RandomForest::ORForest<T>::Train(const std::shared_ptr<std::vector<std::shared_ptr<std::vector<T> > > > i_trainData)
{
    if(trees.size()==0 || onlineUpdate==false)
    {
        int posN=0, negN=0;
        while(posN==0 || negN==0)
        {
            GetTrainAndTestData(i_trainData);
            for(int i=0;i<trainData->size();i++)
            {
                T tempL=trainData->at(i)->back();
                if(tempL==1.0)posN++;
                else negN++;
            }
        }

        trees.resize(treeNumber);
        for(int i=0;i<treeNumber;i++)
		{
            ODTree<T> tempTree;
            trees[i]=tempTree;
			trees[i].SetDepthUpperBound(maxDepth);
            trees[i].SetSampleNumberThreshold(leastNsample);
            trees[i].SetBalanceType(balanceType);
            trees[i].SetSamplingType(samplingType);
			trees[i].Train(trainData);
		}
	}
	else
	{
        // combine old training data and newly arrived training data
        GetTrainAndTestData(i_trainData);
		for(int i=0;i<treeNumber;i++)
		{
			trees[i].Train(trainData);
			T oobe=trees[i].GetOOBE(testData);
			if(oobe>0.4)
			{
                trees[i].Reset();
				trees[i].Train(trainData);
			}
		}
	}
}

template<typename T>
void RandomForest::ORForest<T>::Predict(const std::shared_ptr<std::vector<std::shared_ptr<std::vector<T> > > > i_testData, std::vector<float>  ** o_predict)
{
    std::vector<float> *sumPredict=new std::vector<float>;
    sumPredict->resize(i_testData->size());
    for(int i=0;i<i_testData->size();i++) sumPredict->at(i)=0;
    for(int i=0;i<treeNumber;i++)
    {
        std::vector<float> * tempPredict;
        trees[i].Predict(i_testData, &tempPredict);
        for(int j=0;j<sumPredict->size();j++)
        {
            sumPredict->at(j)+=tempPredict->at(j);
        }
    }
    for(int i=0;i<sumPredict->size();i++)
    {
        sumPredict->at(i)=sumPredict->at(i)/treeNumber;
    }
    *o_predict=sumPredict;
}

template<typename T>
int RandomForest::ORForest<T>::GetActureMaxTreeDepth() const
{
    int tempD=0;
    for(int i=0;i<treeNumber;i++)
    {
        if(tempD < trees[i].GetActureTreeDepth())
        {
            tempD = trees[i].GetActureTreeDepth();
        }
    }
    return tempD;
}

template<typename T>
int RandomForest::ORForest<T>::GetActureMaxTreeNode() const
{
    int n=0;
    for(int i=0;i<treeNumber;i++)
    {
        if(n<trees[i].GetActureTreeNode())
        {
            n=trees[i].GetActureTreeNode();
        }
    }
    return n;
}

template<typename T>
double RandomForest::ORForest<T>::GetAverageOOBE() const
{
    double oobe=0;
    for(int i=0;i<treeNumber;i++)
    {
        oobe+=trees[i].GetOOBE(testData);
    }
    return oobe/treeNumber;
}

template<typename T>
void RandomForest::ORForest<T>::GetRankedGiniImportance( std::shared_ptr<std::vector<int> > * o_featureIndexList,  std::shared_ptr<std::vector<double> > * o_giniImportanceList)
{
    std::shared_ptr<std::vector<int> > featureIndexList=* o_featureIndexList;
    std::shared_ptr<std::vector<double> > giniImportanceList=* o_giniImportanceList;
    featureIndexList->resize(trainData->at(0)->size()-1);
    giniImportanceList->resize(trainData->at(0)->size()-1);
    for(int i=0;i<featureIndexList->size();i++)
    {
        featureIndexList->at(i)=i;
        giniImportanceList->at(i)=0.0;
    }
    for(int i=0;i<treeNumber;i++)
    {
        trees[i].UpdateGiniImportance();
        for(int j=0;j<featureIndexList->size();j++)
        {
            giniImportanceList->at(j)=giniImportanceList->at(j)+trees[i].GetGiniImportance()->at(j);
        }
    }

    // sorting
    for(int i=0;i<featureIndexList->size()-1;i++)
    {
        for(int j=i+1;j<featureIndexList->size();j++)
        {
            double leftValue=giniImportanceList->at(i);
            double rightValue=giniImportanceList->at(j);
            if(leftValue<rightValue)
            {
                int leftIndex=featureIndexList->at(i);
                int rightIndex=featureIndexList->at(j);
                giniImportanceList->at(i)=rightValue;
                giniImportanceList->at(j)=leftValue;
                featureIndexList->at(i)=rightIndex;
                featureIndexList->at(j)=leftIndex;
            }
        }
    }

    *o_featureIndexList=featureIndexList;
    *o_giniImportanceList=giniImportanceList;
}

template class RandomForest::ORForest<float>;
template class RandomForest::ORForest<double>;
<commit_msg>Fixed typo<commit_after>//
//  ORForest.cpp
//
//  Created on: 17 Mar 2015
//      Author: Guotai Wang
//
// Copyright (c) 2014-2016 University College London, United Kingdom. All rights reserved.
// http://cmictig.cs.ucl.ac.uk
//
// Distributed under the BSD-3 licence. Please see the file licence.txt
//

#include "ORForest.h"
#include "Node.h"
#include <iostream>
#include <ctime>

template<typename T>
RandomForest::ORForest<T>::ORForest()
{
	treeNumber=20;
	maxDepth=10;
	leastNsample=10;
    trainData=nullptr;;
    testData=nullptr;
    testDataRatio=0.3;
    balanceType=SingleParameterBoostrap;
    samplingType=DownSamplingMajority;
    onlineUpdate=true;
}

template<typename T>
RandomForest::ORForest<T>::~ORForest()
{
}

template<typename T>
void RandomForest::ORForest<T>::Init(int Ntree,int treeDepth, int leastNsampleForSplit)
{
	treeNumber=Ntree;
	maxDepth=treeDepth;
	leastNsample=leastNsampleForSplit;
}

template<typename T>
void RandomForest::ORForest<T>::Clear()
{
    if(trees.size()>0)
    {
        trees.resize(0);
    }
    if(trainData)
    {
        trainData.reset();
        trainData=nullptr;;
    }
    if(testData)
    {
        testData.reset();
        testData=nullptr;
    }
}

template<typename T>
void RandomForest::ORForest<T>::SetBalanceType(BalanceType type)
{
    balanceType=type;
}

template<typename T>
RandomForest::BalanceType RandomForest::ORForest<T>::GetBalanceType() const
{
    return balanceType;
}

template<typename T>
void RandomForest::ORForest<T>::SetSamplingType(RandomForest::SamplingType type)
{
    samplingType=type;
}

template<typename T>
RandomForest::SamplingType RandomForest::ORForest<T>::GetSamplingType() const
{
    return samplingType;
}

template<typename T>
void RandomForest::ORForest<T>::DisableOnlineUpdate()
{
    onlineUpdate=false;
}

template<typename T>
void RandomForest::ORForest<T>::GetTrainAndTestData(const std::shared_ptr<std::vector<std::shared_ptr<std::vector<T> > > > i_trainData)
{
    int i_addNs=i_trainData->size();
    int posNadd=0, negNadd=0;
    for(int i=0;i<i_addNs;i++)
    {
        T tempLabel=i_trainData->at(i)->back();
        if(tempLabel==1)
        {
            posNadd++;
        }
        else
        {
            negNadd++;
        }
    }
    if(trainData==nullptr && (posNadd==0 || negNadd==0))
    {
        printf("addNs %d, posNadd %d, negNadd %d, the class number of training data shoule be 2!\n", i_addNs, posNadd, negNadd);
        return;
    }

    int addNTest=i_addNs*testDataRatio;
    int addNTrain=i_addNs-addNTest;

    // select train and test data randomly
    std::vector<bool> TestDataMask;
    for(int i=0;i<i_addNs;i++) TestDataMask.push_back(false);

    int add_nTest=0;
    while(add_nTest<addNTest)
    {
        double randf=static_cast<double>(rand())/RAND_MAX;
        int tempIdx=i_addNs*randf;
        if(TestDataMask[tempIdx]==false)
        {
            TestDataMask[tempIdx]=true;
            add_nTest++;
        }
    }

    std::shared_ptr<std::vector<std::shared_ptr<std::vector<T> > > > addTrainData(new std::vector<std::shared_ptr<std::vector<T> > >);
    std::shared_ptr<std::vector<std::shared_ptr<std::vector<T> > > > addTestData(new std::vector<std::shared_ptr<std::vector<T> > >);
    addTrainData->reserve(addNTrain);
    addTestData->reserve(addNTest);
    for(int i=0;i<i_addNs;i++)
    {
        if(TestDataMask[i])
        {
            addTestData->push_back(i_trainData->at(i));
        }
        else{
            addTrainData->push_back(i_trainData->at(i));
        }
    }

    std::shared_ptr<std::vector<std::shared_ptr<std::vector<T> > > > newTrainData(new std::vector<std::shared_ptr<std::vector<T> > >);
    std::shared_ptr<std::vector<std::shared_ptr<std::vector<T> > > > newTestData(new std::vector<std::shared_ptr<std::vector<T> > >);
    if(trainData==nullptr)
    {
        newTrainData=addTrainData;
        newTestData=addTestData;
    }
    else{
        newTrainData->insert(newTrainData->end(), trainData->begin(), trainData->end());
        newTrainData->insert(newTrainData->end(),addTrainData->begin(), addTrainData->end());

        newTestData->insert(newTestData->end(), testData->begin(), testData->end());
        newTestData->insert(newTestData->end(), addTestData->begin(), addTestData->end());
    }
    trainData=newTrainData;
    testData=newTestData;
 }

template<typename T>
void RandomForest::ORForest<T>::Train(const T *i_trainData, int i_Ns, int i_Nfp1)
{
    std::shared_ptr<std::vector<std::shared_ptr<std::vector<T> > > > tempTrainData(new std::vector<std::shared_ptr<std::vector<T> > >);
    tempTrainData->resize(i_Ns);
    for(int i=0;i<i_Ns;i++)
    {
        std::shared_ptr<std::vector<T> > tempSample(new std::vector<T>);
        tempSample->resize(i_Nfp1);
        for(int j=0;j<i_Nfp1;j++)
        {
            tempSample->at(j)=*(i_trainData+i*i_Nfp1+j);
        }
        tempTrainData->at(i)=tempSample;
    }
    Train(tempTrainData);
}
template<typename T>
void RandomForest::ORForest<T>::Train(const std::shared_ptr<std::vector<std::shared_ptr<std::vector<T> > > > i_trainData)
{
    if(trees.size()==0 || onlineUpdate==false)
    {
        int posN=0, negN=0;
        while(posN==0 || negN==0)
        {
            GetTrainAndTestData(i_trainData);
            for(int i=0;i<trainData->size();i++)
            {
                T tempL=trainData->at(i)->back();
                if(tempL==1.0)posN++;
                else negN++;
            }
        }

        trees.resize(treeNumber);
        for(int i=0;i<treeNumber;i++)
		{
            ODTree<T> tempTree;
            trees[i]=tempTree;
			trees[i].SetDepthUpperBound(maxDepth);
            trees[i].SetSampleNumberThreshold(leastNsample);
            trees[i].SetBalanceType(balanceType);
            trees[i].SetSamplingType(samplingType);
			trees[i].Train(trainData);
		}
	}
	else
	{
        // combine old training data and newly arrived training data
        GetTrainAndTestData(i_trainData);
		for(int i=0;i<treeNumber;i++)
		{
			trees[i].Train(trainData);
			T oobe=trees[i].GetOOBE(testData);
			if(oobe>0.4)
			{
                trees[i].Reset();
				trees[i].Train(trainData);
			}
		}
	}
}

template<typename T>
void RandomForest::ORForest<T>::Predict(const std::shared_ptr<std::vector<std::shared_ptr<std::vector<T> > > > i_testData, std::vector<float>  ** o_predict)
{
    std::vector<float> *sumPredict=new std::vector<float>;
    sumPredict->resize(i_testData->size());
    for(int i=0;i<i_testData->size();i++) sumPredict->at(i)=0;
    for(int i=0;i<treeNumber;i++)
    {
        std::vector<float> * tempPredict;
        trees[i].Predict(i_testData, &tempPredict);
        for(int j=0;j<sumPredict->size();j++)
        {
            sumPredict->at(j)+=tempPredict->at(j);
        }
    }
    for(int i=0;i<sumPredict->size();i++)
    {
        sumPredict->at(i)=sumPredict->at(i)/treeNumber;
    }
    *o_predict=sumPredict;
}

template<typename T>
int RandomForest::ORForest<T>::GetActureMaxTreeDepth() const
{
    int tempD=0;
    for(int i=0;i<treeNumber;i++)
    {
        if(tempD < trees[i].GetActureTreeDepth())
        {
            tempD = trees[i].GetActureTreeDepth();
        }
    }
    return tempD;
}

template<typename T>
int RandomForest::ORForest<T>::GetActureMaxTreeNode() const
{
    int n=0;
    for(int i=0;i<treeNumber;i++)
    {
        if(n<trees[i].GetActureTreeNode())
        {
            n=trees[i].GetActureTreeNode();
        }
    }
    return n;
}

template<typename T>
double RandomForest::ORForest<T>::GetAverageOOBE() const
{
    double oobe=0;
    for(int i=0;i<treeNumber;i++)
    {
        oobe+=trees[i].GetOOBE(testData);
    }
    return oobe/treeNumber;
}

template<typename T>
void RandomForest::ORForest<T>::GetRankedGiniImportance( std::shared_ptr<std::vector<int> > * o_featureIndexList,  std::shared_ptr<std::vector<double> > * o_giniImportanceList)
{
    std::shared_ptr<std::vector<int> > featureIndexList=* o_featureIndexList;
    std::shared_ptr<std::vector<double> > giniImportanceList=* o_giniImportanceList;
    featureIndexList->resize(trainData->at(0)->size()-1);
    giniImportanceList->resize(trainData->at(0)->size()-1);
    for(int i=0;i<featureIndexList->size();i++)
    {
        featureIndexList->at(i)=i;
        giniImportanceList->at(i)=0.0;
    }
    for(int i=0;i<treeNumber;i++)
    {
        trees[i].UpdateGiniImportance();
        for(int j=0;j<featureIndexList->size();j++)
        {
            giniImportanceList->at(j)=giniImportanceList->at(j)+trees[i].GetGiniImportance()->at(j);
        }
    }

    // sorting
    for(int i=0;i<featureIndexList->size()-1;i++)
    {
        for(int j=i+1;j<featureIndexList->size();j++)
        {
            double leftValue=giniImportanceList->at(i);
            double rightValue=giniImportanceList->at(j);
            if(leftValue<rightValue)
            {
                int leftIndex=featureIndexList->at(i);
                int rightIndex=featureIndexList->at(j);
                giniImportanceList->at(i)=rightValue;
                giniImportanceList->at(j)=leftValue;
                featureIndexList->at(i)=rightIndex;
                featureIndexList->at(j)=leftIndex;
            }
        }
    }

    *o_featureIndexList=featureIndexList;
    *o_giniImportanceList=giniImportanceList;
}

template class RandomForest::ORForest<float>;
template class RandomForest::ORForest<double>;
<|endoftext|>