diff --git a/config/trajectory/trajectory_files/trajectory_point_lio.txt b/config/trajectory/trajectory_files/trajectory_point_lio.txt new file mode 100644 index 0000000..e82dba2 --- /dev/null +++ b/config/trajectory/trajectory_files/trajectory_point_lio.txt @@ -0,0 +1,375 @@ +0,0,3,0 +0,0,3,0 +0,0,3,0 +0,0,3,0 +0,0,3,0 +0,0,3,0 +0,0,3,0 +0,0,3,0 +0,0,3,0 +0,0,3,0 +0,0,3,0 +0,0,3,0 +0,0,3,0 +0,0,3,0 +0,0,3,0 +0,0,3,0 +0,0,3,0 +0,0,3,0 +0,0,3,0 +0,0,3,0 +0,0,3,0 +0,0,3,0 +0,0,3,0 +0,0,3,0 +0,0,3,0 +0.0,-0.2,3.0,0.0 +0.0,-0.4,3.0,0.0 +0.0,-0.6000000000000001,3.0,0.0 +0.0,-0.8,3.0,0.0 +0.0,-1.0,3.0,0.0 +0.0,-1.2000000000000002,3.0,0.0 +0.0,-1.4000000000000001,3.0,0.0 +0.0,-1.6,3.0,0.0 +0.0,-1.8,3.0,0.0 +0.0,-2.0,3.0,0.0 +0.0,-2.2,3.0,0.0 +0.0,-2.4000000000000004,3.0,0.0 +0.0,-2.6,3.0,0.0 +0.0,-2.8000000000000003,3.0,0.0 +0.0,-3.0,3.0,0.0 +0.0,-3.2,3.0,0.0 +0.0,-3.4000000000000004,3.0,0.0 +0.0,-3.6,3.0,0.0 +0.0,-3.8000000000000003,3.0,0.0 +0.0,-4.0,3.0,0.0 +0.0,-4.2,3.0,0.0 +0.0,-4.4,3.0,0.0 +0.0,-4.6000000000000005,3.0,0.0 +0.0,-4.800000000000001,3.0,0.0 +0.0,-5.0,3.0,0.0 +0.0,-5.2,3.0,0.0 +0.0,-5.4,3.0,0.0 +0.0,-5.6000000000000005,3.0,0.0 +0.0,-5.800000000000001,3.0,0.0 +0.0,-6.0,3.0,0.0 +0.0,-6.2,3.0,0.0 +0.0,-6.4,3.0,0.0 +0.0,-6.6000000000000005,3.0,0.0 +0.0,-6.800000000000001,3.0,0.0 +0.0,-7.0,3.0,0.0 +0.0,-7.2,3.0,0.0 +0.0,-7.4,3.0,0.0 +0.0,-7.6000000000000005,3.0,0.0 +0.0,-7.800000000000001,3.0,0.0 +0.0,-8.0,3.0,0.0 +0.0,-8.200000000000001,3.0,0.0 +0.0,-8.4,3.0,0.0 +0.0,-8.6,3.0,0.0 +0.0,-8.8,3.0,0.0 +0.0,-9.0,3.0,0.0 +0.0,-9.200000000000001,3.0,0.0 +0.0,-9.4,3.0,0.0 +0.0,-9.600000000000001,3.0,0.0 +0.0,-9.8,3.0,0.0 +0.0,-10.0,3.0,0.0 +0.0,-10.200000000000001,3.0,0.0 +0.0,-10.4,3.0,0.0 +0.0,-10.600000000000001,3.0,0.0 +0.0,-10.8,3.0,0.0 +0.0,-11.0,3.0,0.0 +0.0,-11.200000000000001,3.0,0.0 +0.0,-11.4,3.0,0.0 +0.0,-11.600000000000001,3.0,0.0 +0.0,-11.8,3.0,0.0 +0.0,-12.0,3.0,0.0 +0.0,-12.200000000000001,3.0,0.0 +0.0,-12.4,3.0,0.0 +0.0,-12.600000000000001,3.0,0.0 +0.0,-12.8,3.0,0.0 +0.0,-13.0,3.0,0.0 +0.0,-13.200000000000001,3.0,0.0 +0.0,-13.4,3.0,0.0 +0.0,-13.600000000000001,3.0,0.0 +0.0,-13.8,3.0,0.0 +0.0,-14.0,3.0,0.0 +0.0,-14.200000000000001,3.0,0.0 +0.0,-14.4,3.0,0.0 +0.0,-14.600000000000001,3.0,0.0 +0.0,-14.8,3.0,0.0 +0.0,-15.0,3.0,0.0 +0.0,-15.200000000000001,3.0,0.0 +0.0,-15.4,3.0,0.0 +0.0,-15.600000000000001,3.0,0.0 +0.0,-15.8,3.0,0.0 +0.0,-16.0,3.0,0.0 +0.0,-16.2,3.0,0.0 +0.0,-16.400000000000002,3.0,0.0 +0.0,-16.6,3.0,0.0 +0.0,-16.8,3.0,0.0 +0.0,-17.0,3.0,0.0 +0.0,-17.2,3.0,0.0 +0.0,-17.400000000000002,3.0,0.0 +0.0,-17.6,3.0,0.0 +0.0,-17.8,3.0,0.0 +0.0,-18.0,3.0,0.0 +0.0,-18.2,3.0,0.0 +0.0,-18.400000000000002,3.0,0.0 +0.0,-18.6,3.0,0.0 +0.0,-18.8,3.0,0.0 +0.0,-19.0,3.0,0.0 +0.0,-19.200000000000003,3.0,0.0 +0.0,-19.400000000000002,3.0,0.0 +0.0,-19.6,3.0,0.0 +0.0,-19.8,3.0,0.0 +0.0,-20.0,3.0,0.0 +0,-20,3,0 +0,-20,3,0 +0,-20,3,0 +0,-20,3,0 +0,-20,3,0 +0,-20,3,0 +0,-20,3,0 +0,-20,3,0 +0,-20,3,0 +0,-20,3,0 +0,-20,3,0 +0,-20,3,0 +0,-20,3,0 +0,-20,3,0 +0,-20,3,0 +0,-20,3,0 +0,-20,3,0 +0,-20,3,0 +0,-20,3,0 +0,-20,3,0 +0,-20,3,0 +0,-20,3,0 +0,-20,3,0 +0,-20,3,0 +0,-20,3,0 +-0.2,-20.0,3.0,0.0 +-0.4,-20.0,3.0,0.0 +-0.6000000000000001,-20.0,3.0,0.0 +-0.8,-20.0,3.0,0.0 +-1.0,-20.0,3.0,0.0 +-1.2000000000000002,-20.0,3.0,0.0 +-1.4000000000000001,-20.0,3.0,0.0 +-1.6,-20.0,3.0,0.0 +-1.8,-20.0,3.0,0.0 +-2.0,-20.0,3.0,0.0 +-2.2,-20.0,3.0,0.0 +-2.4000000000000004,-20.0,3.0,0.0 +-2.6,-20.0,3.0,0.0 +-2.8000000000000003,-20.0,3.0,0.0 +-3.0,-20.0,3.0,0.0 +-3.2,-20.0,3.0,0.0 +-3.4000000000000004,-20.0,3.0,0.0 +-3.6,-20.0,3.0,0.0 +-3.8000000000000003,-20.0,3.0,0.0 +-4.0,-20.0,3.0,0.0 +-4.2,-20.0,3.0,0.0 +-4.4,-20.0,3.0,0.0 +-4.6000000000000005,-20.0,3.0,0.0 +-4.800000000000001,-20.0,3.0,0.0 +-5.0,-20.0,3.0,0.0 +-5,-20,3,0 +-5,-20,3,0 +-5,-20,3,0 +-5,-20,3,0 +-5,-20,3,0 +-5,-20,3,0 +-5,-20,3,0 +-5,-20,3,0 +-5,-20,3,0 +-5,-20,3,0 +-5,-20,3,0 +-5,-20,3,0 +-5,-20,3,0 +-5,-20,3,0 +-5,-20,3,0 +-5,-20,3,0 +-5,-20,3,0 +-5,-20,3,0 +-5,-20,3,0 +-5,-20,3,0 +-5,-20,3,0 +-5,-20,3,0 +-5,-20,3,0 +-5,-20,3,0 +-5,-20,3,0 +-5.0,-19.8,3.0,0.0 +-5.0,-19.6,3.0,0.0 +-5.0,-19.4,3.0,0.0 +-5.0,-19.2,3.0,0.0 +-5.0,-19.0,3.0,0.0 +-5.0,-18.8,3.0,0.0 +-5.0,-18.6,3.0,0.0 +-5.0,-18.4,3.0,0.0 +-5.0,-18.2,3.0,0.0 +-5.0,-18.0,3.0,0.0 +-5.0,-17.8,3.0,0.0 +-5.0,-17.6,3.0,0.0 +-5.0,-17.4,3.0,0.0 +-5.0,-17.2,3.0,0.0 +-5.0,-17.0,3.0,0.0 +-5.0,-16.8,3.0,0.0 +-5.0,-16.6,3.0,0.0 +-5.0,-16.4,3.0,0.0 +-5.0,-16.2,3.0,0.0 +-5.0,-16.0,3.0,0.0 +-5.0,-15.8,3.0,0.0 +-5.0,-15.6,3.0,0.0 +-5.0,-15.399999999999999,3.0,0.0 +-5.0,-15.2,3.0,0.0 +-5.0,-15.0,3.0,0.0 +-5.0,-14.8,3.0,0.0 +-5.0,-14.6,3.0,0.0 +-5.0,-14.399999999999999,3.0,0.0 +-5.0,-14.2,3.0,0.0 +-5.0,-14.0,3.0,0.0 +-5.0,-13.8,3.0,0.0 +-5.0,-13.6,3.0,0.0 +-5.0,-13.399999999999999,3.0,0.0 +-5.0,-13.2,3.0,0.0 +-5.0,-13.0,3.0,0.0 +-5.0,-12.8,3.0,0.0 +-5.0,-12.6,3.0,0.0 +-5.0,-12.399999999999999,3.0,0.0 +-5.0,-12.2,3.0,0.0 +-5.0,-12.0,3.0,0.0 +-5.0,-11.799999999999999,3.0,0.0 +-5.0,-11.6,3.0,0.0 +-5.0,-11.4,3.0,0.0 +-5.0,-11.2,3.0,0.0 +-5.0,-11.0,3.0,0.0 +-5.0,-10.799999999999999,3.0,0.0 +-5.0,-10.6,3.0,0.0 +-5.0,-10.399999999999999,3.0,0.0 +-5.0,-10.2,3.0,0.0 +-5.0,-10.0,3.0,0.0 +-5.0,-9.799999999999999,3.0,0.0 +-5.0,-9.6,3.0,0.0 +-5.0,-9.399999999999999,3.0,0.0 +-5.0,-9.2,3.0,0.0 +-5.0,-9.0,3.0,0.0 +-5.0,-8.799999999999999,3.0,0.0 +-5.0,-8.6,3.0,0.0 +-5.0,-8.399999999999999,3.0,0.0 +-5.0,-8.2,3.0,0.0 +-5.0,-8.0,3.0,0.0 +-5.0,-7.799999999999999,3.0,0.0 +-5.0,-7.6,3.0,0.0 +-5.0,-7.399999999999999,3.0,0.0 +-5.0,-7.199999999999999,3.0,0.0 +-5.0,-7.0,3.0,0.0 +-5.0,-6.799999999999999,3.0,0.0 +-5.0,-6.6,3.0,0.0 +-5.0,-6.399999999999999,3.0,0.0 +-5.0,-6.199999999999999,3.0,0.0 +-5.0,-6.0,3.0,0.0 +-5.0,-5.799999999999999,3.0,0.0 +-5.0,-5.6,3.0,0.0 +-5.0,-5.399999999999999,3.0,0.0 +-5.0,-5.199999999999999,3.0,0.0 +-5.0,-5.0,3.0,0.0 +-5.0,-4.799999999999999,3.0,0.0 +-5.0,-4.6,3.0,0.0 +-5.0,-4.399999999999999,3.0,0.0 +-5.0,-4.199999999999999,3.0,0.0 +-5.0,-4.0,3.0,0.0 +-5.0,-3.8000000000000007,3.0,0.0 +-5.0,-3.599999999999998,3.0,0.0 +-5.0,-3.3999999999999986,3.0,0.0 +-5.0,-3.1999999999999993,3.0,0.0 +-5.0,-3.0,3.0,0.0 +-5.0,-2.8000000000000007,3.0,0.0 +-5.0,-2.599999999999998,3.0,0.0 +-5.0,-2.3999999999999986,3.0,0.0 +-5.0,-2.1999999999999993,3.0,0.0 +-5.0,-2.0,3.0,0.0 +-5.0,-1.8000000000000007,3.0,0.0 +-5.0,-1.5999999999999979,3.0,0.0 +-5.0,-1.3999999999999986,3.0,0.0 +-5.0,-1.1999999999999993,3.0,0.0 +-5.0,-1.0,3.0,0.0 +-5.0,-0.7999999999999972,3.0,0.0 +-5.0,-0.5999999999999979,3.0,0.0 +-5.0,-0.3999999999999986,3.0,0.0 +-5.0,-0.1999999999999993,3.0,0.0 +-5.0,0.0,3.0,0.0 +-5,0,3,0 +-5,0,3,0 +-5,0,3,0 +-5,0,3,0 +-5,0,3,0 +-5,0,3,0 +-5,0,3,0 +-5,0,3,0 +-5,0,3,0 +-5,0,3,0 +-5,0,3,0 +-5,0,3,0 +-5,0,3,0 +-5,0,3,0 +-5,0,3,0 +-5,0,3,0 +-5,0,3,0 +-5,0,3,0 +-5,0,3,0 +-5,0,3,0 +-5,0,3,0 +-5,0,3,0 +-5,0,3,0 +-5,0,3,0 +-5,0,3,0 +-4.8,0.0,3.0,0.0 +-4.6,0.0,3.0,0.0 +-4.4,0.0,3.0,0.0 +-4.2,0.0,3.0,0.0 +-4.0,0.0,3.0,0.0 +-3.8,0.0,3.0,0.0 +-3.5999999999999996,0.0,3.0,0.0 +-3.4,0.0,3.0,0.0 +-3.2,0.0,3.0,0.0 +-3.0,0.0,3.0,0.0 +-2.8,0.0,3.0,0.0 +-2.5999999999999996,0.0,3.0,0.0 +-2.4,0.0,3.0,0.0 +-2.1999999999999997,0.0,3.0,0.0 +-2.0,0.0,3.0,0.0 +-1.7999999999999998,0.0,3.0,0.0 +-1.5999999999999996,0.0,3.0,0.0 +-1.4,0.0,3.0,0.0 +-1.1999999999999997,0.0,3.0,0.0 +-1.0,0.0,3.0,0.0 +-0.7999999999999998,0.0,3.0,0.0 +-0.5999999999999996,0.0,3.0,0.0 +-0.39999999999999947,0.0,3.0,0.0 +-0.1999999999999993,0.0,3.0,0.0 +0.0,0.0,3.0,0.0 +0,0,3,0 +0,0,3,0 +0,0,3,0 +0,0,3,0 +0,0,3,0 +0,0,3,0 +0,0,3,0 +0,0,3,0 +0,0,3,0 +0,0,3,0 +0,0,3,0 +0,0,3,0 +0,0,3,0 +0,0,3,0 +0,0,3,0 +0,0,3,0 +0,0,3,0 +0,0,3,0 +0,0,3,0 +0,0,3,0 +0,0,3,0 +0,0,3,0 +0,0,3,0 +0,0,3,0 +0,0,3,0 diff --git a/src/detector.cpp b/src/detector.cpp index 8b7a408..dd15e49 100644 --- a/src/detector.cpp +++ b/src/detector.cpp @@ -672,12 +672,18 @@ class UVDARDetector : public nodelet::Nodelet{ /* VisualizationThread() //{ */ void VisualizationThread([[maybe_unused]] const ros::TimerEvent& te) { if (initialized_){ - //generateVisualization(image_visualization_); + + if(_adaptive_threshold_){ int image_index = 0; cv::Mat visualization_image; cv::Mat white_background = cv::Mat::ones(images_current_[image_index].size(),images_current_[image_index].type()) * 255; uvda_[image_index]->generateVisualizationAdaptive(white_background,visualization_image,adaptive_detected_points_[image_index]); //publishVisualizationImage(visualization_image); + } + else{ + generateVisualization(image_visualization_); + } + if ((visualization_image.cols != 0) && (visualization_image.rows != 0)){ if (_publish_visualization_){ ROS_INFO_STREAM("[UVDARDetector]: Publishing visualization.");