text:
//  Duplex optimization tool
//
//  Created by Adel Ahmadyan on 4/23/15.
//  Copyright (c) 2015-2017 Adel Ahmadyan. All rights reserved.
//

#include "RRT.h"
#include "classification.h"
#include "clustering.h"
#include "configuration.h"
#include "duplex.h"
#include "graphics.h"
#include "log.h"
#include "plotFactory.h"
#include "system.h"
#include <boost/program_options.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/xml_parser.hpp>
#include <config4cpp/Configuration.h>
#include <iostream>
#include <string>

#include "descentOptimizer.h"
#include "functionalOptimizer.h"
#include "nonconvexOptimizer.h"
#include "systemOptimizer.h"

using namespace config4cpp;

#if (_MSC_VER == 1900) // Visual studio 2015
// temporary code, visual studio 2015 refactored CRT libraries. Now, some codes
// (specifically config4cpp) will produce link libraries because they cannot
// find _iob_func (used in _sprintf). one solution is to recompile them using
// visual studio 2015 with legacy_stdio_definitions.lib additional dependency.
// this is a temporary fix.
FILE _iob[] = {*stdin, *stdout, *stderr};
extern "C" FILE *__cdecl __iob_func(void) { return _iob; }
#endif

namespace {
const size_t ERROR_IN_COMMAND_LINE = 1;
const size_t SUCCESS = 0;
const size_t ERROR_UNHANDLED_EXCEPTION = 2;
} // namespace

enum class mode {
  load,
  duplex,
  fopt,
  opt,
  sa,
  clustering,
  system,
  classification,
  invalid,
  RRT
};

mode getMode(Settings *settings) {
  auto m = mode::invalid;
  if (settings->check("mode", "load")) {
    m = mode::load;
  } else if (settings->check("mode", "duplex")) {
    m = mode::duplex;
  } else if (settings->check("mode", "fopt")) {
    m = mode::fopt;
  } else if (settings->check("mode", "opt")) {
    m = mode::opt;
  } else if (settings->check("mode", "simulated-annealing")) {
    m = mode::sa;
  } else if (settings->check("mode", "clustering")) {
    m = mode::clustering;
  } else if (settings->check("mode", "system")) {
    m = mode::system;
  } else if (settings->check("mode", "classification")) {
    m = mode::classification;
  } else if (settings->check("mode", "RRT")) {
    m = mode::RRT;
  }
  return m;
}

void autodiffTest();

int main(int argc, char **argv) {
  autodiffTest();
  Log log;
  log << "Duplex optimization tool." << endl;
  Settings *settings = new Settings();
  srand((unsigned int)time(0));
  log << "settings created, procceeding to parsing arguments" << endl;
  try {
    namespace po = boost::program_options;
    po::options_description desc("Options");
    desc.add_options()("help", "Print help messages")(
        "verbose,v", po::value<int>(), "print words with verbosity (0 or 1)")(
        "config,c", po::value<string>(),
        "Specifies the configuration file")("like", "this");
    po::variables_map vm;
    try {
      po::store(po::parse_command_line(argc, argv, desc), vm); // can throw
      // --help option
      if (vm.count("help")) {
        std::cout << "Duplex optimization engine. " << std::endl
                  << "Version 0.1    (C) 2015 Seyed Nematollah (Adel) "
                     "Ahmadyan, All right reserved."
                  << std::endl
                  << desc << std::endl;
        return SUCCESS;
      }
      po::notify(vm); // throws on error, so do after help in case there are
                      // any problems
    } catch (po::error &e) {
      std::cerr << "ERROR: " << e.what() << std::endl << std::endl;
      std::cerr << desc << std::endl;
      cin.get();
      return ERROR_IN_COMMAND_LINE;
    }
    cout << "Parsing arguments complete. Proceed to parsing config file ..."
         << endl;
    settings->parse(vm["config"].as<std::string>().c_str(), "Duplex");

    bool verbose = true;
    try {
      verbose = vm["verbose"].as<int>();
    } catch (exception &e) {
      verbose = settings->lookupBoolean("verbose");
    }
    log << "Parsing config file " << vm["config"].as<std::string>().c_str()
        << " complete." << endl;

    Duplex *duplex = NULL;
    Clustering *clustering = NULL;
    boost::property_tree::ptree ptree; // used to load/save the data

    Data *trainingData;
    Data *testData;
    // determining which algorithm to execute
    auto m = getMode(settings);

    log.tick();
    switch (m) {
    // -----------------------------------------------------
    case mode::load:
      // Load the XML file into the property tree.
      duplex = new NonconvexOptimizer(settings);
      read_xml(settings->lookupString("savefile"), ptree);
      duplex->load(&ptree);
      break;
    // -----------------------------------------------------
    case mode::duplex:
      duplex = new NonconvexOptimizer(settings);
      duplex->train();
      break;

    // -----------------------------------------------------
    case mode::fopt:
      duplex = new FunctionalOptimizer(settings);
      duplex->train();
      break;

    // -----------------------------------------------------
    case mode::opt:
      duplex = new DescentOptimizer(settings);
      duplex->train();
      break;

    // -----------------------------------------------------
    case mode::sa:
      duplex = new NonconvexOptimizer(settings);
      // duplex->simulated_annealing();
      break;

    case mode::system:
      duplex = new SystemOptimizer(settings);
      duplex->train();
      break;
    case mode::RRT:
      duplex = new RRT(settings);
      duplex->train();
      break;

    // -----------------------------------------------------
    case mode::clustering:
      trainingData = new Data(settings);
      clustering = new Clustering(settings, trainingData);
      clustering->train(settings->lookupString("clustering.mode"));
      break;

    case mode::classification:
      trainingData = new Data(settings);
      duplex = new Classifier(settings, trainingData);
      duplex->train();
      break;

    // -----------------------------------------------------
    case mode::invalid:
      log << "Unknown optimization mode is selected.";
      return 0;
    }
    log.tock("Duplex main optimizatio loop");

    // saving the results into an xml file
    // duplex->save(&ptree);
    // ofstream savefile(settings->lookupString("savefile"));
    // write_xml(savefile, ptree);
    // savefile.close();

    // Plotting the results
    if (settings->check("plot.enable", "true")) {
      PlotFactory *pf = new PlotFactory(settings, duplex, clustering);
      vector<string> plots = settings->listVariables("plot", "uid-plot");
      for (int i = 0; i < plots.size(); i++) {
        Graphics *graphic =
            new Graphics(settings->lookupString("plot.gnuplot"));
        string plotStr = pf->getPlot(i);
        graphic->execute(plotStr);
        // graphic->execute(duplex->draw(i));
        // graphic->saveToPdf(settings->lookupString("output"));
        delete graphic;
      }
    }

    // clean-up
    if (clustering)
      delete clustering;
    if (duplex)
      delete duplex;
    if (settings)
      delete settings;

  } catch (SettingsException se) {
    std::cerr << "CONFIG ERROR: " << se.what() << std::endl << std::endl;
    cin.get();
    return ERROR_IN_COMMAND_LINE;
  } catch (std::exception &e) {
    std::cerr << "Unhandled Exception reached the top of main: " << e.what()
              << ", application will now exit" << std::endl;
    cin.get();
    return ERROR_UNHANDLED_EXCEPTION;
  }

  log << "Shutting down" << endl;
  cin.get();
  return SUCCESS;
}

ext: cpp

max_stars_repo_name: ahmadyan/Duplex

max_issues_repo_name: ahmadyan/Duplex

max_forks_repo_name: ahmadyan/Duplex


text:
//---------------------------------------------------------------------------

#pragma hdrstop

#include <algorithm>
#include <numeric>
#include <iterator>

#include <boost/tuple/tuple.hpp>

#include "GridMapper.h"

using std::lower_bound;
using std::iota;
using std::for_each;
using std::stable_sort;
using std::begin;
using std::end;

//---------------------------------------------------------------------------

#pragma package(smart_init)

//---------------------------------------------------------------------------

void ViewRowGridMapper::ClearMap()
{
    mapped_ = false;
    view2Model_.clear();
    model2view_.clear();
}
//---------------------------------------------------------------------------

int ViewRowGridMapper::Map( int RowCount )
{
    ViewToModelCont View2Model( RowCount );
    iota( begin( View2Model ), end( View2Model ), 0 );

    BuildModelToView( View2Model );

    view2Model_.swap( View2Model );
    mapped_ = true;
    return GetCount();
}
//---------------------------------------------------------------------------

template<size_t TupleFieldIdx>
struct ModelIndexCompare {
    template<typename T, typename U>
    bool operator()( T const & Tuple, U Value ) const {
        return Tuple.template get<TupleFieldIdx>() < Value;
    }
};

int ViewRowGridMapper::DoIdx2Num( int Idx ) const
{
    if ( mapped_ ) {
        ModelToViewCont::const_iterator It =
            lower_bound(
                begin( model2view_ ), end( model2view_ ), Idx,
                ModelIndexCompare<(size_t)ModelToRowField::ModelIdx>()
            );
        if ( It != end( model2view_ ) &&
             !( Idx < It->get<(int)ModelToRowField::ModelIdx>() ) )
        {
            return It->get<(size_t)ModelToRowField::ViewIdx>();
        }
        return -1;
    }
    else {
        return Idx;
    }
}
//---------------------------------------------------------------------------

int ViewRowGridMapper::DoNum2Idx( int ARow ) const
{
    return mapped_ && ARow < static_cast<int>( view2Model_.size() ) ?
             view2Model_[ARow]
           :
             ARow;
}
//---------------------------------------------------------------------------

template<typename C>
class CopyIotaType {
public:
    explicit CopyIotaType( C& Cont ) : n_( 0 ), cont_( Cont ) {}

    template<typename T>
    void operator()( T const & Val ) {
        cont_.push_back( boost::make_tuple( Val, n_++ ) );
    }
private:
    int n_;
    C& cont_;
};

template<typename C>
inline CopyIotaType<C> CopyIota( C& Cont ) { return CopyIotaType<C>( Cont ); }

void ViewRowGridMapper::BuildModelToView( ViewToModelCont const & View2Model )
{
    ModelToViewCont Model2View;
    Model2View.reserve( View2Model.size() );
    for_each( begin( View2Model ), end( View2Model ), CopyIota( Model2View ) );
    stable_sort( begin( Model2View ), end( Model2View ) );
    model2view_.swap( Model2View );
}
//---------------------------------------------------------------------------

max_stars_repo_name: gcardi/PugiXmlTest

max_issues_repo_name: gcardi/PugiXmlTest

max_forks_repo_name: gcardi/PugiXmlTest


text:
//
// Boost.Pointer Container
//
//  Copyright Thorsten Ottosen 2003-2005. Use, modification and
//  distribution is subject to the Boost Software License, Version
//  1.0. (See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)
//
// For more information, see http://www.boost.org/libs/ptr_container/
//

#include <boost/ptr_container/ptr_vector.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/lambda/lambda.hpp>
#include <algorithm>

using namespace std;

//
// A simple polymorphic class
//
class Poly
{
    int        i_;
    static int cnt_;

public:
    Poly() : i_( cnt_++ )  { }
    virtual ~Poly()        { }
    void foo()             { doFoo(); }

private:
    virtual void doFoo()   { ++i_; }

public:
    friend inline bool operator>( const Poly& l, const Poly r )
    {
        return l.i_ > r.i_;
    }
};

int Poly::cnt_ = 0;

//
// Normally we need something like this to compare pointers to objects
//
template< typename T >
struct sptr_greater
{
    bool operator()( const boost::shared_ptr<T>& l, const boost::shared_ptr<T>& r ) const
    {
        return *l > *r;
    }
};

//
// one doesn't need to introduce new names or live with long ones
//
typedef boost::shared_ptr<Poly> PolyPtr;

void simple_test()
{
    enum { size = 2000 };
    typedef vector<PolyPtr>          vector_t;
    typedef boost::ptr_vector<Poly>  ptr_vector_t;
    vector_t                         svec;
    ptr_vector_t                     pvec;

    for( int i = 0; i < size; ++i )
    {
        svec.push_back( PolyPtr( new Poly ) );
        pvec.push_back( new Poly );  // no extra syntax
    }

    for( int i = 0; i < size; ++i )
    {
        svec[i]->foo();
        pvec[i].foo(); // automatic indirection
        svec[i] = PolyPtr( new Poly );
        pvec.replace( i, new Poly ); // direct pointer assignment not possible, original element is deleted
    }

    for( vector_t::iterator i = svec.begin(); i != svec.end(); ++i )
        (*i)->foo();

    for( ptr_vector_t::iterator i = pvec.begin(); i != pvec.end(); ++i )
        i->foo(); // automatic indirection
}

#include <boost/test/unit_test.hpp>
using boost::unit_test::test_suite;

test_suite* init_unit_test_suite( int argc, char* argv[] )
{
    test_suite* test = BOOST_TEST_SUITE( "Pointer Container Test Suite" );

    test->add( BOOST_TEST_CASE( &simple_test ) );

    return test;
}

max_stars_repo_name: zyiacas/boost-doc-zh

