From 7dd097d003fa9add14328ef83c600a586260b13b Mon Sep 17 00:00:00 2001 From: Yohei Mishina <66298900+YoheiMishina@users.noreply.github.com> Date: Mon, 6 Dec 2021 13:38:24 +0900 Subject: [PATCH] feat: add lidar apollo instance segmentation package (#115) * release v0.4.0 * check if gdown command exists (#707) * Fix/cublas dependency (#849) * fix cublas depencency * fix cublas depencency * remove ROS1 packages temporarily Signed-off-by: mitsudome-r * Revert "remove ROS1 packages temporarily" This reverts commit b641153235b8c3ec5b5c5a052179c53434e66d7c. Signed-off-by: mitsudome-r * add COLCON_IGNORE to ros1 packages Signed-off-by: mitsudome-r * Rename launch files to launch.xml (#28) * Port lidar apollo instance segmentation (#98) * port lidar_apollo_instance_segmentation Signed-off-by: mitsudome-r * move shared_from_this out of constructor Signed-off-by: mitsudome-r * fix launch file Signed-off-by: mitsudome-r * fix warnings Signed-off-by: mitsudome-r * change rclcpp::Node::SharedPtr to raw pointer Signed-off-by: mitsudome-r * uncomment publish of debug topic Signed-off-by: mitsudome-r * Rename h files to hpp (#142) * Change includes * Rename files * Adjustments to make things compile * Other packages * Adjust copyright notice on 532 out of 699 source files (#143) * Use quotes for includes where appropriate (#144) * Use quotes for includes where appropriate * Fix lint tests * Make tests pass hopefully * Run uncrustify on the entire Pilot.Auto codebase (#151) * Run uncrustify on the entire Pilot.Auto codebase * Exclude open PRs * [tensorrt_yolo3][lidar_apollo_instance_segmentation] fix objection recognition headers (#197) Signed-off-by: mitsudome-r * adding linters to lidar_apollo_instance_segmentation (#173) * Ros2 v0.8.0 lidar apollo instance segmentation (#256) * restore file name for v0.8.0 update Signed-off-by: wep21 * fix typos in perception (#862) * Revert "restore file name for v0.8.0 update" This reverts commit f8e44568acf0613a0385c9323f2d25bcf45025d2. Co-authored-by: Kazuki Miyahara * add use_sim-time option (#454) * Format launch files (#1219) Signed-off-by: Kenji Miyake * fix reliability (#1222) * fix reliability * use sensor data qos * Unify Apache-2.0 license name (#1242) * Remove use_sim_time for set_parameter (#1260) Signed-off-by: wep21 * Perception components (#1368) * [bev_optical_flow]: component node Signed-off-by: wep21 * [object_merger]: component node Signed-off-by: wep21 * [object_range_splitter]: component node Signed-off-by: wep21 * [shape_estimation]: component node Signed-off-by: wep21 * [map_based_prediction]: component node Signed-off-by: wep21 * [naive_path_prediction]: component node Signed-off-by: wep21 * [roi_image_saver]: component node Signed-off-by: wep21 * [lidar_apollo_instance_segmentation]: component node Signed-off-by: wep21 * [object_flow_fusion]: component node Signed-off-by: wep21 * [traffic_light_map_based_detector]: component node Signed-off-by: wep21 * [dynamic_object_visualization]: component node Signed-off-by: wep21 * Fix typo Signed-off-by: wep21 * [lidar_apollo_instance_segmentation]: Change subscriber queue size(5 to 1) (#1570) Signed-off-by: wep21 * Add pre-commit (#1560) * add pre-commit * add pre-commit-config * add additional settings for private repository * use default pre-commit-config * update pre-commit setting * Ignore whitespace for line breaks in markdown * Update .github/workflows/pre-commit.yml Co-authored-by: Kazuki Miyahara * exclude svg * remove pretty-format-json * add double-quote-string-fixer * consider COLCON_IGNORE file when seaching modified package * format file * pre-commit fixes * Update pre-commit.yml * Update .pre-commit-config.yaml Co-authored-by: Kazuki Miyahara Co-authored-by: pre-commit Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> * Fix build error with TensorRT v8 (#1612) * Fix build error with TensorRT v8 Signed-off-by: wep21 * Fix typo Signed-off-by: wep21 * Add markdownlint and prettier (#1661) * Add markdownlint and prettier Signed-off-by: Kenji Miyake * Ignore .param.yaml Signed-off-by: Kenji Miyake * Apply format Signed-off-by: Kenji Miyake * Fix MD029 (#1813) Signed-off-by: Kenji Miyake * Fix -Wunused-parameter (#1836) * Fix -Wunused-parameter Signed-off-by: Kenji Miyake * Fix mistake Signed-off-by: Kenji Miyake * fix spell * Fix lint issues Signed-off-by: Kenji Miyake * Ignore flake8 warnings Signed-off-by: Kenji Miyake Co-authored-by: Hiroki OTA * suppress warnings in perception directory (#1866) * add maybe unused * add Werror * Ignore -Wdeprecated-declarations in lidar_apollo_instance_segmentation (#1875) Signed-off-by: Kenji Miyake * Refactor shape estimation for detection by tracking (#1861) * refactor * modification due to refactoring * cosmetic change and add virtual * change to lib * fix typo * bug fix * cosmetic change * bug fix * add constexpr * cosmetic change * cosmetic change * cosmetic change * cosmetic change * add sort-package-xml hook in pre-commit (#1881) * add sort xml hook in pre-commit * change retval to exit_status * rename * add prettier plugin-xml * use early return * add license note * add tier4 license * restore prettier * change license order * move local hooks to public repo * move prettier-xml to pre-commit-hooks-ros * update version for bug-fix * apply pre-commit * Detection by tracker (#1910) * initial commit * backup * apply format * cosmetic change * implement divided under segmenterd clusters * cosmetic change * bug fix * bug fix * bug fix * modify launch * add debug and bug fix * bug fix * bug fix * add no found tracked object * modify parameters and cmake * bug fix * remove debug info * add readme * modify clustering launch * run pre-commit * cosmetic change * cosmetic change * cosmetic change * apply markdownlint * modify launch * modify for cpplint * modify qos * change int to size_T * bug fix * change perception qos * Update perception/object_recognition/detection/detection_by_tracker/package.xml Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> * cosmetic change * cosmetic change * fix launch * Update perception/object_recognition/detection/detection_by_tracker/src/utils.cpp Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> * modify header include order * change include order * Update perception/object_recognition/detection/detection_by_tracker/src/detection_by_tracker_core.cpp Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> * change to std::optional * cosmetic change * Update perception/object_recognition/detection/detection_by_tracker/src/detection_by_tracker_core.cpp Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> * Update perception/object_recognition/detection/detection_by_tracker/src/detection_by_tracker_core.cpp Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> * bug fix * modify readme Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> * Change formatter to clang-format and black (#2332) * Revert "Temporarily comment out pre-commit hooks" This reverts commit 748e9cdb145ce12f8b520bcbd97f5ff899fc28a3. * Replace ament_lint_common with autoware_lint_common Signed-off-by: Kenji Miyake * Remove ament_cmake_uncrustify and ament_clang_format Signed-off-by: Kenji Miyake * Apply Black Signed-off-by: Kenji Miyake * Apply clang-format Signed-off-by: Kenji Miyake * Fix build errors Signed-off-by: Kenji Miyake * Fix for cpplint * Fix include double quotes to angle brackets Signed-off-by: Kenji Miyake * Apply clang-format Signed-off-by: Kenji Miyake * Fix build errors Signed-off-by: Kenji Miyake * Add COLCON_IGNORE (#500) Signed-off-by: Kenji Miyake * Port lidar apollo instance segmentation to .auto (#548) * Port lidar apollo instance segmentation to .auto Signed-off-by: wep21 * Run pre-commit Signed-off-by: wep21 * Update document for lidar apollo instance segmentation (#597) Signed-off-by: wep21 * Sync .auto branch with the latest branch in internal repository (#691) * add trajectory point offset in rviz plugin (#2270) * sync rc rc/v0.23.0 (#2258) * fix interpolation for insert point (#2228) * fix interpolation for insert point * to prev interpolation pkg * Revert "to prev interpolation pkg" This reverts commit 9eb145b5d36e297186015fb17c267ccd5b3c21ef. Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Co-authored-by: taikitanaka * fix topic name (#2266) Signed-off-by: Takamasa Horibe * Add namespace to diag for dual_return_filter (#2269) * Add a function to make 'geometry_msgs::msg::TransformStamped' (#2250) * Add a function to make 'geometry_msgs::msg::TransformStamped' * Add 'child_frame_id' as an argument of 'pose2transform' * Simplify marker scale initialization (#2286) * Fix/crosswalk polygon (#2279) * extend crosswalk polygon * improve readability * fix polygon shape * Add warning when decel distance calculation fails (#2289) Signed-off-by: Makoto Kurihara * [motion_velocity_smoother] ignore debug print (#2292) * cosmetic change Signed-off-by: Takamasa Horibe * cahnge severity from WARN to DEBUG for debug info Signed-off-by: Takamasa Horibe * use util for stop_watch Signed-off-by: Takamasa Horibe * fix map based prediction (#2200) * fix map based prediction * fix format * change map based prediction * fix spells * fix spells in comments * fix for cpplint * fix some problems * fix format and code for clang-tidy * fix space for cpplint * Update Readme.md * Update perception/object_recognition/prediction/map_based_prediction/Readme.md * Update perception/object_recognition/prediction/map_based_prediction/Readme.md * Update perception/object_recognition/prediction/map_based_prediction/Readme.md * Update perception/object_recognition/prediction/map_based_prediction/Readme.md * Update perception/object_recognition/prediction/map_based_prediction/Readme.md * Update perception/object_recognition/prediction/map_based_prediction/Readme.md * fix vector access method * fix readme format * add parameter * Update perception/object_recognition/prediction/map_based_prediction/Readme.md Co-authored-by: Kazuki Miyahara * Update perception/object_recognition/prediction/map_based_prediction/Readme.md Co-authored-by: Kazuki Miyahara * Update perception/object_recognition/prediction/map_based_prediction/Readme.md Co-authored-by: Kazuki Miyahara * Update Readme.md * Update perception/object_recognition/prediction/map_based_prediction/Readme.md Co-authored-by: Kazuki Miyahara Co-authored-by: tkimura4 Co-authored-by: Kazuki Miyahara * remove failure condition for 0 velocity trajectory (#2295) Signed-off-by: Takamasa Horibe * [mpc_follower] remove stop distance condition from stopState decision (#1916) * [mpc_follower] remove stop distance condition from stopState decision Signed-off-by: Takamasa Horibe * add invalid index handling Signed-off-by: Takamasa Horibe * Move the debug marker initialization part to another file (#2288) * Move the debug marker initialization part to 'debug.cpp' * Make 'isLocalOptimalSolutionOscillation' independent from 'NDTScanMatcher' (#2300) * Remove an unused function 'getTransform' (#2301) * Simplify iteration of initial poses (#2310) * Make a transform object const (#2311) * Represent poses in 'std::vector' instead of 'geometry_msgs::msg::PoseArray' (#2312) * Feature/no stopping area (#2163) * add no stopping area module to behavior velocity planner * apply utils * add polygon interpolation module order stopline around area is considered * devide jpass udge with stop line polygon * update docs * rename file name * update to latest * minor change for marker * update license Co-authored-by: Yukihiro Saito * update license Co-authored-by: Yukihiro Saito * update license Co-authored-by: Yukihiro Saito * update license Co-authored-by: Yukihiro Saito * minor fix * add parameter tuning at experiment * update readme * format doc * apply comments * add exception gurd * cosmetic change * fix ament * fix typo and remove for statement * & to " " * better ns * return pass judge param * add missing stoppable condition * add clear pass judge and stoppable flag * add comment * precommit fix * cpplint Co-authored-by: Yukihiro Saito * sync rc rc/v0.23.0 (#2281) * Fix side shift planner (#2171) (#2172) * add print debug Signed-off-by: TakaHoribe * remove forward shift points when adding new point Signed-off-by: TakaHoribe * remove debug print Signed-off-by: TakaHoribe * format Signed-off-by: TakaHoribe * Fix remove threshold Co-authored-by: Fumiya Watanabe Co-authored-by: Takamasa Horibe * Fix/pull out and pull over (#2175) * delete unnecessary check * fix condition of starting pull out * Add emergency status API (#2174) (#2182) * Fix/mpc reset prev result (#2185) (#2195) * reset prev result * clean code * reset only raw_steer_cmd * Update control/mpc_follower/src/mpc_follower_core.cpp Co-authored-by: Takamasa Horibe Co-authored-by: Takamasa Horibe Co-authored-by: Takamasa Horibe * [hotfix] 1 path point exception after resampling (#2204) * fix 1 path point exception after resampling Signed-off-by: TakaHoribe * Apply suggestions from code review * Apply suggestions from code review Co-authored-by: Takamasa Horibe Co-authored-by: tkimura4 * [hotfix] Fix lane ids (#2211) * Fix lane ids * Prevent acceleration on avoidance (#2214) * prevent acceleration on avoidance Signed-off-by: TakaHoribe * fix param name Signed-off-by: TakaHoribe * parametrize avoidance acc Signed-off-by: Takamasa Horibe * change param name Signed-off-by: Takamasa Horibe * fix typo Signed-off-by: Takamasa Horibe * Fix qos in roi cluster fusion (#2218) * fix confidence (#2220) * too high confidence (#2229) * Fix/obstacle stop 0.23.0 (#2232) * fix unexpected slow down in sharp curves (#2181) * Fix/insert implementation (#2186) Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> * [hotfix] Remove exception in avoidance module (#2233) * Remove exception * Fix clock * Remove blank line * Update traffic light state if ref stop point is ahead of previous one (#2197) Signed-off-by: wep21 * fix interpolation for insert point (#2228) * fix interpolation for insert point * to prev interpolation pkg * fix index (#2265) * turn signal calculation (#2280) * add turn signal funtion in path shifter * add ros parameters Co-authored-by: Fumiya Watanabe Co-authored-by: Takamasa Horibe Co-authored-by: tkimura4 Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Co-authored-by: Yukihiro Saito Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Co-authored-by: Sugatyon <32741405+Sugatyon@users.noreply.github.com> * [behavior_path_planner] fix sudden path change around ego (#2305) (#2318) * fix return-from-ego shift point generation logic Signed-off-by: Takamasa Horibe * change param for trimSimilarGradShiftPoint Signed-off-by: Takamasa Horibe * add comment for issue Signed-off-by: Takamasa Horibe * update comment Signed-off-by: Takamasa Horibe * replace code with function (logic has not changed) Signed-off-by: Takamasa Horibe * move func to cpp Signed-off-by: Takamasa Horibe * add comment for issue Signed-off-by: Takamasa Horibe * fix typo Signed-off-by: Takamasa Horibe * Update planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp Co-authored-by: Kazuki Miyahara * Update planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp Co-authored-by: Kazuki Miyahara Co-authored-by: Kazuki Miyahara Co-authored-by: Takamasa Horibe Co-authored-by: Kazuki Miyahara * Add functions to make stamped scalar messages (#2317) * Fix/object yaw in intersection module (#2294) * fix object orientation * fix function name * add guard (#2321) * reduce cost (double to float) (#2298) * Add detail collision check (#2274) * Add detail collision check Signed-off-by: wep21 * Remove unused function Signed-off-by: wep21 * Fix arc length Signed-off-by: wep21 * Seperate time margin Signed-off-by: wep21 * Fix parameter name Signed-off-by: wep21 * Update Readme Signed-off-by: wep21 * Address review: Add comment for TimeDistanceArray Signed-off-by: wep21 * Run pre-commit Signed-off-by: wep21 * Fix cpplint Signed-off-by: wep21 * Add return for empty polygon Signed-off-by: wep21 * update CenterPoint (#2222) * update to model trained by mmdet3d * add vizualizer (debug) * for multi-frame inputs * chagne config * use autoware_utils::pi * project specific model and param * rename vfe -> encoder * rename general to common * update download link * update * fix * rename model_name * change training toolbox link * chage lint package * fix test error * commit suggestion * Feature/lane change detection (#2331) * add old information deleter * fix access bug * change to deque * update obstacle buffer * fix some bugs * add lane change detector * make a update lanelet function * fix code style * parameterize essential values * Update perception/object_recognition/prediction/map_based_prediction/src/map_based_prediction_ros.cpp Co-authored-by: Kazuki Miyahara * fix slash position * remove unnecessary lines * fix format * fix format * change to new enum * fix format * fix typo and add guard * change funciton name * add lane change description Co-authored-by: Kazuki Miyahara * Add Planning Evaluator (#2293) * Add prototype planning evaluator Produced data for dist between points, curvature, and relative angle * Cleanup the code to make adding metrics easier * Add remaining basic metrics (length, duration, vel, accel, jerk) * Add motion_evaluator to evaluate the actual ego motion + code cleanup * Add deviation metrics * Add naive stability metric * Handle invalid stat (TODO: fix the output file formatting) * Add parameter file and cleanup * Add basic obstacle metric (TTC not yet implemented) and fix output file format * Add basic time to collision * Add lateral-distance based stability metric * Add check (at init time) that metrics' maps are complete * Publish metrics as ParamaterDeclaration msg (for openscenario) * Use lookahead and start from ego_pose when calculating stability metrics * Code cleanup * Fix lint * Add tests * Fix bug with Frechet dist and the last traj point * Finish implementing tests * Fix lint * Code cleanup * Update README.md * Remove unused metric * Change msg type of published metrics to DiagnosticArray * fix format to fix pre-commit check Signed-off-by: Takamasa Horibe * fix yaml format to fix pre-commit check Signed-off-by: Takamasa Horibe * fix yaml format Signed-off-by: Takamasa Horibe * apply clang-format Signed-off-by: Takamasa Horibe * apply clang-format Signed-off-by: Takamasa Horibe * Update planning/planning_diagnostics/planning_evaluator/include/planning_evaluator/planning_evaluator_node.hpp * Update planning/planning_diagnostics/planning_evaluator/test/test_planning_evaluator_node.cpp * Update planning/planning_diagnostics/planning_evaluator/test/test_planning_evaluator_node.cpp * change lint format to autoware_lint_common Signed-off-by: Takamasa Horibe Co-authored-by: Takamasa Horibe * Add keep braking function at driving state (#2346) * Add keep braking function at driving state Signed-off-by: Makoto Kurihara * Remove debug messages Signed-off-by: Makoto Kurihara * Fix format Signed-off-by: Makoto Kurihara * Change diag_updater's pediod from default to 0.1sec (#2348) * add cross judgement and common signal function (#2319) * merge branch turn_signal_common * add turn signal function in signal decider * add cross judge in path_utilities and delete from turn_signal_decider * remove original signal calculation in lane change * omit substitution * replace turn signal decider in pull over function * modify cross judge logic * replace turn signal decider in avoidance * add readme of turn signal * update * delete print debug * update * delete lane change decider in path shifter * delete blank line * fix indent * fix typo * fix typo * decrease nest * run pre commit * Add 0 limit at forward jerk velocity filter (#2340) Signed-off-by: Makoto Kurihara * add time offset param to point cloud concatenation (#2303) * add offset param * clang-format Co-authored-by: Akihito OHSATO * Feature/add doc for keep braking function at driving state (#2366) * Add the description of brake keeping Signed-off-by: Makoto Kurihara * Add the english document Signed-off-by: Makoto Kurihara * Improve description Signed-off-by: Makoto Kurihara * Add english description Signed-off-by: Makoto Kurihara * Fix include files (#2339) Signed-off-by: Kenji Miyake * fix behavior intersection module * fix behavior no stopping area module * fix planning_evaluator * fix motion_velocity_smoother * rename variable * Revert "[mpc_follower] remove stop distance condition from stopState decision (#1916)" This reverts commit ff4f0b5a844d1f835f1b93bd3b36a76747b0cd02. * Revert "Add keep braking function at driving state (#2346)" This reverts commit f0478187db4c28bf6092c198723dcc5ec11a9c70. * Revert "Feature/add doc for keep braking function at driving state (#2366)" This reverts commit 66de2f3924a479049fce2d5c5c6b579cacbd3e49. * Fix orientation availability in centerpoint Signed-off-by: wep21 * fix test_trajectory.cpp * add target link libraries * Use .auto msg in test code for planniing evaluator Signed-off-by: wep21 * fix include Signed-off-by: wep21 Co-authored-by: Takayuki Murooka Co-authored-by: autoware-iv-sync-ci[bot] <87871706+autoware-iv-sync-ci[bot]@users.noreply.github.com> Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Co-authored-by: taikitanaka Co-authored-by: Takamasa Horibe Co-authored-by: Takeshi Miura <57553950+1222-takeshi@users.noreply.github.com> Co-authored-by: Takeshi Ishita Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Co-authored-by: Makoto Kurihara Co-authored-by: purewater0901 <43805014+purewater0901@users.noreply.github.com> Co-authored-by: tkimura4 Co-authored-by: Kazuki Miyahara Co-authored-by: Yukihiro Saito Co-authored-by: Fumiya Watanabe Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Co-authored-by: Sugatyon <32741405+Sugatyon@users.noreply.github.com> Co-authored-by: s-murakami-esol <81723883+s-murakami-esol@users.noreply.github.com> Co-authored-by: Yusuke Muramatsu Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Co-authored-by: Shunsuke Miura <37187849+miursh@users.noreply.github.com> Co-authored-by: Shinnosuke Hirakawa <8327162+0x126@users.noreply.github.com> Co-authored-by: Akihito OHSATO Co-authored-by: Kenji Miyake Co-authored-by: mitsudome-r Co-authored-by: Daichi Murakami Co-authored-by: Yukihiro Saito Co-authored-by: Nikolai Morin Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Co-authored-by: nik-tier4 <71747268+nik-tier4@users.noreply.github.com> Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Co-authored-by: Kazuki Miyahara Co-authored-by: tkimura4 Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Co-authored-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> Co-authored-by: pre-commit Co-authored-by: Hiroki OTA Co-authored-by: Takayuki Murooka Co-authored-by: autoware-iv-sync-ci[bot] <87871706+autoware-iv-sync-ci[bot]@users.noreply.github.com> Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Co-authored-by: taikitanaka Co-authored-by: Takamasa Horibe Co-authored-by: Takeshi Miura <57553950+1222-takeshi@users.noreply.github.com> Co-authored-by: Takeshi Ishita Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Co-authored-by: Makoto Kurihara Co-authored-by: purewater0901 <43805014+purewater0901@users.noreply.github.com> Co-authored-by: Fumiya Watanabe Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Co-authored-by: Sugatyon <32741405+Sugatyon@users.noreply.github.com> Co-authored-by: s-murakami-esol <81723883+s-murakami-esol@users.noreply.github.com> Co-authored-by: Yusuke Muramatsu Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Co-authored-by: Shunsuke Miura <37187849+miursh@users.noreply.github.com> Co-authored-by: Shinnosuke Hirakawa <8327162+0x126@users.noreply.github.com> Co-authored-by: Akihito OHSATO Co-authored-by: Kenji Miyake --- .../.gitignore | 2 + .../CMakeLists.txt | 175 +++++ .../README.md | 143 ++++ .../config/hdl-64.param.yaml | 8 + .../config/vlp-16.param.yaml | 8 + .../config/vls-128.param.yaml | 8 + .../data/hdl-64.prototxt | 685 +++++++++++++++++ .../data/vlp-16.prototxt | 685 +++++++++++++++++ .../data/vls-128.prototxt | 686 ++++++++++++++++++ .../cluster2d.hpp | 162 +++++ .../debugger.hpp | 31 + .../detector.hpp | 55 ++ .../disjoint_set.hpp | 75 ++ .../feature_generator.hpp | 42 ++ .../feature_map.hpp | 65 ++ .../log_table.hpp | 16 + .../node.hpp | 46 ++ .../util.hpp | 59 ++ ...ar_apollo_instance_segmentation.launch.xml | 27 + .../lib/LICENSE | 21 + .../lib/README.md | 28 + .../lib/include/TrtNet.hpp | 115 +++ .../lib/include/Utils.hpp | 134 ++++ .../lib/src/TrtNet.cpp | 152 ++++ .../package.xml | 31 + .../src/cluster2d.cpp | 372 ++++++++++ .../src/debugger.cpp | 100 +++ .../src/detector.cpp | 167 +++++ .../src/feature_generator.cpp | 95 +++ .../src/feature_map.cpp | 164 +++++ .../src/log_table.cpp | 45 ++ .../src/node.cpp | 44 ++ 32 files changed, 4446 insertions(+) create mode 100644 perception/lidar_apollo_instance_segmentation/.gitignore create mode 100644 perception/lidar_apollo_instance_segmentation/CMakeLists.txt create mode 100644 perception/lidar_apollo_instance_segmentation/README.md create mode 100644 perception/lidar_apollo_instance_segmentation/config/hdl-64.param.yaml create mode 100644 perception/lidar_apollo_instance_segmentation/config/vlp-16.param.yaml create mode 100644 perception/lidar_apollo_instance_segmentation/config/vls-128.param.yaml create mode 100644 perception/lidar_apollo_instance_segmentation/data/hdl-64.prototxt create mode 100644 perception/lidar_apollo_instance_segmentation/data/vlp-16.prototxt create mode 100644 perception/lidar_apollo_instance_segmentation/data/vls-128.prototxt create mode 100644 perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/cluster2d.hpp create mode 100644 perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/debugger.hpp create mode 100644 perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/detector.hpp create mode 100644 perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/disjoint_set.hpp create mode 100644 perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/feature_generator.hpp create mode 100644 perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/feature_map.hpp create mode 100644 perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/log_table.hpp create mode 100644 perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/node.hpp create mode 100644 perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/util.hpp create mode 100644 perception/lidar_apollo_instance_segmentation/launch/lidar_apollo_instance_segmentation.launch.xml create mode 100644 perception/lidar_apollo_instance_segmentation/lib/LICENSE create mode 100644 perception/lidar_apollo_instance_segmentation/lib/README.md create mode 100644 perception/lidar_apollo_instance_segmentation/lib/include/TrtNet.hpp create mode 100644 perception/lidar_apollo_instance_segmentation/lib/include/Utils.hpp create mode 100644 perception/lidar_apollo_instance_segmentation/lib/src/TrtNet.cpp create mode 100755 perception/lidar_apollo_instance_segmentation/package.xml create mode 100644 perception/lidar_apollo_instance_segmentation/src/cluster2d.cpp create mode 100644 perception/lidar_apollo_instance_segmentation/src/debugger.cpp create mode 100644 perception/lidar_apollo_instance_segmentation/src/detector.cpp create mode 100644 perception/lidar_apollo_instance_segmentation/src/feature_generator.cpp create mode 100644 perception/lidar_apollo_instance_segmentation/src/feature_map.cpp create mode 100644 perception/lidar_apollo_instance_segmentation/src/log_table.cpp create mode 100644 perception/lidar_apollo_instance_segmentation/src/node.cpp diff --git a/perception/lidar_apollo_instance_segmentation/.gitignore b/perception/lidar_apollo_instance_segmentation/.gitignore new file mode 100644 index 0000000000000..356b207498044 --- /dev/null +++ b/perception/lidar_apollo_instance_segmentation/.gitignore @@ -0,0 +1,2 @@ +data/*.caffemodel +data/*.engine diff --git a/perception/lidar_apollo_instance_segmentation/CMakeLists.txt b/perception/lidar_apollo_instance_segmentation/CMakeLists.txt new file mode 100644 index 0000000000000..41f3bd75bc722 --- /dev/null +++ b/perception/lidar_apollo_instance_segmentation/CMakeLists.txt @@ -0,0 +1,175 @@ +cmake_minimum_required(VERSION 3.5) +project(lidar_apollo_instance_segmentation) + +option(CUDA_VERBOSE "Verbose output of CUDA modules" OFF) + +# set flags for CUDA availability +option(CUDA_AVAIL "CUDA available" OFF) +find_package(CUDA) +if (CUDA_FOUND) + find_library(CUBLAS_LIBRARIES cublas HINTS + ${CUDA_TOOLKIT_ROOT_DIR}/lib64 + ${CUDA_TOOLKIT_ROOT_DIR}/lib + ) + if (CUDA_VERBOSE) + message("CUDA is available!") + message("CUDA Libs: ${CUDA_LIBRARIES}") + message("CUDA Headers: ${CUDA_INCLUDE_DIRS}") + endif () + set(CUDA_AVAIL ON) +else() + message("CUDA NOT FOUND") + set(CUDA_AVAIL OFF) +endif (CUDA_FOUND) + +# set flags for TensorRT availability +option(TRT_AVAIL "TensorRT available" OFF) +# try to find the tensorRT modules +find_library(NVINFER NAMES nvinfer) +find_library(NVPARSERS NAMES nvparsers) +find_library(NVCAFFE_PARSER NAMES nvcaffe_parser) +find_library(NVINFER_PLUGIN NAMES nvinfer_plugin) +if(NVINFER AND NVPARSERS AND NVCAFFE_PARSER AND NVINFER_PLUGIN) + if (CUDA_VERBOSE) + message("TensorRT is available!") + message("NVINFER: ${NVINFER}") + message("NVPARSERS: ${NVPARSERS}") + message("NVCAFFE_PARSER: ${NVCAFFE_PARSER}") + endif () + set(TRT_AVAIL ON) +else() + message("TensorRT is NOT Available") + set(TRT_AVAIL OFF) +endif() + +# set flags for CUDNN availability +option(CUDNN_AVAIL "CUDNN available" OFF) +# try to find the CUDNN module +find_library(CUDNN_LIBRARY +NAMES libcudnn.so${__cudnn_ver_suffix} libcudnn${__cudnn_ver_suffix}.dylib ${__cudnn_lib_win_name} +PATHS $ENV{LD_LIBRARY_PATH} ${__libpath_cudart} ${CUDNN_ROOT_DIR} ${PC_CUDNN_LIBRARY_DIRS} ${CMAKE_INSTALL_PREFIX} +PATH_SUFFIXES lib lib64 bin +DOC "CUDNN library." ) +if(CUDNN_LIBRARY) + if (CUDA_VERBOSE) + message("CUDNN is available!") + message("CUDNN_LIBRARY: ${CUDNN_LIBRARY}") + endif () + set(CUDNN_AVAIL ON) +else() + message("CUDNN is NOT Available") + set(CUDNN_AVAIL OFF) +endif() + +find_program(GDOWN_AVAIL "gdown") +if (NOT GDOWN_AVAIL) + message("gdown: command not found. External files could not be downloaded.") +endif() +if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) + # download weight files + set(PATH "${CMAKE_CURRENT_SOURCE_DIR}/data") + if (NOT EXISTS "${PATH}") + execute_process(COMMAND mkdir -p ${PATH}) + endif() + set(FILE "${PATH}/vlp-16.caffemodel") + message(STATUS "Checking and downloading vlp-16.caffemodel") + if (NOT EXISTS "${FILE}") + message(STATUS "... file does not exist. Downloading now ...") + execute_process(COMMAND gdown "https://drive.google.com/uc?id=1bbO_eeGG5HUqyUiAYjOd6hUn-Zma0mMJ" -O ${PATH}/vlp-16.caffemodel) + endif() + set(FILE "${PATH}/hdl-64.caffemodel") + message(STATUS "Checking and downloading hdl-64.caffemodel") + if (NOT EXISTS "${FILE}") + message(STATUS "... file does not exist. Downloading now ...") + execute_process(COMMAND gdown "https://drive.google.com/uc?id=1ZdB6V3jua3GmtYuY9NR1nc9QZe_ChjkP" -O ${PATH}/hdl-64.caffemodel) + endif() + set(FILE "${PATH}/vls-128.caffemodel") + message(STATUS "Checking and downloading vls-128.caffemodel") + if (NOT EXISTS "${FILE}") + message(STATUS "... file does not exist. Downloading now ...") + execute_process(COMMAND gdown "https://drive.google.com/uc?id=1izpNotNxS6mrYIzJwHQ_EyX_IPDU-MBr" -O ${PATH}/vls-128.caffemodel) + endif() + + find_package(ament_cmake_auto REQUIRED) + ament_auto_find_build_dependencies() + find_package(PCL REQUIRED) + + if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) + endif() + if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Wno-deprecated-declarations -Werror) + endif() + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14 -O3") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -D_MWAITXINTRIN_H_INCLUDED") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -D_FORCE_INLINES") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -D__STRICT_ANSI__") + + include_directories( + lib/include + ${CUDA_INCLUDE_DIRS} + ${PCL_INCLUDE_DIRS} + ) + + ament_auto_add_library(tensorrt_apollo_cnn_lib SHARED + lib/src/TrtNet.cpp + ) + + target_link_libraries(tensorrt_apollo_cnn_lib + ${NVINFER} + ${NVCAFFE_PARSER} + ${NVINFER_PLUGIN} + ${CUDA_LIBRARIES} + ${CUBLAS_LIBRARIES} + ${CUDA_curand_LIBRARY} + ${CUDNN_LIBRARY} + ${PCL_LIBRARIES} + ) + + ament_auto_add_library(lidar_apollo_instance_segmentation SHARED + src/node.cpp + src/detector.cpp + src/log_table.cpp + src/feature_generator.cpp + src/feature_map.cpp + src/cluster2d.cpp + src/debugger.cpp + ) + + target_link_libraries(lidar_apollo_instance_segmentation + tensorrt_apollo_cnn_lib + ) + + rclcpp_components_register_node(lidar_apollo_instance_segmentation + PLUGIN "LidarInstanceSegmentationNode" + EXECUTABLE lidar_apollo_instance_segmentation_node + ) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + + ament_auto_package(INSTALL_TO_SHARE + launch + config + data + ) + +else() + find_package(ament_cmake_auto REQUIRED) + ament_auto_find_build_dependencies() + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + + # to avoid launch file missing without a gpu + ament_auto_package(INSTALL_TO_SHARE + launch + ) + +endif() diff --git a/perception/lidar_apollo_instance_segmentation/README.md b/perception/lidar_apollo_instance_segmentation/README.md new file mode 100644 index 0000000000000..80cd5f24e843c --- /dev/null +++ b/perception/lidar_apollo_instance_segmentation/README.md @@ -0,0 +1,143 @@ +# lidar_apollo_instance_segmentation + +![Peek 2020-04-07 00-17](https://user-images.githubusercontent.com/8327598/78574862-92507d80-7865-11ea-9a2d-56d3453bdb7a.gif) + +## Purpose + +This node segments 3D pointcloud data from lidar sensors into obstacles, e.g., cars, trucks, bicycles, and pedestrians +based on CNN based model and obstacle clustering method. + +## Inner-workings / Algorithms + +See the [original design](https://github.com/ApolloAuto/apollo/blob/master/docs/specs/3d_obstacle_perception.md) by Apollo. + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| ------------------ | ------------------------- | ---------------------------------- | +| `input/pointcloud` | `sensor_msgs/PointCloud2` | Pointcloud data from lidar sensors | + +### Output + +| Name | Type | Description | +| --------------------------- | ----------------------------------------------------- | ------------------------------------------------- | +| `output/labeled_clusters` | `autoware_perception_msgs/DetectedObjectsWithFeature` | Detected objects with labeled pointcloud cluster. | +| `debug/instance_pointcloud` | `sensor_msgs/PointCloud2` | Segmented pointcloud for visualization. | + +## Parameters + +### Node Parameters + +None + +### Core Parameters + +| Name | Type | Default Value | Description | +| ----------------------- | ------ | -------------------- | ---------------------------------------------------------------------------------- | +| `score_threshold` | double | 0.8 | If the score of a detected object is lower than this value, the object is ignored. | +| `range` | int | 60 | Half of the length of feature map sides. [m] | +| `width` | int | 640 | The grid width of feature map. | +| `height` | int | 640 | The grid height of feature map. | +| `engine_file` | string | "vls-128.engine" | The name of TensorRT engine file for CNN model. | +| `prototxt_file` | string | "vls-128.prototxt" | The name of prototxt file for CNN model. | +| `caffemodel_file` | string | "vls-128.caffemodel" | The name of caffemodel file for CNN model. | +| `use_intensity_feature` | bool | true | The flag to use intensity feature of pointcloud. | +| `use_constant_feature` | bool | false | The flag to use direction and distance feature of pointcloud. | +| `target_frame` | string | "base_link" | Pointcloud data is transformed into this frame. | +| `z_offset` | int | 2 | z offset from target frame. [m] | + +## Assumptions / Known limits + +There is no training code for CNN model. + +### Note + +This package makes use of three external codes. +The trained files are provided by apollo. The trained files are automatically downloaded when you build. + +Original URL + +- VLP-16 : + +- HDL-64 : + +- VLS-128 : + + +Supported lidars are velodyne 16, 64 and 128, but you can also use velodyne 32 and other lidars with good accuracy. + +1. [apollo 3D Obstacle Perception description](https://github.com/ApolloAuto/apollo/blob/master/docs/specs/3d_obstacle_perception.md) + + ```txt + /****************************************************************************** + * Copyright 2017 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + ``` + +2. [tensorRTWrapper](https://github.com/lewes6369/tensorRTWrapper) : + It is used under the lib directory. + + ```txt + MIT License + + Copyright (c) 2018 lewes6369 + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in all + copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + SOFTWARE. + ``` + +3. [autoware_perception description](https://github.com/k0suke-murakami/autoware_perception/tree/feature/integration_baidu_seg/lidar_apollo_cnn_seg_detect) + + ```txt + /* + * Copyright 2018-2019 Autoware Foundation. All rights reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + ``` + +### Special thanks + +- [Apollo Project](https://github.com/ApolloAuto/apollo) +- [lewes6369](https://github.com/lewes6369) +- [Autoware Foundation](https://github.com/autowarefoundation/autoware) +- [Kosuke Takeuchi](https://github.com/kosuke55) (TierIV Part timer) diff --git a/perception/lidar_apollo_instance_segmentation/config/hdl-64.param.yaml b/perception/lidar_apollo_instance_segmentation/config/hdl-64.param.yaml new file mode 100644 index 0000000000000..9198af79c3773 --- /dev/null +++ b/perception/lidar_apollo_instance_segmentation/config/hdl-64.param.yaml @@ -0,0 +1,8 @@ +/**: + ros__parameters: + score_threshold: 0.1 + range: 70 + width: 672 + height: 672 + use_intensity_feature: true + use_constant_feature: false diff --git a/perception/lidar_apollo_instance_segmentation/config/vlp-16.param.yaml b/perception/lidar_apollo_instance_segmentation/config/vlp-16.param.yaml new file mode 100644 index 0000000000000..9198af79c3773 --- /dev/null +++ b/perception/lidar_apollo_instance_segmentation/config/vlp-16.param.yaml @@ -0,0 +1,8 @@ +/**: + ros__parameters: + score_threshold: 0.1 + range: 70 + width: 672 + height: 672 + use_intensity_feature: true + use_constant_feature: false diff --git a/perception/lidar_apollo_instance_segmentation/config/vls-128.param.yaml b/perception/lidar_apollo_instance_segmentation/config/vls-128.param.yaml new file mode 100644 index 0000000000000..727fa03063661 --- /dev/null +++ b/perception/lidar_apollo_instance_segmentation/config/vls-128.param.yaml @@ -0,0 +1,8 @@ +/**: + ros__parameters: + score_threshold: 0.1 + range: 90 + width: 864 + height: 864 + use_intensity_feature: false + use_constant_feature: false diff --git a/perception/lidar_apollo_instance_segmentation/data/hdl-64.prototxt b/perception/lidar_apollo_instance_segmentation/data/hdl-64.prototxt new file mode 100644 index 0000000000000..80c62b001c367 --- /dev/null +++ b/perception/lidar_apollo_instance_segmentation/data/hdl-64.prototxt @@ -0,0 +1,685 @@ +name: "pcd_parsing" +layer { + name: "input" + type: "Input" + top: "data" + input_param { + shape { + dim: 1 + dim: 6 + dim: 672 + dim: 672 + } + } +} +layer { + name: "conv0_1" + type: "Convolution" + bottom: "data" + top: "conv0_1" + convolution_param { + num_output: 24 + bias_term: true + pad: 0 + kernel_size: 1 + stride: 1 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_conv0_1" + type: "ReLU" + bottom: "conv0_1" + top: "conv0_1" +} +layer { + name: "conv0" + type: "Convolution" + bottom: "conv0_1" + top: "conv0" + convolution_param { + num_output: 24 + bias_term: true + pad: 1 + kernel_size: 3 + stride: 1 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_conv0" + type: "ReLU" + bottom: "conv0" + top: "conv0" +} +layer { + name: "conv1_1" + type: "Convolution" + bottom: "conv0" + top: "conv1_1" + convolution_param { + num_output: 48 + bias_term: true + pad: 1 + kernel_size: 3 + stride: 2 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_conv1_1" + type: "ReLU" + bottom: "conv1_1" + top: "conv1_1" +} +layer { + name: "conv1" + type: "Convolution" + bottom: "conv1_1" + top: "conv1" + convolution_param { + num_output: 48 + bias_term: true + pad: 1 + kernel_size: 3 + stride: 1 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_conv1" + type: "ReLU" + bottom: "conv1" + top: "conv1" +} +layer { + name: "conv2_1" + type: "Convolution" + bottom: "conv1" + top: "conv2_1" + convolution_param { + num_output: 64 + bias_term: true + pad: 1 + kernel_size: 3 + stride: 2 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_conv2_1" + type: "ReLU" + bottom: "conv2_1" + top: "conv2_1" +} +layer { + name: "conv2_2" + type: "Convolution" + bottom: "conv2_1" + top: "conv2_2" + convolution_param { + num_output: 64 + bias_term: true + pad: 1 + kernel_size: 3 + stride: 1 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_conv2_2" + type: "ReLU" + bottom: "conv2_2" + top: "conv2_2" +} +layer { + name: "conv2" + type: "Convolution" + bottom: "conv2_2" + top: "conv2" + convolution_param { + num_output: 64 + bias_term: true + pad: 1 + kernel_size: 3 + stride: 1 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_conv2" + type: "ReLU" + bottom: "conv2" + top: "conv2" +} +layer { + name: "conv3_1" + type: "Convolution" + bottom: "conv2" + top: "conv3_1" + convolution_param { + num_output: 96 + bias_term: true + pad: 1 + kernel_size: 3 + stride: 2 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_conv3_1" + type: "ReLU" + bottom: "conv3_1" + top: "conv3_1" +} +layer { + name: "conv3_2" + type: "Convolution" + bottom: "conv3_1" + top: "conv3_2" + convolution_param { + num_output: 96 + bias_term: true + pad: 1 + kernel_size: 3 + stride: 1 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_conv3_2" + type: "ReLU" + bottom: "conv3_2" + top: "conv3_2" +} +layer { + name: "conv3" + type: "Convolution" + bottom: "conv3_2" + top: "conv3" + convolution_param { + num_output: 96 + bias_term: true + pad: 1 + kernel_size: 3 + stride: 1 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_conv3" + type: "ReLU" + bottom: "conv3" + top: "conv3" +} +layer { + name: "conv4_1" + type: "Convolution" + bottom: "conv3" + top: "conv4_1" + convolution_param { + num_output: 128 + bias_term: true + pad: 1 + kernel_size: 3 + stride: 2 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_conv4_1" + type: "ReLU" + bottom: "conv4_1" + top: "conv4_1" +} +layer { + name: "conv4_2" + type: "Convolution" + bottom: "conv4_1" + top: "conv4_2" + convolution_param { + num_output: 128 + bias_term: true + pad: 1 + kernel_size: 3 + stride: 1 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_conv4_2" + type: "ReLU" + bottom: "conv4_2" + top: "conv4_2" +} +layer { + name: "conv4" + type: "Convolution" + bottom: "conv4_2" + top: "conv4" + convolution_param { + num_output: 128 + bias_term: true + pad: 1 + kernel_size: 3 + stride: 1 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_conv4" + type: "ReLU" + bottom: "conv4" + top: "conv4" +} +layer { + name: "conv5_1" + type: "Convolution" + bottom: "conv4" + top: "conv5_1" + convolution_param { + num_output: 192 + bias_term: true + pad: 1 + kernel_size: 3 + stride: 2 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_conv5_1" + type: "ReLU" + bottom: "conv5_1" + top: "conv5_1" +} +layer { + name: "conv5" + type: "Convolution" + bottom: "conv5_1" + top: "conv5" + convolution_param { + num_output: 192 + bias_term: true + pad: 1 + kernel_size: 3 + stride: 1 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_conv5" + type: "ReLU" + bottom: "conv5" + top: "conv5" +} +layer { + name: "deconv5_1" + type: "Convolution" + bottom: "conv5" + top: "deconv5_1" + convolution_param { + num_output: 192 + bias_term: true + pad: 1 + kernel_size: 3 + stride: 1 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_deconv5_1" + type: "ReLU" + bottom: "deconv5_1" + top: "deconv5_1" +} +layer { + name: "deconv4" + type: "Deconvolution" + bottom: "deconv5_1" + top: "deconv4" + convolution_param { + num_output: 128 + bias_term: true + pad: 1 + kernel_size: 4 + stride: 2 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_deconv4" + type: "ReLU" + bottom: "deconv4" + top: "deconv4" +} +layer { + name: "Concat_concat4" + type: "Concat" + bottom: "conv4" + bottom: "deconv4" + top: "concat4" +} +layer { + name: "deconv4_1" + type: "Convolution" + bottom: "concat4" + top: "deconv4_1" + convolution_param { + num_output: 128 + bias_term: true + pad: 1 + kernel_size: 3 + stride: 1 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_deconv4_1" + type: "ReLU" + bottom: "deconv4_1" + top: "deconv4_1" +} +layer { + name: "deconv3" + type: "Deconvolution" + bottom: "deconv4_1" + top: "deconv3" + convolution_param { + num_output: 96 + bias_term: true + pad: 1 + kernel_size: 4 + stride: 2 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_deconv3" + type: "ReLU" + bottom: "deconv3" + top: "deconv3" +} +layer { + name: "Concat_concat3" + type: "Concat" + bottom: "conv3" + bottom: "deconv3" + top: "concat3" +} +layer { + name: "deconv3_1" + type: "Convolution" + bottom: "concat3" + top: "deconv3_1" + convolution_param { + num_output: 96 + bias_term: true + pad: 1 + kernel_size: 3 + stride: 1 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_deconv3_1" + type: "ReLU" + bottom: "deconv3_1" + top: "deconv3_1" +} +layer { + name: "deconv2" + type: "Deconvolution" + bottom: "deconv3_1" + top: "deconv2" + convolution_param { + num_output: 64 + bias_term: true + pad: 1 + kernel_size: 4 + stride: 2 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_deconv2" + type: "ReLU" + bottom: "deconv2" + top: "deconv2" +} +layer { + name: "Concat_concat2" + type: "Concat" + bottom: "conv2" + bottom: "deconv2" + top: "concat2" +} +layer { + name: "deconv2_1" + type: "Convolution" + bottom: "concat2" + top: "deconv2_1" + convolution_param { + num_output: 64 + bias_term: true + pad: 1 + kernel_size: 3 + stride: 1 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_deconv2_1" + type: "ReLU" + bottom: "deconv2_1" + top: "deconv2_1" +} +layer { + name: "deconv1" + type: "Deconvolution" + bottom: "deconv2_1" + top: "deconv1" + convolution_param { + num_output: 48 + bias_term: true + pad: 1 + kernel_size: 4 + stride: 2 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_deconv1" + type: "ReLU" + bottom: "deconv1" + top: "deconv1" +} +layer { + name: "Concat_concat1" + type: "Concat" + bottom: "conv1" + bottom: "deconv1" + top: "concat1" +} +layer { + name: "deconv1_1" + type: "Convolution" + bottom: "concat1" + top: "deconv1_1" + convolution_param { + num_output: 48 + bias_term: true + pad: 1 + kernel_size: 3 + stride: 1 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_deconv1_1" + type: "ReLU" + bottom: "deconv1_1" + top: "deconv1_1" +} +layer { + name: "deconv0" + type: "Deconvolution" + bottom: "deconv1_1" + top: "deconv0" + convolution_param { + num_output: 12 + pad: 1 + kernel_size: 4 + stride: 2 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} diff --git a/perception/lidar_apollo_instance_segmentation/data/vlp-16.prototxt b/perception/lidar_apollo_instance_segmentation/data/vlp-16.prototxt new file mode 100644 index 0000000000000..80c62b001c367 --- /dev/null +++ b/perception/lidar_apollo_instance_segmentation/data/vlp-16.prototxt @@ -0,0 +1,685 @@ +name: "pcd_parsing" +layer { + name: "input" + type: "Input" + top: "data" + input_param { + shape { + dim: 1 + dim: 6 + dim: 672 + dim: 672 + } + } +} +layer { + name: "conv0_1" + type: "Convolution" + bottom: "data" + top: "conv0_1" + convolution_param { + num_output: 24 + bias_term: true + pad: 0 + kernel_size: 1 + stride: 1 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_conv0_1" + type: "ReLU" + bottom: "conv0_1" + top: "conv0_1" +} +layer { + name: "conv0" + type: "Convolution" + bottom: "conv0_1" + top: "conv0" + convolution_param { + num_output: 24 + bias_term: true + pad: 1 + kernel_size: 3 + stride: 1 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_conv0" + type: "ReLU" + bottom: "conv0" + top: "conv0" +} +layer { + name: "conv1_1" + type: "Convolution" + bottom: "conv0" + top: "conv1_1" + convolution_param { + num_output: 48 + bias_term: true + pad: 1 + kernel_size: 3 + stride: 2 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_conv1_1" + type: "ReLU" + bottom: "conv1_1" + top: "conv1_1" +} +layer { + name: "conv1" + type: "Convolution" + bottom: "conv1_1" + top: "conv1" + convolution_param { + num_output: 48 + bias_term: true + pad: 1 + kernel_size: 3 + stride: 1 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_conv1" + type: "ReLU" + bottom: "conv1" + top: "conv1" +} +layer { + name: "conv2_1" + type: "Convolution" + bottom: "conv1" + top: "conv2_1" + convolution_param { + num_output: 64 + bias_term: true + pad: 1 + kernel_size: 3 + stride: 2 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_conv2_1" + type: "ReLU" + bottom: "conv2_1" + top: "conv2_1" +} +layer { + name: "conv2_2" + type: "Convolution" + bottom: "conv2_1" + top: "conv2_2" + convolution_param { + num_output: 64 + bias_term: true + pad: 1 + kernel_size: 3 + stride: 1 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_conv2_2" + type: "ReLU" + bottom: "conv2_2" + top: "conv2_2" +} +layer { + name: "conv2" + type: "Convolution" + bottom: "conv2_2" + top: "conv2" + convolution_param { + num_output: 64 + bias_term: true + pad: 1 + kernel_size: 3 + stride: 1 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_conv2" + type: "ReLU" + bottom: "conv2" + top: "conv2" +} +layer { + name: "conv3_1" + type: "Convolution" + bottom: "conv2" + top: "conv3_1" + convolution_param { + num_output: 96 + bias_term: true + pad: 1 + kernel_size: 3 + stride: 2 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_conv3_1" + type: "ReLU" + bottom: "conv3_1" + top: "conv3_1" +} +layer { + name: "conv3_2" + type: "Convolution" + bottom: "conv3_1" + top: "conv3_2" + convolution_param { + num_output: 96 + bias_term: true + pad: 1 + kernel_size: 3 + stride: 1 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_conv3_2" + type: "ReLU" + bottom: "conv3_2" + top: "conv3_2" +} +layer { + name: "conv3" + type: "Convolution" + bottom: "conv3_2" + top: "conv3" + convolution_param { + num_output: 96 + bias_term: true + pad: 1 + kernel_size: 3 + stride: 1 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_conv3" + type: "ReLU" + bottom: "conv3" + top: "conv3" +} +layer { + name: "conv4_1" + type: "Convolution" + bottom: "conv3" + top: "conv4_1" + convolution_param { + num_output: 128 + bias_term: true + pad: 1 + kernel_size: 3 + stride: 2 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_conv4_1" + type: "ReLU" + bottom: "conv4_1" + top: "conv4_1" +} +layer { + name: "conv4_2" + type: "Convolution" + bottom: "conv4_1" + top: "conv4_2" + convolution_param { + num_output: 128 + bias_term: true + pad: 1 + kernel_size: 3 + stride: 1 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_conv4_2" + type: "ReLU" + bottom: "conv4_2" + top: "conv4_2" +} +layer { + name: "conv4" + type: "Convolution" + bottom: "conv4_2" + top: "conv4" + convolution_param { + num_output: 128 + bias_term: true + pad: 1 + kernel_size: 3 + stride: 1 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_conv4" + type: "ReLU" + bottom: "conv4" + top: "conv4" +} +layer { + name: "conv5_1" + type: "Convolution" + bottom: "conv4" + top: "conv5_1" + convolution_param { + num_output: 192 + bias_term: true + pad: 1 + kernel_size: 3 + stride: 2 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_conv5_1" + type: "ReLU" + bottom: "conv5_1" + top: "conv5_1" +} +layer { + name: "conv5" + type: "Convolution" + bottom: "conv5_1" + top: "conv5" + convolution_param { + num_output: 192 + bias_term: true + pad: 1 + kernel_size: 3 + stride: 1 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_conv5" + type: "ReLU" + bottom: "conv5" + top: "conv5" +} +layer { + name: "deconv5_1" + type: "Convolution" + bottom: "conv5" + top: "deconv5_1" + convolution_param { + num_output: 192 + bias_term: true + pad: 1 + kernel_size: 3 + stride: 1 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_deconv5_1" + type: "ReLU" + bottom: "deconv5_1" + top: "deconv5_1" +} +layer { + name: "deconv4" + type: "Deconvolution" + bottom: "deconv5_1" + top: "deconv4" + convolution_param { + num_output: 128 + bias_term: true + pad: 1 + kernel_size: 4 + stride: 2 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_deconv4" + type: "ReLU" + bottom: "deconv4" + top: "deconv4" +} +layer { + name: "Concat_concat4" + type: "Concat" + bottom: "conv4" + bottom: "deconv4" + top: "concat4" +} +layer { + name: "deconv4_1" + type: "Convolution" + bottom: "concat4" + top: "deconv4_1" + convolution_param { + num_output: 128 + bias_term: true + pad: 1 + kernel_size: 3 + stride: 1 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_deconv4_1" + type: "ReLU" + bottom: "deconv4_1" + top: "deconv4_1" +} +layer { + name: "deconv3" + type: "Deconvolution" + bottom: "deconv4_1" + top: "deconv3" + convolution_param { + num_output: 96 + bias_term: true + pad: 1 + kernel_size: 4 + stride: 2 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_deconv3" + type: "ReLU" + bottom: "deconv3" + top: "deconv3" +} +layer { + name: "Concat_concat3" + type: "Concat" + bottom: "conv3" + bottom: "deconv3" + top: "concat3" +} +layer { + name: "deconv3_1" + type: "Convolution" + bottom: "concat3" + top: "deconv3_1" + convolution_param { + num_output: 96 + bias_term: true + pad: 1 + kernel_size: 3 + stride: 1 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_deconv3_1" + type: "ReLU" + bottom: "deconv3_1" + top: "deconv3_1" +} +layer { + name: "deconv2" + type: "Deconvolution" + bottom: "deconv3_1" + top: "deconv2" + convolution_param { + num_output: 64 + bias_term: true + pad: 1 + kernel_size: 4 + stride: 2 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_deconv2" + type: "ReLU" + bottom: "deconv2" + top: "deconv2" +} +layer { + name: "Concat_concat2" + type: "Concat" + bottom: "conv2" + bottom: "deconv2" + top: "concat2" +} +layer { + name: "deconv2_1" + type: "Convolution" + bottom: "concat2" + top: "deconv2_1" + convolution_param { + num_output: 64 + bias_term: true + pad: 1 + kernel_size: 3 + stride: 1 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_deconv2_1" + type: "ReLU" + bottom: "deconv2_1" + top: "deconv2_1" +} +layer { + name: "deconv1" + type: "Deconvolution" + bottom: "deconv2_1" + top: "deconv1" + convolution_param { + num_output: 48 + bias_term: true + pad: 1 + kernel_size: 4 + stride: 2 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_deconv1" + type: "ReLU" + bottom: "deconv1" + top: "deconv1" +} +layer { + name: "Concat_concat1" + type: "Concat" + bottom: "conv1" + bottom: "deconv1" + top: "concat1" +} +layer { + name: "deconv1_1" + type: "Convolution" + bottom: "concat1" + top: "deconv1_1" + convolution_param { + num_output: 48 + bias_term: true + pad: 1 + kernel_size: 3 + stride: 1 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_deconv1_1" + type: "ReLU" + bottom: "deconv1_1" + top: "deconv1_1" +} +layer { + name: "deconv0" + type: "Deconvolution" + bottom: "deconv1_1" + top: "deconv0" + convolution_param { + num_output: 12 + pad: 1 + kernel_size: 4 + stride: 2 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} diff --git a/perception/lidar_apollo_instance_segmentation/data/vls-128.prototxt b/perception/lidar_apollo_instance_segmentation/data/vls-128.prototxt new file mode 100644 index 0000000000000..a2d918b760626 --- /dev/null +++ b/perception/lidar_apollo_instance_segmentation/data/vls-128.prototxt @@ -0,0 +1,686 @@ +name: "pcd_parsing" +layer { + name: "input" + type: "Input" + top: "data" + input_param { + shape { + dim: 1 + dim: 4 + dim: 864 + dim: 864 + } + } +} + +layer { + name: "conv0_1" + type: "Convolution" + bottom: "data" + top: "conv0_1" + convolution_param { + num_output: 24 + bias_term: true + pad: 0 + kernel_size: 1 + stride: 1 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_conv0_1" + type: "ReLU" + bottom: "conv0_1" + top: "conv0_1" +} +layer { + name: "conv0" + type: "Convolution" + bottom: "conv0_1" + top: "conv0" + convolution_param { + num_output: 24 + bias_term: true + pad: 1 + kernel_size: 3 + stride: 1 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_conv0" + type: "ReLU" + bottom: "conv0" + top: "conv0" +} +layer { + name: "conv1_1" + type: "Convolution" + bottom: "conv0" + top: "conv1_1" + convolution_param { + num_output: 48 + bias_term: true + pad: 1 + kernel_size: 3 + stride: 2 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_conv1_1" + type: "ReLU" + bottom: "conv1_1" + top: "conv1_1" +} +layer { + name: "conv1" + type: "Convolution" + bottom: "conv1_1" + top: "conv1" + convolution_param { + num_output: 48 + bias_term: true + pad: 1 + kernel_size: 3 + stride: 1 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_conv1" + type: "ReLU" + bottom: "conv1" + top: "conv1" +} +layer { + name: "conv2_1" + type: "Convolution" + bottom: "conv1" + top: "conv2_1" + convolution_param { + num_output: 64 + bias_term: true + pad: 1 + kernel_size: 3 + stride: 2 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_conv2_1" + type: "ReLU" + bottom: "conv2_1" + top: "conv2_1" +} +layer { + name: "conv2_2" + type: "Convolution" + bottom: "conv2_1" + top: "conv2_2" + convolution_param { + num_output: 64 + bias_term: true + pad: 1 + kernel_size: 3 + stride: 1 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_conv2_2" + type: "ReLU" + bottom: "conv2_2" + top: "conv2_2" +} +layer { + name: "conv2" + type: "Convolution" + bottom: "conv2_2" + top: "conv2" + convolution_param { + num_output: 64 + bias_term: true + pad: 1 + kernel_size: 3 + stride: 1 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_conv2" + type: "ReLU" + bottom: "conv2" + top: "conv2" +} +layer { + name: "conv3_1" + type: "Convolution" + bottom: "conv2" + top: "conv3_1" + convolution_param { + num_output: 96 + bias_term: true + pad: 1 + kernel_size: 3 + stride: 2 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_conv3_1" + type: "ReLU" + bottom: "conv3_1" + top: "conv3_1" +} +layer { + name: "conv3_2" + type: "Convolution" + bottom: "conv3_1" + top: "conv3_2" + convolution_param { + num_output: 96 + bias_term: true + pad: 1 + kernel_size: 3 + stride: 1 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_conv3_2" + type: "ReLU" + bottom: "conv3_2" + top: "conv3_2" +} +layer { + name: "conv3" + type: "Convolution" + bottom: "conv3_2" + top: "conv3" + convolution_param { + num_output: 96 + bias_term: true + pad: 1 + kernel_size: 3 + stride: 1 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_conv3" + type: "ReLU" + bottom: "conv3" + top: "conv3" +} +layer { + name: "conv4_1" + type: "Convolution" + bottom: "conv3" + top: "conv4_1" + convolution_param { + num_output: 128 + bias_term: true + pad: 1 + kernel_size: 3 + stride: 2 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_conv4_1" + type: "ReLU" + bottom: "conv4_1" + top: "conv4_1" +} +layer { + name: "conv4_2" + type: "Convolution" + bottom: "conv4_1" + top: "conv4_2" + convolution_param { + num_output: 128 + bias_term: true + pad: 1 + kernel_size: 3 + stride: 1 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_conv4_2" + type: "ReLU" + bottom: "conv4_2" + top: "conv4_2" +} +layer { + name: "conv4" + type: "Convolution" + bottom: "conv4_2" + top: "conv4" + convolution_param { + num_output: 128 + bias_term: true + pad: 1 + kernel_size: 3 + stride: 1 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_conv4" + type: "ReLU" + bottom: "conv4" + top: "conv4" +} +layer { + name: "conv5_1" + type: "Convolution" + bottom: "conv4" + top: "conv5_1" + convolution_param { + num_output: 192 + bias_term: true + pad: 1 + kernel_size: 3 + stride: 2 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_conv5_1" + type: "ReLU" + bottom: "conv5_1" + top: "conv5_1" +} +layer { + name: "conv5" + type: "Convolution" + bottom: "conv5_1" + top: "conv5" + convolution_param { + num_output: 192 + bias_term: true + pad: 1 + kernel_size: 3 + stride: 1 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_conv5" + type: "ReLU" + bottom: "conv5" + top: "conv5" +} +layer { + name: "deconv5_1" + type: "Convolution" + bottom: "conv5" + top: "deconv5_1" + convolution_param { + num_output: 192 + bias_term: true + pad: 1 + kernel_size: 3 + stride: 1 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_deconv5_1" + type: "ReLU" + bottom: "deconv5_1" + top: "deconv5_1" +} +layer { + name: "deconv4" + type: "Deconvolution" + bottom: "deconv5_1" + top: "deconv4" + convolution_param { + num_output: 128 + bias_term: true + pad: 1 + kernel_size: 4 + stride: 2 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_deconv4" + type: "ReLU" + bottom: "deconv4" + top: "deconv4" +} +layer { + name: "Concat_concat4" + type: "Concat" + bottom: "conv4" + bottom: "deconv4" + top: "concat4" +} +layer { + name: "deconv4_1" + type: "Convolution" + bottom: "concat4" + top: "deconv4_1" + convolution_param { + num_output: 128 + bias_term: true + pad: 1 + kernel_size: 3 + stride: 1 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_deconv4_1" + type: "ReLU" + bottom: "deconv4_1" + top: "deconv4_1" +} +layer { + name: "deconv3" + type: "Deconvolution" + bottom: "deconv4_1" + top: "deconv3" + convolution_param { + num_output: 96 + bias_term: true + pad: 1 + kernel_size: 4 + stride: 2 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_deconv3" + type: "ReLU" + bottom: "deconv3" + top: "deconv3" +} +layer { + name: "Concat_concat3" + type: "Concat" + bottom: "conv3" + bottom: "deconv3" + top: "concat3" +} +layer { + name: "deconv3_1" + type: "Convolution" + bottom: "concat3" + top: "deconv3_1" + convolution_param { + num_output: 96 + bias_term: true + pad: 1 + kernel_size: 3 + stride: 1 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_deconv3_1" + type: "ReLU" + bottom: "deconv3_1" + top: "deconv3_1" +} +layer { + name: "deconv2" + type: "Deconvolution" + bottom: "deconv3_1" + top: "deconv2" + convolution_param { + num_output: 64 + bias_term: true + pad: 1 + kernel_size: 4 + stride: 2 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_deconv2" + type: "ReLU" + bottom: "deconv2" + top: "deconv2" +} +layer { + name: "Concat_concat2" + type: "Concat" + bottom: "conv2" + bottom: "deconv2" + top: "concat2" +} +layer { + name: "deconv2_1" + type: "Convolution" + bottom: "concat2" + top: "deconv2_1" + convolution_param { + num_output: 64 + bias_term: true + pad: 1 + kernel_size: 3 + stride: 1 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_deconv2_1" + type: "ReLU" + bottom: "deconv2_1" + top: "deconv2_1" +} +layer { + name: "deconv1" + type: "Deconvolution" + bottom: "deconv2_1" + top: "deconv1" + convolution_param { + num_output: 48 + bias_term: true + pad: 1 + kernel_size: 4 + stride: 2 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_deconv1" + type: "ReLU" + bottom: "deconv1" + top: "deconv1" +} +layer { + name: "Concat_concat1" + type: "Concat" + bottom: "conv1" + bottom: "deconv1" + top: "concat1" +} +layer { + name: "deconv1_1" + type: "Convolution" + bottom: "concat1" + top: "deconv1_1" + convolution_param { + num_output: 48 + bias_term: true + pad: 1 + kernel_size: 3 + stride: 1 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} +layer { + name: "relu_deconv1_1" + type: "ReLU" + bottom: "deconv1_1" + top: "deconv1_1" +} +layer { + name: "deconv0" + type: "Deconvolution" + bottom: "deconv1_1" + top: "deconv0" + convolution_param { + num_output: 12 + pad: 1 + kernel_size: 4 + stride: 2 + weight_filler { + type: "xavier" + } + bias_filler { + type: "constant" + value: 0.0 + } + } +} diff --git a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/cluster2d.hpp b/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/cluster2d.hpp new file mode 100644 index 0000000000000..ee750a35156da --- /dev/null +++ b/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/cluster2d.hpp @@ -0,0 +1,162 @@ +// Copyright 2020 TierIV +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +/* + * Copyright 2018-2019 Autoware Foundation. All rights reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/****************************************************************************** + * Copyright 2017 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#ifndef LIDAR_APOLLO_INSTANCE_SEGMENTATION__CLUSTER2D_HPP_ +#define LIDAR_APOLLO_INSTANCE_SEGMENTATION__CLUSTER2D_HPP_ + +#include "disjoint_set.hpp" +#include "util.hpp" + +#include +#include +#include + +#include +#include +#include + +#include +#include + +enum MetaType { + META_UNKNOWN, + META_SMALLMOT, + META_BIGMOT, + META_NONMOT, + META_PEDESTRIAN, + MAX_META_TYPE +}; + +struct Obstacle +{ + std::vector grids; + pcl::PointCloud::Ptr cloud_ptr; + float score; + float height; + float heading; + MetaType meta_type; + std::vector meta_type_probs; + + Obstacle() : score(0.0), height(-5.0), heading(0.0), meta_type(META_UNKNOWN) + { + cloud_ptr.reset(new pcl::PointCloud); + meta_type_probs.assign(MAX_META_TYPE, 0.0); + } +}; + +class Cluster2D +{ +public: + Cluster2D(const int rows, const int cols, const float range); + + ~Cluster2D() {} + + void cluster( + const std::shared_ptr & inferred_data, + const pcl::PointCloud::Ptr & pc_ptr, const pcl::PointIndices & valid_indices, + float objectness_thresh, bool use_all_grids_for_clustering); + + void filter(const std::shared_ptr & inferred_data); + void classify(const std::shared_ptr & inferred_data); + + void getObjects( + const float confidence_thresh, const float height_thresh, const int min_pts_num, + autoware_perception_msgs::msg::DetectedObjectsWithFeature & objects, + const std_msgs::msg::Header & in_header); + + autoware_perception_msgs::msg::DetectedObjectWithFeature obstacleToObject( + const Obstacle & in_obstacle, const std_msgs::msg::Header & in_header); + +private: + int rows_; + int cols_; + int siz_; + float range_; + float scale_; + float inv_res_x_; + float inv_res_y_; + std::vector point2grid_; + std::vector obstacles_; + std::vector id_img_; + + pcl::PointCloud::Ptr pc_ptr_; + const std::vector * valid_indices_in_pc_ = nullptr; + + struct Node + { + Node * center_node; + Node * parent; + char node_rank; + char traversed; + bool is_center; + bool is_object; + int point_num; + int obstacle_id; + + Node() + { + center_node = nullptr; + parent = nullptr; + node_rank = 0; + traversed = 0; + is_center = false; + is_object = false; + point_num = 0; + obstacle_id = -1; + } + }; + + inline bool IsValidRowCol(int row, int col) const { return IsValidRow(row) && IsValidCol(col); } + + inline bool IsValidRow(int row) const { return row >= 0 && row < rows_; } + + inline bool IsValidCol(int col) const { return col >= 0 && col < cols_; } + + inline int RowCol2Grid(int row, int col) const { return row * cols_ + col; } + + void traverse(Node * x); +}; + +#endif // LIDAR_APOLLO_INSTANCE_SEGMENTATION__CLUSTER2D_HPP_ diff --git a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/debugger.hpp b/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/debugger.hpp new file mode 100644 index 0000000000000..140ffab4b4767 --- /dev/null +++ b/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/debugger.hpp @@ -0,0 +1,31 @@ +// Copyright 2020 TierIV +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include + +#include + +class Debugger +{ +public: + explicit Debugger(rclcpp::Node * node); + ~Debugger() {} + void publishColoredPointCloud( + const autoware_perception_msgs::msg::DetectedObjectsWithFeature & input); + +private: + rclcpp::Publisher::SharedPtr instance_pointcloud_pub_; +}; diff --git a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/detector.hpp b/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/detector.hpp new file mode 100644 index 0000000000000..ce2da6f32bc06 --- /dev/null +++ b/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/detector.hpp @@ -0,0 +1,55 @@ +// Copyright 2020 TierIV +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include "cluster2d.hpp" +#include "feature_generator.hpp" +#include "lidar_apollo_instance_segmentation/node.hpp" + +#include + +#include +#include +#include +#include + +#include +#include + +class LidarApolloInstanceSegmentation : public LidarInstanceSegmentationInterface +{ +public: + explicit LidarApolloInstanceSegmentation(rclcpp::Node * node); + ~LidarApolloInstanceSegmentation() {} + bool detectDynamicObjects( + const sensor_msgs::msg::PointCloud2 & input, + autoware_perception_msgs::msg::DetectedObjectsWithFeature & output) override; + +private: + bool transformCloud( + const sensor_msgs::msg::PointCloud2 & input, sensor_msgs::msg::PointCloud2 & transformed_cloud, + float z_offset); + + rclcpp::Node * node_; + std::unique_ptr net_ptr_; + std::shared_ptr cluster2d_; + std::shared_ptr feature_generator_; + float score_threshold_; + + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + std::string target_frame_; + float z_offset_; +}; diff --git a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/disjoint_set.hpp b/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/disjoint_set.hpp new file mode 100644 index 0000000000000..fc3cc6b18a5ab --- /dev/null +++ b/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/disjoint_set.hpp @@ -0,0 +1,75 @@ +/****************************************************************************** + * Copyright 2017 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#ifndef MODULES_PERCEPTION_OBSTACLE_COMMON_DISJOINT_SET_H_ +#define MODULES_PERCEPTION_OBSTACLE_COMMON_DISJOINT_SET_H_ + +template +void DisjointSetMakeSet(T * x) +{ + x->parent = x; + x->node_rank = 0; +} + +template +T * DisjointSetFindRecursive(T * x) +{ + if (x->parent != x) { + x->parent = DisjointSetFindRecursive(x->parent); + } + return x->parent; +} + +template +T * DisjointSetFind(T * x) +{ + T * y = x->parent; + if (y == x || y->parent == y) { + return y; + } + T * root = DisjointSetFindRecursive(y->parent); + x->parent = root; + y->parent = root; + return root; +} + +template +void DisjointSetMerge([[maybe_unused]] T * x, [[maybe_unused]] const T * y) +{ +} + +template +void DisjointSetUnion(T * x, T * y) +{ + x = DisjointSetFind(x); + y = DisjointSetFind(y); + if (x == y) { + return; + } + if (x->node_rank < y->node_rank) { + x->parent = y; + DisjointSetMerge(y, x); + } else if (y->node_rank < x->node_rank) { + y->parent = x; + DisjointSetMerge(x, y); + } else { + y->parent = x; + x->node_rank++; + DisjointSetMerge(x, y); + } +} + +#endif // MODULES_PERCEPTION_OBSTACLE_COMMON_DISJOINT_SET_H_ diff --git a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/feature_generator.hpp b/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/feature_generator.hpp new file mode 100644 index 0000000000000..ec9f200994396 --- /dev/null +++ b/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/feature_generator.hpp @@ -0,0 +1,42 @@ +// Copyright 2020 TierIV +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include "lidar_apollo_instance_segmentation/feature_map.hpp" +#include "util.hpp" + +#include +#include + +#include + +class FeatureGenerator +{ +private: + float min_height_; + float max_height_; + bool use_intensity_feature_; + bool use_constant_feature_; + std::shared_ptr map_ptr_; + +public: + FeatureGenerator( + const int width, const int height, const int range, const bool use_intensity_feature, + const bool use_constant_feature); + ~FeatureGenerator() {} + + std::shared_ptr generate( + const pcl::PointCloud::Ptr & pc_ptr); +}; diff --git a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/feature_map.hpp b/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/feature_map.hpp new file mode 100644 index 0000000000000..2f54bbe0c960e --- /dev/null +++ b/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/feature_map.hpp @@ -0,0 +1,65 @@ +// Copyright 2020 TierIV +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#pragma once +#include +#include + +struct FeatureMapInterface +{ +public: + int channels; + int width; + int height; + int range; + float * max_height_data; // channel 0 + float * mean_height_data; // channel 1 + float * count_data; // channel 2 + float * direction_data; // channel 3 + float * top_intensity_data; // channel 4 + float * mean_intensity_data; // channel 5 + float * distance_data; // channel 6 + float * nonempty_data; // channel 7 + std::vector map_data; + virtual void initializeMap(std::vector & map) = 0; + virtual void resetMap(std::vector & map) = 0; + FeatureMapInterface(const int _channels, const int _width, const int _height, const int _range); +}; + +struct FeatureMap : public FeatureMapInterface +{ + FeatureMap(const int width, const int height, const int range); + void initializeMap(std::vector & map) override; + void resetMap(std::vector & map) override; +}; + +struct FeatureMapWithIntensity : public FeatureMapInterface +{ + FeatureMapWithIntensity(const int width, const int height, const int range); + void initializeMap(std::vector & map) override; + void resetMap(std::vector & map) override; +}; + +struct FeatureMapWithConstant : public FeatureMapInterface +{ + FeatureMapWithConstant(const int width, const int height, const int range); + void initializeMap(std::vector & map) override; + void resetMap(std::vector & map) override; +}; + +struct FeatureMapWithConstantAndIntensity : public FeatureMapInterface +{ + FeatureMapWithConstantAndIntensity(const int width, const int height, const int range); + void initializeMap(std::vector & map) override; + void resetMap(std::vector & map) override; +}; diff --git a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/log_table.hpp b/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/log_table.hpp new file mode 100644 index 0000000000000..73c29de6cb613 --- /dev/null +++ b/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/log_table.hpp @@ -0,0 +1,16 @@ +// Copyright 2020 TierIV +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#pragma once + +float calcApproximateLog(float num); diff --git a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/node.hpp b/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/node.hpp new file mode 100644 index 0000000000000..a2e38b3e860e4 --- /dev/null +++ b/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/node.hpp @@ -0,0 +1,46 @@ +// Copyright 2020 TierIV +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +#pragma once +#include "lidar_apollo_instance_segmentation/debugger.hpp" + +#include + +#include +#include + +#include + +class LidarInstanceSegmentationInterface +{ +public: + LidarInstanceSegmentationInterface() {} + virtual ~LidarInstanceSegmentationInterface() {} + virtual bool detectDynamicObjects( + const sensor_msgs::msg::PointCloud2 & input, + autoware_perception_msgs::msg::DetectedObjectsWithFeature & output) = 0; +}; + +class LidarInstanceSegmentationNode : public rclcpp::Node +{ +private: + rclcpp::Subscription::SharedPtr pointcloud_sub_; + rclcpp::Publisher::SharedPtr + dynamic_objects_pub_; + std::shared_ptr detector_ptr_; + std::shared_ptr debugger_ptr_; + void pointCloudCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); + +public: + explicit LidarInstanceSegmentationNode(const rclcpp::NodeOptions & node_options); +}; diff --git a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/util.hpp b/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/util.hpp new file mode 100644 index 0000000000000..50318e633039b --- /dev/null +++ b/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/util.hpp @@ -0,0 +1,59 @@ +// Copyright 2018-2019 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/****************************************************************************** + * Copyright 2017 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#ifndef MODULES_PERCEPTION_OBSTACLE_LIDAR_SEGMENTATION_CNNSEG_UTIL_H_ +#define MODULES_PERCEPTION_OBSTACLE_LIDAR_SEGMENTATION_CNNSEG_UTIL_H_ + +#include +#include + +// project point cloud to 2d map. calc in which grid point is. +// pointcloud to pixel +inline int F2I(float val, float ori, float scale) +{ + return static_cast(std::floor((ori - val) * scale)); +} + +inline int Pc2Pixel(float in_pc, float in_range, float out_size) +{ + float inv_res = 0.5 * out_size / in_range; + return static_cast(std::floor((in_range - in_pc) * inv_res)); +} + +// return the distance from my car to center of the grid. +// Pc means point cloud = real world scale. so transform pixel scale to real +// world scale +inline float Pixel2Pc(int in_pixel, float in_size, float out_range) +{ + float res = 2.0 * out_range / in_size; + return out_range - (static_cast(in_pixel) + 0.5f) * res; +} + +#endif // MODULES_PERCEPTION_OBSTACLE_LIDAR_SEGMENTATION_CNNSEG_UTIL_H_ diff --git a/perception/lidar_apollo_instance_segmentation/launch/lidar_apollo_instance_segmentation.launch.xml b/perception/lidar_apollo_instance_segmentation/launch/lidar_apollo_instance_segmentation.launch.xml new file mode 100644 index 0000000000000..9aebe7a701a42 --- /dev/null +++ b/perception/lidar_apollo_instance_segmentation/launch/lidar_apollo_instance_segmentation.launch.xml @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/perception/lidar_apollo_instance_segmentation/lib/LICENSE b/perception/lidar_apollo_instance_segmentation/lib/LICENSE new file mode 100644 index 0000000000000..4527cae1a769f --- /dev/null +++ b/perception/lidar_apollo_instance_segmentation/lib/LICENSE @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2018 lewes6369 + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/perception/lidar_apollo_instance_segmentation/lib/README.md b/perception/lidar_apollo_instance_segmentation/lib/README.md new file mode 100644 index 0000000000000..38cb4cafd6c4c --- /dev/null +++ b/perception/lidar_apollo_instance_segmentation/lib/README.md @@ -0,0 +1,28 @@ +### Note + +This library is customized. +Original repository : + +```txt +MIT License + +Copyright (c) 2018 lewes6369 + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. +``` diff --git a/perception/lidar_apollo_instance_segmentation/lib/include/TrtNet.hpp b/perception/lidar_apollo_instance_segmentation/lib/include/TrtNet.hpp new file mode 100644 index 0000000000000..c4194a3f5c1e8 --- /dev/null +++ b/perception/lidar_apollo_instance_segmentation/lib/include/TrtNet.hpp @@ -0,0 +1,115 @@ +/* + * MIT License + * + * Copyright (c) 2018 lewes6369 + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ +#ifndef __TRT_NET_H_ +#define __TRT_NET_H_ + +#include "Utils.hpp" + +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace Tn +{ +enum class RUN_MODE { FLOAT32 = 0, FLOAT16 = 1, INT8 = 2 }; + +class trtNet +{ +public: + // Load from engine file + explicit trtNet(const std::string & engineFile); + + ~trtNet() + { + // Release the stream and the buffers + cudaStreamSynchronize(mTrtCudaStream); + cudaStreamDestroy(mTrtCudaStream); + for (auto & item : mTrtCudaBuffer) { + cudaFree(item); + } + + if (!mTrtRunTime) { + mTrtRunTime->destroy(); + } + if (!mTrtContext) { + mTrtContext->destroy(); + } + if (!mTrtEngine) { + mTrtEngine->destroy(); + } + } + + void saveEngine(std::string fileName) + { + if (mTrtEngine) { + nvinfer1::IHostMemory * data = mTrtEngine->serialize(); + std::ofstream file; + file.open(fileName, std::ios::binary | std::ios::out); + if (!file.is_open()) { + std::cout << "read create engine file" << fileName << " failed" << std::endl; + return; + } + + file.write((const char *)data->data(), data->size()); + file.close(); + } + } + + void doInference(const void * inputData, void * outputData); + + inline size_t getInputSize() + { + return std::accumulate( + mTrtBindBufferSize.begin(), mTrtBindBufferSize.begin() + mTrtInputCount, 0); + } + + inline size_t getOutputSize() + { + return std::accumulate( + mTrtBindBufferSize.begin() + mTrtInputCount, mTrtBindBufferSize.end(), 0); + } + +private: + void InitEngine(); + + nvinfer1::IExecutionContext * mTrtContext; + nvinfer1::ICudaEngine * mTrtEngine; + nvinfer1::IRuntime * mTrtRunTime; + cudaStream_t mTrtCudaStream; + Profiler mTrtProfiler; + RUN_MODE mTrtRunMode; + + std::vector mTrtCudaBuffer; + std::vector mTrtBindBufferSize; + int mTrtInputCount; +}; +} // namespace Tn + +#endif // __TRT_NET_H_ diff --git a/perception/lidar_apollo_instance_segmentation/lib/include/Utils.hpp b/perception/lidar_apollo_instance_segmentation/lib/include/Utils.hpp new file mode 100644 index 0000000000000..8813206b965b3 --- /dev/null +++ b/perception/lidar_apollo_instance_segmentation/lib/include/Utils.hpp @@ -0,0 +1,134 @@ +/* + * MIT License + * + * Copyright (c) 2018 lewes6369 + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#ifndef __TRT_UTILS_H_ +#define __TRT_UTILS_H_ + +#include +#include + +#include +#include +#include +#include +#include + +#ifndef CUDA_CHECK + +#define CUDA_CHECK(callstr) \ + { \ + cudaError_t error_code = callstr; \ + if (error_code != cudaSuccess) { \ + std::cerr << "CUDA error " << error_code << " at " << __FILE__ << ":" << __LINE__; \ + assert(0); \ + } \ + } + +#endif + +namespace Tn +{ +class Profiler : public nvinfer1::IProfiler +{ +public: + void printLayerTimes(int iterationsTimes) + { + float totalTime = 0; + for (size_t i = 0; i < mProfile.size(); i++) { + printf("%-40.40s %4.3fms\n", mProfile[i].first.c_str(), mProfile[i].second / iterationsTimes); + totalTime += mProfile[i].second; + } + printf("Time over all layers: %4.3f\n", totalTime / iterationsTimes); + } + +private: + typedef std::pair Record; + std::vector mProfile; + + void reportLayerTime(const char * layerName, float ms) noexcept override + { + auto record = std::find_if( + mProfile.begin(), mProfile.end(), [&](const Record & r) { return r.first == layerName; }); + if (record == mProfile.end()) { + mProfile.push_back(std::make_pair(layerName, ms)); + } else { + record->second += ms; + } + } +}; + +// Logger for TensorRT info/warning/errors +class Logger : public nvinfer1::ILogger +{ +public: + Logger() : Logger(Severity::kWARNING) {} + + explicit Logger(Severity severity) : reportableSeverity(severity) {} + + void log(Severity severity, const char * msg) noexcept override + { + // suppress messages with severity enum value greater than the reportable + if (severity > reportableSeverity) { + return; + } + + switch (severity) { + case Severity::kINTERNAL_ERROR: + std::cerr << "INTERNAL_ERROR: "; + break; + case Severity::kERROR: + std::cerr << "ERROR: "; + break; + case Severity::kWARNING: + std::cerr << "WARNING: "; + break; + case Severity::kINFO: + std::cerr << "INFO: "; + break; + default: + std::cerr << "UNKNOWN: "; + break; + } + std::cerr << msg << std::endl; + } + + Severity reportableSeverity{Severity::kWARNING}; +}; + +template +void write(char *& buffer, const T & val) +{ + *reinterpret_cast(buffer) = val; + buffer += sizeof(T); +} + +template +void read(const char *& buffer, T & val) +{ + val = *reinterpret_cast(buffer); + buffer += sizeof(T); +} +} // namespace Tn + +#endif diff --git a/perception/lidar_apollo_instance_segmentation/lib/src/TrtNet.cpp b/perception/lidar_apollo_instance_segmentation/lib/src/TrtNet.cpp new file mode 100644 index 0000000000000..c51adc83e07c1 --- /dev/null +++ b/perception/lidar_apollo_instance_segmentation/lib/src/TrtNet.cpp @@ -0,0 +1,152 @@ +/* + * MIT License + * + * Copyright (c) 2018 lewes6369 + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +using namespace nvinfer1; // NOLINT +using namespace nvcaffeparser1; // NOLINT +using namespace plugin; // NOLINT + +static Tn::Logger gLogger; + +inline void * safeCudaMalloc(size_t memSize) +{ + void * deviceMem; + CUDA_CHECK(cudaMalloc(&deviceMem, memSize)); + if (deviceMem == nullptr) { + std::cerr << "Out of memory" << std::endl; + exit(1); + } + return deviceMem; +} + +inline int64_t volume(const nvinfer1::Dims & d) +{ + return std::accumulate(d.d, d.d + d.nbDims, 1, std::multiplies()); +} + +inline unsigned int getElementSize(nvinfer1::DataType t) +{ + switch (t) { + case nvinfer1::DataType::kINT32: + return 4; + case nvinfer1::DataType::kFLOAT: + return 4; + case nvinfer1::DataType::kHALF: + return 2; + case nvinfer1::DataType::kINT8: + return 1; + default: + throw std::runtime_error("Invalid DataType."); + return 0; + } +} + +namespace Tn +{ +trtNet::trtNet(const std::string & engineFile) +: mTrtContext(nullptr), + mTrtEngine(nullptr), + mTrtRunTime(nullptr), + mTrtRunMode(RUN_MODE::FLOAT32), + mTrtInputCount(0) +{ + using namespace std; // NOLINT + fstream file; + + file.open(engineFile, ios::binary | ios::in); + if (!file.is_open()) { + cout << "read engine file" << engineFile << " failed" << endl; + return; + } + file.seekg(0, ios::end); + int length = file.tellg(); + file.seekg(0, ios::beg); + std::unique_ptr data(new char[length]); + file.read(data.get(), length); + file.close(); + + mTrtRunTime = createInferRuntime(gLogger); + assert(mTrtRunTime != nullptr); + mTrtEngine = mTrtRunTime->deserializeCudaEngine(data.get(), length, nullptr); + assert(mTrtEngine != nullptr); + + InitEngine(); +} + +void trtNet::InitEngine() +{ + const int maxBatchSize = 1; + mTrtContext = mTrtEngine->createExecutionContext(); + assert(mTrtContext != nullptr); + mTrtContext->setProfiler(&mTrtProfiler); + + int nbBindings = mTrtEngine->getNbBindings(); + + mTrtCudaBuffer.resize(nbBindings); + mTrtBindBufferSize.resize(nbBindings); + for (int i = 0; i < nbBindings; ++i) { + Dims dims = mTrtEngine->getBindingDimensions(i); + DataType dtype = mTrtEngine->getBindingDataType(i); + int64_t totalSize = volume(dims) * maxBatchSize * getElementSize(dtype); + mTrtBindBufferSize[i] = totalSize; + mTrtCudaBuffer[i] = safeCudaMalloc(totalSize); + if (mTrtEngine->bindingIsInput(i)) { + mTrtInputCount++; + } + } + + CUDA_CHECK(cudaStreamCreate(&mTrtCudaStream)); +} + +void trtNet::doInference(const void * inputData, void * outputData) +{ + static const int batchSize = 1; + assert(mTrtInputCount == 1); + + int inputIndex = 0; + CUDA_CHECK(cudaMemcpyAsync( + mTrtCudaBuffer[inputIndex], inputData, mTrtBindBufferSize[inputIndex], cudaMemcpyHostToDevice, + mTrtCudaStream)); + + mTrtContext->execute(batchSize, &mTrtCudaBuffer[inputIndex]); + + for (size_t bindingIdx = mTrtInputCount; bindingIdx < mTrtBindBufferSize.size(); ++bindingIdx) { + auto size = mTrtBindBufferSize[bindingIdx]; + CUDA_CHECK(cudaMemcpyAsync( + outputData, mTrtCudaBuffer[bindingIdx], size, cudaMemcpyDeviceToHost, mTrtCudaStream)); + } +} +} // namespace Tn diff --git a/perception/lidar_apollo_instance_segmentation/package.xml b/perception/lidar_apollo_instance_segmentation/package.xml new file mode 100755 index 0000000000000..faa637801cb06 --- /dev/null +++ b/perception/lidar_apollo_instance_segmentation/package.xml @@ -0,0 +1,31 @@ + + + lidar_apollo_instance_segmentation + 0.1.0 + lidar_apollo_instance_segmentation + + Yukihiro Saito + Kosuke Takeuchi + Yukihiro Saito + + Apache License 2.0 + + ament_cmake_auto + + autoware_auto_perception_msgs + autoware_perception_msgs + libpcl-all-dev + pcl_conversions + rclcpp + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + + ament_cmake_cppcheck + ament_lint_auto + + + ament_cmake + + diff --git a/perception/lidar_apollo_instance_segmentation/src/cluster2d.cpp b/perception/lidar_apollo_instance_segmentation/src/cluster2d.cpp new file mode 100644 index 0000000000000..53f3c751a6184 --- /dev/null +++ b/perception/lidar_apollo_instance_segmentation/src/cluster2d.cpp @@ -0,0 +1,372 @@ +// Copyright 2020 TierIV +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* + * Copyright 2018-2019 Autoware Foundation. All rights reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/****************************************************************************** + * Copyright 2017 The Apollo Authors. All Rights Reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + *****************************************************************************/ + +#include "lidar_apollo_instance_segmentation/cluster2d.hpp" + +#include +#include + +#include +#include + +geometry_msgs::msg::Quaternion getQuaternionFromRPY(const double r, const double p, const double y) +{ + tf2::Quaternion q; + q.setRPY(r, p, y); + return tf2::toMsg(q); +} + +Cluster2D::Cluster2D(const int rows, const int cols, const float range) +{ + rows_ = rows; + cols_ = cols; + siz_ = rows * cols; + range_ = range; + scale_ = 0.5 * static_cast(rows_) / range_; + inv_res_x_ = 0.5 * static_cast(cols_) / range_; + inv_res_y_ = 0.5 * static_cast(rows_) / range_; + point2grid_.clear(); + id_img_.assign(siz_, -1); + pc_ptr_.reset(); + valid_indices_in_pc_ = nullptr; +} + +void Cluster2D::traverse(Node * x) +{ + std::vector p; + p.clear(); + + while (x->traversed == 0) { + p.push_back(x); + x->traversed = 2; + x = x->center_node; + } + if (x->traversed == 2) { + for (int i = static_cast(p.size()) - 1; i >= 0 && p[i] != x; i--) { + p[i]->is_center = true; + } + x->is_center = true; + } + for (size_t i = 0; i < p.size(); i++) { + Node * y = p[i]; + y->traversed = 1; + y->parent = x->parent; + } +} + +void Cluster2D::cluster( + const std::shared_ptr & inferred_data, const pcl::PointCloud::Ptr & pc_ptr, + const pcl::PointIndices & valid_indices, float objectness_thresh, + bool use_all_grids_for_clustering) +{ + const float * category_pt_data = inferred_data.get(); + const float * instance_pt_x_data = inferred_data.get() + siz_; + const float * instance_pt_y_data = inferred_data.get() + siz_ * 2; + + pc_ptr_ = pc_ptr; + + std::vector> nodes(rows_, std::vector(cols_, Node())); + + valid_indices_in_pc_ = &(valid_indices.indices); + point2grid_.assign(valid_indices_in_pc_->size(), -1); + + for (size_t i = 0; i < valid_indices_in_pc_->size(); ++i) { + int point_id = valid_indices_in_pc_->at(i); + const auto & point = pc_ptr_->points[point_id]; + // * the coordinates of x and y have been exchanged in feature generation + // step, + // so we swap them back here. + int pos_x = F2I(point.y, range_, inv_res_x_); // col + int pos_y = F2I(point.x, range_, inv_res_y_); // row + if (IsValidRowCol(pos_y, pos_x)) { + point2grid_[i] = RowCol2Grid(pos_y, pos_x); + nodes[pos_y][pos_x].point_num++; + } + } + + for (int row = 0; row < rows_; ++row) { + for (int col = 0; col < cols_; ++col) { + int grid = RowCol2Grid(row, col); + Node * node = &nodes[row][col]; + DisjointSetMakeSet(node); + node->is_object = (use_all_grids_for_clustering || nodes[row][col].point_num > 0) && + (*(category_pt_data + grid) >= objectness_thresh); + int center_row = std::round(row + instance_pt_x_data[grid] * scale_); + int center_col = std::round(col + instance_pt_y_data[grid] * scale_); + center_row = std::min(std::max(center_row, 0), rows_ - 1); + center_col = std::min(std::max(center_col, 0), cols_ - 1); + node->center_node = &nodes[center_row][center_col]; + } + } + + for (int row = 0; row < rows_; ++row) { + for (int col = 0; col < cols_; ++col) { + Node * node = &nodes[row][col]; + if (node->is_object && node->traversed == 0) { + traverse(node); + } + } + } + + for (int row = 0; row < rows_; ++row) { + for (int col = 0; col < cols_; ++col) { + Node * node = &nodes[row][col]; + if (!node->is_center) { + continue; + } + for (int row2 = row - 1; row2 <= row + 1; ++row2) { + for (int col2 = col - 1; col2 <= col + 1; ++col2) { + if ((row2 == row || col2 == col) && IsValidRowCol(row2, col2)) { + Node * node2 = &nodes[row2][col2]; + if (node2->is_center) { + DisjointSetUnion(node, node2); + } + } + } + } + } + } + + int count_obstacles = 0; + obstacles_.clear(); + id_img_.assign(siz_, -1); + for (int row = 0; row < rows_; ++row) { + for (int col = 0; col < cols_; ++col) { + Node * node = &nodes[row][col]; + if (!node->is_object) { + continue; + } + Node * root = DisjointSetFind(node); + if (root->obstacle_id < 0) { + root->obstacle_id = count_obstacles++; + obstacles_.push_back(Obstacle()); + } + int grid = RowCol2Grid(row, col); + id_img_[grid] = root->obstacle_id; + obstacles_[root->obstacle_id].grids.push_back(grid); + } + } + filter(inferred_data); + classify(inferred_data); +} + +void Cluster2D::filter(const std::shared_ptr & inferred_data) +{ + const float * confidence_pt_data = inferred_data.get() + siz_ * 3; + const float * heading_pt_x_data = inferred_data.get() + siz_ * 9; + const float * heading_pt_y_data = inferred_data.get() + siz_ * 10; + const float * height_pt_data = inferred_data.get() + siz_ * 11; + + for (size_t obstacle_id = 0; obstacle_id < obstacles_.size(); obstacle_id++) { + Obstacle * obs = &obstacles_[obstacle_id]; + double score = 0.0; + double height = 0.0; + double vec_x = 0.0; + double vec_y = 0.0; + for (int grid : obs->grids) { + score += static_cast(confidence_pt_data[grid]); + height += static_cast(height_pt_data[grid]); + vec_x += heading_pt_x_data[grid]; + vec_y += heading_pt_y_data[grid]; + } + obs->score = score / static_cast(obs->grids.size()); + obs->height = height / static_cast(obs->grids.size()); + obs->heading = std::atan2(vec_y, vec_x) * 0.5; + obs->cloud_ptr.reset(new pcl::PointCloud); + } +} + +void Cluster2D::classify(const std::shared_ptr & inferred_data) +{ + const float * classify_pt_data = inferred_data.get() + siz_ * 4; + int num_classes = 5; + for (size_t obs_id = 0; obs_id < obstacles_.size(); obs_id++) { + Obstacle * obs = &obstacles_[obs_id]; + + for (size_t grid_id = 0; grid_id < obs->grids.size(); grid_id++) { + int grid = obs->grids[grid_id]; + for (int k = 0; k < num_classes; k++) { + obs->meta_type_probs[k] += classify_pt_data[k * siz_ + grid]; + } + } + int meta_type_id = 0; + for (int k = 0; k < num_classes; k++) { + obs->meta_type_probs[k] /= obs->grids.size(); + if (obs->meta_type_probs[k] > obs->meta_type_probs[meta_type_id]) { + meta_type_id = k; + } + } + obs->meta_type = static_cast(meta_type_id); + } +} + +autoware_perception_msgs::msg::DetectedObjectWithFeature Cluster2D::obstacleToObject( + const Obstacle & in_obstacle, const std_msgs::msg::Header & in_header) +{ + using autoware_auto_perception_msgs::msg::DetectedObjectKinematics; + using autoware_auto_perception_msgs::msg::ObjectClassification; + + autoware_perception_msgs::msg::DetectedObjectWithFeature resulting_object; + // pcl::PointCloud in_cluster = *(in_obstacle.cloud_ptr); + + resulting_object.object.classification.emplace_back( + autoware_auto_perception_msgs::build() + .label(ObjectClassification::UNKNOWN) + .probability(in_obstacle.score)); + if (in_obstacle.meta_type == MetaType::META_PEDESTRIAN) { + resulting_object.object.classification.front().label = ObjectClassification::PEDESTRIAN; + } else if (in_obstacle.meta_type == MetaType::META_NONMOT) { + resulting_object.object.classification.front().label = ObjectClassification::MOTORCYCLE; + } else if (in_obstacle.meta_type == MetaType::META_SMALLMOT) { + resulting_object.object.classification.front().label = ObjectClassification::CAR; + } else if (in_obstacle.meta_type == MetaType::META_BIGMOT) { + resulting_object.object.classification.front().label = ObjectClassification::BUS; + } else { + // resulting_object.object.classification.front().label = ObjectClassification::PEDESTRIAN; + resulting_object.object.classification.front().label = ObjectClassification::UNKNOWN; + } + + pcl::PointXYZ min_point; + pcl::PointXYZ max_point; + for (auto pit = in_obstacle.cloud_ptr->points.begin(); pit != in_obstacle.cloud_ptr->points.end(); + ++pit) { + if (pit->x < min_point.x) { + min_point.x = pit->x; + } + if (pit->y < min_point.y) { + min_point.y = pit->y; + } + if (pit->z < min_point.z) { + min_point.z = pit->z; + } + if (pit->x > max_point.x) { + max_point.x = pit->x; + } + if (pit->y > max_point.y) { + max_point.y = pit->y; + } + if (pit->z > max_point.z) { + max_point.z = pit->z; + } + } + + // cluster and ground filtering + pcl::PointCloud cluster; + const float min_height = min_point.z + ((max_point.z - min_point.z) * 0.1f); + for (auto pit = in_obstacle.cloud_ptr->points.begin(); pit != in_obstacle.cloud_ptr->points.end(); + ++pit) { + if (min_height < pit->z) { + cluster.points.push_back(*pit); + } + } + min_point.z = 0.0; + max_point.z = 0.0; + for (auto pit = cluster.points.begin(); pit != cluster.points.end(); ++pit) { + if (pit->z < min_point.z) { + min_point.z = pit->z; + } + if (pit->z > max_point.z) { + max_point.z = pit->z; + } + } + sensor_msgs::msg::PointCloud2 ros_pc; + pcl::toROSMsg(cluster, ros_pc); + resulting_object.feature.cluster = ros_pc; + resulting_object.feature.cluster.header = in_header; + + // position + const float height = max_point.z - min_point.z; + const float length = max_point.x - min_point.x; + const float width = max_point.y - min_point.y; + resulting_object.object.kinematics.pose_with_covariance.pose.position.x = + min_point.x + length / 2; + resulting_object.object.kinematics.pose_with_covariance.pose.position.y = min_point.y + width / 2; + resulting_object.object.kinematics.pose_with_covariance.pose.position.z = + min_point.z + height / 2; + + resulting_object.object.kinematics.pose_with_covariance.pose.orientation = + getQuaternionFromRPY(0.0, 0.0, in_obstacle.heading); + resulting_object.object.kinematics.orientation_availability = + DetectedObjectKinematics::SIGN_UNKNOWN; + + return resulting_object; +} + +void Cluster2D::getObjects( + const float confidence_thresh, const float height_thresh, const int min_pts_num, + autoware_perception_msgs::msg::DetectedObjectsWithFeature & objects, + const std_msgs::msg::Header & in_header) +{ + for (size_t i = 0; i < point2grid_.size(); ++i) { + int grid = point2grid_[i]; + if (grid < 0) { + continue; + } + + int obstacle_id = id_img_[grid]; + + int point_id = valid_indices_in_pc_->at(i); + + if (obstacle_id >= 0 && obstacles_[obstacle_id].score >= confidence_thresh) { + if ( + height_thresh < 0 || + pc_ptr_->points[point_id].z <= obstacles_[obstacle_id].height + height_thresh) { + obstacles_[obstacle_id].cloud_ptr->push_back(pc_ptr_->points[point_id]); + } + } + } + + for (size_t obstacle_id = 0; obstacle_id < obstacles_.size(); obstacle_id++) { + Obstacle * obs = &obstacles_[obstacle_id]; + if (static_cast(obs->cloud_ptr->size()) < min_pts_num) { + continue; + } + autoware_perception_msgs::msg::DetectedObjectWithFeature out_obj = + obstacleToObject(*obs, in_header); + objects.feature_objects.push_back(out_obj); + } + objects.header = in_header; +} diff --git a/perception/lidar_apollo_instance_segmentation/src/debugger.cpp b/perception/lidar_apollo_instance_segmentation/src/debugger.cpp new file mode 100644 index 0000000000000..f1d2616628332 --- /dev/null +++ b/perception/lidar_apollo_instance_segmentation/src/debugger.cpp @@ -0,0 +1,100 @@ +// Copyright 2020 TierIV +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "lidar_apollo_instance_segmentation/debugger.hpp" + +#include +#include + +#include +#include +#include + +Debugger::Debugger(rclcpp::Node * node) +{ + instance_pointcloud_pub_ = + node->create_publisher("debug/instance_pointcloud", 1); +} + +void Debugger::publishColoredPointCloud( + const autoware_perception_msgs::msg::DetectedObjectsWithFeature & input) +{ + using autoware_auto_perception_msgs::msg::ObjectClassification; + pcl::PointCloud colored_pointcloud; + for (size_t i = 0; i < input.feature_objects.size(); i++) { + pcl::PointCloud object_pointcloud; + pcl::fromROSMsg(input.feature_objects.at(i).feature.cluster, object_pointcloud); + + int red = 0, green = 0, blue = 0; + switch (input.feature_objects.at(i).object.classification.front().label) { + case ObjectClassification::CAR: { + red = 255; + green = 0; + blue = 0; + break; + } + case ObjectClassification::TRUCK: { + red = 255; + green = 127; + blue = 0; + break; + } + case ObjectClassification::BUS: { + red = 255; + green = 0; + blue = 127; + break; + } + case ObjectClassification::PEDESTRIAN: { + red = 0; + green = 255; + blue = 0; + break; + } + case ObjectClassification::BICYCLE: { + red = 0; + green = 0; + blue = 255; + break; + } + case ObjectClassification::MOTORCYCLE: { + red = 0; + green = 127; + blue = 255; + break; + } + case ObjectClassification::UNKNOWN: { + red = 255; + green = 255; + blue = 255; + break; + } + } + + for (size_t i = 0; i < object_pointcloud.size(); i++) { + pcl::PointXYZRGB colored_point; + colored_point.x = object_pointcloud[i].x; + colored_point.y = object_pointcloud[i].y; + colored_point.z = object_pointcloud[i].z; + colored_point.r = red; + colored_point.g = green; + colored_point.b = blue; + colored_pointcloud.push_back(colored_point); + } + } + sensor_msgs::msg::PointCloud2 output_msg; + pcl::toROSMsg(colored_pointcloud, output_msg); + output_msg.header = input.header; + instance_pointcloud_pub_->publish(output_msg); +} diff --git a/perception/lidar_apollo_instance_segmentation/src/detector.cpp b/perception/lidar_apollo_instance_segmentation/src/detector.cpp new file mode 100644 index 0000000000000..ab42a2381d3c6 --- /dev/null +++ b/perception/lidar_apollo_instance_segmentation/src/detector.cpp @@ -0,0 +1,167 @@ +// Copyright 2020 TierIV +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "lidar_apollo_instance_segmentation/detector.hpp" + +#include "lidar_apollo_instance_segmentation/feature_map.hpp" + +#include +#include +#include + +LidarApolloInstanceSegmentation::LidarApolloInstanceSegmentation(rclcpp::Node * node) +: node_(node), tf_buffer_(node_->get_clock()), tf_listener_(tf_buffer_) +{ + int range, width, height; + bool use_intensity_feature, use_constant_feature; + std::string engine_file; + std::string prototxt_file; + std::string caffemodel_file; + score_threshold_ = node_->declare_parameter("score_threshold", 0.8); + range = node_->declare_parameter("range", 60); + width = node_->declare_parameter("width", 640); + height = node_->declare_parameter("height", 640); + engine_file = node_->declare_parameter("engine_file", "vls-128.engine"); + prototxt_file = node_->declare_parameter("prototxt_file", "vls-128.prototxt"); + caffemodel_file = node_->declare_parameter("caffemodel_file", "vls-128.caffemodel"); + use_intensity_feature = node_->declare_parameter("use_intensity_feature", true); + use_constant_feature = node_->declare_parameter("use_constant_feature", true); + target_frame_ = node_->declare_parameter("target_frame", "base_link"); + z_offset_ = node_->declare_parameter("z_offset", 2); + + // load weight file + std::ifstream fs(engine_file); + if (!fs.is_open()) { + RCLCPP_INFO( + node_->get_logger(), + "Could not find %s. try making TensorRT engine from caffemodel and prototxt", + engine_file.c_str()); + Tn::Logger logger; + nvinfer1::IBuilder * builder = nvinfer1::createInferBuilder(logger); + nvinfer1::INetworkDefinition * network = builder->createNetworkV2(0U); + nvcaffeparser1::ICaffeParser * parser = nvcaffeparser1::createCaffeParser(); + nvinfer1::IBuilderConfig * config = builder->createBuilderConfig(); + const nvcaffeparser1::IBlobNameToTensor * blob_name2tensor = parser->parse( + prototxt_file.c_str(), caffemodel_file.c_str(), *network, nvinfer1::DataType::kFLOAT); + std::string output_node = "deconv0"; + auto output = blob_name2tensor->find(output_node.c_str()); + if (output == nullptr) { + RCLCPP_ERROR(node_->get_logger(), "can not find output named %s", output_node.c_str()); + } + network->markOutput(*output); + const int batch_size = 1; + builder->setMaxBatchSize(batch_size); + config->setMaxWorkspaceSize(1 << 30); + nvinfer1::ICudaEngine * engine = builder->buildEngineWithConfig(*network, *config); + nvinfer1::IHostMemory * trt_model_stream = engine->serialize(); + assert(trt_model_stream != nullptr); + std::ofstream outfile(engine_file, std::ofstream::binary); + assert(!outfile.fail()); + outfile.write(reinterpret_cast(trt_model_stream->data()), trt_model_stream->size()); + outfile.close(); + network->destroy(); + parser->destroy(); + builder->destroy(); + config->destroy(); + } + net_ptr_.reset(new Tn::trtNet(engine_file)); + + // feature map generator: pre process + feature_generator_ = std::make_shared( + width, height, range, use_intensity_feature, use_constant_feature); + + // cluster: post process + cluster2d_ = std::make_shared(width, height, range); +} + +bool LidarApolloInstanceSegmentation::transformCloud( + const sensor_msgs::msg::PointCloud2 & input, sensor_msgs::msg::PointCloud2 & transformed_cloud, + float z_offset) +{ + // TODO(mitsudome-r): remove conversion once pcl_ros transform are available. + pcl::PointCloud pcl_input, pcl_transformed_cloud; + pcl::fromROSMsg(input, pcl_input); + + // transform pointcloud to target_frame + if (target_frame_ != input.header.frame_id) { + try { + geometry_msgs::msg::TransformStamped transform_stamped; + transform_stamped = tf_buffer_.lookupTransform( + target_frame_, input.header.frame_id, input.header.stamp, std::chrono::milliseconds(500)); + Eigen::Matrix4f affine_matrix = + tf2::transformToEigen(transform_stamped.transform).matrix().cast(); + pcl::transformPointCloud(pcl_input, pcl_transformed_cloud, affine_matrix); + transformed_cloud.header.frame_id = target_frame_; + } catch (tf2::TransformException & ex) { + RCLCPP_WARN(node_->get_logger(), "%s", ex.what()); + return false; + } + } else { + pcl_transformed_cloud = pcl_input; + } + + // move pointcloud z_offset in z axis + pcl::PointCloud pointcloud_with_z_offset; + Eigen::Affine3f z_up_translation(Eigen::Translation3f(0, 0, z_offset)); + Eigen::Matrix4f z_up_transform = z_up_translation.matrix(); + pcl::transformPointCloud(pcl_transformed_cloud, pcl_transformed_cloud, z_up_transform); + + pcl::toROSMsg(pcl_transformed_cloud, transformed_cloud); + + return true; +} + +bool LidarApolloInstanceSegmentation::detectDynamicObjects( + const sensor_msgs::msg::PointCloud2 & input, + autoware_perception_msgs::msg::DetectedObjectsWithFeature & output) +{ + // move up pointcloud z_offset in z axis + sensor_msgs::msg::PointCloud2 transformed_cloud; + transformCloud(input, transformed_cloud, z_offset_); + + // convert from ros to pcl + pcl::PointCloud::Ptr pcl_pointcloud_raw_ptr(new pcl::PointCloud); + pcl::fromROSMsg(transformed_cloud, *pcl_pointcloud_raw_ptr); + + // generate feature map + std::shared_ptr feature_map_ptr = + feature_generator_->generate(pcl_pointcloud_raw_ptr); + + // inference + std::shared_ptr inferred_data(new float[net_ptr_->getOutputSize() / sizeof(float)]); + net_ptr_->doInference(feature_map_ptr->map_data.data(), inferred_data.get()); + + // post process + const float objectness_thresh = 0.5; + pcl::PointIndices valid_idx; + valid_idx.indices.resize(pcl_pointcloud_raw_ptr->size()); + std::iota(valid_idx.indices.begin(), valid_idx.indices.end(), 0); + cluster2d_->cluster( + inferred_data, pcl_pointcloud_raw_ptr, valid_idx, objectness_thresh, + true /*use all grids for clustering*/); + const float height_thresh = 0.5; + const int min_pts_num = 3; + cluster2d_->getObjects( + score_threshold_, height_thresh, min_pts_num, output, transformed_cloud.header); + + // move down pointcloud z_offset in z axis + for (std::size_t i = 0; i < output.feature_objects.size(); i++) { + sensor_msgs::msg::PointCloud2 transformed_cloud; + transformCloud(output.feature_objects.at(i).feature.cluster, transformed_cloud, -z_offset_); + output.feature_objects.at(i).feature.cluster = transformed_cloud; + } + + output.header = transformed_cloud.header; + return true; +} diff --git a/perception/lidar_apollo_instance_segmentation/src/feature_generator.cpp b/perception/lidar_apollo_instance_segmentation/src/feature_generator.cpp new file mode 100644 index 0000000000000..5a4b4a6b79f6c --- /dev/null +++ b/perception/lidar_apollo_instance_segmentation/src/feature_generator.cpp @@ -0,0 +1,95 @@ +// Copyright 2020 TierIV +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "lidar_apollo_instance_segmentation/feature_generator.hpp" + +#include "lidar_apollo_instance_segmentation/log_table.hpp" + +namespace +{ +inline float normalizeIntensity(float intensity) { return intensity / 255.0f; } +} // namespace + +FeatureGenerator::FeatureGenerator( + const int width, const int height, const int range, const bool use_intensity_feature, + const bool use_constant_feature) +: min_height_(-5.0), + max_height_(5.0), + use_intensity_feature_(use_intensity_feature), + use_constant_feature_(use_constant_feature) +{ + // select feature map type + if (use_constant_feature && use_intensity_feature) { + map_ptr_ = std::make_shared(width, height, range); + } else if (use_constant_feature) { + map_ptr_ = std::make_shared(width, height, range); + } else if (use_intensity_feature) { + map_ptr_ = std::make_shared(width, height, range); + } else { + map_ptr_ = std::make_shared(width, height, range); + } + map_ptr_->initializeMap(map_ptr_->map_data); +} + +std::shared_ptr FeatureGenerator::generate( + const pcl::PointCloud::Ptr & pc_ptr) +{ + const double epsilon = 1e-6; + map_ptr_->resetMap(map_ptr_->map_data); + + int size = map_ptr_->height * map_ptr_->width; + + const float inv_res_x = 0.5 * map_ptr_->width / map_ptr_->range; + const float inv_res_y = 0.5 * map_ptr_->height / map_ptr_->range; + + for (size_t i = 0; i < pc_ptr->points.size(); ++i) { + if (pc_ptr->points[i].z <= min_height_ || max_height_ <= pc_ptr->points[i].z) { + continue; + } + + const int pos_x = std::floor((map_ptr_->range - pc_ptr->points[i].y) * inv_res_x); // x on grid + const int pos_y = std::floor((map_ptr_->range - pc_ptr->points[i].x) * inv_res_y); // y on grid + if (pos_x < 0 || map_ptr_->width <= pos_x || pos_y < 0 || map_ptr_->height <= pos_y) { + continue; + } + + const int idx = pos_y * map_ptr_->width + pos_x; + + if (map_ptr_->max_height_data[idx] < pc_ptr->points[i].z) { + map_ptr_->max_height_data[idx] = pc_ptr->points[i].z; + if (map_ptr_->top_intensity_data != nullptr) { + map_ptr_->top_intensity_data[idx] = normalizeIntensity(pc_ptr->points[i].intensity); + } + } + map_ptr_->mean_height_data[idx] += static_cast(pc_ptr->points[i].z); + if (map_ptr_->mean_intensity_data != nullptr) { + map_ptr_->mean_intensity_data[idx] += normalizeIntensity(pc_ptr->points[i].intensity); + } + map_ptr_->count_data[idx] += 1.0f; + } + + for (int i = 0; i < size; ++i) { + if (map_ptr_->count_data[i] < epsilon) { + map_ptr_->max_height_data[i] = 0.0f; + } else { + map_ptr_->mean_height_data[i] /= map_ptr_->count_data[i]; + if (map_ptr_->mean_intensity_data != nullptr) { + map_ptr_->mean_intensity_data[i] /= map_ptr_->count_data[i]; + } + map_ptr_->nonempty_data[i] = 1.0f; + } + map_ptr_->count_data[i] = calcApproximateLog(map_ptr_->count_data[i]); + } + return map_ptr_; +} diff --git a/perception/lidar_apollo_instance_segmentation/src/feature_map.cpp b/perception/lidar_apollo_instance_segmentation/src/feature_map.cpp new file mode 100644 index 0000000000000..63cb84c8bfe9a --- /dev/null +++ b/perception/lidar_apollo_instance_segmentation/src/feature_map.cpp @@ -0,0 +1,164 @@ +// Copyright 2020 TierIV +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "lidar_apollo_instance_segmentation/feature_map.hpp" + +#include "lidar_apollo_instance_segmentation/util.hpp" + +#include + +FeatureMapInterface::FeatureMapInterface( + const int _channels, const int _width, const int _height, const int _range) +: channels(_channels), + width(_width), + height(_height), + range(_range), + max_height_data(nullptr), + mean_height_data(nullptr), + count_data(nullptr), + direction_data(nullptr), + top_intensity_data(nullptr), + mean_intensity_data(nullptr), + distance_data(nullptr), + nonempty_data(nullptr) +{ + map_data.resize(width * height * channels); +} + +FeatureMap::FeatureMap(const int width, const int height, const int range) +: FeatureMapInterface(4, width, height, range) +{ + max_height_data = &(map_data[0]) + width * height * 0; + mean_height_data = &(map_data[0]) + width * height * 1; + count_data = &(map_data[0]) + width * height * 2; + nonempty_data = &(map_data[0]) + width * height * 3; +} +void FeatureMap::initializeMap([[maybe_unused]] std::vector & map) {} +void FeatureMap::resetMap([[maybe_unused]] std::vector & map) +{ + const int size = width * height; + for (int i = 0; i < size; ++i) { + max_height_data[i] = -5.0f; + mean_height_data[i] = 0.0f; + count_data[i] = 0.0f; + nonempty_data[i] = 0.0f; + } +} + +FeatureMapWithIntensity::FeatureMapWithIntensity(const int width, const int height, const int range) +: FeatureMapInterface(6, width, height, range) +{ + max_height_data = &(map_data[0]) + width * height * 0; + mean_height_data = &(map_data[0]) + width * height * 1; + count_data = &(map_data[0]) + width * height * 2; + top_intensity_data = &(map_data[0]) + width * height * 3; + mean_intensity_data = &(map_data[0]) + width * height * 4; + nonempty_data = &(map_data[0]) + width * height * 5; +} +void FeatureMapWithIntensity::initializeMap([[maybe_unused]] std::vector & map) {} +void FeatureMapWithIntensity::resetMap([[maybe_unused]] std::vector & map) +{ + const int size = width * height; + for (int i = 0; i < size; ++i) { + max_height_data[i] = -5.0f; + mean_height_data[i] = 0.0f; + count_data[i] = 0.0f; + top_intensity_data[i] = 0.0f; + mean_intensity_data[i] = 0.0f; + nonempty_data[i] = 0.0f; + } +} + +FeatureMapWithConstant::FeatureMapWithConstant(const int width, const int height, const int range) +: FeatureMapInterface(6, width, height, range) +{ + max_height_data = &(map_data[0]) + width * height * 0; + mean_height_data = &(map_data[0]) + width * height * 1; + count_data = &(map_data[0]) + width * height * 2; + direction_data = &(map_data[0]) + width * height * 3; + distance_data = &(map_data[0]) + width * height * 4; + nonempty_data = &(map_data[0]) + width * height * 5; +} +void FeatureMapWithConstant::initializeMap([[maybe_unused]] std::vector & map) +{ + for (int row = 0; row < height; ++row) { + for (int col = 0; col < width; ++col) { + int idx = row * width + col; + // * row <-> x, column <-> y + // return the distance from my car to center of the grid. + // Pc means point cloud = real world scale. so transform pixel scale to + // real world scale + float center_x = Pixel2Pc(row, height, range); + float center_y = Pixel2Pc(col, width, range); + // normalization. -0.5~0.5 + direction_data[idx] = static_cast(std::atan2(center_y, center_x) / (2.0 * M_PI)); + distance_data[idx] = static_cast(std::hypot(center_x, center_y) / 60.0 - 0.5); + } + } +} + +void FeatureMapWithConstant::resetMap([[maybe_unused]] std::vector & map) +{ + const int size = width * height; + for (int i = 0; i < size; ++i) { + max_height_data[i] = -5.0f; + mean_height_data[i] = 0.0f; + count_data[i] = 0.0f; + nonempty_data[i] = 0.0f; + } +} + +FeatureMapWithConstantAndIntensity::FeatureMapWithConstantAndIntensity( + const int width, const int height, const int range) +: FeatureMapInterface(8, width, height, range) +{ + max_height_data = &(map_data[0]) + width * height * 0; + mean_height_data = &(map_data[0]) + width * height * 1; + count_data = &(map_data[0]) + width * height * 2; + direction_data = &(map_data[0]) + width * height * 3; + top_intensity_data = &(map_data[0]) + width * height * 4; + mean_intensity_data = &(map_data[0]) + width * height * 5; + distance_data = &(map_data[0]) + width * height * 6; + nonempty_data = &(map_data[0]) + width * height * 7; +} +void FeatureMapWithConstantAndIntensity::initializeMap([[maybe_unused]] std::vector & map) +{ + for (int row = 0; row < height; ++row) { + for (int col = 0; col < width; ++col) { + int idx = row * width + col; + // * row <-> x, column <-> y + // return the distance from my car to center of the grid. + // Pc means point cloud = real world scale. so transform pixel scale to + // real world scale + float center_x = Pixel2Pc(row, height, range); + float center_y = Pixel2Pc(col, width, range); + // normalization. -0.5~0.5 + direction_data[idx] = static_cast(std::atan2(center_y, center_x) / (2.0 * M_PI)); + distance_data[idx] = static_cast(std::hypot(center_x, center_y) / 60.0 - 0.5); + } + } +} + +void FeatureMapWithConstantAndIntensity::resetMap([[maybe_unused]] std::vector & map) +{ + const int size = width * height; + for (int i = 0; i < size; ++i) { + max_height_data[i] = -5.0f; + mean_height_data[i] = 0.0f; + count_data[i] = 0.0f; + top_intensity_data[i] = 0.0f; + mean_intensity_data[i] = 0.0f; + nonempty_data[i] = 0.0f; + } +} diff --git a/perception/lidar_apollo_instance_segmentation/src/log_table.cpp b/perception/lidar_apollo_instance_segmentation/src/log_table.cpp new file mode 100644 index 0000000000000..fa5006f3090d4 --- /dev/null +++ b/perception/lidar_apollo_instance_segmentation/src/log_table.cpp @@ -0,0 +1,45 @@ +// Copyright 2020 TierIV +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "lidar_apollo_instance_segmentation/log_table.hpp" + +#include +#include +#include + +namespace +{ +struct LogTable +{ + std::vector data; + LogTable() + { + data.resize(256 * 10); + for (size_t i = 0; i < data.size(); ++i) { + data[i] = std::log1p(static_cast(i / 10.0)); + } + } +}; +} // namespace + +static ::LogTable log_table; + +float calcApproximateLog(float num) +{ + int integer_num = static_cast(num * 10.0); + if (integer_num < static_cast(log_table.data.size())) { + return log_table.data[integer_num]; + } + return std::log(static_cast(1.0 + num)); +} diff --git a/perception/lidar_apollo_instance_segmentation/src/node.cpp b/perception/lidar_apollo_instance_segmentation/src/node.cpp new file mode 100644 index 0000000000000..3715748856d98 --- /dev/null +++ b/perception/lidar_apollo_instance_segmentation/src/node.cpp @@ -0,0 +1,44 @@ +// Copyright 2020 TierIV +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "lidar_apollo_instance_segmentation/node.hpp" + +#include "lidar_apollo_instance_segmentation/detector.hpp" + +LidarInstanceSegmentationNode::LidarInstanceSegmentationNode( + const rclcpp::NodeOptions & node_options) +: Node("lidar_apollo_instance_segmentation_node", node_options) +{ + using std::placeholders::_1; + detector_ptr_ = std::make_shared(this); + debugger_ptr_ = std::make_shared(this); + pointcloud_sub_ = this->create_subscription( + "input/pointcloud", rclcpp::SensorDataQoS().keep_last(1), + std::bind(&LidarInstanceSegmentationNode::pointCloudCallback, this, _1)); + dynamic_objects_pub_ = + this->create_publisher( + "output/labeled_clusters", rclcpp::QoS{1}); +} + +void LidarInstanceSegmentationNode::pointCloudCallback( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) +{ + autoware_perception_msgs::msg::DetectedObjectsWithFeature output_msg; + detector_ptr_->detectDynamicObjects(*msg, output_msg); + dynamic_objects_pub_->publish(output_msg); + debugger_ptr_->publishColoredPointCloud(output_msg); +} + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(LidarInstanceSegmentationNode)