Class SLOAMTest

sloam/src/tests/core_test.cpp:53–126

}

class SLOAMTest : public ::testing::Test
{
protected:
  void SetUp() override
  {
    setupParams();

    // t0
    readInputData(t0Input, "still", "t0");
    t0Input.poseEstimate = SE3();

    // t1
    readInputData(t1Input, "still", "t1");
    t1Input.poseEstimate = SE3();

    if (ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug))
      ros::console::notifyLoggerLevelsChanged();
  }

  void readInputData(SloamInput& inp, std::string prefix, std::string stamp){
    std::string base_dir = "/opt/sloam_ws/src/sloam/src/tests/aux/";
    std::string landmarks_path = base_dir + prefix + "_landmarks_" + stamp;
    std::string ground_path = base_dir + prefix + "_ground_" + stamp + std::string(".pcd");

    // read class state from archive
    std::ifstream ifs(landmarks_path);
    boost::archive::text_iarchive ia(ifs);
    ia >> inp.landmarks;

    for(auto& tree : inp.landmarks){
      for(auto& vtx : tree){
        vtx.points.resize(5);
      }
    }
    
    // CloudT::Ptr rawGround(new CloudT); 
    // pcl::io::loadPCDFile<PointT>(ground_path, *rawGround);
    // filterPC(rawGround, inp.groundCloud, 1);
    pcl::io::loadPCDFile<PointT>(ground_path, *inp.groundCloud);
  }

  void setupParams()
  {

    params.scansPerSweep = 1;
    params.maxLidarDist = 15.0;
    params.maxGroundLidarDist = 30.0;
    params.minGroundLidarDist = 0.0;

    params.groundRadiiBins = 1;
    params.groundThetaBins = 18;
    params.groundMatchThresh = 2.0;
    params.groundRetainThresh = 0.05;

    params.maxTreeRadius = 0.3;
    params.maxAxisTheta = 10;
    params.maxFocusOutlierDistance = 0.5;
    params.roughTreeMatchThresh = 3.0;