max_issues_repo_name: sdfict/boost-doc-zh

max_forks_repo_name: sdfict/boost-doc-zh


text:
#include <Eigen/Dense>
#include <cmath>
#include <iostream>
#include <thread>

#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Odometry.h>
#include <visualization_msgs/MarkerArray.h>

#include <tf/transform_datatypes.h>

#include <igvc_msgs/trajectory.h>
#include <parameter_assertions/assertions.h>

#include "motion_profiler.h"
#include "trajectory_planner.h"

TrajectoryPlanner::TrajectoryPlanner()
{
  ros::NodeHandle nh;
  ros::NodeHandle pNh("~");

  double axle_length;
  SmoothControlOptions smooth_control_options{};
  PathGenerationOptions path_generation_options{};
  TargetSelectionOptions target_selection_options{};
  CurvatureBlendingOptions curvature_blending_options{};

  WheelConstraint wheel_constraints{};
  RobotConstraint robot_constraints{};
  MotionProfilerOptions motion_profiler_options{};
  double target_velocity;

  assertions::param(pNh, "axle_length", axle_length, 0.52);
  assertions::param(pNh, "path_timeout", path_timeout_, 3.0);
  assertions::param(pNh, "node/loop_hz", loop_hz_, 20.0);

  assertions::param(pNh, "smooth_control/k1", smooth_control_options.k1, 1.0);
  assertions::param(pNh, "smooth_control/k2", smooth_control_options.k2, 3.0);

  assertions::param(pNh, "path_generation/simulation_frequency", path_generation_options.simulation_frequency, 100.0);
  assertions::param(pNh, "path_generation/simulation_horizon", path_generation_options.simulation_horizon, 5.0);
  assertions::param(pNh, "path_generation/velocity", path_generation_options.simulation_velocity, 1.0);

  assertions::param(pNh, "target_selection/lookahead_dist", target_selection_options.lookahead_dist, 2.0);
  assertions::param(pNh, "target_selection/reached_distance", target_selection_options.target_reached_distance, 0.05);
  assertions::param(pNh, "target_selection/move_threshold", target_selection_options.target_move_threshold, 0.05);

  assertions::param(pNh, "curvature_blending/blending_distance", curvature_blending_options.blending_distance, 1.0);

  assertions::param(pNh, "wheel_constraints/velocity", wheel_constraints.velocity, 3.0);
  assertions::param(pNh, "wheel_constraints/acceleration", wheel_constraints.acceleration, 5.0);
  assertions::param(pNh, "robot_constraints/velocity", robot_constraints.velocity, 2.0);
  assertions::param(pNh, "robot_constraints/acceleration", robot_constraints.acceleration, 5.0);

  assertions::param(pNh, "motion_profiler/target_velocity", target_velocity, 1.0);
  assertions::param(pNh, "motion_profiler/beta", motion_profiler_options.beta, 2.0);
  assertions::param(pNh, "motion_profiler/lambda", motion_profiler_options.lambda, 1.0);

  assertions::getParam(pNh, "end_distance_threshold", end_distance_threshold);

  double linear_acceleration_curvature_threshold;
  assertions::param(pNh, "linear_velocity_curvature_threshold", linear_acceleration_curvature_threshold, 1e-2);

  if (path_generation_options.simulation_frequency <= 0)
  {
    ROS_WARN_STREAM("Simulation frequency (currently " << path_generation_options.simulation_frequency
                                                       << ") should be greater than 0. Setting to 1 for now.");
    path_generation_options.simulation_frequency = 1;
  }
  controller_ = std::make_unique<SmoothControl>(smooth_control_options, path_generation_options,
                                                target_selection_options, curvature_blending_options, axle_length);
  motion_profiler_ =
      std::make_unique<MotionProfiler>(axle_length, wheel_constraints, robot_constraints, motion_profiler_options,
                                       target_velocity, linear_acceleration_curvature_threshold);

  // load global parameters
  assertions::param(pNh, "stop_dist", stop_dist_, 0.9);

  ros::Subscriber path_sub = nh.subscribe("/path", 1, &TrajectoryPlanner::pathCallback, this);
  ros::Subscriber encoder_sub = nh.subscribe("/encoders", 1, &TrajectoryPlanner::encoderCallback, this);
  ros::Subscriber pose_sub = nh.subscribe("/odometry/filtered", 1, &TrajectoryPlanner::positionCallback, this);
  ros::Subscriber waypoint_sub = nh.subscribe("/waypoint", 1, &TrajectoryPlanner::waypointCallback, this);

  cmd_pub_ = nh.advertise<igvc_msgs::velocity_pair>("/motors", 1);
  target_pub_ = nh.advertise<geometry_msgs::PointStamped>("/target_point", 1);
  trajectory_pub_ = nh.advertise<igvc_msgs::trajectory>("/trajectory", 1);
  smoothed_pub_ = nh.advertise<nav_msgs::Path>("/smoothed", 1);
  debug_pub_ = nh.advertise<nav_msgs::Path>("/trajectory_planner/debug", 1);

  std::thread trajectory_plan_thread(&TrajectoryPlanner::updateTrajectory, this);

  ros::spin();
  ROS_INFO("Shutting down...");
  trajectory_plan_thread.join();
}

void TrajectoryPlanner::pathCallback(const nav_msgs::PathConstPtr& msg)
{
  ROS_DEBUG_STREAM("Follower got path. Size: " << msg->poses.size());
  nav_msgs::PathConstPtr path = getPatchedPath(msg);
  smoothed_pub_.publish(path);
  std::lock_guard<std::mutex> path_mutex(path_mutex_);
  path_ = path;
  last_path_time_ = ros::Time::now();
}

void TrajectoryPlanner::waypointCallback(const geometry_msgs::PointStampedConstPtr& msg)
{
  waypoint_ = msg;
}

/**
Constructs a new trajectory to follow using the current path msg and publishes
the first velocity command from this trajectory.
*/
void TrajectoryPlanner::positionCallback(const nav_msgs::OdometryConstPtr& msg)
{
  state_.setState(msg);
  last_time_ = msg->header.stamp;
}

nav_msgs::PathConstPtr TrajectoryPlanner::getPatchedPath(const nav_msgs::PathConstPtr& msg) const
{
  // Get points between each point to smooth it out
  nav_msgs::PathPtr smoothed_path = boost::make_shared<nav_msgs::Path>();
  smoothed_path->poses.emplace_back(msg->poses.front());
  for (size_t i = 0; i < msg->poses.size() - 1; i++)
  {
    double mid_x = (msg->poses[i].pose.position.x + msg->poses[i + 1].pose.position.x) / 2;
    double mid_y = (msg->poses[i].pose.position.y + msg->poses[i + 1].pose.position.y) / 2;
    geometry_msgs::PoseStamped pose;
    pose.pose.position.x = mid_x;
    pose.pose.position.y = mid_y;
    smoothed_path->poses.emplace_back(pose);
  }
  smoothed_path->poses.emplace_back(msg->poses.back());

  // Patch heading
  for (size_t i = 0; i < smoothed_path->poses.size(); i++)
  {
    double delta_x = smoothed_path->poses[i + 1].pose.position.x - smoothed_path->poses[i].pose.position.x;
    double delta_y = smoothed_path->poses[i + 1].pose.position.y - smoothed_path->poses[i].pose.position.y;
    double heading = atan2(delta_y, delta_x);
    smoothed_path->poses[i].pose.orientation = tf::createQuaternionMsgFromYaw(heading);
  }
  smoothed_path->poses.back().pose.orientation = smoothed_path->poses[msg->poses.size() - 2].pose.orientation;
  smoothed_path->header = msg->header;
  return smoothed_path;
}

void TrajectoryPlanner::encoderCallback(const igvc_msgs::velocity_pairConstPtr& msg)
{
  std::lock_guard<std::mutex> state_lock(state_mutex_);
  state_.setVelocity(msg);
  last_time_ = msg->header.stamp;
}

void TrajectoryPlanner::updateTrajectory()
{
  ros::Rate rate(loop_hz_);
  while (ros::ok())
  {
    std::optional<igvc_msgs::trajectoryPtr> trajectory = getSmoothPath();
    if ((ros::Time::now() - last_path_time_).toSec() > path_timeout_)
    {
      ROS_INFO_STREAM_THROTTLE(1, "TIMEOUT ON TRAJ PLANNER " << (ros::Time::now() - last_path_time_).toSec());
      igvc_msgs::trajectoryPtr empty_traj = boost::make_shared<igvc_msgs::trajectory>();
      empty_traj->header.stamp = ros::Time::now();
      empty_traj->header.frame_id = "/odom";
      publishTrajectory(empty_traj);
    }

    if (!trajectory || trajectory.value().get() == nullptr || trajectory.value()->trajectory.empty())
    {
      publishStop();
      continue;
    }

    publishDebug(*trajectory);
    motion_profiler_->profileTrajectory(*trajectory, state_);
    publishTrajectory(*trajectory);
    rate.sleep();
  }
}

void TrajectoryPlanner::publishStop()
{
  igvc_msgs::trajectory trajectory;
  igvc_msgs::trajectory_point trajectory_point;
  trajectory_point.velocity = 0.0;
  trajectory_point.curvature = 0.0;
  trajectory.trajectory.emplace_back(trajectory_point);

  trajectory_pub_.publish(trajectory);
}

std::optional<igvc_msgs::trajectoryPtr> TrajectoryPlanner::getSmoothPath()
{
  if (waypoint_.get() == nullptr)
  {
    ROS_INFO_THROTTLE(1, "Waypoint empty.");
    return std::nullopt;
  }
  if (path_.get() == nullptr || path_->poses.empty() || path_->poses.size() < 2)
  {
    ROS_INFO_THROTTLE(1, "Path empty.");
    return std::nullopt;
  }

  double goal_dist = state_.distTo(waypoint_->point.x, waypoint_->point.y);

  ROS_DEBUG_STREAM_THROTTLE(1, "Distance to waypoint: " << goal_dist << "(m.)");

  igvc_msgs::trajectoryPtr trajectory = boost::make_shared<igvc_msgs::trajectory>();

  std::lock_guard<std::mutex> state_lock(state_mutex_);
  std::lock_guard<std::mutex> path_lock(path_mutex_);
  controller_->getPath(path_, trajectory, state_, end_distance_threshold);
  return trajectory;
}

void TrajectoryPlanner::publishTrajectory(const igvc_msgs::trajectoryConstPtr& trajectory)
{
  trajectory_pub_.publish(trajectory);
}

void TrajectoryPlanner::publishDebug(const igvc_msgs::trajectoryConstPtr& trajectory)
{
  igvc_msgs::trajectory_point point = trajectory->trajectory.front();
  nav_msgs::Path path;
  for (const auto& point : trajectory->trajectory)
  {
    geometry_msgs::PoseStamped pose;
    pose.pose = point.pose;
    pose.header.stamp = point.header.stamp;
    pose.header.frame_id = "/odom";

    path.poses.emplace_back(pose);
  }

  path.header.stamp = trajectory->header.stamp;
  path.header.frame_id = "/odom";
  debug_pub_.publish(path);
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "trajectory_planner");
  TrajectoryPlanner path_follower;
  return 0;
}

max_stars_repo_name: jiajunmao/igvc-software

max_issues_repo_name: jiajunmao/igvc-software

max_forks_repo_name: jiajunmao/igvc-software


text:
#include "test_utils.hpp"

#include <binlog/Entries.hpp> // Event
#include <binlog/EventStream.hpp>
#include <binlog/PrettyPrinter.hpp>

#include <boost/test/unit_test.hpp>

#include <ctime>
#include <sstream>

TestStream& TestStream::write(const char* data, std::streamsize size)
{
  buffer.insert(buffer.end(), data, data + size);
  return *this;
}

binlog::Range TestStream::nextEntryPayload()
{
  binlog::Range input(buffer.data() + readPos, buffer.data() + buffer.size());
  if (input.empty()) { return {}; }

  const std::uint32_t size = input.read<std::uint32_t>();
  binlog::Range result(input.view(size), size);
  readPos += sizeof(size) + size;
  return result;
}

std::vector<std::string> streamToEvents(TestStream& input, const char* eventFormat)
{
  std::vector<std::string> result;

  binlog::EventStream eventStream;
  binlog::PrettyPrinter pp(eventFormat, "%Y.%m.%d %H:%M:%S");

  while (const binlog::Event* event = eventStream.nextEvent(input))
  {
    std::ostringstream str;
    pp.printEvent(str, *event, eventStream.writerProp(), eventStream.clockSync());
    result.push_back(str.str());
  }

  return result;
}

std::vector<std::string> getEvents(binlog::Session& session, const char* eventFormat)
{
  TestStream stream;
  const binlog::Session::ConsumeResult cr = session.consume(stream);
  BOOST_TEST(stream.buffer.size() == cr.bytesConsumed);
  return streamToEvents(stream, eventFormat);
}

std::string timePointToString(std::chrono::system_clock::time_point tp)
{
  char buffer[128] = {0};
  const std::time_t tt = std::chrono::system_clock::to_time_t(tp);
  const std::tm* tm = std::localtime(&tt);
  strftime(buffer, 128, "%Y.%m.%d %H:%M:%S", tm);
  return buffer;
}

std::size_t countTags(TestStream& input, std::uint64_t tagToCount)
{
  std::size_t result = 0;
  const std::size_t oldReadPos = input.readPos;

  while (binlog::Range payload = input.nextEntryPayload())
  {
    const std::uint64_t tag = payload.read<std::uint64_t>();
    if (tag == tagToCount) { ++result; }
  }

  input.readPos = oldReadPos;

  return result;
}

max_stars_repo_name: linhanwang/binlog

max_issues_repo_name: linhanwang/binlog

max_forks_repo_name: linhanwang/binlog


text:
/*
  Copyright (c) 2015, 2017, Oracle and/or its affiliates. All rights reserved.

  This program is free software; you can redistribute it and/or modify
  it under the terms of the GNU General Public License as published by
  the Free Software Foundation; version 2 of the License.

  This program 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 General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with this program; if not, write to the Free Software
  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301  USA
*/

#include "mysql_object_reader.h"
#include <boost/algorithm/string.hpp>

using namespace Mysql::Tools::Dump;

void Mysql_object_reader::Rows_fetching_context::acquire_fields_information(
  MYSQL_RES* mysql_result)
{
  MYSQL_FIELD* fields= mysql_fetch_fields(mysql_result);
  uint field_count= mysql_num_fields(mysql_result);
  m_fields.reserve(field_count);
  for (uint i= 0; i < field_count; ++i)
    m_fields.push_back(Mysql_field(&fields[i]));
}

void Mysql_object_reader::Rows_fetching_context::process_buffer()
{
  if (m_row_group.m_rows.size() == 0)
    return;
  m_parent->format_rows(m_item_processing, &m_row_group);

  m_row_group.m_rows.clear();
}

int64 Mysql_object_reader::Rows_fetching_context::result_callback(
  const Mysql::Tools::Base::Mysql_query_runner::Row& row_data)
{
  if (unlikely(m_fields.size() == 0))
  {
    this->acquire_fields_information(
      row_data.get_mysql_result_info());
  }
  m_row_group.m_rows.push_back(new Row(row_data));

  if (m_row_group.m_rows.size() >= m_parent->m_options->m_row_group_size)
  {
    this->process_buffer();
  }

  return 0;
}

Mysql_object_reader::Rows_fetching_context::Rows_fetching_context(
  Mysql_object_reader* parent, Item_processing_data* item_processing,
  bool has_generated_column)
  : m_parent(parent),
  m_item_processing(item_processing),
  m_row_group((Table*)item_processing
    ->get_process_task_object()->get_related_db_object(), m_fields,
    has_generated_column)
{
  m_row_group.m_rows.reserve(
    (size_t)m_parent->m_options->m_row_group_size);
}

bool Mysql_object_reader::Rows_fetching_context::is_all_rows_processed()
{
  return m_row_group.m_rows.size() == 0;
}

void Mysql_object_reader::read_table_rows_task(
  Table_rows_dump_task* table_rows_dump_task,
  Item_processing_data* item_to_process)
{
  bool has_generated_columns= 0;
  Mysql::Tools::Base::Mysql_query_runner* runner= this->get_runner();
  Table* table= table_rows_dump_task->get_related_table();

  std::vector<const Mysql::Tools::Base::Mysql_query_runner::Row*> columns;
  std::vector<std::string> field_names;

  runner->run_query_store(
    "SELECT `COLUMN_NAME`, `EXTRA` FROM " +
    this->get_quoted_object_full_name("INFORMATION_SCHEMA", "COLUMNS") +
    "WHERE TABLE_SCHEMA ='" + runner->escape_string(table->get_schema()) +
    "' AND TABLE_NAME ='" + runner->escape_string(table->get_name()) + "'",
    &columns);

  std::string column_names;
  for (std::vector<const Mysql::Tools::Base::Mysql_query_runner::Row*>::iterator
    it= columns.begin(); it != columns.end(); ++it)
  {
    const Mysql::Tools::Base::Mysql_query_runner::Row& column_data= **it;
    if (column_data[1] == "STORED GENERATED" ||
        column_data[1] == "VIRTUAL GENERATED")
      has_generated_columns= 1;
    else
      column_names+= this->quote_name(column_data[0]) + ",";
  }
  /* remove last comma from column_names */
  column_names= boost::algorithm::replace_last_copy(column_names, ",", "");

  Mysql::Tools::Base::Mysql_query_runner::cleanup_result(&columns);

  Rows_fetching_context* row_fetching_context=
    new Rows_fetching_context(this, item_to_process, has_generated_columns);

  runner->run_query(
    "SELECT " + column_names + "  FROM " +
    this->get_quoted_object_full_name(table),
    new Mysql::Instance_callback<
      int64, const Mysql::Tools::Base::Mysql_query_runner::Row&,
        Rows_fetching_context>(
          row_fetching_context, &Rows_fetching_context::result_callback));

  row_fetching_context->process_buffer();
  if (row_fetching_context->is_all_rows_processed())
    delete row_fetching_context;
  delete runner;
}

void Mysql_object_reader::format_rows(Item_processing_data* item_to_process,
  Row_group_dump_task* row_group)
{
  this->format_object(this->new_chain_created(
    item_to_process, row_group));
}

void Mysql_object_reader::read_object(Item_processing_data* item_to_process)
{
  this->object_processing_starts(item_to_process);

  if (!(this->try_process_task<Table_rows_dump_task>
    (item_to_process, &Mysql_object_reader::read_table_rows_task)))
  {
    this->format_object(item_to_process);
  }

  this->object_processing_ends(item_to_process);
}

Mysql_object_reader::Mysql_object_reader(
  I_connection_provider* connection_provider,
  Mysql::I_callable<bool, const Mysql::Tools::Base::Message_data&>*
    message_handler, Simple_id_generator* object_id_generator,
  const Mysql_object_reader_options* options)
  : Abstract_data_formatter_wrapper(message_handler, object_id_generator),
  Abstract_mysql_chain_element_extension(connection_provider,
  message_handler, options->m_mysql_chain_element_options),
  m_options(options)
{}

ext: cc

max_stars_repo_name: SJTU-IPADS/dst

max_issues_repo_name: SJTU-IPADS/dst

max_forks_repo_name: SJTU-IPADS/dst


text: #include <boost/typeof/std/list.hpp>

ext: hpp

max_stars_repo_name: miathedev/BoostForArduino

max_issues_repo_name: miathedev/BoostForArduino

max_forks_repo_name: miathedev/BoostForArduino


text:
// Copyright 2019 ETH Zürich, Thomas Schöps
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// 1. Redistributions of source code must retain the above copyright notice,
//    this list of conditions and the following disclaimer.
//
// 2. 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.
//
// 3. Neither the name of the copyright holder 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 HOLDER 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.

#include "badslam/gui_settings_window.h"

#include <boost/filesystem.hpp>
#include <QFileDialog>
#include <QString>
#include <QLabel>
#include <QLineEdit>
#include <QTabWidget>
#include <QPushButton>
#include <QBoxLayout>
#include <QGridLayout>
#include <QCheckBox>
#include <QMessageBox>
#include <QSettings>

namespace vis {

bool ShowSettingsWindow(string* dataset_path, BadSlamConfig* config, bool* start_paused) {
  QString qdataset_path = QString::fromStdString(*dataset_path);

  SettingsDialog settings_dialog(&qdataset_path, config, start_paused);
  QSize bestSize = settings_dialog.sizeHint();
  bestSize.setWidth(bestSize.width() * 1.75f);
  settings_dialog.resize(bestSize);
  if (settings_dialog.exec() == QDialog::Rejected) {
    return false;
  }

  *dataset_path = qdataset_path.toStdString();
  return true;
}

SettingsDialog::SettingsDialog(QString* dataset_path, BadSlamConfig* config, bool* start_paused, QWidget* parent)
    : QDialog(parent),
      config(config),
      start_paused(start_paused),
      dataset_path(dataset_path) {
  setWindowTitle(tr("BAD SLAM Settings"));
  setWindowIcon(QIcon(":/badslam/badslam.png"));

  // Warning for live changes
  live_changes_warning = new QLabel(tr("Warning: For many settings, changes after the reconstruction has been started will not be handled correctly."));
  live_changes_warning->setVisible(false);

  // Dataset path line
  QLabel* dataset_path_label = new QLabel(tr("Dataset: "));
  dataset_path_edit = new QLineEdit(*dataset_path);
  QPushButton* dataset_choose_button = new QPushButton(tr("..."));
  connect(dataset_choose_button, &QPushButton::clicked, this, &SettingsDialog::ChooseDatasetClicked);

  QHBoxLayout* dataset_layout = new QHBoxLayout();
  dataset_layout->addWidget(dataset_path_label);
  dataset_layout->addWidget(dataset_path_edit, 1);
  dataset_layout->addWidget(dataset_choose_button);

#ifdef BAD_SLAM_HAVE_REALSENSE
  QPushButton* realsense_button = new QPushButton(tr("RealSense live input"));
  connect(realsense_button, &QPushButton::clicked, this, &SettingsDialog::RealSenseLiveInputClicked);
  dataset_layout->addWidget(realsense_button);
#endif

#ifdef HAVE_STRUCTURE
  QPushButton* structure_button = new QPushButton(tr("Structure live input"));
  connect(structure_button, &QPushButton::clicked, this, &SettingsDialog::StructureLiveInputClicked);
  dataset_layout->addWidget(structure_button);
#endif

#ifdef HAVE_K4A
  QPushButton* k4a_button = new QPushButton(tr("K4A live input"));
  connect(k4a_button, &QPushButton::clicked, this, &SettingsDialog::K4ALiveInputClicked);
  dataset_layout->addWidget(k4a_button);
#endif

  // TODO: Allow selecting the ground truth trajectory file

  // TODO: Maybe allow selecting some presets here in a second line (real-time, offline - low quality, offline - high quality)?

  // TODO: The help text must be available for the options as well. Also, this
  //       text should be stored only once for the GUI and command line interfaces,
  //       ideally in the config->h file such that we can also de-duplicate
  //       it with the comments there.
  auto add_option = [&](const QString& label, QWidget* widget, QGridLayout* layout, int* row) {
    layout->addWidget(new QLabel(label), *row, 0);
    layout->addWidget(widget, *row, 1);
    ++ *row;
  };

  // Settings tab: Dataset playback
  QWidget* dataset_playback_tab = new QWidget();
  QGridLayout* dataset_playback_layout = new QGridLayout();
  int row = 0;

  raw_to_float_depth_edit = new QLineEdit(QString::number(config->raw_to_float_depth));
  add_option(tr("Raw to metric depth conversion factor: "), raw_to_float_depth_edit, dataset_playback_layout, &row);

  target_frame_rate_edit = new QLineEdit(QString::number(config->target_frame_rate));
  add_option(tr("Target frame rate: "), target_frame_rate_edit, dataset_playback_layout, &row);

  restrict_fps_to_edit = new QLineEdit(QString::number(config->fps_restriction));
  add_option(tr("Restrict FPS to: "), restrict_fps_to_edit, dataset_playback_layout, &row);

  start_frame_edit = new QLineEdit(QString::number(config->start_frame));
  add_option(tr("Start frame: "), start_frame_edit, dataset_playback_layout, &row);

  end_frame_edit = new QLineEdit(QString::number(config->end_frame));
  add_option(tr("End frame: "), end_frame_edit, dataset_playback_layout, &row);

  pyramid_level_for_depth_edit = new QLineEdit(QString::number(config->pyramid_level_for_depth));
  add_option(tr("Pyramid level for depth: "), pyramid_level_for_depth_edit, dataset_playback_layout, &row);

  pyramid_level_for_color_edit = new QLineEdit(QString::number(config->pyramid_level_for_color));
  add_option(tr("Pyramid level for color: "), pyramid_level_for_color_edit, dataset_playback_layout, &row);

  load_poses_path_edit = new QLineEdit("");
  add_option(tr("Load poses from file: "), load_poses_path_edit, dataset_playback_layout, &row);

  dataset_playback_layout->setRowStretch(row, 1);
  dataset_playback_tab->setLayout(dataset_playback_layout);

  // Settings tab: Odometry
  QWidget* odometry_tab = new QWidget();
  QGridLayout* odometry_layout = new QGridLayout();
  row = 0;

  num_scales_edit = new QLineEdit(QString::number(config->num_scales));
  add_option(tr("Number of pyramid levels: "), num_scales_edit, odometry_layout, &row);

  use_motion_model_checkbox = new QCheckBox(tr("Use motion model"));
  use_motion_model_checkbox->setChecked(config->use_motion_model);
  odometry_layout->addWidget(use_motion_model_checkbox, row, 0, 1, 2);
  ++ row;

  odometry_layout->setRowStretch(row, 1);
  odometry_tab->setLayout(odometry_layout);

  // Settings tab: Bundle Adjustment
  QWidget* ba_tab = new QWidget();
  QGridLayout* ba_layout = new QGridLayout();
  row = 0;

  keyframe_interval_edit = new QLineEdit(QString::number(config->keyframe_interval));
  add_option(tr("Keyframe interval: "), keyframe_interval_edit, ba_layout, &row);

  max_num_ba_iterations_per_keyframe_edit = new QLineEdit(QString::number(config->max_num_ba_iterations_per_keyframe));
  add_option(tr("Max BA iteration count per keyframe: "), max_num_ba_iterations_per_keyframe_edit, ba_layout, &row);

  enable_deactivation_checkbox = new QCheckBox(tr("Enable deactivation of surfels and keyframes"));
  enable_deactivation_checkbox->setChecked(!config->disable_deactivation);
  ba_layout->addWidget(enable_deactivation_checkbox, row, 0, 1, 2);
  ++ row;

  use_geometric_residuals_checkbox = new QCheckBox(tr("Use geometric residuals"));
  use_geometric_residuals_checkbox->setChecked(config->use_geometric_residuals);
  ba_layout->addWidget(use_geometric_residuals_checkbox, row, 0, 1, 2);
  ++ row;

  use_photometric_residuals_checkbox = new QCheckBox(tr("Use photometric residuals"));
  use_photometric_residuals_checkbox->setChecked(config->use_photometric_residuals);
  ba_layout->addWidget(use_photometric_residuals_checkbox, row, 0, 1, 2);
  ++ row;

  optimize_intrinsics_checkbox = new QCheckBox(tr("Optimize camera intrinsics and depth deformation"));
  optimize_intrinsics_checkbox->setChecked(config->optimize_intrinsics);
  ba_layout->addWidget(optimize_intrinsics_checkbox, row, 0, 1, 2);
  ++ row;

  intrinsics_optimization_interval_edit = new QLineEdit(QString::number(config->intrinsics_optimization_interval));
  add_option(tr("Interval for intrinsics optimization (every Xth time BA is run): "), intrinsics_optimization_interval_edit, ba_layout, &row);

  // TODO: not in config
//   QLineEdit* final_ba_iterations_edit = new QLineEdit(QString::number(config->final_ba_iterations));
//   add_option(tr("Number of BA iterations to run after playback finishes: "), final_ba_iterations_edit, ba_layout, &row);

  do_surfel_updates_checkbox = new QCheckBox(tr("Perform surfel updates"));
  do_surfel_updates_checkbox->setChecked(config->do_surfel_updates);
  ba_layout->addWidget(do_surfel_updates_checkbox, row, 0, 1, 2);
  ++ row;

  parallel_ba_checkbox = new QCheckBox(tr("Run BA in parallel with odometry"));
  parallel_ba_checkbox->setChecked(config->parallel_ba);
  ba_layout->addWidget(parallel_ba_checkbox, row, 0, 1, 2);
  ++ row;

  use_pcg_checkbox = new QCheckBox(tr("Use PCG-based solver"));
  use_pcg_checkbox->setChecked(config->use_pcg);
  ba_layout->addWidget(use_pcg_checkbox, row, 0, 1, 2);
  ++ row;

  estimate_poses_checkbox = new QCheckBox(tr("Estimate poses"));
  estimate_poses_checkbox->setChecked(config->estimate_poses);
  ba_layout->addWidget(estimate_poses_checkbox, row, 0, 1, 2);
  ++ row;

  ba_layout->setRowStretch(row, 1);
  ba_tab->setLayout(ba_layout);

  // Settings tab: Memory
  QWidget* memory_tab = new QWidget();
  QGridLayout* memory_layout = new QGridLayout();
  row = 0;

  min_free_gpu_memory_mb_edit = new QLineEdit(QString::number(config->min_free_gpu_memory_mb));
  add_option(tr("Minimum GPU memory to remain free, in MB: "), min_free_gpu_memory_mb_edit, memory_layout, &row);

  memory_layout->setRowStretch(row, 1);
  memory_tab->setLayout(memory_layout);

  // Settings tab: Surfel reconstruction
  QWidget* surfel_reconstruction_tab = new QWidget();
  QGridLayout* surfel_reconstruction_layout = new QGridLayout();
  row = 0;

  max_surfel_count_edit = new QLineEdit(QString::number(config->max_surfel_count));
  add_option(tr("Maximum surfel count: "), max_surfel_count_edit, surfel_reconstruction_layout, &row);

  sparse_surfel_cell_size_edit = new QLineEdit(QString::number(config->sparse_surfel_cell_size));
  add_option(tr("Cell size for sparse surfel creation in pixels: "), sparse_surfel_cell_size_edit, surfel_reconstruction_layout, &row);

  surfel_merge_dist_factor_edit = new QLineEdit(QString::number(config->surfel_merge_dist_factor));
  add_option(tr("Factor on surfel merge distance: "), surfel_merge_dist_factor_edit, surfel_reconstruction_layout, &row);

  min_observation_count_while_bootstrapping_1_edit = new QLineEdit(QString::number(config->min_observation_count_while_bootstrapping_1));
  add_option(tr("Minimum surfel observation count (while bootstrapping, 1): "), min_observation_count_while_bootstrapping_1_edit, surfel_reconstruction_layout, &row);

  min_observation_count_while_bootstrapping_2_edit = new QLineEdit(QString::number(config->min_observation_count_while_bootstrapping_2));
  add_option(tr("Minimum surfel observation count (while bootstrapping, 2): "), min_observation_count_while_bootstrapping_2_edit, surfel_reconstruction_layout, &row);

  min_observation_count_edit = new QLineEdit(QString::number(config->min_observation_count));
  add_option(tr("Minimum surfel observation count: "), min_observation_count_edit, surfel_reconstruction_layout, &row);

  // TODO: Non-included option:
//   int reconstruction_sparse_surfel_cell_size = 1;
//   cmd_parser.NamedParameter(
//       "--reconstruction_sparsification",
//       &reconstruction_sparse_surfel_cell_size, /*required*/ false,
//       "Sparse surfel cell size for the final reconstruction that is done for"
//       " --export_reconstruction. See --sparsification.");

  surfel_reconstruction_layout->setRowStretch(row, 1);
  surfel_reconstruction_tab->setLayout(surfel_reconstruction_layout);

  // Settings tab: Loop closure
  QWidget* loop_closure_tab = new QWidget();
  QGridLayout* loop_closure_layout = new QGridLayout();
  row = 0;

  loop_closure_checkbox = new QCheckBox(tr("Enable loop detection"));
  loop_closure_checkbox->setChecked(config->enable_loop_detection);
  loop_closure_layout->addWidget(loop_closure_checkbox, row, 0, 1, 2);
  ++ row;

  parallel_loop_detection_checkbox = new QCheckBox(tr("Run loop detection in parallel with other tasks"));
  parallel_loop_detection_checkbox->setChecked(config->parallel_loop_detection);
  loop_closure_layout->addWidget(parallel_loop_detection_checkbox, row, 0, 1, 2);
  ++ row;

  loop_detection_image_frequency_edit = new QLineEdit(QString::number(config->loop_detection_image_frequency));
  add_option(tr("Image frequency: "), loop_detection_image_frequency_edit, loop_closure_layout, &row);

  loop_closure_layout->setRowStretch(row, 1);
  loop_closure_tab->setLayout(loop_closure_layout);

  // Settings tab: Depth preprocessing
  QWidget* depth_preprocessing_tab = new QWidget();
  QGridLayout* depth_preprocessing_layout = new QGridLayout();
  row = 0;

  max_depth_edit = new QLineEdit(QString::number(config->max_depth));
  add_option(tr("Maximum depth to use: "), max_depth_edit, depth_preprocessing_layout, &row);

  baseline_fx_edit = new QLineEdit(QString::number(config->baseline_fx));
  add_option(tr("Baseline * fx: "), baseline_fx_edit, depth_preprocessing_layout, &row);

  median_filter_and_densify_iterations_edit = new QLineEdit(QString::number(config->median_filter_and_densify_iterations));
  add_option(tr("Median filter and densify iterations: "), median_filter_and_densify_iterations_edit, depth_preprocessing_layout, &row);

  bilateral_filter_sigma_xy_edit = new QLineEdit(QString::number(config->bilateral_filter_sigma_xy));
  add_option(tr("Bilateral filter: sigma_xy: "), bilateral_filter_sigma_xy_edit, depth_preprocessing_layout, &row);

  bilateral_filter_radius_factor_edit = new QLineEdit(QString::number(config->bilateral_filter_radius_factor));
  add_option(tr("Bilateral filter: radius_factor: "), bilateral_filter_radius_factor_edit, depth_preprocessing_layout, &row);

  bilateral_filter_sigma_inv_depth_edit = new QLineEdit(QString::number(config->bilateral_filter_sigma_inv_depth));
  add_option(tr("Bilateral filter: sigma_inv_depth: "), bilateral_filter_sigma_inv_depth_edit, depth_preprocessing_layout, &row);

  depth_preprocessing_layout->setRowStretch(row, 1);
  depth_preprocessing_tab->setLayout(depth_preprocessing_layout);

  // Settings tab: Structure
#ifdef HAVE_STRUCTURE
  QWidget* structure_tab = new QWidget();
  QGridLayout* structure_layout = new QGridLayout();
  row = 0;

  structure_layout->addWidget(new QLabel(tr("Depth range: ")), row, 0);
  structure_depth_range_combo = new QComboBox();
  structure_depth_range_combo->addItem(tr("Default"));
  structure_depth_range_values.push_back("Default");
  structure_depth_range_combo->addItem(tr("BodyScanning"));
  structure_depth_range_values.push_back("BodyScanning");
  structure_depth_range_combo->addItem(tr("VeryShort (0.35m to 0.92m)"));
  structure_depth_range_values.push_back("VeryShort");
  structure_depth_range_combo->addItem(tr("Short (0.41m to 1.36m)"));
  structure_depth_range_values.push_back("Short");
  structure_depth_range_combo->addItem(tr("Medium (0.52m to 5.23m)"));
  structure_depth_range_values.push_back("Medium");
  structure_depth_range_combo->addItem(tr("Long (0.58m to 8.0m)"));
  structure_depth_range_values.push_back("Long");
  structure_depth_range_combo->addItem(tr("VeryLong (0.58m to 10.0m)"));
  structure_depth_range_values.push_back("VeryLong");
  structure_depth_range_combo->addItem(tr("Hybrid (0.35m to 10.0m)"));
  structure_depth_range_values.push_back("Hybrid");
  bool found = false;
  for (usize i = 0; i < structure_depth_range_values.size(); ++ i) {
    if (config->structure_depth_range == structure_depth_range_values[i]) {
      structure_depth_range_combo->setCurrentIndex(i);
      found = true;
      break;
    }
  }
  if (!found) {
    LOG(ERROR) << "Did not find the given Structure depth range, choosing Default instead.";
    structure_depth_range_combo->setCurrentIndex(0);
  }
  structure_layout->addWidget(structure_depth_range_combo, row, 1);
  ++ row;

  structure_depth_only_checkbox = new QCheckBox(tr("Stream only depth (without reprojection), but not visible images"));
  structure_depth_only_checkbox->setChecked(config->structure_depth_only);
  structure_layout->addWidget(structure_depth_only_checkbox, row, 0, 1, 2);
  ++ row;

  structure_layout->addWidget(new QLabel(tr("Depth resolution (only relevant if depth-only stream): ")), row, 0);
  structure_depth_resolution_combo = new QComboBox();
  structure_depth_resolution_combo->addItem("320x240");
  structure_depth_resolution_combo->addItem("640x480");
  structure_depth_resolution_combo->addItem("1280x960");
  if (config->structure_depth_resolution == string("320x240")) {
    structure_depth_resolution_combo->setCurrentIndex(0);
  } else if (config->structure_depth_resolution == string("640x480")) {
    structure_depth_resolution_combo->setCurrentIndex(1);
  } else if (config->structure_depth_resolution == string("1280x960")) {
    structure_depth_resolution_combo->setCurrentIndex(2);
  } else {
    LOG(ERROR) << "Did not find the given Structure depth resolution, choosing 640x480 instead.";
    structure_depth_range_combo->setCurrentIndex(1);
  }
  structure_layout->addWidget(structure_depth_resolution_combo, row, 1);
  ++ row;

  structure_expensive_correction_checkbox = new QCheckBox(tr("Use the Structure SDK's 'expensive correction' for depth images"));
  structure_expensive_correction_checkbox->setChecked(config->structure_expensive_correction);
  structure_layout->addWidget(structure_expensive_correction_checkbox, row, 0, 1, 2);
  ++ row;

  structure_one_shot_dynamic_calibration_checkbox = new QCheckBox(tr("Use the Structure SDK's one-shot dynamic calibration"));
  structure_one_shot_dynamic_calibration_checkbox->setChecked(config->structure_one_shot_dynamic_calibration);
  structure_layout->addWidget(structure_one_shot_dynamic_calibration_checkbox, row, 0, 1, 2);
  ++ row;

  structure_depth_diff_threshold_edit = new QLineEdit(QString::number(config->structure_depth_diff_threshold));
  add_option(tr("Depth difference threshold for continuous reprojection: "), structure_depth_diff_threshold_edit, structure_layout, &row);

  structure_infrared_auto_exposure_checkbox = new QCheckBox(tr("Infrared auto exposure"));
  structure_infrared_auto_exposure_checkbox->setChecked(config->structure_infrared_auto_exposure);
  structure_layout->addWidget(structure_infrared_auto_exposure_checkbox, row, 0, 1, 2);
  ++ row;

  structure_visible_exposure_time_edit = new QLineEdit(QString::number(config->structure_visible_exposure_time));
  add_option(tr("Fixed exposure time for visible camera [seconds]: "), structure_visible_exposure_time_edit, structure_layout, &row);

  structure_layout->setRowStretch(row, 1);
  structure_tab->setLayout(structure_layout);
#endif

  // Settings tab: K4A preprocessing
#ifdef HAVE_K4A
  QWidget* k4a_tab = new QWidget();
  QGridLayout* k4a_layout = new QGridLayout();
  row = 0;

  k4a_layout->addWidget(new QLabel(tr("FOV mode: ")), row, 0);
  k4a_mode_combo = new QComboBox();
  k4a_mode_combo->addItem(tr("nfov"));
  k4a_mode_values.push_back("nfov");
  k4a_mode_combo->addItem(tr("nfov2x2"));
  k4a_mode_values.push_back("nfov2x2");
  k4a_mode_combo->addItem(tr("wfov"));
  k4a_mode_values.push_back("wfov");
  k4a_mode_combo->addItem(tr("wfov2x2"));
  k4a_mode_values.push_back("wfov2x2");
  found = false;
  for (usize i = 0; i < k4a_mode_values.size(); ++ i) {
    if (config->k4a_mode == k4a_mode_values[i]) {
      k4a_mode_combo->setCurrentIndex(i);
      found = true;
      break;
    }
  }
  if (!found) {
    LOG(ERROR) << "Did not find the given K4A FOV mode, choosing the default instead.";
    k4a_mode_combo->setCurrentIndex(0);
  }
  k4a_layout->addWidget(k4a_mode_combo, row, 1);
  ++ row;

  k4a_layout->addWidget(new QLabel(tr("FPS: ")), row, 0);
  k4a_fps_combo = new QComboBox();
  k4a_fps_combo->addItem(tr("30"));
  k4a_fps_values.push_back(30);
  k4a_fps_combo->addItem(tr("15"));
  k4a_fps_values.push_back(15);
  k4a_fps_combo->addItem(tr("5"));
  k4a_fps_values.push_back(5);
  found = false;
  for (usize i = 0; i < k4a_fps_values.size(); ++ i) {
    if (config->k4a_fps == k4a_fps_values[i]) {
      k4a_fps_combo->setCurrentIndex(i);
      found = true;
      break;
    }
  }
  if (!found) {
    LOG(ERROR) << "Did not find the given K4A FPS setting, choosing the default instead.";
    k4a_fps_combo->setCurrentIndex(0);
  }
  k4a_layout->addWidget(k4a_fps_combo, row, 1);
  ++ row;

  k4a_layout->addWidget(new QLabel(tr("RGB resolution: ")), row, 0);
  k4a_resolution_combo = new QComboBox();
  k4a_resolution_combo->addItem(tr("1280 * 720 (16:9)"));
  k4a_resolution_values.push_back(720);
  k4a_resolution_combo->addItem(tr("1920 * 1080 (16:9)"));
  k4a_resolution_values.push_back(1080);
  k4a_resolution_combo->addItem(tr("2560 * 1440 (16:9)"));
  k4a_resolution_values.push_back(1440);
  k4a_resolution_combo->addItem(tr("2048 * 1536 (4:3)"));
  k4a_resolution_values.push_back(1536);
  k4a_resolution_combo->addItem(tr("3840 * 2160 (16:9)"));
  k4a_resolution_values.push_back(2160);
  k4a_resolution_combo->addItem(tr("4096 * 3072 (4:3)"));
  k4a_resolution_values.push_back(3072);
  found = false;
  for (usize i = 0; i < k4a_resolution_values.size(); ++ i) {
    if (config->k4a_resolution == k4a_resolution_values[i]) {
      k4a_resolution_combo->setCurrentIndex(i);
      found = true;
      break;
    }
  }
  if (!found) {
    LOG(ERROR) << "Did not find the given K4A resolution setting, choosing the default instead.";
    k4a_resolution_combo->setCurrentIndex(0);
  }
  k4a_layout->addWidget(k4a_resolution_combo, row, 1);
  ++ row;

  k4a_factor_edit = new QLineEdit(QString::number(config->k4a_factor));
  add_option(tr("Downscaling factor: "), k4a_factor_edit, k4a_layout, &row);

  k4a_use_ir_checkbox = new QCheckBox(tr("Use IR instead of RGB colors (this way, the full depth camera FOV will be used)"));
  k4a_use_ir_checkbox->setChecked(config->k4a_use_ir);
  k4a_layout->addWidget(k4a_use_ir_checkbox, row, 0, 1, 2);
  ++ row;

  k4a_exposure_edit = new QLineEdit(QString::number(config->k4a_exposure));
  add_option(tr("RGB exposure time in us (0 for auto-exposure): "), k4a_exposure_edit, k4a_layout, &row);

  k4a_record_path_edit = new QLineEdit(QString::fromStdString(config->k4a_record_path));
  add_option(tr("Path to an .mkv file to record: "), k4a_record_path_edit, k4a_layout, &row);

  k4a_layout->setRowStretch(row, 1);
  k4a_tab->setLayout(k4a_layout);
#endif

  // Settings tab widget
  QTabWidget* tab_widget = new QTabWidget(this);
  tab_widget->setElideMode(Qt::TextElideMode::ElideRight);
  tab_widget->addTab(dataset_playback_tab, tr("Dataset playback"));
  tab_widget->addTab(depth_preprocessing_tab, tr("Depth preprocessing"));
  tab_widget->addTab(surfel_reconstruction_tab, tr("Surfel Reconstruction"));
  tab_widget->addTab(odometry_tab, tr("Odometry"));
  tab_widget->addTab(ba_tab, tr("Bundle Adjustment"));
  tab_widget->addTab(loop_closure_tab, tr("Loop closure"));
  tab_widget->addTab(memory_tab, tr("Memory"));
#ifdef HAVE_STRUCTURE
  tab_widget->addTab(structure_tab, tr("Structure"));
#endif
#ifdef HAVE_K4A
  tab_widget->addTab(k4a_tab, tr("Azure Kinect"));
#endif

  // Action buttons
  QHBoxLayout* buttons_layout = new QHBoxLayout();
  buttons_layout->addStretch(1);
  if (start_paused) {
    QPushButton* start_button = new QPushButton(tr("Start"));
    start_button->setDefault(true);
    connect(start_button, &QPushButton::clicked, this, &SettingsDialog::StartClicked);

    QPushButton* start_paused_button = new QPushButton(tr("Start (paused)"));
    connect(start_paused_button, &QPushButton::clicked, this, &SettingsDialog::StartPausedClicked);

    QPushButton* quit_button = new QPushButton(tr("Quit"));
    connect(quit_button, &QPushButton::clicked, this, &QDialog::reject);

    buttons_layout->addWidget(start_button);
    buttons_layout->addWidget(start_paused_button);
    buttons_layout->addWidget(quit_button);
  } else {
    QPushButton* ok_button = new QPushButton(tr("Ok"));
    ok_button->setDefault(true);
    connect(ok_button, &QPushButton::clicked, this, &SettingsDialog::OkClicked);

    QPushButton* cancel_button = new QPushButton(tr("Cancel"));
    connect(cancel_button, &QPushButton::clicked, this, &QDialog::reject);

    buttons_layout->addWidget(ok_button);
    buttons_layout->addWidget(cancel_button);
  }

  QVBoxLayout* dialog_layout = new QVBoxLayout(this);
  dialog_layout->addWidget(live_changes_warning);
  dialog_layout->addLayout(dataset_layout);
  dialog_layout->addWidget(tab_widget);
  dialog_layout->addLayout(buttons_layout);
  setLayout(dialog_layout);
}

void SettingsDialog::DisablePathEditing() {
  dataset_path_edit->setEnabled(false);
}

void SettingsDialog::ShowWarningForLiveChanges() {
  live_changes_warning->setVisible(true);
}

void SettingsDialog::ChooseDatasetClicked() {
  QSettings settings;
  QString dataset_path = QFileDialog::getExistingDirectory(
      this,
      tr("Choose dataset"),
      dataset_path_edit->text().isEmpty() ? settings.value("dataset_path").toString() : dataset_path_edit->text());
  if (dataset_path.isEmpty()) {
    return;
  }
  dataset_path_edit->setText(dataset_path);
  settings.setValue("dataset_path", *this->dataset_path);
}

void SettingsDialog::RealSenseLiveInputClicked() {
  dataset_path_edit->setText("live://realsense");

  if (QMessageBox::question(
      this,
      tr("RealSense live input"),
      tr("Set recommended default settings for Intel D435 live input?"
         " This will disable photometric residuals, set --bilateral_filter_sigma_inv_depth to 0.01, set --max_depth to 4, and set --restrict_fps_to 0."),
      QMessageBox::StandardButton::Yes | QMessageBox::StandardButton::No) == QMessageBox::StandardButton::Yes) {
    use_photometric_residuals_checkbox->setChecked(false);
    bilateral_filter_sigma_inv_depth_edit->setText("0.01");
    max_depth_edit->setText("4");
    restrict_fps_to_edit->setText("0");
  }
}

void SettingsDialog::StructureLiveInputClicked() {
  dataset_path_edit->setText("live://structure");

  if (QMessageBox::question(
      this,
      tr("Structure live input"),
      tr("Set recommended default settings for Structure Core live input?"
         " This will set --bilateral_filter_sigma_inv_depth to 0.0075, --max_depth to 10, and --restrict_fps_to 0."),
      QMessageBox::StandardButton::Yes | QMessageBox::StandardButton::No) == QMessageBox::StandardButton::Yes) {
    bilateral_filter_sigma_inv_depth_edit->setText("0.0075");
    max_depth_edit->setText("10");
    restrict_fps_to_edit->setText("0");
  }
}

void SettingsDialog::K4ALiveInputClicked() {
  dataset_path_edit->setText("live://k4a");

  if (QMessageBox::question(
      this,
      tr("K4A live input"),
      tr("Set recommended default settings for Azure Kinect live input?"
         " This will disable photometric residuals, set --max_depth to 10, and set --restrict_fps_to 0."),
      QMessageBox::StandardButton::Yes | QMessageBox::StandardButton::No) == QMessageBox::StandardButton::Yes) {
    use_photometric_residuals_checkbox->setChecked(false);
    max_depth_edit->setText("10");
    restrict_fps_to_edit->setText("0");
  }
}

void SettingsDialog::StartClicked() {
  if (ParseSettings()) {
    *start_paused = false;
    accept();
  }
}

void SettingsDialog::StartPausedClicked() {
  if (ParseSettings()) {
    *start_paused = true;
    accept();
  }
}

void SettingsDialog::OkClicked() {
  if (ParseSettings()) {
    accept();
  }
}

bool SettingsDialog::ParseSettings() {
  bool ok = true;

  auto report_error = [&](const QString& option_name, const QString& invalid_value) {
    QMessageBox::warning(this, tr("Error"), tr("Entered value for %1 is invalid: %2").arg(option_name).arg(invalid_value));
  };

  // Validate the dataset path
  if (dataset_path_edit->text().isEmpty()) {
    QMessageBox::warning(this, tr("Error"), tr("Please choose a dataset or a source of live input."));
    return false;
  }
  *this->dataset_path = dataset_path_edit->text();

  if (!this->dataset_path->startsWith("live://") &&
      !this->dataset_path->endsWith(".mkv")) {
    boost::filesystem::path dataset_path = this->dataset_path->toStdString();
    if (!boost::filesystem::exists(dataset_path / "depth") ||
        !boost::filesystem::exists(dataset_path / "rgb") ||
        !boost::filesystem::exists(dataset_path / "calibration.txt") ||
        !boost::filesystem::exists(dataset_path / "associated.txt")) {
      QMessageBox::warning(this, tr("Error"), tr("There does not seem to be a dataset at the chosen dataset path. It must contain the \"depth\" and \"rgb\" folders and the \"calibration.txt\" and \"associated.txt\" files."));
      return false;
    }
  }

  // Dataset playback settings
  config->raw_to_float_depth = raw_to_float_depth_edit->text().toDouble(&ok);
  if (!ok) { report_error("raw_to_float_depth", raw_to_float_depth_edit->text()); return false; }

  config->target_frame_rate = target_frame_rate_edit->text().toDouble(&ok);
  if (!ok) { report_error("target_frame_rate", target_frame_rate_edit->text()); return false; }

  config->fps_restriction = restrict_fps_to_edit->text().toInt(&ok);
  if (!ok) { report_error("fps_restriction", restrict_fps_to_edit->text()); return false; }

  config->start_frame = start_frame_edit->text().toInt(&ok);
  if (!ok) { report_error("start_frame", start_frame_edit->text()); return false; }

  config->end_frame = end_frame_edit->text().toInt(&ok);
  if (!ok) { report_error("end_frame", end_frame_edit->text()); return false; }

  config->pyramid_level_for_depth = pyramid_level_for_depth_edit->text().toInt(&ok);
  if (!ok) { report_error("pyramid_level_for_depth", pyramid_level_for_depth_edit->text()); return false; }

  config->pyramid_level_for_color = pyramid_level_for_color_edit->text().toInt(&ok);
  if (!ok) { report_error("pyramid_level_for_color", pyramid_level_for_color_edit->text()); return false; }

  // TODO: load_poses_path_edit

  // Odometry settings
  config->num_scales = num_scales_edit->text().toInt(&ok);
  if (!ok) { report_error("num_scales", num_scales_edit->text()); return false; }

  config->use_motion_model = use_motion_model_checkbox->isChecked();

  // BA settings
  config->keyframe_interval = keyframe_interval_edit->text().toInt(&ok);
  if (!ok) { report_error("keyframe_interval", keyframe_interval_edit->text()); return false; }

  config->max_num_ba_iterations_per_keyframe = max_num_ba_iterations_per_keyframe_edit->text().toInt(&ok);
  if (!ok) { report_error("max_num_ba_iterations_per_keyframe", max_num_ba_iterations_per_keyframe_edit->text()); return false; }

  config->disable_deactivation = !enable_deactivation_checkbox->isChecked();

  config->use_geometric_residuals = use_geometric_residuals_checkbox->isChecked();

  config->use_photometric_residuals = use_photometric_residuals_checkbox->isChecked();

  config->optimize_intrinsics = optimize_intrinsics_checkbox->isChecked();

  config->intrinsics_optimization_interval = intrinsics_optimization_interval_edit->text().toInt(&ok);
  if (!ok) { report_error("intrinsics_optimization_interval", intrinsics_optimization_interval_edit->text()); return false; }

  config->do_surfel_updates = do_surfel_updates_checkbox->isChecked();

  config->parallel_ba = parallel_ba_checkbox->isChecked();

  config->use_pcg = use_pcg_checkbox->isChecked();

  config->estimate_poses = estimate_poses_checkbox->isChecked();

  // Memory settings
  config->min_free_gpu_memory_mb = min_free_gpu_memory_mb_edit->text().toInt(&ok);
  if (!ok) { report_error("min_free_gpu_memory_mb", min_free_gpu_memory_mb_edit->text()); return false; }

  // Surfel reconstruction settings
  config->max_surfel_count = max_surfel_count_edit->text().toInt(&ok);
  if (!ok) { report_error("max_surfel_count", max_surfel_count_edit->text()); return false; }

  config->sparse_surfel_cell_size = sparse_surfel_cell_size_edit->text().toInt(&ok);
  if (!ok) { report_error("sparse_surfel_cell_size", sparse_surfel_cell_size_edit->text()); return false; }

  config->surfel_merge_dist_factor = surfel_merge_dist_factor_edit->text().toDouble(&ok);
  if (!ok) { report_error("surfel_merge_dist_factor", surfel_merge_dist_factor_edit->text()); return false; }

  config->min_observation_count_while_bootstrapping_1 = min_observation_count_while_bootstrapping_1_edit->text().toInt(&ok);
  if (!ok) { report_error("min_observation_count_while_bootstrapping_1", min_observation_count_while_bootstrapping_1_edit->text()); return false; }

  config->min_observation_count_while_bootstrapping_2 = min_observation_count_while_bootstrapping_2_edit->text().toInt(&ok);
  if (!ok) { report_error("min_observation_count_while_bootstrapping_2", min_observation_count_while_bootstrapping_2_edit->text()); return false; }

  config->min_observation_count = min_observation_count_edit->text().toInt(&ok);
  if (!ok) { report_error("min_observation_count", min_observation_count_edit->text()); return false; }

  // Loop closure settings
  config->enable_loop_detection = loop_closure_checkbox->isChecked();

  config->parallel_loop_detection = parallel_loop_detection_checkbox->isChecked();

  config->loop_detection_image_frequency = loop_detection_image_frequency_edit->text().toInt(&ok);
  if (!ok) { report_error("loop_detection_image_frequency", loop_detection_image_frequency_edit->text()); return false; }

  // Depth preprocessing settings
  config->max_depth = max_depth_edit->text().toDouble(&ok);
  if (!ok) { report_error("max_depth", max_depth_edit->text()); return false; }

  config->baseline_fx = baseline_fx_edit->text().toDouble(&ok);
  if (!ok) { report_error("baseline_fx", baseline_fx_edit->text()); return false; }

  config->median_filter_and_densify_iterations = median_filter_and_densify_iterations_edit->text().toInt(&ok);
  if (!ok) { report_error("median_filter_and_densify_iterations", median_filter_and_densify_iterations_edit->text()); return false; }

  config->bilateral_filter_sigma_xy = bilateral_filter_sigma_xy_edit->text().toDouble(&ok);
  if (!ok) { report_error("bilateral_filter_sigma_xy", bilateral_filter_sigma_xy_edit->text()); return false; }

  config->bilateral_filter_radius_factor = bilateral_filter_radius_factor_edit->text().toDouble(&ok);
  if (!ok) { report_error("bilateral_filter_radius_factor", bilateral_filter_radius_factor_edit->text()); return false; }

  config->bilateral_filter_sigma_inv_depth = bilateral_filter_sigma_inv_depth_edit->text().toDouble(&ok);
  if (!ok) { report_error("bilateral_filter_sigma_inv_depth", bilateral_filter_sigma_inv_depth_edit->text()); return false; }

  // Structure settings
#ifdef HAVE_STRUCTURE
  config->structure_depth_range = structure_depth_range_values[structure_depth_range_combo->currentIndex()];

  config->structure_depth_only = structure_depth_only_checkbox->isChecked();

  config->structure_depth_resolution = structure_depth_resolution_combo->currentText().toStdString();

  config->structure_expensive_correction = structure_expensive_correction_checkbox->isChecked();

  config->structure_one_shot_dynamic_calibration = structure_one_shot_dynamic_calibration_checkbox->isChecked();

  config->structure_depth_diff_threshold = structure_depth_diff_threshold_edit->text().toDouble(&ok);
  if (!ok) { report_error("structure_depth_diff_threshold", structure_depth_diff_threshold_edit->text()); return false; }

  config->structure_infrared_auto_exposure = structure_infrared_auto_exposure_checkbox->isChecked();

  config->structure_visible_exposure_time = structure_visible_exposure_time_edit->text().toDouble(&ok);
  if (!ok) { report_error("structure_visible_exposure_time", structure_visible_exposure_time_edit->text()); return false; }
#endif

  // k4a settings
#ifdef HAVE_K4A
  config->k4a_mode = k4a_mode_values[k4a_mode_combo->currentIndex()];
  config->k4a_fps = k4a_fps_values[k4a_fps_combo->currentIndex()];
  config->k4a_resolution = k4a_resolution_values[k4a_resolution_combo->currentIndex()];
  config->k4a_factor = k4a_factor_edit->text().toInt(&ok);
  config->k4a_use_ir = k4a_use_ir_checkbox->isChecked();
  config->k4a_exposure = k4a_exposure_edit->text().toInt(&ok);
  config->k4a_record_path = k4a_record_path_edit->text().toStdString();
#endif

  return true;
}

}

max_stars_repo_name: dianaleeg/badslam

max_issues_repo_name: dianaleeg/badslam

max_forks_repo_name: dianaleeg/badslam


text: #include <boost/spirit/home/karma/format.hpp>


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

/* \author
 *      Jacob Schloss (jacob.schloss@urbanrobotics.net),
 *      Justin Rosen (jmylesrosen@gmail.com),
 *      Stephen Fox (foxstephend@gmail.com)
 */

#include <gtest/gtest.h>

#include <vector>
#include <cstdio>
#include <iostream>
using namespace std;

#include <pcl/common/time.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

using namespace pcl;

#include <pcl/outofcore/outofcore.h>
#include <pcl/outofcore/outofcore_impl.h>

#include <sensor_msgs/PointCloud2.h>

using namespace pcl::outofcore;

#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/foreach.hpp>

/** \brief Unit tests for UR out of core octree code which test public interface
 * of OutofcoreOctreeBase
 */

// For doing exhaustive checks this is set low remove those, and this can be
// set much higher
const static uint64_t numPts(10000);

const static boost::uint32_t rngseed = 0xAAFF33DD;

const static boost::filesystem::path filename_otreeA =
    "treeA/tree_test.oct_idx";
const static boost::filesystem::path filename_otreeB =
    "treeB/tree_test.oct_idx";

const static boost::filesystem::path filename_otreeA_LOD =
    "treeA_LOD/tree_test.oct_idx";
const static boost::filesystem::path filename_otreeB_LOD =
    "treeB_LOD/tree_test.oct_idx";

const static boost::filesystem::path
    outofcore_path("point_cloud_octree/tree_test.oct_idx");

typedef pcl::PointXYZ PointT;

// UR Typedefs
typedef OutofcoreOctreeBase<OutofcoreOctreeDiskContainer<PointT>, PointT>
    octree_disk;
typedef OutofcoreOctreeBaseNode<OutofcoreOctreeDiskContainer<PointT>, PointT>
    octree_disk_node;

typedef OutofcoreOctreeBase<OutofcoreOctreeRamContainer<PointT>, PointT>
    octree_ram;
typedef OutofcoreOctreeBaseNode<OutofcoreOctreeRamContainer<PointT>, PointT>
    octree_ram_node;

typedef std::vector<PointT, Eigen::aligned_allocator<PointT>>
    AlignedPointTVector;

AlignedPointTVector points;

/** \brief helper function to compare two points. is there a templated function
 * in pcl to do this for arbitrary point types?*/
bool compPt(const PointT &p1, const PointT &p2) {
    if (p1.x != p2.x || p1.y != p2.y || p1.z != p2.z)
        return false;

    return true;
}

TEST(PCL, Outofcore_Octree_Build) {

    boost::filesystem::remove_all(filename_otreeA.parent_path());
    boost::filesystem::remove_all(filename_otreeB.parent_path());

    Eigen::Vector3d min(-32.0, -32.0, -32.0);
    Eigen::Vector3d max(32.0, 32.0, 32.0);

    // Build two trees using each constructor
    // depth of treeA will be same as B because 1/2^3 > .1 and 1/2^4 < .1
    // depth really affects performance
    octree_disk treeA(min, max, .1, filename_otreeA, "ECEF");
    octree_disk treeB(4, min, max, filename_otreeB, "ECEF");

    // Equidistributed uniform pseudo-random number generator
    boost::mt19937 rng(rngseed);

    // For testing sparse
    // boost::uniform_real<double> dist(0,1);

    // For testing less sparse
    boost::normal_distribution<float> dist(0.5f, .1f);

    // Create a point
    PointT p;
    points.resize(numPts);

    // ignore these fields from the UR point for now
    // p.r = p.g = p.b = 0;
    // p.nx = p.ny = p.nz = 1;
    // p.cameraCount = 0;
    // p.error = 0;
    // p.triadID = 0;

    // Radomize it's position in space
    for (size_t i = 0; i < numPts; i++) {
        p.x = dist(rng);
        p.y = dist(rng);
        p.z = dist(rng);

        points[i] = p;
    }

    // Add to tree
    treeA.addDataToLeaf(points);

    // Add to tree
    treeB.addDataToLeaf(points);
}

TEST(PCL, Outofcore_Octree_Build_LOD) {

    boost::filesystem::remove_all(filename_otreeA_LOD.parent_path());
    boost::filesystem::remove_all(filename_otreeB_LOD.parent_path());

    Eigen::Vector3d min(0.0, 0.0, 0.0);
    Eigen::Vector3d max(1.0, 1.0, 1.0);

    // Build two trees using each constructor
    octree_disk treeA(min, max, .1, filename_otreeA_LOD, "ECEF");
    octree_disk treeB(4, min, max, filename_otreeB_LOD, "ECEF");

    // Equidistributed uniform pseudo-random number generator
    boost::mt19937 rng(rngseed);
    // For testing sparse
    // boost::uniform_real<double> dist(0,1);
    // For testing less sparse
    boost::normal_distribution<float> dist(0.5f, .1f);

    // Create a point
    PointT p;

    /*
      p.r = p.g = p.b = 0;
      p.nx = p.ny = p.nz = 1;
      p.cameraCount = 0;
      p.error = 0;
      p.triadID = 0;
    */
    points.resize(numPts);

    // Radomize it's position in space
    for (size_t i = 0; i < numPts; i++) {
        p.x = dist(rng);
        p.y = dist(rng);
        p.z = dist(rng);

        points[i] = p;
    }

    // Add to tree
    treeA.addDataToLeaf_and_genLOD(points);

    // Add to tree
    treeB.addDataToLeaf_and_genLOD(points);
}

TEST(PCL, Outofcore_Bounding_Box) {

    Eigen::Vector3d min(-32.0, -32.0, -32.0);
    Eigen::Vector3d max(32.0, 32.0, 32.0);

    octree_disk treeA(filename_otreeA, false);
    octree_disk treeB(filename_otreeB, false);

    Eigen::Vector3d min_otreeA;
    Eigen::Vector3d max_otreeA;
    treeA.getBoundingBox(min_otreeA, max_otreeA);

    Eigen::Vector3d min_otreeB;
    Eigen::Vector3d max_otreeB;
    treeB.getBoundingBox(min_otreeB, max_otreeB);

    for (int i = 0; i < 3; i++) {
        // octree adds an epsilon to bounding box
        EXPECT_LE(min_otreeA[i], min[i]);
        EXPECT_NEAR(min_otreeA[i], min[i], 1e4);

        EXPECT_GE(max_otreeA[i], max[i]);
        EXPECT_NEAR(max_otreeA[i], max[i], 1e4);

        EXPECT_LE(min_otreeB[i], min[i]);
        EXPECT_NEAR(min_otreeB[i], min[i], 1e4);

        EXPECT_GE(max_otreeB[i], max[i]);
        EXPECT_NEAR(max_otreeB[i], max[i], 1e4);
    }
}

void point_test(octree_disk &t) {
    boost::mt19937 rng(rngseed);
    boost::uniform_real<float> dist(0, 1);

    Eigen::Vector3d query_box_min;
    Eigen::Vector3d qboxmax;

    for (int i = 0; i < 10; i++) {
        // std::cout << "query test round " << i << std::endl;
        for (int j = 0; j < 3; j++) {
            query_box_min[j] = dist(rng);
            qboxmax[j] = dist(rng);

            if (qboxmax[j] < query_box_min[j]) {
                std::swap(query_box_min[j], qboxmax[j]);
            }
        }

        // query the trees
        AlignedPointTVector p_ot;

        t.queryBBIncludes(query_box_min, qboxmax, t.getDepth(), p_ot);

        // query the list
        AlignedPointTVector pointsinregion;

        for (AlignedPointTVector::iterator pointit = points.begin();
             pointit != points.end(); ++pointit) {
            if ((query_box_min[0] <= pointit->x) && (pointit->x < qboxmax[0]) &&
                (query_box_min[1] < pointit->y) && (pointit->y < qboxmax[1]) &&
                (query_box_min[2] <= pointit->z) && (pointit->z < qboxmax[2])) {
                pointsinregion.push_back(*pointit);
            }
        }

        EXPECT_EQ(p_ot.size(), pointsinregion.size());

        // very slow exhaustive comparison
        while (!p_ot.empty()) {
            AlignedPointTVector::iterator it;
            it = std::find_first_of(p_ot.begin(), p_ot.end(),
                                    pointsinregion.begin(),
                                    pointsinregion.end(), compPt);

            if (it != p_ot.end()) {
                p_ot.erase(it);
            } else {
                FAIL() << "Dropped Point from tree1!" << std::endl;
                break;
            }
        }

        EXPECT_TRUE(p_ot.empty());
    }
}

TEST(PCL, Outofcore_Point_Query) {
    octree_disk treeA(filename_otreeA, false);
    octree_disk treeB(filename_otreeB, false);

    point_test(treeA);
    point_test(treeB);
}

#if 0 // this class will be deprecated soon.
TEST (PCL, Outofcore_Ram_Tree)
{
  Eigen::Vector3d min (0.0,0.0,0.0);
  Eigen::Vector3d max (1.0, 1.0, 1.0);

  const boost::filesystem::path filename_otreeA = "ram_tree/ram_tree.oct_idx";

  octree_ram t (min, max, .1, filename_otreeA, "ECEF");

  boost::mt19937 rng (rngseed);
  //boost::uniform_real<double> dist(0,1);//for testing sparse
  boost::normal_distribution<float> dist (0.5f, .1f);//for testing less sparse
  PointT p;

  points.resize (numPts);
  for (size_t i = 0; i < numPts; i++)
  {
    p.x = dist(rng);
    p.y = dist(rng);
    p.z = dist(rng);

    points[i] = p;
  }

  t.addDataToLeaf_and_genLOD (points);
  //t.addDataToLeaf(points);

  Eigen::Vector3d qboxmin;
  Eigen::Vector3d qboxmax;
  for (int i = 0; i < 10; i++)
  {
    //std::cout << "query test round " << i << std::endl;
    for (int j = 0; j < 3; j++)
    {
      qboxmin[j] = dist (rng);
      qboxmax[j] = dist (rng);

      if (qboxmax[j] < qboxmin[j])
      {
        std::swap (qboxmin[j], qboxmax[j]);
      }
    }

    //query the trees
    AlignedPointTVector p_ot1;
    t.queryBBIncludes (qboxmin, qboxmax, t.getDepth (), p_ot1);

    //query the list
    AlignedPointTVector pointsinregion;
    BOOST_FOREACH(const PointT& p, points)
    {
      if ((qboxmin[0] <= p.x) && (p.x <= qboxmax[0]) && (qboxmin[1] <= p.y) && (p.y <= qboxmax[1]) && (qboxmin[2] <= p.z) && (p.z <= qboxmax[2]))
      {
        pointsinregion.push_back (p);
      }
    }

    EXPECT_EQ (p_ot1.size (), pointsinregion.size ());

    //very slow exhaustive comparison
    while (!p_ot1.empty ())
    {
      AlignedPointTVector::iterator it;
      it = std::find_first_of (p_ot1.begin (), p_ot1.end (), pointsinregion.begin (), pointsinregion.end (), compPt);

      if (it != p_ot1.end ())
      {
        p_ot1.erase(it);
      }
      else
      {
        break;
        FAIL () <<  "Dropped Point from tree1!" << std::endl;
      }
    }

    EXPECT_TRUE (p_ot1.empty ());
  }
}
#endif
class OutofcoreTest : public testing::Test {
  protected:
    OutofcoreTest() : smallest_voxel_dim() {}

    virtual void SetUp() { smallest_voxel_dim = 3.0f; }

    virtual void TearDown() {}

    void cleanUpFilesystem() {
        // clear existing trees from test path

        boost::filesystem::remove_all(filename_otreeA.parent_path());
        boost::filesystem::remove_all(filename_otreeB.parent_path());

        boost::filesystem::remove_all(filename_otreeA_LOD.parent_path());
        boost::filesystem::remove_all(filename_otreeB_LOD.parent_path());

        boost::filesystem::remove_all(outofcore_path.parent_path());
    }

    double smallest_voxel_dim;
};

/** \brief Thorough test of the constructors, including exceptions and specified
 * behavior */
TEST_F(OutofcoreTest, Outofcore_Constructors) {
    // Case 1: create octree on-disk by resolution
    // Case 2: create octree on-disk by depth
    // Case 3: try to create an octree in existing tree and handle exception
    // Case 4: load existing octree from disk
    // Case 5: try to load non-existent octree from disk

    cleanUpFilesystem();

    // Specify the lower corner of the axis-aligned bounding box
    const Eigen::Vector3d min(-1024.0, -1024.0, -1024.0);
    // Specify the upper corner of the axis-aligned bounding box
    const Eigen::Vector3d max(1024.0, 1024.0, 1024.0);

    AlignedPointTVector some_points;

    for (unsigned int i = 0; i < numPts; i++)
        some_points.push_back(PointT(static_cast<float>(rand() % 1024),
                                     static_cast<float>(rand() % 1024),
                                     static_cast<float>(rand() % 1024)));

    //(Case 1)
    // Create Octree based on resolution of smallest voxel, automatically
    // computing depth
    octree_disk octreeA(min, max, smallest_voxel_dim, filename_otreeA, "ECEF");

    EXPECT_EQ(some_points.size(), octreeA.addDataToLeaf(some_points))
        << "Dropped points in voxel resolution constructor\n";

    EXPECT_EQ(some_points.size(),
              octreeA.getNumPointsAtDepth(octreeA.getDepth()));

    //(Case 2)
    // create Octree by prespecified depth in constructor
    int depth = 2;
    octree_disk octreeB(depth, min, max, filename_otreeB, "ECEF");
    EXPECT_EQ(some_points.size(), octreeB.addDataToLeaf(some_points))
        << "Dropped points in fixed-depth constructor\n";

    EXPECT_EQ(some_points.size(),
              octreeB.getNumPointsAtDepth(octreeB.getDepth()));
}

TEST_F(OutofcoreTest, Outofcore_ConstructorSafety) {
    // Specify the lower corner of the axis-aligned bounding box
    const Eigen::Vector3d min(-1024, -1024, -1024);
    // Specify the upper corner of the axis-aligned bounding box
    const Eigen::Vector3d max(1024, 1024, 1024);

    int depth = 2;

    //(Case 3) Constructor Safety. These should throw OCT_CHILD_EXISTS
    //exceptions and write an error message of conflicting file path
    ASSERT_TRUE(boost::filesystem::exists(filename_otreeA))
        << "No tree detected on disk. This test will fail. Perhaps this test "
           "was run out of order.\n";
    ASSERT_TRUE(boost::filesystem::exists(filename_otreeB))
        << "No tree detected on disk. This test will fail. Perhaps this test "
           "was run out of order.\n";

    EXPECT_ANY_THROW({
        octree_disk octreeC(min, max, smallest_voxel_dim, filename_otreeA,
                            "ECEF");
    }) << "Failure to detect existing tree on disk with the same name. Data "
          "may be overwritten.\n";
    EXPECT_ANY_THROW({
        octree_disk octreeD(depth, min, max, filename_otreeB, "ECEF");
    }) << "Failure to detect existing tree on disk with the same name. Data "
          "may be overwritten.\n";

    //(Case 4): Load existing tree from disk
    octree_disk octree_from_disk(filename_otreeB, true);
    vector<uint64_t> numPoints = octree_from_disk.getNumPointsVector();
    EXPECT_EQ(numPts,
              octree_from_disk.getNumPointsAtDepth(octree_from_disk.getDepth()))
        << "Failure to count the number of points in a tree already existing "
           "on disk\n";
}

TEST_F(OutofcoreTest, Outofcore_ConstructorBadPaths) {
    //(Case 5): Try to load non-existent tree from disk
    // root node should be created at this point
    /// \todo Shouldn't these throw an exception for bad path?
    boost::filesystem::path non_existent_path_name(
        "treeBogus/tree_bogus.oct_idx");
    boost::filesystem::path bad_extension_path(
        "treeBadExtension/tree_bogus.bad_extension");

    EXPECT_FALSE(boost::filesystem::exists(non_existent_path_name));
    EXPECT_ANY_THROW(
        { octree_disk octree_bogus_path(non_existent_path_name, true); });

    EXPECT_FALSE(boost::filesystem::exists(bad_extension_path));
    EXPECT_ANY_THROW(
        { octree_disk octree_bad_extension(bad_extension_path, true); });
}

TEST_F(OutofcoreTest, Outofcore_PointcloudConstructor) {
    cleanUpFilesystem();

    // Specify the lower corner of the axis-aligned bounding box
    const Eigen::Vector3d min(-1, -1, -1);
    // Specify the upper corner of the axis-aligned bounding box
    const Eigen::Vector3d max(1024, 1024, 1024);

    // create a point cloud
    PointCloud<PointT>::Ptr test_cloud(new PointCloud<PointT>());

    test_cloud->width = numPts;
    test_cloud->height = 1;
    test_cloud->reserve(numPts);

    // generate some random points
    for (size_t i = 0; i < numPts; i++) {
        PointT tmp(static_cast<float>(i % 1024), static_cast<float>(i % 1024),
                   static_cast<float>(i % 1024));

        test_cloud->points.push_back(tmp);
    }

    EXPECT_EQ(numPts, test_cloud->points.size());

    octree_disk pcl_cloud(min, max, 4, outofcore_path, "ECEF");

    pcl_cloud.addPointCloud(test_cloud);

    EXPECT_EQ(test_cloud->points.size(),
              pcl_cloud.getNumPointsAtDepth(pcl_cloud.getDepth()));

    cleanUpFilesystem();
}

TEST_F(OutofcoreTest, Outofcore_PointsOnBoundaries) {
    cleanUpFilesystem();

    const Eigen::Vector3d min(-1, -1, -1);
    const Eigen::Vector3d max(1, 1, 1);

    PointCloud<PointT>::Ptr cloud(new PointCloud<PointT>());
    cloud->width = 8;
    cloud->height = 1;
    cloud->reserve(8);

    for (int i = 0; i < 8; i++) {
        PointT tmp;
        tmp.x = static_cast<float>(pow(-1.0, i)) * 1.0f;
        tmp.y = static_cast<float>(pow(-1.0, i + 1)) * 1.0f;
        tmp.z = static_cast<float>(pow(-1.0, 3 * i)) * 1.0f;

        cloud->points.push_back(tmp);
    }

    octree_disk octree(min, max, 4, outofcore_path, "ECEF");

    octree.addPointCloud(cloud);

    EXPECT_EQ(8, octree.getNumPointsAtDepth(octree.getDepth()));
}

/*
TEST_F (OutofcoreTest, Outofcore_PointCloud2Basic)
{
  cleanUpFilesystem ();

  const double min[3] = { -1.0, -1.0, -1.0 };
  const double max[3] = { 1.0, 1.0, 1.0 };

  sensor_msgs::PointCloud2::Ptr cloud (new sensor_msgs::PointCloud2 ());

}
*/

TEST_F(OutofcoreTest, Outofcore_MultiplePointClouds) {
    cleanUpFilesystem();

    // Specify the lower corner of the axis-aligned bounding box
    const Eigen::Vector3d min(-1024, -1024, -1024);

    // Specify the upper corner of the axis-aligned bounding box
    const Eigen::Vector3d max(1024, 1024, 1024);

    // create a point cloud
    PointCloud<PointT>::Ptr test_cloud(new PointCloud<PointT>());
    PointCloud<PointT>::Ptr second_cloud(new PointCloud<PointT>());

    test_cloud->width = numPts;
    test_cloud->height = 1;
    test_cloud->reserve(numPts);

    second_cloud->width = numPts;
    second_cloud->height = 1;
    second_cloud->reserve(numPts);

    // generate some random points
    for (size_t i = 0; i < numPts; i++) {
        PointT tmp(static_cast<float>(i % 1024), static_cast<float>(i % 1024),
                   static_cast<float>(i % 1024));

        test_cloud->points.push_back(tmp);
    }

    for (size_t i = 0; i < numPts; i++) {
        PointT tmp(static_cast<float>(i % 1024), static_cast<float>(i % 1024),
                   static_cast<float>(i % 1024));

        second_cloud->points.push_back(tmp);
    }

    octree_disk pcl_cloud(min, max, 4, outofcore_path, "ECEF");

    EXPECT_EQ(test_cloud->points.size(), pcl_cloud.addPointCloud(test_cloud));

    EXPECT_EQ(numPts, pcl_cloud.getNumPointsAtDepth(pcl_cloud.getDepth()));

    pcl_cloud.addPointCloud(second_cloud);

    EXPECT_EQ(2 * numPts, pcl_cloud.getNumPointsAtDepth(pcl_cloud.getDepth()))
        << "Points are lost when two points clouds are added to the outofcore "
           "file system\n";
    cleanUpFilesystem();
}

TEST_F(OutofcoreTest, Outofcore_PointCloudInput_LOD) {
    cleanUpFilesystem();

    // Specify the lower corner of the axis-aligned bounding box
    const Eigen::Vector3d min(-1024, -1024, -1024);

    // Specify the upper corner of the axis-aligned bounding box
    const Eigen::Vector3d max(1024, 1024, 1024);

    // create a point cloud
    PointCloud<PointT>::Ptr test_cloud(new PointCloud<PointT>());
    PointCloud<PointT>::Ptr second_cloud(new PointCloud<PointT>());

    test_cloud->width = numPts;
    test_cloud->height = 1;
    test_cloud->reserve(numPts);

    second_cloud->width = numPts;
    second_cloud->height = 1;
    second_cloud->reserve(numPts);

    // generate some random points
    for (size_t i = 0; i < numPts; i++) {
        PointT tmp(static_cast<float>(i % 1024), static_cast<float>(i % 1024),
                   static_cast<float>(i % 1024));

        test_cloud->points.push_back(tmp);
    }

    for (size_t i = 0; i < numPts; i++) {
        PointT tmp(static_cast<float>(i % 1024), static_cast<float>(i % 1024),
                   static_cast<float>(i % 1024));

        second_cloud->points.push_back(tmp);
    }

    octree_disk pcl_cloud(min, max, 4, outofcore_path, "ECEF");

    pcl_cloud.addPointCloud_and_genLOD(second_cloud);

    //  EXPECT_EQ (2*numPts, pcl_cloud.getNumPointsAtDepth (pcl_cloud.getDepth
    //  ())) << "Points are lost when two points clouds are added to the
    //  outofcore file system\n";
    cleanUpFilesystem();
}

TEST_F(OutofcoreTest, PointCloud2_Constructors) {
    cleanUpFilesystem();

    // Specify the bounding box of the point clouds
    const Eigen::Vector3d min(-100.1, -100.1, -100.1);
    const Eigen::Vector3d max(100.1, 100.1, 100.1);

    const boost::uint64_t depth = 2;

    // create a point cloud
    PointCloud<PointT>::Ptr test_cloud(new PointCloud<PointT>());

    test_cloud->width = numPts;
    test_cloud->height = 1;
    test_cloud->reserve(numPts);

    // generate some random points
    for (size_t i = 0; i < numPts; i++) {
        PointT tmp(static_cast<float>(i % 200) - 99,
                   static_cast<float>(i % 200) - 99,
                   static_cast<float>(i % 200) - 99);

        test_cloud->points.push_back(tmp);
    }

    boost::shared_ptr<sensor_msgs::PointCloud2> point_cloud(
        new sensor_msgs::PointCloud2());

    pcl::toROSMsg(*test_cloud, *point_cloud);

    octree_disk octreeA(depth, min, max, filename_otreeA, "ECEF");
    octree_disk octreeB(depth, min, max, filename_otreeB, "ECEF");

    EXPECT_EQ(octreeA.addPointCloud(point_cloud, false),
              point_cloud->width * point_cloud->height)
        << "Number of points returned by constructor indicates some points "
           "were not properly inserted to the outofcore cloud\n";

    EXPECT_EQ(octreeB.addPointCloud_and_genLOD(point_cloud),
              point_cloud->width * point_cloud->height)
        << "Number of points inserted when generating LOD does not match the "
           "size of the point cloud\n";
}

TEST_F(OutofcoreTest, PointCloud2_Insertion) {
    cleanUpFilesystem();

    const Eigen::Vector3d min(-11, -11, -11);
    const Eigen::Vector3d max(11, 11, 11);

    pcl::PointCloud<pcl::PointXYZ> point_cloud;

    point_cloud.points.reserve(numPts);
    point_cloud.width = static_cast<uint32_t>(numPts);
    point_cloud.height = 1;

    for (size_t i = 0; i < numPts; i++)
        point_cloud.points.push_back(PointT(static_cast<float>(rand() % 10),
                                            static_cast<float>(rand() % 10),
                                            static_cast<float>(rand() % 10)));

    sensor_msgs::PointCloud2::Ptr input_cloud(new sensor_msgs::PointCloud2());

    toROSMsg<PointXYZ>(point_cloud, *input_cloud);
    ASSERT_EQ(point_cloud.width * point_cloud.height,
              input_cloud->width * input_cloud->height);

    octree_disk octreeA(min, max, smallest_voxel_dim, filename_otreeA, "ECEF");
    octree_disk octreeB(1, min, max, filename_otreeB, "ECEF");

    // make sure the number of points successfully added are the same as how
    // many we input
    uint64_t points_in_input_cloud = input_cloud->width * input_cloud->height;
    EXPECT_EQ(octreeA.addPointCloud(input_cloud, false), points_in_input_cloud)
        << "Insertion failure. Number of points successfully added does not "
           "match size of input cloud\n";
    EXPECT_EQ(octreeB.addPointCloud(input_cloud, false), points_in_input_cloud)
        << "Insertion failure. Number of points successfully added does not "
           "match size of input cloud\n";
}

TEST_F(OutofcoreTest, PointCloud2_MultiplePointCloud) {

    cleanUpFilesystem();

    // Specify the bounding box of the point clouds
    const Eigen::Vector3d min(-100.1, -100.1, -100.1);
    const Eigen::Vector3d max(100.1, 100.1, 100.1);

    // create a point cloud
    PointCloud<PointT>::Ptr first_cloud(new PointCloud<PointT>());
    PointCloud<PointT>::Ptr second_cloud(new PointCloud<PointT>());

    first_cloud->width = numPts;
    first_cloud->height = 1;
    first_cloud->reserve(numPts);

    second_cloud->width = numPts;
    second_cloud->height = 1;
    second_cloud->reserve(numPts);

    // generate some random points
    for (size_t i = 0; i < numPts; i++) {
        PointT tmp(static_cast<float>(i % 50), static_cast<float>(i % 50),
                   static_cast<float>(i % 50));

        first_cloud->points.push_back(tmp);
    }

    for (size_t i = 0; i < numPts; i++) {
        PointT tmp(static_cast<float>(i % 50), static_cast<float>(i % 50),
                   static_cast<float>(i % 50));

        second_cloud->points.push_back(tmp);
    }

    sensor_msgs::PointCloud2::Ptr first_cloud_ptr(
        new sensor_msgs::PointCloud2());
    sensor_msgs::PointCloud2::Ptr second_cloud_ptr(
        new sensor_msgs::PointCloud2());

    toROSMsg<PointT>(*first_cloud, *first_cloud_ptr);
    toROSMsg<PointT>(*second_cloud, *second_cloud_ptr);

    // Create an outofcore tree which just concatenates the two clouds into a
    // single PCD in the root node. Check that the number of points is correct.
    octree_disk shallow_outofcore(0 /*depth*/, min, max, filename_otreeB,
                                  "ECEF");

    shallow_outofcore.addPointCloud(first_cloud);
    shallow_outofcore.addPointCloud(second_cloud);

    sensor_msgs::PointCloud2::Ptr result(new sensor_msgs::PointCloud2());
    shallow_outofcore.queryBBIncludes(min, max, 0, result);

    size_t num_points_queried = result->width * result->height;
    size_t num_points_inserted = first_cloud->width * first_cloud->height +
                                 second_cloud->width * second_cloud->height;

    EXPECT_EQ(num_points_inserted, num_points_queried)
        << "If num_points_inserted > num_points_on_disk, then points were "
           "dropped on insertion of multiple clouds in the outofcore octree";
}

// test that the PointCloud2 query returns the same points as the templated
// queries
TEST_F(OutofcoreTest, PointCloud2_Query) {

    cleanUpFilesystem();

    // Specify the bounding box of the point clouds
    const Eigen::Vector3d min(-100.1, -100.1, -100.1);
    const Eigen::Vector3d max(100.1, 100.1, 100.1);

    const boost::uint64_t depth = 2;

    // create a point cloud
    PointCloud<PointT>::Ptr test_cloud(new PointCloud<PointT>());

    test_cloud->width = numPts;
    test_cloud->height = 1;
    test_cloud->reserve(numPts);

    // generate some random points
    for (size_t i = 0; i < numPts; i++) {
        PointT tmp(static_cast<float>(i % 50) - 50,
                   static_cast<float>(i % 50) - 50,
                   static_cast<float>(i % 50) - 50);

        test_cloud->points.push_back(tmp);
    }

    sensor_msgs::PointCloud2::Ptr dst_blob(new sensor_msgs::PointCloud2());

    pcl::toROSMsg(*test_cloud, *dst_blob);

    octree_disk octreeA(depth, min, max, filename_otreeA, "ECEF");
    octree_disk octreeB(depth, min, max, filename_otreeB, "ECEF");

    uint64_t points_added = octreeA.addPointCloud(dst_blob, false);
    uint64_t LOD_points_added = octreeB.addPointCloud_and_genLOD(dst_blob);

    ASSERT_EQ(points_added, dst_blob->width * dst_blob->height)
        << "Number of points returned by addPointCloud does not match the "
           "number of poitns in the input point cloud\n";
    ASSERT_EQ(LOD_points_added, dst_blob->width * dst_blob->height)
        << "Number of points returned by addPointCloud_and_genLOD does not "
           "match the number of points in the input point cloud\n";

    sensor_msgs::PointCloud2::Ptr query_result_a(
        new sensor_msgs::PointCloud2());
    sensor_msgs::PointCloud2::Ptr query_result_b(
        new sensor_msgs::PointCloud2());

    octreeA.queryBBIncludes(min, max, int(octreeA.getDepth()), query_result_a);

    EXPECT_EQ(test_cloud->width * test_cloud->height,
              query_result_a->width * query_result_a->height)
        << "PointCloud2 Query number of points returned failed\n";

    uint64_t total_octreeB_LOD_query = 0;

    for (int i = 0; i <= octreeB.getDepth(); i++) {
        octreeB.queryBBIncludes(min, max, i, query_result_b);
        total_octreeB_LOD_query +=
            query_result_b->width * query_result_b->height;
        query_result_b->data.clear();
        query_result_b->width = 0;
        query_result_b->height = 0;
    }

    EXPECT_EQ(test_cloud->width * test_cloud->height, total_octreeB_LOD_query)
        << "PointCloud2 Query number of points returned failed\n";

    cleanUpFilesystem();
}

/* [--- */
int main(int argc, char **argv) {
    testing::InitGoogleTest(&argc, argv);
    return (RUN_ALL_TESTS());
}
/* ]--- */

max_stars_repo_name: yxlao/StanfordPCL

max_issues_repo_name: yxlao/StanfordPCL

max_forks_repo_name: yxlao/StanfordPCL