├── .vscode └── settings.json ├── fidget_dataset └── model_meshes │ ├── table1.ply │ ├── stage_1.ply │ ├── stage_2.ply │ ├── stage_3.ply │ ├── stage_4.ply │ ├── left_gear.ply │ ├── right_gear.ply │ ├── top_casing.ply │ └── bottom_casing.ply ├── data_generation ├── models │ ├── Nema │ │ ├── cover │ │ │ ├── cover.ply │ │ │ ├── istockphoto-476817442-170667a.jpg │ │ │ ├── model.config │ │ │ └── model.sdf │ │ ├── planet │ │ │ ├── planet.ply │ │ │ ├── istockphoto-476817442-170667a.jpg │ │ │ ├── model.config │ │ │ └── model.sdf │ │ ├── carrier │ │ │ ├── carrier.ply │ │ │ ├── istockphoto-476817442-170667a.jpg │ │ │ ├── model.config │ │ │ └── model.sdf │ │ ├── housing │ │ │ ├── housing.ply │ │ │ ├── istockphoto-476817442-170667a.jpg │ │ │ ├── model.config │ │ │ └── model.sdf │ │ ├── sun_gear │ │ │ ├── sun_gear.ply │ │ │ ├── istockphoto-476817442-170667a.jpg │ │ │ ├── model.config │ │ │ └── model.sdf │ │ └── Nema17 │ │ │ ├── istockphoto-476817442-170667a.jpg │ │ │ ├── istockphoto-476817442-170667a.jpg.001.jpg │ │ │ ├── model.config │ │ │ └── model.sdf │ ├── table │ │ ├── meshes │ │ │ ├── table.stl │ │ │ └── SF0202S.stl │ │ ├── model.config │ │ └── model.sdf │ ├── Planetary │ │ ├── case │ │ │ ├── case.ply │ │ │ ├── istockphoto-476817442-170667a.jpg │ │ │ ├── model.config │ │ │ └── model.sdf │ │ ├── cover │ │ │ ├── cover.ply │ │ │ ├── istockphoto-476817442-170667a.jpg │ │ │ ├── model.config │ │ │ └── model.sdf │ │ ├── gear_0 │ │ │ ├── gear_0.ply │ │ │ ├── istockphoto-476817442-170667a.jpg │ │ │ ├── model.config │ │ │ └── model.sdf │ │ ├── gear_1 │ │ │ ├── gear_1.ply │ │ │ ├── istockphoto-476817442-170667a.jpg │ │ │ ├── model.config │ │ │ └── model.sdf │ │ ├── gear_carrier │ │ │ ├── gear_carrier.ply │ │ │ ├── istockphoto-476817442-170667a.jpg │ │ │ ├── model.config │ │ │ └── model.sdf │ │ └── gear_1_g_testing │ │ │ ├── gear_1_g_testing.ply │ │ │ ├── istockphoto-476817442-170667a.jpg │ │ │ ├── model.config │ │ │ └── model.sdf │ ├── table1 │ │ ├── meshes │ │ │ └── table.ply │ │ ├── model.config │ │ └── model.sdf │ ├── arm_part │ │ ├── materials │ │ │ ├── textures │ │ │ │ └── parts.png │ │ │ └── scripts │ │ │ │ └── arm_part.material │ │ ├── model.config │ │ └── model.sdf │ ├── coke_can │ │ ├── materials │ │ │ └── textures │ │ │ │ └── coke_can.png │ │ ├── model.config │ │ ├── model-1_3.sdf │ │ ├── model-1_2.sdf │ │ ├── model.sdf │ │ └── model-1_4.sdf │ ├── extra │ │ ├── Fidget2 │ │ │ ├── left_gear │ │ │ │ ├── meshes │ │ │ │ │ └── 1290039926.png │ │ │ │ ├── model.config │ │ │ │ └── model.sdf │ │ │ ├── right_gear │ │ │ │ ├── meshes │ │ │ │ │ └── 1290039926.png │ │ │ │ ├── model.config │ │ │ │ └── model.sdf │ │ │ ├── top_casing │ │ │ │ ├── meshes │ │ │ │ │ └── 1290039926.png │ │ │ │ ├── model.config │ │ │ │ └── model.sdf │ │ │ └── bottom_casing │ │ │ │ ├── meshes │ │ │ │ └── 1899226145.png │ │ │ │ ├── model.config │ │ │ │ └── model.sdf │ │ ├── fidget_old_set │ │ │ ├── left_gear │ │ │ │ ├── meshes │ │ │ │ │ ├── fidget.jpeg │ │ │ │ │ └── left_gear.stl │ │ │ │ ├── materials │ │ │ │ │ └── textures │ │ │ │ │ │ └── parts.png │ │ │ │ ├── model.config │ │ │ │ └── model.sdf │ │ │ ├── right_gear │ │ │ │ ├── meshes │ │ │ │ │ ├── fidget.jpeg │ │ │ │ │ └── right_gear.stl │ │ │ │ ├── model.config │ │ │ │ └── model.sdf │ │ │ ├── top_casing │ │ │ │ ├── meshes │ │ │ │ │ ├── fidget.jpeg │ │ │ │ │ └── top_casing.stl │ │ │ │ ├── model.config │ │ │ │ └── model.sdf │ │ │ └── bottom_casing │ │ │ │ ├── meshes │ │ │ │ ├── fidget.jpeg │ │ │ │ └── bottom_casing.stl │ │ │ │ ├── model.config │ │ │ │ └── model.sdf │ │ ├── left_gear │ │ │ ├── model.config │ │ │ └── model.sdf │ │ ├── right_gear │ │ │ ├── model.config │ │ │ └── model.sdf │ │ ├── top_casing │ │ │ ├── model.config │ │ │ └── model.sdf │ │ └── bottom_casing │ │ │ ├── model.config │ │ │ └── model.sdf │ └── Fidget │ │ ├── left_gear │ │ ├── meshes │ │ │ └── istockphoto-476817442-170667a.jpg │ │ ├── model.config │ │ └── model.sdf │ │ ├── right_gear │ │ ├── meshes │ │ │ └── istockphoto-476817442-170667a.jpg │ │ ├── model.config │ │ └── model.sdf │ │ ├── top_casing │ │ ├── meshes │ │ │ └── istockphoto-476817442-170667a.jpg │ │ ├── model.config │ │ └── model.sdf │ │ └── bottom_casing │ │ ├── meshes │ │ └── istockphoto-476817442-170667a.jpg │ │ ├── model.config │ │ └── model.sdf ├── launch │ ├── nema17.launch │ ├── fidget.launch │ └── planetary.launch ├── package.xml ├── Worlds │ ├── planetary.world │ ├── metrics.world │ ├── Nema17.world │ ├── metrics2.world │ └── fidget_world.sdf └── CMakeLists.txt ├── Nema17_reducer_dataset └── model_meshes │ ├── cover.ply │ ├── Nema17.ply │ ├── carrier.ply │ ├── housing.ply │ ├── planet.ply │ ├── stage_1.ply │ ├── stage_2.ply │ ├── stage_3.ply │ ├── stage_4.ply │ ├── sun_gear.ply │ └── table1.ply ├── 6DApose.repos ├── README.md ├── icp_utils.py ├── Fidget_Ground_truth_preprocess.ipynb ├── Nema17_dataset_GT_preprocess.ipynb ├── echo_tfs.py ├── 6DAPose.py ├── requirements.txt └── render_icp_data.py /.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "ros.distro": "humble" 3 | } -------------------------------------------------------------------------------- /fidget_dataset/model_meshes/table1.ply: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/fidget_dataset/model_meshes/table1.ply -------------------------------------------------------------------------------- /fidget_dataset/model_meshes/stage_1.ply: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/fidget_dataset/model_meshes/stage_1.ply -------------------------------------------------------------------------------- /fidget_dataset/model_meshes/stage_2.ply: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/fidget_dataset/model_meshes/stage_2.ply -------------------------------------------------------------------------------- /fidget_dataset/model_meshes/stage_3.ply: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/fidget_dataset/model_meshes/stage_3.ply -------------------------------------------------------------------------------- /fidget_dataset/model_meshes/stage_4.ply: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/fidget_dataset/model_meshes/stage_4.ply -------------------------------------------------------------------------------- /data_generation/models/Nema/cover/cover.ply: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/data_generation/models/Nema/cover/cover.ply -------------------------------------------------------------------------------- /fidget_dataset/model_meshes/left_gear.ply: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/fidget_dataset/model_meshes/left_gear.ply -------------------------------------------------------------------------------- /fidget_dataset/model_meshes/right_gear.ply: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/fidget_dataset/model_meshes/right_gear.ply -------------------------------------------------------------------------------- /fidget_dataset/model_meshes/top_casing.ply: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/fidget_dataset/model_meshes/top_casing.ply -------------------------------------------------------------------------------- /Nema17_reducer_dataset/model_meshes/cover.ply: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/Nema17_reducer_dataset/model_meshes/cover.ply -------------------------------------------------------------------------------- /data_generation/models/Nema/planet/planet.ply: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/data_generation/models/Nema/planet/planet.ply -------------------------------------------------------------------------------- /data_generation/models/table/meshes/table.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/data_generation/models/table/meshes/table.stl -------------------------------------------------------------------------------- /fidget_dataset/model_meshes/bottom_casing.ply: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/fidget_dataset/model_meshes/bottom_casing.ply -------------------------------------------------------------------------------- /Nema17_reducer_dataset/model_meshes/Nema17.ply: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/Nema17_reducer_dataset/model_meshes/Nema17.ply -------------------------------------------------------------------------------- /Nema17_reducer_dataset/model_meshes/carrier.ply: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/Nema17_reducer_dataset/model_meshes/carrier.ply -------------------------------------------------------------------------------- /Nema17_reducer_dataset/model_meshes/housing.ply: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/Nema17_reducer_dataset/model_meshes/housing.ply -------------------------------------------------------------------------------- /Nema17_reducer_dataset/model_meshes/planet.ply: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/Nema17_reducer_dataset/model_meshes/planet.ply -------------------------------------------------------------------------------- /Nema17_reducer_dataset/model_meshes/stage_1.ply: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/Nema17_reducer_dataset/model_meshes/stage_1.ply -------------------------------------------------------------------------------- /Nema17_reducer_dataset/model_meshes/stage_2.ply: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/Nema17_reducer_dataset/model_meshes/stage_2.ply -------------------------------------------------------------------------------- /Nema17_reducer_dataset/model_meshes/stage_3.ply: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/Nema17_reducer_dataset/model_meshes/stage_3.ply -------------------------------------------------------------------------------- /Nema17_reducer_dataset/model_meshes/stage_4.ply: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/Nema17_reducer_dataset/model_meshes/stage_4.ply -------------------------------------------------------------------------------- /Nema17_reducer_dataset/model_meshes/sun_gear.ply: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/Nema17_reducer_dataset/model_meshes/sun_gear.ply -------------------------------------------------------------------------------- /Nema17_reducer_dataset/model_meshes/table1.ply: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/Nema17_reducer_dataset/model_meshes/table1.ply -------------------------------------------------------------------------------- /data_generation/models/Nema/carrier/carrier.ply: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/data_generation/models/Nema/carrier/carrier.ply -------------------------------------------------------------------------------- /data_generation/models/Nema/housing/housing.ply: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/data_generation/models/Nema/housing/housing.ply -------------------------------------------------------------------------------- /data_generation/models/Planetary/case/case.ply: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/data_generation/models/Planetary/case/case.ply -------------------------------------------------------------------------------- /data_generation/models/Planetary/cover/cover.ply: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/data_generation/models/Planetary/cover/cover.ply -------------------------------------------------------------------------------- /data_generation/models/table/meshes/SF0202S.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/data_generation/models/table/meshes/SF0202S.stl -------------------------------------------------------------------------------- /data_generation/models/table1/meshes/table.ply: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/data_generation/models/table1/meshes/table.ply -------------------------------------------------------------------------------- /data_generation/models/Nema/sun_gear/sun_gear.ply: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/data_generation/models/Nema/sun_gear/sun_gear.ply -------------------------------------------------------------------------------- /data_generation/models/Planetary/gear_0/gear_0.ply: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/data_generation/models/Planetary/gear_0/gear_0.ply -------------------------------------------------------------------------------- /data_generation/models/Planetary/gear_1/gear_1.ply: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/data_generation/models/Planetary/gear_1/gear_1.ply -------------------------------------------------------------------------------- /data_generation/models/arm_part/materials/textures/parts.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/data_generation/models/arm_part/materials/textures/parts.png -------------------------------------------------------------------------------- /data_generation/models/Planetary/gear_carrier/gear_carrier.ply: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/data_generation/models/Planetary/gear_carrier/gear_carrier.ply -------------------------------------------------------------------------------- /data_generation/models/coke_can/materials/textures/coke_can.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/data_generation/models/coke_can/materials/textures/coke_can.png -------------------------------------------------------------------------------- /data_generation/models/Nema/Nema17/istockphoto-476817442-170667a.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/data_generation/models/Nema/Nema17/istockphoto-476817442-170667a.jpg -------------------------------------------------------------------------------- /data_generation/models/Nema/cover/istockphoto-476817442-170667a.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/data_generation/models/Nema/cover/istockphoto-476817442-170667a.jpg -------------------------------------------------------------------------------- /data_generation/models/Nema/planet/istockphoto-476817442-170667a.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/data_generation/models/Nema/planet/istockphoto-476817442-170667a.jpg -------------------------------------------------------------------------------- /data_generation/models/extra/Fidget2/left_gear/meshes/1290039926.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/data_generation/models/extra/Fidget2/left_gear/meshes/1290039926.png -------------------------------------------------------------------------------- /data_generation/models/Nema/carrier/istockphoto-476817442-170667a.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/data_generation/models/Nema/carrier/istockphoto-476817442-170667a.jpg -------------------------------------------------------------------------------- /data_generation/models/Nema/housing/istockphoto-476817442-170667a.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/data_generation/models/Nema/housing/istockphoto-476817442-170667a.jpg -------------------------------------------------------------------------------- /data_generation/models/Nema/sun_gear/istockphoto-476817442-170667a.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/data_generation/models/Nema/sun_gear/istockphoto-476817442-170667a.jpg -------------------------------------------------------------------------------- /data_generation/models/Planetary/gear_1_g_testing/gear_1_g_testing.ply: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/data_generation/models/Planetary/gear_1_g_testing/gear_1_g_testing.ply -------------------------------------------------------------------------------- /data_generation/models/extra/Fidget2/right_gear/meshes/1290039926.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/data_generation/models/extra/Fidget2/right_gear/meshes/1290039926.png -------------------------------------------------------------------------------- /data_generation/models/extra/Fidget2/top_casing/meshes/1290039926.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/data_generation/models/extra/Fidget2/top_casing/meshes/1290039926.png -------------------------------------------------------------------------------- /data_generation/models/Planetary/case/istockphoto-476817442-170667a.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/data_generation/models/Planetary/case/istockphoto-476817442-170667a.jpg -------------------------------------------------------------------------------- /data_generation/models/Planetary/cover/istockphoto-476817442-170667a.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/data_generation/models/Planetary/cover/istockphoto-476817442-170667a.jpg -------------------------------------------------------------------------------- /data_generation/models/Planetary/gear_0/istockphoto-476817442-170667a.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/data_generation/models/Planetary/gear_0/istockphoto-476817442-170667a.jpg -------------------------------------------------------------------------------- /data_generation/models/Planetary/gear_1/istockphoto-476817442-170667a.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/data_generation/models/Planetary/gear_1/istockphoto-476817442-170667a.jpg -------------------------------------------------------------------------------- /data_generation/models/extra/Fidget2/bottom_casing/meshes/1899226145.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/data_generation/models/extra/Fidget2/bottom_casing/meshes/1899226145.png -------------------------------------------------------------------------------- /data_generation/models/extra/fidget_old_set/left_gear/meshes/fidget.jpeg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/data_generation/models/extra/fidget_old_set/left_gear/meshes/fidget.jpeg -------------------------------------------------------------------------------- /data_generation/models/extra/fidget_old_set/right_gear/meshes/fidget.jpeg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/data_generation/models/extra/fidget_old_set/right_gear/meshes/fidget.jpeg -------------------------------------------------------------------------------- /data_generation/models/extra/fidget_old_set/top_casing/meshes/fidget.jpeg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/data_generation/models/extra/fidget_old_set/top_casing/meshes/fidget.jpeg -------------------------------------------------------------------------------- /data_generation/models/extra/fidget_old_set/left_gear/meshes/left_gear.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/data_generation/models/extra/fidget_old_set/left_gear/meshes/left_gear.stl -------------------------------------------------------------------------------- /data_generation/models/Nema/Nema17/istockphoto-476817442-170667a.jpg.001.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/data_generation/models/Nema/Nema17/istockphoto-476817442-170667a.jpg.001.jpg -------------------------------------------------------------------------------- /data_generation/models/extra/fidget_old_set/bottom_casing/meshes/fidget.jpeg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/data_generation/models/extra/fidget_old_set/bottom_casing/meshes/fidget.jpeg -------------------------------------------------------------------------------- /data_generation/models/extra/fidget_old_set/right_gear/meshes/right_gear.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/data_generation/models/extra/fidget_old_set/right_gear/meshes/right_gear.stl -------------------------------------------------------------------------------- /data_generation/models/extra/fidget_old_set/top_casing/meshes/top_casing.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/data_generation/models/extra/fidget_old_set/top_casing/meshes/top_casing.stl -------------------------------------------------------------------------------- /data_generation/models/Fidget/left_gear/meshes/istockphoto-476817442-170667a.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/data_generation/models/Fidget/left_gear/meshes/istockphoto-476817442-170667a.jpg -------------------------------------------------------------------------------- /data_generation/models/Planetary/gear_carrier/istockphoto-476817442-170667a.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/data_generation/models/Planetary/gear_carrier/istockphoto-476817442-170667a.jpg -------------------------------------------------------------------------------- /data_generation/models/Fidget/right_gear/meshes/istockphoto-476817442-170667a.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/data_generation/models/Fidget/right_gear/meshes/istockphoto-476817442-170667a.jpg -------------------------------------------------------------------------------- /data_generation/models/Fidget/top_casing/meshes/istockphoto-476817442-170667a.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/data_generation/models/Fidget/top_casing/meshes/istockphoto-476817442-170667a.jpg -------------------------------------------------------------------------------- /data_generation/models/Planetary/gear_1_g_testing/istockphoto-476817442-170667a.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/data_generation/models/Planetary/gear_1_g_testing/istockphoto-476817442-170667a.jpg -------------------------------------------------------------------------------- /data_generation/models/extra/fidget_old_set/bottom_casing/meshes/bottom_casing.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/data_generation/models/extra/fidget_old_set/bottom_casing/meshes/bottom_casing.stl -------------------------------------------------------------------------------- /data_generation/models/extra/fidget_old_set/left_gear/materials/textures/parts.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/data_generation/models/extra/fidget_old_set/left_gear/materials/textures/parts.png -------------------------------------------------------------------------------- /data_generation/models/Fidget/bottom_casing/meshes/istockphoto-476817442-170667a.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KulunuOS/6DAPose/HEAD/data_generation/models/Fidget/bottom_casing/meshes/istockphoto-476817442-170667a.jpg -------------------------------------------------------------------------------- /data_generation/models/arm_part/materials/scripts/arm_part.material: -------------------------------------------------------------------------------- 1 | material ArmPart/Diffuse 2 | { 3 | receive_shadows off 4 | technique 5 | { 6 | pass 7 | { 8 | texture_unit 9 | { 10 | texture parts.png 11 | } 12 | } 13 | } 14 | } 15 | -------------------------------------------------------------------------------- /data_generation/models/extra/left_gear/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | left_gear 4 | 1.0 5 | model.sdf 6 | 7 | Generated by blender SDF tools 8 | 9 | 10 | -------------------------------------------------------------------------------- /data_generation/models/extra/right_gear/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | right_gear 4 | 1.0 5 | model.sdf 6 | 7 | Generated by blender SDF tools 8 | 9 | 10 | -------------------------------------------------------------------------------- /data_generation/models/extra/top_casing/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | top_casing 4 | 1.0 5 | model.sdf 6 | 7 | Generated by blender SDF tools 8 | 9 | 10 | -------------------------------------------------------------------------------- /data_generation/models/extra/Fidget2/left_gear/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | left_gear 4 | 1.0 5 | model.sdf 6 | 7 | Generated by blender SDF tools 8 | 9 | 10 | -------------------------------------------------------------------------------- /data_generation/models/extra/bottom_casing/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | bottom_casing 4 | 1.0 5 | model.sdf 6 | 7 | Generated by blender SDF tools 8 | 9 | 10 | -------------------------------------------------------------------------------- /data_generation/models/extra/Fidget2/right_gear/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | right_gear 4 | 1.0 5 | model.sdf 6 | 7 | Generated by blender SDF tools 8 | 9 | 10 | -------------------------------------------------------------------------------- /data_generation/models/extra/Fidget2/top_casing/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | top_casing 4 | 1.0 5 | model.sdf 6 | 7 | Generated by blender SDF tools 8 | 9 | 10 | -------------------------------------------------------------------------------- /data_generation/models/extra/Fidget2/bottom_casing/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | bottom_casing 4 | 1.0 5 | model.sdf 6 | 7 | Generated by blender SDF tools 8 | 9 | 10 | -------------------------------------------------------------------------------- /data_generation/models/Nema/cover/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | cover 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Nate Koenig 10 | nate@osrfoundation.org 11 | 12 | 13 | 14 | cover 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /data_generation/launch/nema17.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /data_generation/models/Fidget/left_gear/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | left_gear 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Nate Koenig 10 | nate@osrfoundation.org 11 | 12 | 13 | 14 | left_gear 15 | 16 | 17 | -------------------------------------------------------------------------------- /data_generation/models/Nema/Nema17/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Nema17 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Nate Koenig 10 | nate@osrfoundation.org 11 | 12 | 13 | 14 | Nema17 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /data_generation/models/Nema/carrier/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | carrier 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Nate Koenig 10 | nate@osrfoundation.org 11 | 12 | 13 | 14 | carrier 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /data_generation/models/Nema/housing/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | housing 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Nate Koenig 10 | nate@osrfoundation.org 11 | 12 | 13 | 14 | housing 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /data_generation/models/Nema/planet/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | planet 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Nate Koenig 10 | nate@osrfoundation.org 11 | 12 | 13 | 14 | planet 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /data_generation/models/Planetary/case/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | case 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Nate Koenig 10 | nate@osrfoundation.org 11 | 12 | 13 | 14 | case 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /data_generation/models/Planetary/cover/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | cover 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Nate Koenig 10 | nate@osrfoundation.org 11 | 12 | 13 | 14 | cover 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /data_generation/launch/fidget.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /data_generation/models/Fidget/right_gear/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | right_gear 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Nate Koenig 10 | nate@osrfoundation.org 11 | 12 | 13 | 14 | right_gear 15 | 16 | 17 | -------------------------------------------------------------------------------- /data_generation/models/Fidget/top_casing/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | top_casing 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Nate Koenig 10 | nate@osrfoundation.org 11 | 12 | 13 | 14 | top_casing 15 | 16 | 17 | -------------------------------------------------------------------------------- /data_generation/models/Nema/sun_gear/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | sun_gear 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Nate Koenig 10 | nate@osrfoundation.org 11 | 12 | 13 | 14 | sun_gear 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /data_generation/models/Planetary/gear_0/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | gear_0 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Nate Koenig 10 | nate@osrfoundation.org 11 | 12 | 13 | 14 | gear_0 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /data_generation/models/Planetary/gear_1/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | gear_1 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Nate Koenig 10 | nate@osrfoundation.org 11 | 12 | 13 | 14 | gear_1 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /data_generation/launch/planetary.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /data_generation/models/table/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | table 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Nate Koenig 10 | nate@osrfoundation.org 11 | 12 | 13 | 14 | A directional light for the sun. 15 | 16 | 17 | -------------------------------------------------------------------------------- /data_generation/models/table1/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | table1 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Nate Koenig 10 | nate@osrfoundation.org 11 | 12 | 13 | 14 | A directional light for the sun. 15 | 16 | 17 | -------------------------------------------------------------------------------- /data_generation/models/Fidget/bottom_casing/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | bottom_casing 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Nate Koenig 10 | nate@osrfoundation.org 11 | 12 | 13 | 14 | bottom_casing 15 | 16 | 17 | -------------------------------------------------------------------------------- /data_generation/models/extra/fidget_old_set/left_gear/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | left_gear 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Nate Koenig 10 | nate@osrfoundation.org 11 | 12 | 13 | 14 | left_gear 15 | 16 | 17 | -------------------------------------------------------------------------------- /data_generation/models/Planetary/gear_carrier/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | gear_carrier 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Nate Koenig 10 | nate@osrfoundation.org 11 | 12 | 13 | 14 | gear_carrier 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /data_generation/models/extra/fidget_old_set/right_gear/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | right_gear 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Nate Koenig 10 | nate@osrfoundation.org 11 | 12 | 13 | 14 | right_gear 15 | 16 | 17 | -------------------------------------------------------------------------------- /data_generation/models/extra/fidget_old_set/top_casing/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | top_casing 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Nate Koenig 10 | nate@osrfoundation.org 11 | 12 | 13 | 14 | top_casing 15 | 16 | 17 | -------------------------------------------------------------------------------- /data_generation/models/extra/fidget_old_set/bottom_casing/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | bottom_casing 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Nate Koenig 10 | nate@osrfoundation.org 11 | 12 | 13 | 14 | bottom_casing 15 | 16 | 17 | -------------------------------------------------------------------------------- /data_generation/models/Planetary/gear_1_g_testing/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | gear_1_g_testing 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Nate Koenig 10 | nate@osrfoundation.org 11 | 12 | 13 | 14 | gear_1_g_testing 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /data_generation/models/coke_can/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Coke Can 5 | 1.0 6 | model-1_2.sdf 7 | model-1_3.sdf 8 | model-1_4.sdf 9 | model.sdf 10 | 11 | 12 | John Hsu 13 | hsu@osrfoundation.org 14 | 15 | 16 | 17 | A can of Coke. 18 | 19 | 20 | -------------------------------------------------------------------------------- /data_generation/models/arm_part/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Arm Part 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Nate Koenig 10 | nate@osrfoundation.org 11 | 12 | 13 | 14 | Cole Biesemeyer 15 | 16 | 17 | 18 | A hypothetical part that might exist in an industrial manufacturing setting. 19 | 20 | 21 | -------------------------------------------------------------------------------- /6DApose.repos: -------------------------------------------------------------------------------- 1 | repositories: 2 | realsense2_description: 3 | type: git 4 | url: https://github.com/issaiass/realsense2_description.git 5 | version: master 6 | realsense_gazebo_plugin: 7 | type: git 8 | url: https://github.com/pal-robotics/realsense_gazebo_plugin.git 9 | version: melodic-devel 10 | bop_toolkit: 11 | type: git 12 | url: https://github.com/thodan/bop_toolkit.git 13 | version: master 14 | geometry2: 15 | type: git 16 | url: https://github.com/ros/geometry2 17 | version: noetic-devel -------------------------------------------------------------------------------- /data_generation/models/Nema/cover/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | false 5 | 6 | 7 | 8 | 9 | 1 1 1 10 | model://Nema/cover/cover.dae 11 | 12 | 13 | 14 | 15 | 16 | 17 | 1 1 1 18 | model://Nema/cover/cover.dae 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /data_generation/models/table/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | false 5 | 6 | 7 | 8 | 9 | 1 1 1 10 | model://table/meshes/table.stl 11 | 12 | 13 | 14 | 15 | 16 | 17 | 1 1 1 18 | model://table/meshes/table.stl 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /data_generation/models/Nema/Nema17/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | false 5 | 6 | 7 | 8 | 9 | 1 1 1 10 | model://Nema/Nema17/Nema17.dae 11 | 12 | 13 | 14 | 15 | 16 | 17 | 1 1 1 18 | model://Nema/Nema17/Nema17.dae 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /data_generation/models/Nema/planet/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | false 5 | 6 | 7 | 8 | 9 | 1 1 1 10 | model://Nema/planet/planet.dae 11 | 12 | 13 | 14 | 15 | 16 | 17 | 1 1 1 18 | model://Nema/planet/planet.dae 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /data_generation/models/Planetary/case/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | false 5 | 6 | 7 | 8 | 9 | 1 1 1 10 | model://Planetary/case/case.dae 11 | 12 | 13 | 14 | 15 | 16 | 17 | 1 1 1 18 | model://Planetary/case/case.dae 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /data_generation/models/Nema/carrier/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | false 5 | 6 | 7 | 8 | 9 | 1 1 1 10 | model://Nema/carrier/carrier.dae 11 | 12 | 13 | 14 | 15 | 16 | 17 | 1 1 1 18 | model://Nema/carrier/carrier.dae 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /data_generation/models/Nema/housing/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | false 5 | 6 | 7 | 8 | 9 | 1 1 1 10 | model://Nema/housing/housing.dae 11 | 12 | 13 | 14 | 15 | 16 | 17 | 1 1 1 18 | model://Nema/housing/housing.dae 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /data_generation/models/Planetary/cover/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | false 5 | 6 | 7 | 8 | 9 | 1 1 1 10 | model://Planetary/cover/cover.dae 11 | 12 | 13 | 14 | 15 | 16 | 17 | 1 1 1 18 | model://Planetary/cover/cover.dae 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /data_generation/models/Nema/sun_gear/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | false 5 | 6 | 7 | 8 | 9 | 1 1 1 10 | model://Nema/sun_gear/sun_gear.dae 11 | 12 | 13 | 14 | 15 | 16 | 17 | 1 1 1 18 | model://Nema/sun_gear/sun_gear.dae 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /data_generation/models/Planetary/gear_0/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | false 5 | 6 | 7 | 8 | 9 | 1 1 1 10 | model://Planetary/gear_0/gear_0.dae 11 | 12 | 13 | 14 | 15 | 16 | 17 | 1 1 1 18 | model://Planetary/gear_0/gear_0.dae 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /data_generation/models/Planetary/gear_1/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | false 5 | 6 | 7 | 8 | 9 | 1 1 1 10 | model://Planetary/gear_1/gear_1.dae 11 | 12 | 13 | 14 | 15 | 16 | 17 | 1 1 1 18 | model://Planetary/gear_1/gear_1.dae 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /data_generation/models/Fidget/left_gear/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | false 5 | 6 | 15 | 16 | 17 | 18 | 1 1 1 19 | file://Fidget/left_gear/meshes/left_gear.dae 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /data_generation/models/Planetary/gear_carrier/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | false 5 | 6 | 7 | 8 | 9 | 1 1 1 10 | model://Planetary/gear_carrier/gear_carrier.dae 11 | 12 | 13 | 14 | 15 | 16 | 17 | 1 1 1 18 | model://Planetary/gear_carrier/gear_carrier.dae 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /data_generation/models/extra/fidget_old_set/left_gear/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | false 5 | 6 | 7 | 8 | 9 | 0.001 0.001 0.001 10 | file://left_gear/meshes/left_gear.dae 11 | 12 | 13 | 14 | 15 | 16 | 17 | 0.001 0.001 0.001 18 | file://left_gear/meshes/left_gear.dae 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /data_generation/models/Fidget/right_gear/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | false 5 | 6 | 15 | 16 | 17 | 18 | 1 1 1 19 | model://Fidget/right_gear/meshes/right_gear.dae 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /data_generation/models/Fidget/top_casing/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | false 5 | 6 | 15 | 16 | 17 | 18 | 1 1 1 19 | model://Fidget/top_casing/meshes/top_casing.dae 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /data_generation/models/extra/fidget_old_set/right_gear/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | false 5 | 6 | 7 | 8 | 9 | 0.001 0.0011 0.001 10 | model://right_gear/meshes/right_gear.dae 11 | 12 | 13 | 14 | 15 | 16 | 17 | 0.001 0.001 0.001 18 | model://right_gear/meshes/right_gear.dae 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /data_generation/models/extra/fidget_old_set/top_casing/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | false 5 | 6 | 7 | 8 | 9 | 0.001 0.001 0.001 10 | model://top_casing/meshes/top_casing.dae 11 | 12 | 13 | 14 | 15 | 16 | 17 | 0.001 0.001 0.001 18 | model://top_casing/meshes/top_casing.dae 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /data_generation/models/Planetary/gear_1_g_testing/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | false 5 | 6 | 7 | 8 | 9 | 1 1 1 10 | model://Planetary/gear_1_g_testing/gear_1_g_testing.dae 11 | 12 | 13 | 14 | 15 | 16 | 17 | 1 1 1 18 | model://Planetary/gear_1_g_testing/gear_1_g_testing.dae 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /data_generation/models/extra/fidget_old_set/bottom_casing/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | false 5 | 6 | 7 | 8 | 9 | 0.001 0.001 0.001 10 | model://bottom_casing/meshes/bottom_casing.dae 11 | 12 | 13 | 14 | 15 | 16 | 17 | 0.001 0.001 0.001 18 | model://bottom_casing/meshes/bottom_casing.dae 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /data_generation/models/Fidget/bottom_casing/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | false 5 | 6 | 16 | 17 | 18 | 19 | 1 1 1 20 | model://Fidget/bottom_casing/meshes/bottom_casing.dae 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /data_generation/models/extra/bottom_casing/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | meshes/model.dae 9 | 10 | bottom_casing 11 | 12 | 13 | 14 | 15 | 1.0 1.0 1.0 1.0 16 | 0.0 0.0 0.0 1.0 17 | 18 | 19 | 20 | 21 | 22 | 23 | meshes/model.dae 24 | 25 | 26 | 27 | 28 | 0x01 29 | 30 | 31 | 32 | true 33 | 34 | -------------------------------------------------------------------------------- /data_generation/models/coke_can/model-1_3.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 0.390 7 | 8 | 0.00055575 9 | 0 10 | 0 11 | 0.00055575 12 | 0 13 | 0.0001755 14 | 15 | 16 | 17 | 0.003937 0.0047244 -0.18 0 0 0 18 | 19 | 20 | model://coke_can/meshes/coke_can.dae 21 | 22 | 23 | 24 | 25 | 0.003937 0.0047244 -0.18 0 0 0 26 | 27 | 28 | model://coke_can/meshes/coke_can.dae 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /data_generation/models/coke_can/model-1_2.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 0.390 7 | 8 | 0.00055575 9 | 0 10 | 0 11 | 0.00055575 12 | 0 13 | 0.0001755 14 | 15 | 16 | 17 | 0.003937 0.0047244 -0.18 0 0 0 18 | 19 | 20 | model://coke_can/meshes/coke_can.dae 21 | 22 | 23 | 24 | 25 | 0.003937 0.0047244 -0.18 0 0 0 26 | 27 | 28 | model://coke_can/meshes/coke_can.dae 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /data_generation/models/extra/left_gear/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | meshes/model.dae 9 | 10 | left_gear 11 | 12 | 13 | 14 | 15 | 1.0 1.0 1.0 1.0 16 | 0.0 0.0 0.0 1.0 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | meshes/model.dae 26 | 27 | 28 | 29 | 30 | 0x01 31 | 32 | 33 | 34 | true 35 | 36 | -------------------------------------------------------------------------------- /data_generation/models/extra/top_casing/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | meshes/model.dae 9 | 10 | top_casing 11 | 12 | 13 | 14 | 15 | 1.0 1.0 1.0 1.0 16 | 0.0 0.0 0.0 1.0 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | meshes/model.dae 26 | 27 | 28 | 29 | 30 | 0x01 31 | 32 | 33 | 34 | true 35 | 36 | -------------------------------------------------------------------------------- /data_generation/models/extra/right_gear/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | meshes/model.dae 9 | 10 | right_gear 11 | 12 | 13 | 14 | 15 | 1.0 1.0 1.0 1.0 16 | 0.0 0.0 0.0 1.0 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | meshes/model.dae 27 | 28 | 29 | 30 | 31 | 0x01 32 | 33 | 34 | 35 | true 36 | 37 | -------------------------------------------------------------------------------- /data_generation/models/extra/Fidget2/left_gear/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | meshes/model.dae 9 | 10 | left_gear 11 | 12 | 13 | 14 | 15 | 1.0 1.0 1.0 1.0 16 | 0.0 0.0 0.0 1.0 17 | 18 | 19 | meshes/1290039926.png 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | meshes/model.dae 29 | 30 | 31 | 32 | 33 | 0x01 34 | 35 | 36 | 37 | true 38 | 39 | -------------------------------------------------------------------------------- /data_generation/models/extra/Fidget2/right_gear/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | meshes/model.dae 9 | 10 | right_gear 11 | 12 | 13 | 14 | 15 | 1.0 1.0 1.0 1.0 16 | 0.0 0.0 0.0 1.0 17 | 18 | 19 | meshes/1290039926.png 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | meshes/model.dae 29 | 30 | 31 | 32 | 33 | 0x01 34 | 35 | 36 | 37 | true 38 | 39 | -------------------------------------------------------------------------------- /data_generation/models/extra/Fidget2/top_casing/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | meshes/model.dae 9 | 10 | top_casing 11 | 12 | 13 | 14 | 15 | 1.0 1.0 1.0 1.0 16 | 0.0 0.0 0.0 1.0 17 | 18 | 19 | meshes/1290039926.png 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | meshes/model.dae 29 | 30 | 31 | 32 | 33 | 0x01 34 | 35 | 36 | 37 | true 38 | 39 | -------------------------------------------------------------------------------- /data_generation/models/extra/Fidget2/bottom_casing/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | meshes/model.dae 9 | 10 | bottom_casing 11 | 12 | 13 | 14 | 15 | 1.0 1.0 1.0 1.0 16 | 0.0 0.0 0.0 1.0 17 | 18 | 19 | meshes/1899226145.png 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | meshes/model.dae 29 | 30 | 31 | 32 | 33 | 0x01 34 | 35 | 36 | 37 | true 38 | 39 | -------------------------------------------------------------------------------- /data_generation/models/table1/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | true 4 | 5 | 0 0 0 0 -0 0 6 | 7 | 0 0.04617 0.38 0 -0 0 8 | 35.4321 9 | 10 | 1.73083 11 | 2.01582e-11 12 | 2.78594e-13 13 | 5.32936 14 | 4.32952e-13 15 | 3.81098 16 | 17 | 18 | 19 | 0 0 0 0 -0 0 20 | 21 | 22 | 1 1 1 23 | model://table1/meshes/table1.dae 24 | 25 | 26 | 27 | 28 | 0 0 0 0 -0 0 29 | 30 | 31 | 1 1 1 32 | model://table1/meshes/table1.dae 33 | 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /data_generation/models/arm_part/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 0 -0.12223 0.101835 0 0 0 7 | 0.5 8 | 9 | 0.00950655468 10 | 0 11 | 0 12 | 0.0054345236 13 | 0 14 | 0.01148428921 15 | 16 | 17 | 18 | 19 | 0 -0.12223 0.101835 0 0 0 20 | 21 | 22 | 0.29824 0.43206 0.20367 23 | 24 | 25 | 26 | 27 | 28 | 29 | model://arm_part/meshes/arm.dae 30 | 31 | 32 | 33 | 38 | 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /data_generation/models/coke_can/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 0 0 0.06 0 0 0 7 | 0.390 8 | 9 | 0.00055575 10 | 0 11 | 0 12 | 0.00055575 13 | 0 14 | 0.0001755 15 | 16 | 17 | 18 | 0.003937 0.0047244 -0.18 0 0 0 19 | 20 | 21 | model://coke_can/meshes/coke_can.dae 22 | 23 | 24 | 25 | 26 | 27 | 1.0 28 | 1.0 29 | 30 | 31 | 32 | 33 | 10000000.0 34 | 1.0 35 | 0.001 36 | 0.1 37 | 38 | 39 | 40 | 41 | 42 | 0.003937 0.0047244 -0.18 0 0 0 43 | 44 | 45 | model://coke_can/meshes/coke_can.dae 46 | 47 | 48 | 49 | 50 | 51 | 52 | -------------------------------------------------------------------------------- /data_generation/models/coke_can/model-1_4.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 0 0 0.06 0 0 0 7 | 0.390 8 | 9 | 0.00055575 10 | 0 11 | 0 12 | 0.00055575 13 | 0 14 | 0.0001755 15 | 16 | 17 | 18 | 0.003937 0.0047244 -0.18 0 0 0 19 | 20 | 21 | model://coke_can/meshes/coke_can.dae 22 | 23 | 24 | 25 | 26 | 27 | 1.0 28 | 1.0 29 | 30 | 31 | 32 | 33 | 10000000.0 34 | 1.0 35 | 0.001 36 | 0.1 37 | 38 | 39 | 40 | 41 | 42 | 0.003937 0.0047244 -0.18 0 0 0 43 | 44 | 45 | model://coke_can/meshes/coke_can.dae 46 | 47 | 48 | 49 | 50 | 51 | 52 | -------------------------------------------------------------------------------- /data_generation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | data_generation 4 | 0.0.0 5 | The data_generation package 6 | 7 | 8 | 9 | 10 | kulunu 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | 53 | gazebo_ros 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # 6DAPose 2 | 3 | This work presents a pointcloud registration based assembly pose estimation method for robotic assembling. The framework consists of semantic 4 | segmentation of the scene and registering point clouds of local surfaces against target point clouds derived from CAD models in order to detect 6D poses. 5 | The method is demonstrated and evaluated on two synthetic object assembly datasets generated in gazebo simulation environment. 6 | 7 | **Download the datasets [here](https://zenodo.org/records/10117869)** 8 | 9 | Please use the following citation if you used this repository in your publications: 10 | 11 | ``` 12 | @article{samarawickrama20236d, 13 | title={6D Assembly Pose Estimation by Point Cloud Registration for Robot Manipulation}, 14 | author={Samarawickrama, Kulunu and Sharma, Gaurang and Angleraud, Alexandre and Pieters, Roel}, 15 | journal={arXiv preprint arXiv:2312.02593}, 16 | year={2023} 17 | } 18 | ``` 19 | 20 | ## Requirements 21 | 22 | The source code is tested under following specifications 23 | 24 | - Ubuntu Linux 20.04 LTS 25 | - Python 3.8 or greater 26 | - ROS Noetic 27 | - Gazebo Classic 28 | 29 | 30 | 31 | Install the required python libraries to your anaconda or virtual environment 32 | 33 | ``` 34 | $ pip install -r requirements.txt 35 | ``` 36 | 37 | ## Installation Instructions 38 | 39 | 1. Create a workspace in your local setup following the directory structure: 40 | 41 | ``` 42 | $ mkdir -p ~/6DAPose/src 43 | $ cd ~/6DAPose/src 44 | $ git clone https://github.com/KulunuOS/6DAPose.git . 45 | ``` 46 | 2. download the dependent repositories using vcs-tool 47 | 48 | ``` 49 | $ vcs import < 6DAPose.repos 50 | ``` 51 | 3. move the echo_tfs.py script to tf2_tools package 52 | 53 | ``` 54 | $ mv echo_tfs.py /geometry2/tf2_tools/scripts/ 55 | ``` 56 | 57 | 4. Finally, build the workspace 58 | 59 | ``` 60 | $ source /opt/ros/noetic/setup.bash 61 | $ colcon build 62 | ``` 63 | 64 | ## Evaluation 65 | 66 | ### Download Dataset 67 | 68 | Download the [datasets](https://zenodo.org/records/10117869) and copy to directories "/fidget_dataset" or "/Nema17_reducer_dataset" 69 | 70 | ### Generate the Dataset locally 71 | 72 | Follow the instructions below if you want to generate the dataset locally 73 | 74 | 1. Launch the gazebo simulation package 75 | ``` 76 | # Terminal 1 77 | $ source /opt/ros/noetic/setup.bash 78 | $ cd ~/6DAPose 79 | $ source devel/setup.bash 80 | 81 | # to launch Nema17 Planetary reducer dataset: 82 | $ roslaunch data_generation nema17.launch 83 | 84 | # to launch fidget dataset: 85 | $ roslaunch data_generation fidget.launch 86 | ``` 87 | 88 | 2. Launch the tf2_tools package for coordinate frames transformation info 89 | ``` 90 | # Terminal 2 91 | $ source /opt/ros/noetic/setup.bash 92 | $ cd ~/6DAPose 93 | $ source devel/setup.bash 94 | # rosrun tf2_tools echo_tfs.py base_link camera_depth_optical_frame 95 | 96 | ``` 97 | 3. Launch the script to generate and save data. set argument -d to define dataset and argument -s to define the assembly steps. 98 | ``` 99 | # to generate fidget dataset: do for each assemble steps 1-3 by setting argument -s 100 | $ python3 render_icp_data.py -m bottom_casing left_gear right_gear top_casing -d fidget -s 1 101 | 102 | # to generate Nema17 reducer dataset: do for each assemble steps 1-4 by setting argument -s 103 | $ python3 render_icp_data.py -m Nema17 sun_gear housing carrier cover -d Nema17_reducer -s 1 104 | 105 | ``` 106 | 4. Generate groundtruth assembly specific relative pose information using the preprocess notebooks [1](https://github.com/KulunuOS/6DAPose/blob/main/Fidget_Ground_truth_preprocess.ipynb) and [2](https://github.com/KulunuOS/6DAPose/blob/main/Nema17_dataset_GT_preprocess.ipynb) 107 | 108 | 109 | ### Run evaluation 110 | ``` 111 | $ python3 6DAPose.py 112 | ``` 113 | 114 | ## Acknowledgement 115 | 116 | Project funding was received from Helsinki Institute of Physics’ Technology Programme (project; ROBOT) and European Union’s Horizon 2020 research and innovation programme, grant agreement no. 871252 (METRICS). 117 | 118 | 119 | 120 | -------------------------------------------------------------------------------- /data_generation/Worlds/planetary.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 1 8 | 0 0 10 0 -0 0 9 | 0.8 0.8 0.8 1 10 | 0.1 0.1 0.1 1 11 | 12 | 1000 13 | 0.9 14 | 0.01 15 | 0.001 16 | 17 | -0.5 0.5 -1 18 | 19 | 20 | 21 | 22 | 1 23 | 24 | 25 | 26 | 27 | 0 0 1 28 | 100 100 29 | 30 | 31 | 32 | 33 | 34 | 100 35 | 50 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 10 47 | 48 | 49 | 0 50 | 51 | 52 | 0 0 1 53 | 100 100 54 | 55 | 56 | 57 | 61 | 62 | 63 | 0 64 | 0 65 | 1 66 | 67 | 68 | 69 | 70 | 0 0 -9.8 71 | 6e-06 2.3e-05 -4.2e-05 72 | 73 | 74 | 0.001 75 | 1 76 | 1000 77 | 78 | 79 | 0.4 0.4 0.4 1 80 | 0.7 0.7 0.7 1 81 | 1 82 | 83 | 84 | EARTH_WGS84 85 | 0 86 | 0 87 | 0 88 | 0 89 | 90 | 91 | 92 | 93 | 0 0 2 0 -0 0 94 | 0.2 0.2 0.2 1 95 | 0.1 0.1 0.1 1 96 | 97 | 20 98 | 0.5 99 | 0.01 100 | 0.001 101 | 102 | 0 103 | 0 0 -1 104 | 105 | 106 | 107 | 108 | 109 | model://table1 110 | table1 111 | -0.475 0 0 0 0 0 112 | 113 | 114 | 115 | 116 | model://Planetary/case 117 | case 118 | -0.04 0.04 0.845 0 0 0 119 | 0 120 | 121 | 122 | 123 | model://Planetary/cover 124 | cover 125 | 0.02 -0.02 0.845 0 0 0 126 | 0 127 | 128 | 129 | 130 | model://Planetary/gear_0 131 | gear_0 132 | -0.02 -0.02 0.845 0 0 0 133 | 0 134 | 135 | 136 | 137 | model://Planetary/gear_1 138 | gear_1 139 | -0.02 0.0 0.845 0 0 0 140 | 0 141 | 142 | 143 | 144 | model://Planetary/gear_1_g_testing 145 | gear_1_g_testing 146 | 0.03 0.04 0.845 0 0 0 147 | 0 148 | 149 | 150 | 151 | model://Planetary/gear_carrier 152 | gear_carrier 153 | 0.0 0.03 0.845 0 0 0 154 | 0 155 | 156 | 157 | 158 | 159 | 2.16101 -0.608536 1.91511 0 0.589796 2.59702 160 | orbit 161 | perspective 162 | 163 | 164 | 165 | 166 | -------------------------------------------------------------------------------- /data_generation/Worlds/metrics.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 1 8 | 0 0 10 0 -0 0 9 | 0.8 0.8 0.8 1 10 | 0.1 0.1 0.1 1 11 | 12 | 1000 13 | 0.9 14 | 0.01 15 | 0.001 16 | 17 | -0.5 0.5 -1 18 | 19 | 20 | 21 | 22 | 1 23 | 24 | 25 | 26 | 27 | 0 0 1 28 | 100 100 29 | 30 | 31 | 32 | 33 | 34 | 100 35 | 50 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 10 47 | 48 | 49 | 0 50 | 51 | 52 | 0 0 1 53 | 100 100 54 | 55 | 56 | 57 | 61 | 62 | 63 | 0 64 | 0 65 | 1 66 | 67 | 68 | 69 | 70 | 0 0 -9.8 71 | 6e-06 2.3e-05 -4.2e-05 72 | 73 | 74 | 0.001 75 | 1 76 | 1000 77 | 78 | 79 | 0.4 0.4 0.4 1 80 | 0.7 0.7 0.7 1 81 | 1 82 | 83 | 84 | EARTH_WGS84 85 | 0 86 | 0 87 | 0 88 | 0 89 | 90 | 91 | 92 | 93 | 0 0 2 0 -0 0 94 | 0.2 0.2 0.2 1 95 | 0.1 0.1 0.1 1 96 | 97 | 20 98 | 0.5 99 | 0.01 100 | 0.001 101 | 102 | 0 103 | 0 0 -1 104 | 105 | 106 | 107 | 114 | 115 | 116 | 117 | model://Fidget/right_gear 118 | right_gear 119 | 0.02 -0.02 0.845 0 0 0 120 | 1 121 | 122 | 123 | 124 | 125 | model://Fidget/top_casing 126 | top_casing 127 | -0.02 -0.02 0.845 0 0 0 128 | 1 129 | 130 | 131 | 132 | 133 | model://table1 134 | table1 135 | -0.475 0 0 0 0 0 136 | 1 137 | 138 | 139 | 140 | 141 | model://Fidget/left_gear 142 | left_gear 143 | 0.0315 0.02 0.85 0 0 0 144 | 1 145 | 146 | 147 | 154 | 155 | 162 | 163 | 164 | 165 | 166 | model://Fidget/bottom_casing 167 | bottom_casing 168 | 0.02 0.02 0.845 0 0 0 169 | 1 170 | 171 | 172 | 173 | 174 | 175 | 2.16101 -0.608536 1.91511 0 0.589796 2.59702 176 | orbit 177 | perspective 178 | 179 | 180 | 181 | 182 | -------------------------------------------------------------------------------- /icp_utils.py: -------------------------------------------------------------------------------- 1 | import open3d as o3d 2 | import numpy as np 3 | import copy 4 | import cv2 5 | 6 | # Portions of this code have been adapted from the Open3D library (https://github.com/intel-isl/Open3D) 7 | # and the PVN3D GitHub repository (https://github.com/ethnhe/PVN3D). 8 | # Credit and thanks to the original authors for their contributions. 9 | 10 | def prepare_dataset(voxel_size, source,target, radius_normal,radius_feature): 11 | #print(":: Load two point clouds and disturb initial pose.") 12 | source_down, source_fpfh = preprocess_point_cloud(source, voxel_size, radius_normal,radius_feature) 13 | target_down, target_fpfh = preprocess_point_cloud(target, voxel_size,radius_normal,radius_feature) 14 | 15 | return source, target, source_down, target_down, source_fpfh, target_fpfh 16 | 17 | def preprocess_point_cloud(pcd, voxel_size, radius_normal,radius_feature ): 18 | 19 | pcd_down = pcd 20 | pcd_down.estimate_normals( 21 | o3d.geometry.KDTreeSearchParamHybrid(radius=radius_normal, max_nn=30)) 22 | 23 | pcd_fpfh = o3d.pipelines.registration.compute_fpfh_feature( 24 | pcd_down, 25 | o3d.geometry.KDTreeSearchParamHybrid(radius=radius_feature, max_nn=100)) 26 | return pcd_down, pcd_fpfh 27 | 28 | def execute_global_registration(source_down, target_down, source_fpfh, 29 | target_fpfh, voxel_size, distance_threshold): 30 | result = o3d.pipelines.registration.registration_ransac_based_on_feature_matching( 31 | source_down, target_down, source_fpfh, target_fpfh, True, 32 | distance_threshold, 33 | o3d.pipelines.registration.TransformationEstimationPointToPlane(), 34 | 5, 35 | [ 36 | o3d.pipelines.registration.CorrespondenceCheckerBasedOnEdgeLength( 37 | 0.09), 38 | o3d.pipelines.registration.CorrespondenceCheckerBasedOnDistance( 39 | distance_threshold) 40 | ], 41 | o3d.pipelines.registration.RANSACConvergenceCriteria(100000, 0.899)) 42 | return result 43 | 44 | 45 | def draw_registration_result(source, target, transformation): 46 | source_temp = copy.deepcopy(source) 47 | target_temp = copy.deepcopy(target) 48 | target_temp.paint_uniform_color([1, 0.706, 0]) 49 | source_temp.paint_uniform_color([0, 0.651, 0.929]) 50 | source_temp.transform(transformation) 51 | o3d.visualization.draw_geometries([source_temp, target_temp]) 52 | 53 | def draw_bounding_box(img, corner, gt=False): 54 | 55 | if gt: 56 | color = (255, 0, 0) 57 | else: 58 | color = (0, 255, 0) 59 | 60 | img = cv2.line(img, tuple(corner[0]), tuple(corner[1]), color, 2) 61 | img = cv2.line(img, tuple(corner[0]), tuple(corner[2]), color,2) 62 | img = cv2.line(img, tuple(corner[2]), tuple(corner[3]), color, 2) 63 | img = cv2.line(img, tuple(corner[3]), tuple(corner[1]), color, 2) 64 | img = cv2.line(img, tuple(corner[4]), tuple(corner[5]), color, 2) 65 | img = cv2.line(img, tuple(corner[5]), tuple(corner[7]), color, 2) 66 | img = cv2.line(img, tuple(corner[6]), tuple(corner[2]), color, 2) 67 | img = cv2.line(img, tuple(corner[3]), tuple(corner[7]), color, 2) 68 | img = cv2.line(img, tuple(corner[6]), tuple(corner[2]), color, 2) 69 | img = cv2.line(img, tuple(corner[4]), tuple(corner[0]), color, 2) 70 | img = cv2.line(img, tuple(corner[4]), tuple(corner[6]), color, 2) 71 | img = cv2.line(img, tuple(corner[6]), tuple(corner[7]), color, 2) 72 | img = cv2.line(img, tuple(corner[1]), tuple(corner[5]), color, 2) 73 | 74 | 75 | return img 76 | 77 | def project_p3d( p3d, cam_scale, K): 78 | if p3d.shape[1]<4: 79 | p3d = p3d * cam_scale 80 | p2d = np.dot(p3d, K.T) 81 | p2d_3 = p2d[:, 2] 82 | p2d_3[np.where(p2d_3 < 1e-8)] = 1.0 83 | p2d[:, 2] = p2d_3 84 | p2d = np.around((p2d[:, :2] / p2d[:, 2:])).astype(np.int32) 85 | return p2d 86 | else: 87 | p3d = p3d * cam_scale 88 | p2d = np.dot(p3d[: , 0:3], K.T) 89 | p2d_3 = p2d[:, 2] 90 | filter = np.where(p2d_3 < 1e-8) 91 | if filter[0].shape[0]>0: 92 | p2d_rgbs = p3d[filter, 3:6] 93 | p2d_3[filter] = 1.0 94 | else: 95 | p2d_rgbs = p3d[:, 3:6] 96 | p2d[:, 2] = p2d_3 97 | p2d = np.around((p2d[:, :2] / p2d[:, 2:])).astype(np.int32) 98 | 99 | return np.concatenate((p2d, p2d_rgbs), axis=1).astype(np.int32) 100 | 101 | #Function to project depth to pointcloud 102 | def dpt_2_cld( depth_frame, K,segMask = None,cam_scale=1): 103 | 104 | w = depth_frame.shape[1] 105 | h = depth_frame.shape[0] 106 | 107 | xmap = np.array([[j for i in range(w)] for j in range(h)]) 108 | ymap = np.array([[i for i in range(w)] for j in range(h)]) 109 | 110 | dpt = np.array(depth_frame, dtype=np.float32) 111 | dpt = dpt/1000 112 | 113 | if len(dpt.shape) > 2: 114 | dpt = dpt[:, :, 0] 115 | msk_dp = dpt > -1 116 | choose = msk_dp.flatten().nonzero()[0].astype(np.uint32) 117 | 118 | if len(choose) < 1: 119 | return None, None 120 | 121 | dpt_mskd = dpt.flatten()[choose][:, np.newaxis].astype(np.float32) 122 | xmap_mskd = xmap.flatten()[choose][:, np.newaxis].astype(np.float32) 123 | ymap_mskd = ymap.flatten()[choose][:, np.newaxis].astype(np.float32) 124 | cam_cx, cam_cy = K[0][2], K[1][2] 125 | cam_fx, cam_fy = K[0][0], K[1][1] 126 | 127 | if segMask is not None: 128 | 129 | focus_points = np.argwhere(segMask != 0) 130 | focus = segMask != 0 131 | 132 | # projecting only the focus 133 | pt2 = dpt_mskd[focus.flatten()] / cam_scale 134 | pt0b= (ymap_mskd[focus.flatten()] - cam_cx) * pt2 / cam_fx 135 | pt1b= (xmap_mskd[focus.flatten()] - cam_cy) * pt2 / cam_fy 136 | focus_points = np.concatenate((pt0b, pt1b, pt2),axis=1) 137 | 138 | return focus_points , choose 139 | 140 | else : 141 | 142 | # projecting the cloud as a whole 143 | pt2 = dpt_mskd / cam_scale 144 | pt0 = (ymap_mskd - cam_cx) * pt2 / cam_fx 145 | pt1 = (xmap_mskd - cam_cy) * pt2 / cam_fy 146 | cld = np.concatenate((pt0, pt1, pt2),axis=1) 147 | 148 | return cld , choose -------------------------------------------------------------------------------- /data_generation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(data_generation) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED) 11 | 12 | ## System dependencies are found with CMake's conventions 13 | # find_package(Boost REQUIRED COMPONENTS system) 14 | 15 | 16 | ## Uncomment this if the package has a setup.py. This macro ensures 17 | ## modules and global scripts declared therein get installed 18 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 19 | # catkin_python_setup() 20 | 21 | ################################################ 22 | ## Declare ROS messages, services and actions ## 23 | ################################################ 24 | 25 | ## To declare and build messages, services or actions from within this 26 | ## package, follow these steps: 27 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 28 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 29 | ## * In the file package.xml: 30 | ## * add a build_depend tag for "message_generation" 31 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 32 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 33 | ## but can be declared for certainty nonetheless: 34 | ## * add a exec_depend tag for "message_runtime" 35 | ## * In this file (CMakeLists.txt): 36 | ## * add "message_generation" and every package in MSG_DEP_SET to 37 | ## find_package(catkin REQUIRED COMPONENTS ...) 38 | ## * add "message_runtime" and every package in MSG_DEP_SET to 39 | ## catkin_package(CATKIN_DEPENDS ...) 40 | ## * uncomment the add_*_files sections below as needed 41 | ## and list every .msg/.srv/.action file to be processed 42 | ## * uncomment the generate_messages entry below 43 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 44 | 45 | ## Generate messages in the 'msg' folder 46 | # add_message_files( 47 | # FILES 48 | # Message1.msg 49 | # Message2.msg 50 | # ) 51 | 52 | ## Generate services in the 'srv' folder 53 | # add_service_files( 54 | # FILES 55 | # Service1.srv 56 | # Service2.srv 57 | # ) 58 | 59 | ## Generate actions in the 'action' folder 60 | # add_action_files( 61 | # FILES 62 | # Action1.action 63 | # Action2.action 64 | # ) 65 | 66 | ## Generate added messages and services with any dependencies listed here 67 | # generate_messages( 68 | # DEPENDENCIES 69 | # std_msgs # Or other packages containing msgs 70 | # ) 71 | 72 | ################################################ 73 | ## Declare ROS dynamic reconfigure parameters ## 74 | ################################################ 75 | 76 | ## To declare and build dynamic reconfigure parameters within this 77 | ## package, follow these steps: 78 | ## * In the file package.xml: 79 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 80 | ## * In this file (CMakeLists.txt): 81 | ## * add "dynamic_reconfigure" to 82 | ## find_package(catkin REQUIRED COMPONENTS ...) 83 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 84 | ## and list every .cfg file to be processed 85 | 86 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 87 | # generate_dynamic_reconfigure_options( 88 | # cfg/DynReconf1.cfg 89 | # cfg/DynReconf2.cfg 90 | # ) 91 | 92 | ################################### 93 | ## catkin specific configuration ## 94 | ################################### 95 | ## The catkin_package macro generates cmake config files for your package 96 | ## Declare things to be passed to dependent projects 97 | ## INCLUDE_DIRS: uncomment this if your package contains header files 98 | ## LIBRARIES: libraries you create in this project that dependent projects also need 99 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 100 | ## DEPENDS: system dependencies of this project that dependent projects also need 101 | catkin_package( 102 | # INCLUDE_DIRS include 103 | # LIBRARIES data_generation 104 | # CATKIN_DEPENDS other_catkin_pkg 105 | # DEPENDS system_lib 106 | ) 107 | 108 | ########### 109 | ## Build ## 110 | ########### 111 | 112 | ## Specify additional locations of header files 113 | ## Your package locations should be listed before other locations 114 | include_directories( 115 | # include 116 | # ${catkin_INCLUDE_DIRS} 117 | ) 118 | 119 | ## Declare a C++ library 120 | # add_library(${PROJECT_NAME} 121 | # src/${PROJECT_NAME}/data_generation.cpp 122 | # ) 123 | 124 | ## Add cmake target dependencies of the library 125 | ## as an example, code may need to be generated before libraries 126 | ## either from message generation or dynamic reconfigure 127 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 128 | 129 | ## Declare a C++ executable 130 | ## With catkin_make all packages are built within a single CMake context 131 | ## The recommended prefix ensures that target names across packages don't collide 132 | # add_executable(${PROJECT_NAME}_node src/data_generation_node.cpp) 133 | 134 | ## Rename C++ executable without prefix 135 | ## The above recommended prefix causes long target names, the following renames the 136 | ## target back to the shorter version for ease of user use 137 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 138 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 139 | 140 | ## Add cmake target dependencies of the executable 141 | ## same as for the library above 142 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 143 | 144 | ## Specify libraries to link a library or executable target against 145 | # target_link_libraries(${PROJECT_NAME}_node 146 | # ${catkin_LIBRARIES} 147 | # ) 148 | 149 | ############# 150 | ## Install ## 151 | ############# 152 | 153 | # all install targets should use catkin DESTINATION variables 154 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 155 | 156 | ## Mark executable scripts (Python etc.) for installation 157 | ## in contrast to setup.py, you can choose the destination 158 | # catkin_install_python(PROGRAMS 159 | # scripts/my_python_script 160 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 161 | # ) 162 | 163 | ## Mark executables for installation 164 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 165 | # install(TARGETS ${PROJECT_NAME}_node 166 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 167 | # ) 168 | 169 | ## Mark libraries for installation 170 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 171 | # install(TARGETS ${PROJECT_NAME} 172 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 173 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 174 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 175 | # ) 176 | 177 | ## Mark cpp header files for installation 178 | # install(DIRECTORY include/${PROJECT_NAME}/ 179 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 180 | # FILES_MATCHING PATTERN "*.h" 181 | # PATTERN ".svn" EXCLUDE 182 | # ) 183 | 184 | ## Mark other files for installation (e.g. launch and bag files, etc.) 185 | # install(FILES 186 | # # myfile1 187 | # # myfile2 188 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 189 | # ) 190 | 191 | ############# 192 | ## Testing ## 193 | ############# 194 | 195 | ## Add gtest based cpp test target and link libraries 196 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_data_generation.cpp) 197 | # if(TARGET ${PROJECT_NAME}-test) 198 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 199 | # endif() 200 | 201 | ## Add folders to be run by python nosetests 202 | # catkin_add_nosetests(test) 203 | -------------------------------------------------------------------------------- /Fidget_Ground_truth_preprocess.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": 2, 6 | "id": "8582d56b", 7 | "metadata": {}, 8 | "outputs": [ 9 | { 10 | "name": "stdout", 11 | "output_type": "stream", 12 | "text": [ 13 | "Jupyter environment detected. Enabling Open3D WebVisualizer.\n", 14 | "[Open3D INFO] WebRTC GUI backend enabled.\n", 15 | "[Open3D INFO] WebRTCWindowSystem: HTTP handshake server disabled.\n" 16 | ] 17 | } 18 | ], 19 | "source": [ 20 | "import matplotlib.pyplot as plt\n", 21 | "import cv2\n", 22 | "import open3d as o3d\n", 23 | "#from open3d.web_visualizer import draw\n", 24 | "from scipy.spatial.transform import Rotation as Rot\n", 25 | "import numpy as np\n", 26 | "import os\n", 27 | "from bop_toolkit_lib import config\n", 28 | "from bop_toolkit_lib import dataset_params\n", 29 | "from bop_toolkit_lib import inout\n", 30 | "from bop_toolkit_lib import misc\n", 31 | "import copy" 32 | ] 33 | }, 34 | { 35 | "cell_type": "code", 36 | "execution_count": 3, 37 | "id": "ee58eacd", 38 | "metadata": {}, 39 | "outputs": [ 40 | { 41 | "name": "stdout", 42 | "output_type": "stream", 43 | "text": [ 44 | "fidget_dataset/model_meshes/left_gear.ply\n", 45 | "fidget_dataset/model_meshes/table1.ply\n", 46 | "fidget_dataset/model_meshes/right_gear.ply\n", 47 | "fidget_dataset/model_meshes/stage_2.ply\n", 48 | "fidget_dataset/model_meshes/stage_1.ply\n", 49 | "fidget_dataset/model_meshes/stage_3.ply\n", 50 | "fidget_dataset/model_meshes/stage_4.ply\n", 51 | "fidget_dataset/model_meshes/top_casing.ply\n", 52 | "fidget_dataset/model_meshes/bottom_casing.ply\n", 53 | "{'left_gear.ply': TriangleMesh with 9082 points and 18164 triangles., 'table1.ply': TriangleMesh with 19496 points and 39204 triangles., 'right_gear.ply': TriangleMesh with 12962 points and 25924 triangles., 'stage_2.ply': TriangleMesh with 11302 points and 22598 triangles., 'stage_1.ply': TriangleMesh with 2220 points and 4434 triangles., 'stage_3.ply': TriangleMesh with 24264 points and 48522 triangles., 'stage_4.ply': TriangleMesh with 25396 points and 50782 triangles., 'top_casing.ply': TriangleMesh with 1132 points and 2260 triangles., 'bottom_casing.ply': TriangleMesh with 2220 points and 4434 triangles.}\n" 54 | ] 55 | } 56 | ], 57 | "source": [ 58 | "dataset = \"fidget_dataset\"\n", 59 | "mesh_directory = dataset+'/model_meshes'\n", 60 | "mesh_list = {}\n", 61 | "mesh_path_list =[]\n", 62 | "\n", 63 | "for filename in os.listdir(mesh_directory):\n", 64 | " f = os.path.join(mesh_directory, filename)\n", 65 | " mesh_path_list.append(f)\n", 66 | " mesh = o3d.io.read_triangle_mesh(f)\n", 67 | " mesh_list[str(os.path.basename(f))]= mesh\n", 68 | " # checking if it is a file\n", 69 | " if os.path.isfile(f):\n", 70 | " print(f)\n", 71 | "\n", 72 | "print(mesh_list)\n", 73 | "\n", 74 | "assembling_order = ['bottom_casing.ply','left_gear.ply','right_gear.ply','top_casing.ply']\n", 75 | "#mesh = o3d.io.read_triangle_mesh(\"top_casing.ply\")" 76 | ] 77 | }, 78 | { 79 | "cell_type": "code", 80 | "execution_count": 4, 81 | "id": "eb37a06c", 82 | "metadata": {}, 83 | "outputs": [], 84 | "source": [ 85 | "# Create the assembly\n", 86 | "base_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.01, origin=[0, 0, 0])\n", 87 | "transformations = {}\n", 88 | "\n", 89 | "def adjust_mesh(name,color,R=[0,0,0],t=[0,0,0]):\n", 90 | " mesh = mesh_list[name]\n", 91 | " T_or = Rot.from_euler('zyx',R).as_matrix()\n", 92 | " T_pos = np.array(t).reshape(3,1)\n", 93 | " T= np.concatenate((T_or, T_pos), axis = 1) \n", 94 | " T= np.vstack((T, [0,0,0,1] ))\n", 95 | " transformations[name] = T\n", 96 | " mesh.transform(T)\n", 97 | " mesh.paint_uniform_color(color)\n", 98 | " \n", 99 | " return mesh\n", 100 | "\n", 101 | "bottom_casing = adjust_mesh(name='bottom_casing.ply', color=[0.561, 0.522, 0.255])\n", 102 | "left_gear = adjust_mesh(name='left_gear.ply', color=[0.561, 0.522, 0.255],t=[0.0115,0,0.005])\n", 103 | "right_gear = adjust_mesh(name='right_gear.ply',color=[0.561, 0.522, 0.255],t=[-0.0115,0,0.005])\n", 104 | "top_casing = adjust_mesh(name='top_casing.ply',color=[0.561, 0.522, 0.255],R =[0.0, 0.0, 3.14],t=[0.00, 0,0.012])\n", 105 | "\n", 106 | "\n", 107 | "o3d.visualization.draw_geometries([bottom_casing, left_gear],mesh_show_wireframe=True)\n", 108 | "#o3d.visualization.draw_geometries([bottom_casing, top_casing,left_gear, right_gear,base_frame],mesh_show_wireframe=True)\n" 109 | ] 110 | }, 111 | { 112 | "cell_type": "code", 113 | "execution_count": 6, 114 | "id": "7db69ab6", 115 | "metadata": {}, 116 | "outputs": [], 117 | "source": [ 118 | "# generate intermediate assembly states and save them as mesh files\n", 119 | "cm_1 = bottom_casing + left_gear\n", 120 | "#o3d.io.write_triangle_mesh(mesh_directory+\"/stage_2.ply\", cm_1)\n", 121 | "\n", 122 | "cm_2 = cm_1 + right_gear\n", 123 | "#o3d.io.write_triangle_mesh(mesh_directory+\"/stage_3.ply\", cm_2)\n", 124 | "\n", 125 | "cm_3 = cm_2 + top_casing\n", 126 | "#o3d.io.write_triangle_mesh(mesh_directory+\"/stage_4.ply\", cm_3)\n", 127 | "\n", 128 | "o3d.visualization.draw_geometries([bottom_casing], mesh_show_wireframe=True)" 129 | ] 130 | }, 131 | { 132 | "cell_type": "code", 133 | "execution_count": null, 134 | "id": "4bf2e587", 135 | "metadata": {}, 136 | "outputs": [], 137 | "source": [ 138 | "# generate assembly_poses_gt.json\n", 139 | "import json\n", 140 | "with open(\"gt_assembly_poses.json\", \"w\") as outfile:\n", 141 | " assembly_poses ={}\n", 142 | " for i, name in enumerate(assembling_order):\n", 143 | " obj_data = {\n", 144 | " \"obj_id\" : i,\n", 145 | " \"obj_name\": name, \n", 146 | " \"R_wrt_bottom_casing\" : np.ravel(np.reshape(transformations[name][0:3,0:3],(1,9))).tolist(),\n", 147 | " \"t_wrt_bottom_casing\" : (transformations[name][0:3,3]*1000).tolist()\n", 148 | " }\n", 149 | " assembly_poses[i]= obj_data\n", 150 | " json.dump(assembly_poses, outfile)\n", 151 | " \n", 152 | "#print(assembly_poses)" 153 | ] 154 | }, 155 | { 156 | "cell_type": "code", 157 | "execution_count": null, 158 | "id": "81e2b5c7", 159 | "metadata": {}, 160 | "outputs": [], 161 | "source": [ 162 | "models_info = {}\n", 163 | "for obj_id, name in enumerate(assembling_order):\n", 164 | " \n", 165 | " misc.log('Processing model of object {}...'.format(name))\n", 166 | " \n", 167 | " model = inout.load_ply(mesh_directory+'/'+name)\n", 168 | " ref_pt = model['pts'].min(axis=0).flatten()\n", 169 | " size =(model['pts'].max(axis=0) - ref_pt)\n", 170 | "\n", 171 | " # Calculated diameter.\n", 172 | " diameter = misc.calc_pts_diameter(model['pts'])\n", 173 | "\n", 174 | " models_info[obj_id] = {\n", 175 | " 'min_x': ref_pt[0], 'min_y': ref_pt[1], 'min_z': ref_pt[2],\n", 176 | " 'size_x': size[0], 'size_y': size[1], 'size_z': size[2],\n", 177 | " 'diameter': diameter\n", 178 | " }\n", 179 | "\n", 180 | "\n", 181 | "# Save the calculated info about the object models.\n", 182 | "inout.save_json(dataset+'/models_info.json', models_info)" 183 | ] 184 | } 185 | ], 186 | "metadata": { 187 | "kernelspec": { 188 | "display_name": "singledemo", 189 | "language": "python", 190 | "name": "singledemo" 191 | }, 192 | "language_info": { 193 | "codemirror_mode": { 194 | "name": "ipython", 195 | "version": 3 196 | }, 197 | "file_extension": ".py", 198 | "mimetype": "text/x-python", 199 | "name": "python", 200 | "nbconvert_exporter": "python", 201 | "pygments_lexer": "ipython3", 202 | "version": "3.8.13" 203 | } 204 | }, 205 | "nbformat": 4, 206 | "nbformat_minor": 5 207 | } 208 | -------------------------------------------------------------------------------- /Nema17_dataset_GT_preprocess.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": 40, 6 | "metadata": {}, 7 | "outputs": [ 8 | { 9 | "name": "stdout", 10 | "output_type": "stream", 11 | "text": [ 12 | "Nema17_reducer_dataset/model_meshes/housing.ply\n", 13 | "Nema17_reducer_dataset/model_meshes/sun_gear.ply\n", 14 | "Nema17_reducer_dataset/model_meshes/stage_2.ply\n", 15 | "Nema17_reducer_dataset/model_meshes/planet.ply\n", 16 | "Nema17_reducer_dataset/model_meshes/carrier.ply\n", 17 | "Nema17_reducer_dataset/model_meshes/stage_3.ply\n", 18 | "Nema17_reducer_dataset/model_meshes/Nema17.ply\n", 19 | "Nema17_reducer_dataset/model_meshes/cover.ply\n", 20 | "Nema17_reducer_dataset/model_meshes/stage_4.ply\n", 21 | "{'housing.ply': TriangleMesh with 2314 points and 4708 triangles., 'sun_gear.ply': TriangleMesh with 660 points and 1332 triangles., 'stage_2.ply': TriangleMesh with 2344 points and 4711 triangles., 'planet.ply': TriangleMesh with 476 points and 952 triangles., 'carrier.ply': TriangleMesh with 408 points and 828 triangles., 'stage_3.ply': TriangleMesh with 4658 points and 9419 triangles., 'Nema17.ply': TriangleMesh with 1684 points and 3379 triangles., 'cover.ply': TriangleMesh with 468 points and 952 triangles., 'stage_4.ply': TriangleMesh with 5534 points and 11199 triangles.}\n" 22 | ] 23 | } 24 | ], 25 | "source": [ 26 | "import matplotlib.pyplot as plt\n", 27 | "import cv2\n", 28 | "import open3d as o3d\n", 29 | "#from open3d.web_visualizer import draw\n", 30 | "from scipy.spatial.transform import Rotation as Rot\n", 31 | "import numpy as np\n", 32 | "import os\n", 33 | "from bop_toolkit_lib import config\n", 34 | "from bop_toolkit_lib import dataset_params\n", 35 | "from bop_toolkit_lib import inout\n", 36 | "from bop_toolkit_lib import misc\n", 37 | "import copy\n", 38 | "\n", 39 | "dataset = \"Nema17_reducer_dataset\"\n", 40 | "mesh_directory = dataset+'/model_meshes'\n", 41 | "mesh_list = {}\n", 42 | "mesh_path_list =[]\n", 43 | "\n", 44 | "for filename in os.listdir(mesh_directory):\n", 45 | " f = os.path.join(mesh_directory, filename)\n", 46 | " mesh_path_list.append(f)\n", 47 | " mesh = o3d.io.read_triangle_mesh(f)\n", 48 | " mesh_list[str(os.path.basename(f))]= mesh\n", 49 | " # checking if it is a file\n", 50 | " if os.path.isfile(f):\n", 51 | " print(f)\n", 52 | "\n", 53 | "print(mesh_list)\n", 54 | "\n", 55 | "assembling_order = ['Nema17.ply','sun_gear.ply','housing.ply','carrier.ply','cover.ply']" 56 | ] 57 | }, 58 | { 59 | "cell_type": "code", 60 | "execution_count": 81, 61 | "metadata": {}, 62 | "outputs": [], 63 | "source": [ 64 | "base_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=0.01, origin=[0, 0, 0])\n", 65 | "transformations = {}\n", 66 | "\n", 67 | "def adjust_mesh(name,color,R=[0,0,0],t=[0,0,0]):\n", 68 | " mesh = mesh_list[name]\n", 69 | " T_or = Rot.from_euler('zyx',R).as_matrix()\n", 70 | " T_pos = np.array(t).reshape(3,1)\n", 71 | " T= np.concatenate((T_or, T_pos), axis = 1) \n", 72 | " T= np.vstack((T, [0,0,0,1] ))\n", 73 | " transformations[name] = T\n", 74 | " mesh = copy.deepcopy(mesh).transform(T)\n", 75 | " mesh.paint_uniform_color(color)\n", 76 | " \n", 77 | " return mesh\n", 78 | "\n", 79 | "Nema17 = adjust_mesh(name='Nema17.ply', color=[1, 0.706, 0])\n", 80 | "sun_gear = adjust_mesh(name='sun_gear.ply', color=[0.5, 0.106, 0],t=[0.0,0.0,0.0585])\n", 81 | "housing = adjust_mesh(name='housing.ply',color=[0.7, 0.806, 0],t=[0,0,0.0505])\n", 82 | "carrier = adjust_mesh(name='carrier.ply',color=[0.9, 0.406, 0],R =[0.0, 0.0, 0],t=[0.00, 0,0.0775])\n", 83 | "cover = adjust_mesh(name='cover.ply',color=[0.9, 0.406, 0],R =[0.0, 0.0, 0],t=[0.00, 0,0.0845])\n", 84 | "\n", 85 | "\n", 86 | "o3d.visualization.draw_geometries([Nema17, sun_gear, housing,carrier, cover])" 87 | ] 88 | }, 89 | { 90 | "cell_type": "code", 91 | "execution_count": 85, 92 | "metadata": {}, 93 | "outputs": [ 94 | { 95 | "data": { 96 | "text/plain": [ 97 | "True" 98 | ] 99 | }, 100 | "execution_count": 85, 101 | "metadata": {}, 102 | "output_type": "execute_result" 103 | } 104 | ], 105 | "source": [ 106 | "cm_1 = Nema17 + sun_gear\n", 107 | "o3d.io.write_triangle_mesh(mesh_directory+\"/stage_2.ply\", cm_1)\n", 108 | "\n", 109 | "cm_2 = cm_1 + housing\n", 110 | "o3d.io.write_triangle_mesh(mesh_directory+\"/stage_3.ply\", cm_2)\n", 111 | "\n", 112 | "cm_3 = cm_2 + carrier\n", 113 | "o3d.io.write_triangle_mesh(mesh_directory+\"/stage_4.ply\", cm_3)\n", 114 | "\n", 115 | "cm_4 = cm_3 + cover\n", 116 | "o3d.io.write_triangle_mesh(mesh_directory+\"/stage_4.ply\", cm_4)" 117 | ] 118 | }, 119 | { 120 | "cell_type": "code", 121 | "execution_count": 86, 122 | "metadata": {}, 123 | "outputs": [], 124 | "source": [ 125 | "# generate assembly_poses_gt.json\n", 126 | "import json\n", 127 | "with open(os.path.join(dataset, \"gt_assembly_poses.json\"), \"w\") as outfile:\n", 128 | " assembly_poses ={}\n", 129 | " for i, name in enumerate(assembling_order):\n", 130 | " obj_data = {\n", 131 | " \"obj_id\" : i,\n", 132 | " \"obj_name\": name, \n", 133 | " \"R_wrt_bottom_casing\" : np.ravel(np.reshape(transformations[name][0:3,0:3],(1,9))).tolist(),\n", 134 | " \"t_wrt_bottom_casing\" : (transformations[name][0:3,3]*1000).tolist()\n", 135 | " }\n", 136 | " assembly_poses[i]= obj_data\n", 137 | " json.dump(assembly_poses, outfile)" 138 | ] 139 | }, 140 | { 141 | "cell_type": "code", 142 | "execution_count": 87, 143 | "metadata": {}, 144 | "outputs": [ 145 | { 146 | "name": "stdout", 147 | "output_type": "stream", 148 | "text": [ 149 | "8/18|05:48:12: Processing model of object Nema17.ply...\n", 150 | "8/18|05:48:12: Processing model of object sun_gear.ply...\n", 151 | "8/18|05:48:12: Processing model of object housing.ply...\n", 152 | "8/18|05:48:12: Processing model of object carrier.ply...\n", 153 | "8/18|05:48:12: Processing model of object cover.ply...\n" 154 | ] 155 | } 156 | ], 157 | "source": [ 158 | "# create models info json for error calculations\n", 159 | "\n", 160 | "models_info = {}\n", 161 | "for obj_id, name in enumerate(assembling_order):\n", 162 | " \n", 163 | " misc.log('Processing model of object {}...'.format(name))\n", 164 | " \n", 165 | " model = inout.load_ply(mesh_directory+'/'+name)\n", 166 | " ref_pt = model['pts'].min(axis=0).flatten()\n", 167 | " size =(model['pts'].max(axis=0) - ref_pt)\n", 168 | "\n", 169 | " # Calculated diameter.\n", 170 | " diameter = misc.calc_pts_diameter(model['pts'])\n", 171 | "\n", 172 | " models_info[obj_id] = {\n", 173 | " 'min_x': ref_pt[0], 'min_y': ref_pt[1], 'min_z': ref_pt[2],\n", 174 | " 'size_x': size[0], 'size_y': size[1], 'size_z': size[2],\n", 175 | " 'diameter': diameter\n", 176 | " }\n", 177 | "\n", 178 | "\n", 179 | "# Save the calculated info about the object models.\n", 180 | "inout.save_json(dataset+'/models_info.json', models_info)" 181 | ] 182 | }, 183 | { 184 | "cell_type": "code", 185 | "execution_count": null, 186 | "metadata": {}, 187 | "outputs": [], 188 | "source": [] 189 | } 190 | ], 191 | "metadata": { 192 | "kernelspec": { 193 | "display_name": "singledemo", 194 | "language": "python", 195 | "name": "singledemo" 196 | }, 197 | "language_info": { 198 | "codemirror_mode": { 199 | "name": "ipython", 200 | "version": 3 201 | }, 202 | "file_extension": ".py", 203 | "mimetype": "text/x-python", 204 | "name": "python", 205 | "nbconvert_exporter": "python", 206 | "pygments_lexer": "ipython3", 207 | "version": "3.8.13" 208 | } 209 | }, 210 | "nbformat": 4, 211 | "nbformat_minor": 4 212 | } 213 | -------------------------------------------------------------------------------- /data_generation/Worlds/Nema17.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 1 8 | 0 0 10 0 -0 0 9 | 0.8 0.8 0.8 1 10 | 0.1 0.1 0.1 1 11 | 12 | 1000 13 | 0.9 14 | 0.01 15 | 0.001 16 | 17 | -0.5 0.5 -1 18 | 19 | 20 | 21 | 22 | 1 23 | 24 | 25 | 26 | 27 | 0 0 1 28 | 100 100 29 | 30 | 31 | 32 | 33 | 34 | 100 35 | 50 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 10 47 | 48 | 49 | 0 50 | 51 | 52 | 0 0 1 53 | 100 100 54 | 55 | 56 | 57 | 61 | 62 | 63 | 0 64 | 0 65 | 1 66 | 67 | 68 | 69 | 70 | 0 0 -9.8 71 | 6e-06 2.3e-05 -4.2e-05 72 | 73 | 74 | 0.001 75 | 1 76 | 1000 77 | 78 | 79 | 0.4 0.4 0.4 1 80 | 0.7 0.7 0.7 1 81 | 1 82 | 83 | 84 | EARTH_WGS84 85 | 0 86 | 0 87 | 0 88 | 0 89 | 90 | 91 | 92 | 93 | 0 0 2 0 -0 0 94 | 0.2 0.2 0.2 1 95 | 0.1 0.1 0.1 1 96 | 97 | 20 98 | 0.5 99 | 0.01 100 | 0.001 101 | 102 | 0 103 | 0 0 -1 104 | 105 | 106 | 113 | 114 | 115 | 116 | 117 | model://table1 118 | table1 119 | -0.475 0 0 0 0 0 120 | 1 121 | 122 | 123 | 124 | 125 | model://Nema/carrier 126 | carrier 127 | -0.04 0.04 0.845 0 0 0 128 | 1 129 | 130 | 131 | 132 | model://Nema/cover 133 | cover 134 | 0.03 -0.05 0.845 0 0 0 135 | 1 136 | 137 | 138 | 139 | model://Nema/housing 140 | housing 141 | -0.04 -0.05 0.845 0 0 0 142 | 1 143 | 144 | 145 | 146 | model://Nema/sun_gear 147 | sun_gear 148 | 0 0.04 0.845 0 0 0 149 | 1 150 | 151 | 152 | 180 | 181 | 209 | 210 | 238 | 239 | 271 | 272 | model://Nema/Nema17 273 | Nema17 274 | 0.04 0.04 0.845 0 0 0 275 | 1 276 | 277 | 278 | 279 | 280 | 2.16101 -0.608536 1.91511 0 0.589796 2.59702 281 | orbit 282 | perspective 283 | 284 | 285 | 286 | 287 | -------------------------------------------------------------------------------- /echo_tfs.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # tf2 echo code Copyright (c) 2018, Lucas Walter 4 | # transformations.py code Copyright (c) 2006-2017, Christoph Gohlke 5 | # transformations.py code Copyright (c) 2006-2017, The Regents of the University of California 6 | # Produced at the Laboratory for Fluorescence Dynamics 7 | # All rights reserved. 8 | # 9 | # Redistribution and use in source and binary forms, with or without 10 | # modification, are permitted provided that the following conditions are met: 11 | # 12 | # * Redistributions of source code must retain the above copyright 13 | # notice, this list of conditions and the following disclaimer. 14 | # * Redistributions in binary form must reproduce the above copyright 15 | # notice, this list of conditions and the following disclaimer in the 16 | # documentation and/or other materials provided with the distribution. 17 | # * Neither the name of the copyright holders nor the names of any 18 | # contributors may be used to endorse or promote products derived 19 | # from this software without specific prior written permission. 20 | # 21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 25 | # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 26 | # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 27 | # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 28 | # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 29 | # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 30 | # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | # POSSIBILITY OF SUCH DAMAGE. 32 | 33 | from __future__ import print_function 34 | 35 | import argparse 36 | import math 37 | import numpy 38 | import rospy 39 | import sys 40 | import tf2_py as tf2 41 | import tf2_ros 42 | from std_msgs.msg import String 43 | from geometry_msgs.msg import TransformStamped, PoseStamped, Pose 44 | # https://github.com/ros/geometry2/issues/222 45 | # from tf import transformations 46 | 47 | """ 48 | The following euler conversion functions are from https://github.com/matthew-brett/transforms3d 49 | which adapted it from transformations.py, it is needed here until transforms3d is available 50 | as a dependency. 51 | They are for internal use only. 52 | """ 53 | 54 | # epsilon for testing whether a number is close to zero 55 | _EPS = numpy.finfo(float).eps * 4.0 56 | 57 | # axis sequences for Euler angles 58 | _NEXT_AXIS = [1, 2, 0, 1] 59 | 60 | # TODO(lucasw) if sxyz works then eliminate the other possibilities 61 | # map axes strings to/from tuples of inner axis, parity, repetition, frame 62 | _AXES2TUPLE = { 63 | 'sxyz': (0, 0, 0, 0), 'sxyx': (0, 0, 1, 0), 'sxzy': (0, 1, 0, 0), 64 | 'sxzx': (0, 1, 1, 0), 'syzx': (1, 0, 0, 0), 'syzy': (1, 0, 1, 0), 65 | 'syxz': (1, 1, 0, 0), 'syxy': (1, 1, 1, 0), 'szxy': (2, 0, 0, 0), 66 | 'szxz': (2, 0, 1, 0), 'szyx': (2, 1, 0, 0), 'szyz': (2, 1, 1, 0), 67 | 'rzyx': (0, 0, 0, 1), 'rxyx': (0, 0, 1, 1), 'ryzx': (0, 1, 0, 1), 68 | 'rxzx': (0, 1, 1, 1), 'rxzy': (1, 0, 0, 1), 'ryzy': (1, 0, 1, 1), 69 | 'rzxy': (1, 1, 0, 1), 'ryxy': (1, 1, 1, 1), 'ryxz': (2, 0, 0, 1), 70 | 'rzxz': (2, 0, 1, 1), 'rxyz': (2, 1, 0, 1), 'rzyz': (2, 1, 1, 1)} 71 | 72 | def _euler_from_matrix(matrix, axes='sxyz'): 73 | """temporaray import from https://github.com/matthew-brett/transforms3d/blob/master/transforms3d/_gohlketransforms.py for internal use only""" 74 | try: 75 | firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()] 76 | except (AttributeError, KeyError): 77 | _TUPLE2AXES[axes] # validation 78 | firstaxis, parity, repetition, frame = axes 79 | 80 | i = firstaxis 81 | j = _NEXT_AXIS[i+parity] 82 | k = _NEXT_AXIS[i-parity+1] 83 | 84 | M = numpy.array(matrix, dtype=numpy.float64, copy=False)[:3, :3] 85 | if repetition: 86 | sy = math.sqrt(M[i, j]*M[i, j] + M[i, k]*M[i, k]) 87 | if sy > _EPS: 88 | ax = math.atan2( M[i, j], M[i, k]) 89 | ay = math.atan2( sy, M[i, i]) 90 | az = math.atan2( M[j, i], -M[k, i]) 91 | else: 92 | ax = math.atan2(-M[j, k], M[j, j]) 93 | ay = math.atan2( sy, M[i, i]) 94 | az = 0.0 95 | else: 96 | cy = math.sqrt(M[i, i]*M[i, i] + M[j, i]*M[j, i]) 97 | if cy > _EPS: 98 | ax = math.atan2( M[k, j], M[k, k]) 99 | ay = math.atan2(-M[k, i], cy) 100 | az = math.atan2( M[j, i], M[i, i]) 101 | else: 102 | ax = math.atan2(-M[j, k], M[j, j]) 103 | ay = math.atan2(-M[k, i], cy) 104 | az = 0.0 105 | 106 | if parity: 107 | ax, ay, az = -ax, -ay, -az 108 | if frame: 109 | ax, az = az, ax 110 | return ax, ay, az 111 | 112 | def _quaternion_matrix(quaternion): 113 | """temporaray import from https://github.com/matthew-brett/transforms3d/blob/master/transforms3d/_gohlketransforms.py for internal use only""" 114 | q = numpy.array(quaternion, dtype=numpy.float64, copy=True) 115 | n = numpy.dot(q, q) 116 | if n < _EPS: 117 | return numpy.identity(4) 118 | q *= math.sqrt(2.0 / n) 119 | q = numpy.outer(q, q) 120 | return numpy.array([ 121 | [1.0-q[2, 2]-q[3, 3], q[1, 2]-q[3, 0], q[1, 3]+q[2, 0], 0.0], 122 | [ q[1, 2]+q[3, 0], 1.0-q[1, 1]-q[3, 3], q[2, 3]-q[1, 0], 0.0], 123 | [ q[1, 3]-q[2, 0], q[2, 3]+q[1, 0], 1.0-q[1, 1]-q[2, 2], 0.0], 124 | [ 0.0, 0.0, 0.0, 1.0]]) 125 | 126 | def _euler_from_quaternion(quaternion, axes='sxyz'): 127 | """temporaray import from https://github.com/matthew-brett/transforms3d/blob/master/transforms3d/_gohlketransforms.py for internal use only""" 128 | return _euler_from_matrix(_quaternion_matrix(quaternion), axes) 129 | 130 | def _euler_from_quaternion_msg(quaternion): 131 | # the above code is from transform3 which changed convention from old transformations.py 132 | # from xyzw to wxyz 133 | # return transformations.euler_from_quaternion([quaternion.x, quaternion.y, quaternion.z, quaternion.w]) 134 | return _euler_from_quaternion([quaternion.w, 135 | quaternion.x, 136 | quaternion.y, 137 | quaternion.z]) 138 | 139 | class Echo(): 140 | def __init__(self, args): 141 | self.tf_buffer = tf2_ros.Buffer(cache_time=args.cache_time) 142 | self.tf_listener = tf2_ros.TransformListener(self.tf_buffer) 143 | self.args = args 144 | 145 | self.count = 0 146 | self.timer = rospy.Timer(rospy.Duration(1.0 / self.args.rate), self.lookup) 147 | self.pub = rospy.Publisher('Cam_2_World', PoseStamped, queue_size=100) 148 | 149 | def lookup(self, event): 150 | self.count += 1 151 | if self.args.limit: 152 | if self.count > self.args.limit: 153 | # TODO(lucasw) is there a better method to stop the spin()? 154 | rospy.signal_shutdown("tf echo finished") 155 | return 156 | 157 | cur_time = rospy.Time.now() 158 | # If the transform is from tf_static the ts.header.stamp will be 0.0 159 | # when offset == 0 or lookup_time is rospy.Time() 160 | if self.args.time: 161 | lookup_time = rospy.Time(self.args.time) 162 | elif self.args.offset: 163 | # If the transform is static this will always work 164 | lookup_time = cur_time + rospy.Duration(self.args.offset) 165 | else: 166 | # Get the most recent transform 167 | lookup_time = rospy.Time() 168 | 169 | try: 170 | ts = self.tf_buffer.lookup_transform(self.args.source_frame, 171 | self.args.target_frame, 172 | lookup_time) 173 | except tf2.LookupException as ex: 174 | msg = "At time {}, (current time {}) ".format(lookup_time.to_sec(), cur_time.to_sec()) 175 | rospy.logerr(msg + str(ex)) 176 | return 177 | except tf2.ExtrapolationException as ex: 178 | msg = "(current time {}) ".format(cur_time.to_sec()) 179 | rospy.logerr(msg + str(ex)) 180 | return 181 | 182 | # The old tf1 static_transform_publisher (which published into /tf, not /tf_static) 183 | # publishes transforms 0.5 seconds into future so the cur_time and header stamp 184 | # will be identical. 185 | msg = "At time {}, (current time {})".format(ts.header.stamp.to_sec(), cur_time.to_sec()) 186 | xyz = ts.transform.translation 187 | msg += "\n- Translation: [{:.{p}f}, {:.{p}f}, {:.{p}f}]\n".format(xyz.x, xyz.y, xyz.z, p=self.args.precision) 188 | quat = ts.transform.rotation 189 | msg += "- Rotation: in Quaternion [{:.{p}f}, {:.{p}f}, {:.{p}f}, {:.{p}f}]\n".format(quat.x, quat.y, quat.z, quat.w, p=self.args.precision) 190 | # TODO(lucasw) need to get quaternion to euler from somewhere, but not tf1 191 | # or a dependency that isn't in Ubuntu or ros repos 192 | euler = _euler_from_quaternion_msg(quat) 193 | msg += " in RPY (radian) " 194 | msg += "[{:.{p}f}, {:.{p}f}, {:.{p}f}]\n".format(euler[0], euler[1], euler[2], p=self.args.precision) 195 | msg += " in RPY (degree) " 196 | msg += "[{:.{p}f}, {:.{p}f}, {:.{p}f}]".format(math.degrees(euler[0]), 197 | math.degrees(euler[1]), 198 | math.degrees(euler[2]), p=self.args.precision) 199 | #print(msg) 200 | 201 | pose_msg = Pose() 202 | pose_msg.position = ts.transform.translation 203 | pose_msg.orientation.x = quat.x 204 | pose_msg.orientation.y = quat.y 205 | pose_msg.orientation.z = quat.z 206 | pose_msg.orientation.w = quat.w 207 | 208 | pose_stamped_msg = PoseStamped() 209 | pose_stamped_msg.header.frame_id = 'cam_2_world' 210 | pose_stamped_msg.header.stamp = rospy.Time.now() 211 | pose_stamped_msg.pose = pose_msg 212 | 213 | self.pub.publish(pose_stamped_msg) 214 | #print(pose_stamped_msg) 215 | 216 | def positive_float(x): 217 | x = float(x) 218 | if x <= 0.0: 219 | raise argparse.ArgumentTypeError("{} must be > 0.0".format(x)) 220 | return x 221 | 222 | def positive_int(x): 223 | x = int(x) 224 | if x <= 0: 225 | raise argparse.ArgumentTypeError("{} must be > 0".format(x)) 226 | return x 227 | 228 | if __name__ == '__main__': 229 | rospy.init_node("echo") 230 | 231 | other_args = rospy.myargv(argv=sys.argv) 232 | precision=3 233 | try: 234 | precision = rospy.get_param('~precision') 235 | rospy.loginfo("Precision default value was overriden, new value: %d", precision) 236 | except KeyError: 237 | pass 238 | 239 | parser = argparse.ArgumentParser() 240 | parser.add_argument("source_frame") # parent 241 | parser.add_argument("target_frame") # child 242 | parser.add_argument("-r", "--rate", 243 | help="update rate, must be > 0.0", 244 | default=1.0, 245 | type=positive_float) 246 | parser.add_argument("-c", "--cache_time", 247 | help="length of tf buffer cache in seconds", 248 | type=positive_float) 249 | parser.add_argument("-o", "--offset", 250 | help="offset the lookup from current time, ignored if using -t", 251 | type=float) 252 | parser.add_argument("-t", "--time", 253 | help="fixed time to do the lookup", 254 | type=float) 255 | parser.add_argument("-l", "--limit", 256 | help="lookup fixed number of times", 257 | type=positive_int) 258 | parser.add_argument("-p", "--precision", 259 | help="output precision", 260 | default=precision, 261 | type=positive_int) 262 | args = parser.parse_args(other_args[1:]) # Remove first arg 263 | echo = Echo(args) 264 | rospy.spin() -------------------------------------------------------------------------------- /6DAPose.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import numpy as np 3 | import open3d as o3d 4 | from scipy.spatial.transform import Rotation as R 5 | import numpy as np 6 | from PIL import Image 7 | import os 8 | import json 9 | import copy 10 | import pandas as pd 11 | from statistics import stdev, mean 12 | import time 13 | from bop_toolkit.bop_toolkit_lib import pose_error,misc 14 | from icp_utils import dpt_2_cld, prepare_dataset, execute_global_registration 15 | 16 | rgb_format = '.png' 17 | dpt_format = '.png' 18 | seg_format = '.npy' 19 | 20 | class this_dataset(): 21 | 22 | def __init__(self, stage, dataset): 23 | 24 | self.rt_dir = dataset 25 | self.base_dir= dataset+'/'+stage 26 | self.rgb_dir = dataset+'/'+stage+'/rgb' 27 | self.dpt_dir = dataset+'/'+stage+'/depth' 28 | self.seg_dir = dataset+'/'+stage+'/seg_maps' 29 | self.mesh_path = dataset +'/model_meshes/' 30 | self.pcld_path = dataset +'/model_pointcloud/' 31 | 32 | def get_item(self, im_id): 33 | 34 | #1. Read RGB frame and depth frame 35 | with Image.open(os.path.join(self.rgb_dir,str(im_id)+ rgb_format)) as ri: 36 | rgb = np.array(ri,dtype=np.uint8)[:, :, :3] 37 | 38 | 39 | with Image.open(os.path.join(self.dpt_dir,str(im_id)+ dpt_format)) as di: 40 | dpt = np.array(di) 41 | 42 | #2. Read segmentation map 43 | segmap = np.load(os.path.join(self.seg_dir,str(im_id)+'_seg_map'+seg_format)) 44 | 45 | #3. Read scene camera ground truth 46 | with open(os.path.join(self.base_dir,'scene_camera.json'),"r") as k: 47 | for i,j in enumerate(k): 48 | im_dict = json.loads(j) 49 | if i == int(im_id): 50 | this_cam = im_dict 51 | 52 | #4. Read scene ground truth wrt camera frame 53 | with open(os.path.join(self.base_dir,'scene_gt.json'),"r") as l: 54 | for i,j in enumerate(l): 55 | scene_dict = json.loads(j) 56 | if i == int(im_id): 57 | this_scene = scene_dict 58 | 59 | #5 Pose relative to base object 60 | with open(os.path.join(self.rt_dir,'gt_assembly_poses.json'),"r") as ap: 61 | pose_dict = json.load(ap) 62 | 63 | with open(os.path.join(self.rt_dir,'models_info.json'),"r") as m: 64 | models_info = json.load(m) 65 | 66 | with open(os.path.join(self.base_dir,'scene_w_gt.json'),"r") as n: 67 | for i,j in enumerate(n): 68 | scene_dict = json.loads(j) 69 | if i == int(im_id): 70 | this_scene_w = scene_dict 71 | 72 | return rgb, dpt, segmap, this_cam[im_id], pose_dict, this_scene_w[im_id], models_info, this_scene[im_id] 73 | 74 | def get_target_pcd(stage, cld ,this_cam): 75 | 76 | target_pcd_wrt_opt = o3d.geometry.PointCloud() 77 | target_pcd_wrt_opt.points = o3d.utility.Vector3dVector(cld) 78 | 79 | # tranformation matrix of depth_optical_frame wrt to world_frame 80 | dptopt_R_w2c = np.array(this_cam['dptopt_cam_R_w2c']).reshape(3,3) 81 | dptopt_t_w2c = np.array(this_cam['dptopt_cam_t_w2c']).reshape(3,1)*0.001 82 | dptopt_world_T = np.hstack((dptopt_R_w2c,dptopt_t_w2c)) 83 | dptopt_world_T = np.vstack((dptopt_world_T, [0,0,0,1])) 84 | 85 | 86 | target_pcd_wrt_world = target_pcd_wrt_opt.transform(dptopt_world_T) 87 | 88 | return target_pcd_wrt_world, dptopt_world_T 89 | 90 | def pcl_registration(target, source_mesh_,dpt_K, dptopt_world_T): 91 | 92 | cam_position = np.transpose(dptopt_world_T[:3,3]) 93 | source_mesh = copy.deepcopy(source_mesh_) 94 | mesh = o3d.t.geometry.TriangleMesh.from_legacy(source_mesh) 95 | mesh.translate(target.get_center()) 96 | 97 | initial_T = np.hstack((np.eye(3,3),np.array(target.get_center()).reshape(3,1))) 98 | initial_T = np.vstack((initial_T, [0,0,0,1])) 99 | 100 | scene = o3d.t.geometry.RaycastingScene() 101 | scene.add_triangles(mesh) 102 | 103 | # https://learnwebgl.brown37.net/07_cameras/camera_introduction.html 104 | rays = o3d.t.geometry.RaycastingScene.create_rays_pinhole( 105 | fov_deg = 60, 106 | center= target.get_center(), 107 | eye= cam_position, 108 | up=[0, 1, 0], 109 | width_px=1280, 110 | height_px=720,) 111 | 112 | ans = scene.cast_rays(rays) 113 | hit = ans['t_hit'].isfinite() 114 | points = rays[hit][:,:3] + rays[hit][:,3:]*ans['t_hit'][hit].reshape((-1,1)) 115 | source_pcd = o3d.t.geometry.PointCloud(points) 116 | source_pcd = o3d.t.geometry.PointCloud.to_legacy(source_pcd).paint_uniform_color([0, 0.651, 0.929]) 117 | 118 | pcd_world_T = np.hstack((np.eye(3,3),np.array(source_pcd.get_center()).reshape(3,1))) 119 | pcd_world_T = np.vstack((pcd_world_T, [0,0,0,1])) 120 | 121 | mesh_world_T = np.hstack((np.eye(3,3),np.array(target.get_center()).reshape(3,1))) 122 | mesh_world_T = np.vstack((mesh_world_T, [0,0,0,1])) 123 | 124 | mesh_pcd_T = np.dot(np.linalg.inv(pcd_world_T), mesh_world_T) 125 | 126 | voxel_size = 0.008 # 0.03 127 | radius_normal = voxel_size*2 128 | radius_feature = voxel_size *3 129 | global_distance_threshold = voxel_size * 1.2 130 | local_distance_threshold = voxel_size * 0.4 131 | 132 | source, target, source_down, target_down, source_fpfh, target_fpfh = prepare_dataset(voxel_size, 133 | source_pcd,target, radius_normal,radius_feature) 134 | 135 | #1. Global registration 136 | result_ransac = execute_global_registration(source_down, target_down, 137 | source_fpfh, target_fpfh, voxel_size, 138 | global_distance_threshold) 139 | 140 | #2. Local registration 141 | reg_p2p = o3d.pipelines.registration.registration_icp( 142 | source, target, local_distance_threshold, result_ransac.transformation, 143 | o3d.pipelines.registration.TransformationEstimationPointToPlane()) 144 | 145 | estimated_base_T = np.dot(mesh_pcd_T,np.dot(reg_p2p.transformation,pcd_world_T)) 146 | 147 | return target,source, reg_p2p ,result_ransac, estimated_base_T 148 | 149 | def eval_assembly_pose(item,stage,models,im_id): 150 | 151 | rgb,dpt,seg_map,this_cam, pose_dict,this_scene_w, models_info, this_scene = item.get_item(im_id) 152 | 153 | cam_K = this_cam['cam_K'] 154 | K = np.array(cam_K).reshape(3,3) 155 | dpt_cam_K = this_cam['dpt_cam_K'] 156 | dpt_K = np.array(dpt_cam_K).reshape(3,3) 157 | 158 | seg_mask = np.zeros_like(seg_map) 159 | 160 | for i in range (1,stage+1): 161 | this_seg_mask = copy.deepcopy(seg_map) 162 | this_seg_mask[np.where(this_seg_mask != i)] = 0 163 | this_seg_mask[np.where(this_seg_mask != 0)] = 1 164 | seg_mask += this_seg_mask 165 | 166 | cld,_ = dpt_2_cld(dpt,dpt_K,seg_mask) 167 | 168 | #1.get target pointcloud <== the pointcloud inferred from camera 169 | target_pcd_wrt_world, dptopt_world_T = get_target_pcd(stage, cld ,this_cam) 170 | 171 | #2.get source mesh 172 | source_mesh = o3d.io.read_triangle_mesh(item.rt_dir+'/model_meshes/stage_'+str(stage)+'.ply') 173 | 174 | #3.do registration 175 | target, source,reg_result,result_ransac, estimated_base_T = pcl_registration(target_pcd_wrt_world, source_mesh, dpt_K, dptopt_world_T) 176 | 177 | # ground-truth assembly pose wrt base 178 | R_wrt_base = np.asarray(pose_dict[str(stage)]["R_wrt_bottom_casing"]).reshape(3,3) 179 | t_wrt_base = np.asarray(pose_dict[str(stage)]["t_wrt_bottom_casing"]).reshape(3,1)*0.001 # already in mm 180 | obj_base_T = np.hstack((R_wrt_base,t_wrt_base)) 181 | obj_base_T = np.vstack((obj_base_T, [0,0,0,1])) 182 | 183 | estimated_assembly_pose = np.dot(estimated_base_T,obj_base_T) 184 | 185 | est_R_world = estimated_assembly_pose[0:3,0:3] 186 | est_t_world = estimated_assembly_pose[:3, 3].reshape(3,1) 187 | 188 | #(this is unnecesssary read gt pose from next stage!) 189 | # ground_truth assembly pose wrt world ==> base_world_T * obj_base_T 190 | gt_R_world = np.array(this_scene_w[0]["cam_R_m2c"]).reshape(3,3) ## cam_R_m2c must be changed in gen script!!! 191 | gt_t_world = np.array(this_scene_w[0]["cam_t_m2c"]).reshape(3,1)*0.001 192 | gt_base_world_T = np.hstack((gt_R_world,gt_t_world)) 193 | gt_base_world_T = np.vstack((gt_base_world_T, [0,0,0,1])) 194 | 195 | gt_assembly_pose = np.dot(gt_base_world_T,obj_base_T) 196 | gt_R_world = gt_assembly_pose[0:3,0:3] 197 | gt_t_world = gt_assembly_pose[:3, 3].reshape(3,1) 198 | 199 | 200 | assembling_pcd = o3d.io.read_point_cloud(item.rt_dir+'/model_pointcloud/'+models[stage]+'.ply') 201 | pts = np.asarray(assembling_pcd.points) 202 | 203 | syms = misc.get_symmetry_transformations(models_info[str(stage)], max(0.015, models_info[str(stage)]['diameter'])) 204 | 205 | 206 | error = pose_error.adi(est_R_world, est_t_world, gt_R_world, gt_t_world, pts) 207 | translation_error = pose_error.te(est_t_world,gt_t_world) 208 | rotational_error = pose_error.re(est_R_world,gt_R_world) 209 | # https://bop.felk.cvut.cz/challenges/bop-challenge-2019/#:~:text=7.-,Evaluation%20methodology,-7.1%20Pose%2Derror 210 | mssd_error = pose_error.mssd(est_R_world, est_t_world, gt_R_world, gt_t_world, pts, syms) 211 | 212 | return reg_result , error, translation_error,rotational_error , mssd_error , gt_assembly_pose , estimated_assembly_pose 213 | 214 | if __name__ == "__main__": 215 | 216 | data_all, data_all_summary = {}, {} 217 | inliers, fitness_vals, ADI_error, translation_error, rotational_error, mssd_error, inference_time = [], [], [], [], [], [], [] 218 | 219 | # Uncomment the dataset to test 220 | 221 | ## Nema17 dataset ############ 222 | dataset = "Nema17_reducer_dataset" 223 | models = ['Nema17','sun_gear','housing','carrier','cover'] 224 | 225 | ## Fidget dataset ############ 226 | #dataset = "fidget_dataset" 227 | #models = ["bottom_casing", "left_gear", "right_gear", "top_casing"] 228 | 229 | stage = 1 # replace this with assembly stage 230 | n_instances = 400 # replace this with number of images in each stage 231 | 232 | for i in range(0, n_instances): 233 | time.sleep(3) 234 | start = time.time() 235 | if i% 100 == 0: 236 | print("calculating instance :", str(i)) 237 | 238 | item = this_dataset(stage,dataset) 239 | reg_result, error,te,re, mssd, gt_assembly_pose = eval_assembly_pose(item,stage,models,im_id=str(i)) 240 | 241 | if reg_result.fitness < 0.6: 242 | print("fitness too low :",reg_result.fitness) 243 | print("calculating instance :", str(i)) 244 | 245 | end = time.time() 246 | run_time = (end-start) # time in milli seeconds 247 | 248 | inference_time.append(run_time) 249 | inliers.append(reg_result.inlier_rmse) 250 | fitness_vals.append(reg_result.fitness) 251 | ADI_error.append(error) 252 | translation_error.append(te) 253 | rotational_error.append(re) 254 | mssd_error.append(mssd) 255 | 256 | stats ={"inliers" :inliers, 257 | "fitness": fitness_vals, 258 | "ADI error": ADI_error, 259 | "translation_error": translation_error, 260 | "rotational_error": rotational_error, 261 | "MSSD_error": mssd_error, 262 | "inference_time":inference_time} 263 | 264 | stats_summary = { 265 | "stage": ["stage_"+str(stage)], 266 | "fitness_mean": [float(mean(fitness_vals))], 267 | "fitness_stdv": [stdev(fitness_vals)], 268 | "inliers_mean": [float(mean(inliers))], 269 | "inliers_stdv": [stdev(inliers)], 270 | "ADI_mean": [mean(ADI_error)], 271 | "ADI_stdv": [stdev(ADI_error)], 272 | "MSSD_mean": [mean(mssd_error)], 273 | "MSSd_stdv": [stdev(mssd_error)], 274 | "mean_time":[mean(inference_time)]} 275 | 276 | print("done") 277 | print("results :", stats_summary) 278 | 279 | #save to pkl files 280 | data = pd.DataFrame(stats) 281 | data_all = {"stage_"+str(stage):data} 282 | 283 | data_summary = pd.DataFrame(stats_summary) 284 | data_all_summary = {"stage_"+str(stage):data_summary} 285 | 286 | data.to_pickle(dataset+"/"+ "stage_"+str(stage)+".pkl") 287 | data_summary.to_pickle(dataset+"/"+ "stage_"+str(stage)+"_summary.pkl") -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | # This file may be used to create an environment using: 2 | # $ conda create --name --file 3 | # platform: linux-64 4 | _libgcc_mutex=0.1=conda_forge 5 | _openmp_mutex=4.5=2_gnu 6 | absl-py=1.2.0=pypi_0 7 | aiohttp=3.8.5=pypi_0 8 | aiosignal=1.3.1=pypi_0 9 | alabaster=0.7.13=pyhd8ed1ab_0 10 | alsa-lib=1.2.6.1=h7f98852_0 11 | ansi2html=1.8.0=pypi_0 12 | antlr4-python3-runtime=4.9.3=pypi_0 13 | anyio=3.5.0=py38h06a4308_0 14 | aom=3.4.0=h27087fc_1 15 | appdirs=1.4.4=pypi_0 16 | apr=1.7.0=h7f98852_5 17 | argon2-cffi=21.3.0=pyhd3eb1b0_0 18 | argon2-cffi-bindings=21.2.0=py38h7f8727e_0 19 | arrow=1.2.3=pyhd8ed1ab_0 20 | astroid=2.5.8=py38h578d9bd_0 21 | asttokens=2.0.5=pyhd3eb1b0_0 22 | async-timeout=4.0.3=pypi_0 23 | atomicwrites=1.4.1=pyhd8ed1ab_0 24 | attr=2.5.1=h166bdaf_1 25 | attrs=22.2.0=pyh71513ae_0 26 | autopep8=1.6.0=pyhd8ed1ab_1 27 | babel=2.11.0=pyhd8ed1ab_0 28 | backcall=0.2.0=pyhd3eb1b0_0 29 | beautifulsoup4=4.11.2=pyha770c72_0 30 | binaryornot=0.4.4=py_1 31 | black=21.4b2=pypi_0 32 | bleach=6.0.0=pyhd8ed1ab_0 33 | boost-cpp=1.72.0=he72f1d9_7 34 | brotli=1.0.9=h166bdaf_8 35 | brotli-bin=1.0.9=h166bdaf_8 36 | brotlipy=0.7.0=py38h0a891b7_1005 37 | bzip2=1.0.8=h7f98852_4 38 | c-ares=1.18.1=h7f98852_0 39 | ca-certificates=2023.01.10=h06a4308_0 40 | cachetools=5.2.0=pypi_0 41 | cairo=1.16.0=ha61ee94_1012 42 | catkin_pkg=0.5.2=pyhd8ed1ab_0 43 | certifi=2022.12.7=py38h06a4308_0 44 | cffi=1.15.1=py38h4a40e3a_3 45 | chardet=5.1.0=py38h578d9bd_0 46 | charset-normalizer=2.1.1=pyhd8ed1ab_0 47 | click=8.1.3=unix_pyhd8ed1ab_2 48 | cloudpickle=2.1.0=pypi_0 49 | cmake=3.24.1=h5432695_0 50 | colorama=0.4.6=pyhd8ed1ab_0 51 | comm=0.1.4=pypi_0 52 | configargparse=1.7=pypi_0 53 | console_bridge=1.0.2=h924138e_1 54 | contourpy=1.0.7=py38hfbd4bf9_0 55 | cookiecutter=2.1.1=pyh6c4a22f_0 56 | cryptography=39.0.0=py38h1724139_0 57 | cssutils=2.7.1=pypi_0 58 | cvbridge3=1.1=pypi_0 59 | cycler=0.11.0=pyhd8ed1ab_0 60 | dash=2.12.1=pypi_0 61 | dash-core-components=2.0.0=pypi_0 62 | dash-html-components=2.0.0=pypi_0 63 | dash-table=5.0.0=pypi_0 64 | dataframe-image=0.1.11=pypi_0 65 | dbus=1.13.6=h5008d03_3 66 | debugpy=1.5.1=py38h295c915_0 67 | decorator=5.1.1=pyhd3eb1b0_0 68 | defusedxml=0.7.1=pyhd8ed1ab_0 69 | detectron2=0.6+cu111=pypi_0 70 | diff-match-patch=20200713=pyh9f0ad1d_0 71 | distro=1.6.0=pyhd8ed1ab_0 72 | docutils=0.19=py38h578d9bd_0 73 | empy=3.3.4=pyh9f0ad1d_1 74 | entrypoints=0.4=py38h06a4308_0 75 | executing=0.8.3=pyhd3eb1b0_0 76 | expat=2.4.8=h27087fc_0 77 | ffmpeg=4.4.2=gpl_hfe78399_107 78 | fftw=3.3.10=nompi_ha7695d1_103 79 | flake8=4.0.1=pyhd8ed1ab_2 80 | font-ttf-dejavu-sans-mono=2.37=hab24e00_0 81 | font-ttf-inconsolata=3.000=h77eed37_0 82 | font-ttf-source-code-pro=2.038=h77eed37_0 83 | font-ttf-ubuntu=0.83=hab24e00_0 84 | fontconfig=2.14.0=h8e229c2_0 85 | fonts-conda-ecosystem=1=0 86 | fonts-conda-forge=1=0 87 | fonttools=4.38.0=py38h0a891b7_1 88 | freeglut=3.2.2=h9c3ff4c_1 89 | freetype=2.12.1=hca18f0e_0 90 | frozenlist=1.4.0=pypi_0 91 | future=0.18.2=pypi_0 92 | fvcore=0.1.5.post20220512=pypi_0 93 | gettext=0.19.8.1=h73d1719_1008 94 | giflib=5.2.1=h36c2ea0_2 95 | glib=2.72.1=h6239696_0 96 | glib-tools=2.72.1=h6239696_0 97 | gmock=1.11.0=h924138e_0 98 | gmp=6.2.1=h58526e2_0 99 | gnutls=3.7.7=hf3e180e_0 100 | google-auth=2.11.0=pypi_0 101 | google-auth-oauthlib=0.4.6=pypi_0 102 | graphite2=1.3.13=h58526e2_1001 103 | grpcio=1.47.0=pypi_0 104 | gst-plugins-base=1.20.3=hf6a322e_0 105 | gstreamer=1.20.3=hd4edc92_0 106 | gtest=1.11.0=h924138e_0 107 | harfbuzz=5.1.0=hf9f4e7c_0 108 | hdf5=1.12.2=nompi_h2386368_100 109 | html2image=2.0.3=pypi_0 110 | hydra-core=1.2.0=pypi_0 111 | icu=70.1=h27087fc_0 112 | idna=3.4=pyhd8ed1ab_0 113 | imageio=2.31.1=pypi_0 114 | imagesize=1.4.1=pyhd8ed1ab_0 115 | importlib-metadata=4.12.0=pypi_0 116 | importlib_metadata=6.0.0=hd8ed1ab_0 117 | importlib_resources=5.10.2=pyhd8ed1ab_0 118 | inflection=0.5.1=pyh9f0ad1d_0 119 | intervaltree=3.0.2=py_0 120 | iopath=0.1.9=pypi_0 121 | ipykernel=6.15.0=pyh210e3f2_0 122 | ipython=7.33.0=py38h578d9bd_0 123 | ipython_genutils=0.2.0=py_1 124 | ipywidgets=8.1.1=pypi_0 125 | isort=5.12.0=pyhd8ed1ab_1 126 | jack=1.9.18=h8c3723f_1002 127 | jaraco.classes=3.2.3=pyhd8ed1ab_0 128 | jasper=2.0.33=ha77e612_0 129 | jedi=0.18.1=py38h06a4308_1 130 | jeepney=0.8.0=pyhd8ed1ab_0 131 | jellyfish=0.9.0=py38h0a891b7_2 132 | jinja2=3.1.2=pyhd8ed1ab_1 133 | jinja2-time=0.2.0=pyhd8ed1ab_3 134 | jpeg=9e=h166bdaf_2 135 | jsonschema=4.17.3=pyhd8ed1ab_0 136 | jupyter_client=7.3.4=pyhd8ed1ab_0 137 | jupyter_core=4.10.0=py38h06a4308_0 138 | jupyter_server=1.23.4=py38h06a4308_0 139 | jupyterlab-widgets=3.0.9=pypi_0 140 | jupyterlab_pygments=0.2.2=pyhd8ed1ab_0 141 | kaleido=0.2.1=pypi_0 142 | keyring=23.13.1=py38h578d9bd_0 143 | keyutils=1.6.1=h166bdaf_0 144 | kiwisolver=1.4.4=py38h43d8883_1 145 | krb5=1.19.3=h3790be6_0 146 | lame=3.100=h7f98852_1001 147 | lazy-loader=0.3=pypi_0 148 | lazy-object-proxy=1.9.0=py38h1de0b5d_0 149 | lcms2=2.14=h6ed2654_0 150 | ld_impl_linux-64=2.36.1=hea4e1c9_2 151 | lerc=4.0.0=h27087fc_0 152 | libapr=1.7.0=h7f98852_5 153 | libapriconv=1.2.2=h7f98852_5 154 | libaprutil=1.6.1=h975c496_5 155 | libblas=3.9.0=16_linux64_openblas 156 | libbrotlicommon=1.0.9=h166bdaf_8 157 | libbrotlidec=1.0.9=h166bdaf_8 158 | libbrotlienc=1.0.9=h166bdaf_8 159 | libcap=2.64=ha37c62d_0 160 | libcblas=3.9.0=16_linux64_openblas 161 | libclang=14.0.6=default_h2e3cab8_0 162 | libclang13=14.0.6=default_h3a83d3e_0 163 | libcups=2.3.3=h3e49a29_2 164 | libcurl=7.83.1=h7bff187_0 165 | libdb=6.2.32=h9c3ff4c_0 166 | libdeflate=1.13=h166bdaf_0 167 | libdrm=2.4.112=h166bdaf_0 168 | libedit=3.1.20191231=he28a2e2_2 169 | libev=4.33=h516909a_1 170 | libevent=2.1.10=h9b69904_4 171 | libffi=3.4.2=h7f98852_5 172 | libflac=1.3.4=h27087fc_0 173 | libgcc-ng=12.1.0=h8d9b700_16 174 | libgfortran-ng=12.1.0=h69a702a_16 175 | libgfortran5=12.1.0=hdcd56e2_16 176 | libglib=2.72.1=h2d90d5f_0 177 | libglu=9.0.0=he1b5a44_1001 178 | libgomp=12.1.0=h8d9b700_16 179 | libiconv=1.16=h516909a_0 180 | libidn2=2.3.3=h166bdaf_0 181 | liblapack=3.9.0=16_linux64_openblas 182 | liblapacke=3.9.0=16_linux64_openblas 183 | libllvm14=14.0.6=he0ac6c6_0 184 | libnghttp2=1.47.0=hdcd2b5c_1 185 | libnsl=2.0.0=h7f98852_0 186 | libogg=1.3.4=h7f98852_1 187 | libopenblas=0.3.21=pthreads_h78a6416_2 188 | libopencv=4.6.0=py38h3504efa_3 189 | libopus=1.3.1=h7f98852_1 190 | libpciaccess=0.16=h516909a_0 191 | libpng=1.6.37=h753d276_4 192 | libpq=14.5=hd77ab85_0 193 | libprotobuf=3.20.1=h6239696_1 194 | libsndfile=1.0.31=h9c3ff4c_1 195 | libsodium=1.0.18=h7b6447c_0 196 | libspatialindex=1.9.3=h9c3ff4c_4 197 | libsqlite=3.39.2=h753d276_1 198 | libssh2=1.10.0=haa6b8db_3 199 | libstdcxx-ng=12.1.0=ha89aaad_16 200 | libtasn1=4.19.0=h166bdaf_0 201 | libtiff=4.4.0=h0e0dad5_3 202 | libtool=2.4.6=h9c3ff4c_1008 203 | libudev1=249=h166bdaf_4 204 | libunistring=0.9.10=h7f98852_0 205 | libuuid=2.32.1=h7f98852_1000 206 | libuv=1.44.2=h166bdaf_0 207 | libva=2.15.0=h166bdaf_0 208 | libvorbis=1.3.7=h9c3ff4c_0 209 | libvpx=1.11.0=h9c3ff4c_3 210 | libwebp=1.2.4=h522a892_0 211 | libwebp-base=1.2.4=h166bdaf_0 212 | libxcb=1.13=h7f98852_1004 213 | libxkbcommon=1.0.3=he3ba5ed_0 214 | libxml2=2.9.14=h22db469_4 215 | libzlib=1.2.12=h166bdaf_2 216 | log4cxx=0.11.0=h291d653_3 217 | lxml=4.9.2=pypi_0 218 | markdown=3.4.1=pypi_0 219 | markupsafe=2.1.1=pypi_0 220 | matplotlib=3.6.3=py38h578d9bd_0 221 | matplotlib-base=3.6.3=py38hd6c3c57_0 222 | matplotlib-inline=0.1.2=pyhd3eb1b0_2 223 | mccabe=0.6.1=py_1 224 | mistune=2.0.4=pyhd8ed1ab_0 225 | more-itertools=9.0.0=pyhd8ed1ab_0 226 | multidict=6.0.4=pypi_0 227 | munkres=1.1.4=pyh9f0ad1d_0 228 | mypy_extensions=0.4.3=py38h578d9bd_6 229 | mysql-common=8.0.30=haf5c9bc_0 230 | mysql-libs=8.0.30=h28c427c_0 231 | nbclassic=0.4.8=py38h06a4308_0 232 | nbclient=0.5.13=pyhd8ed1ab_0 233 | nbconvert=7.2.9=pyhd8ed1ab_0 234 | nbconvert-core=7.2.9=pyhd8ed1ab_0 235 | nbconvert-pandoc=7.2.9=pyhd8ed1ab_0 236 | nbformat=5.7.0=pypi_0 237 | ncurses=6.3=h27087fc_1 238 | nest-asyncio=1.5.5=py38h06a4308_0 239 | netifaces=0.11.0=py38h0a891b7_0 240 | nettle=3.8.1=hc379101_1 241 | networkx=3.1=pypi_0 242 | nose=1.3.7=py_1006 243 | notebook=6.5.2=py38h06a4308_0 244 | notebook-shim=0.2.2=py38h06a4308_0 245 | nspr=4.32=h9c3ff4c_1 246 | nss=3.78=h2350873_0 247 | numpy=1.23.2=py38h3a7f9d9_0 248 | numpydoc=1.5.0=pyhd8ed1ab_0 249 | oauthlib=3.2.0=pypi_0 250 | omegaconf=2.2.3=pypi_0 251 | open3d=0.17.0=pypi_0 252 | opencv=4.6.0=py38h578d9bd_3 253 | openh264=2.3.0=h27087fc_0 254 | openjpeg=2.5.0=h7d73246_1 255 | openssl=1.1.1s=h7f8727e_0 256 | p11-kit=0.24.1=hc5aa10d_0 257 | packaging=23.0=pyhd8ed1ab_0 258 | pandoc=2.19.2=ha770c72_0 259 | pandocfilters=1.5.0=pyhd8ed1ab_0 260 | parso=0.8.3=pyhd3eb1b0_0 261 | pathspec=0.11.0=pyhd8ed1ab_0 262 | pcre=8.45=h9c3ff4c_0 263 | pexpect=4.8.0=pyhd3eb1b0_3 264 | pickleshare=0.7.5=pyhd3eb1b0_1003 265 | pillow=9.5.0=pypi_0 266 | pip=23.2.1=pypi_0 267 | pixman=0.40.0=h36c2ea0_0 268 | pkg-config=0.29.2=h36c2ea0_1008 269 | pkgutil-resolve-name=1.3.10=pyhd8ed1ab_0 270 | platformdirs=2.6.2=pyhd8ed1ab_0 271 | plotly=5.15.0=py_0 272 | plotly-gif=0.0.4=pypi_0 273 | pluggy=1.0.0=pyhd8ed1ab_5 274 | ply=3.11=py_1 275 | portalocker=2.5.1=pypi_0 276 | portaudio=19.6.0=h57a0ea0_5 277 | prometheus_client=0.14.1=py38h06a4308_0 278 | prompt-toolkit=3.0.20=pyhd3eb1b0_0 279 | protobuf=3.19.4=pypi_0 280 | psutil=5.9.4=py38h0a891b7_0 281 | pthread-stubs=0.4=h36c2ea0_1001 282 | ptyprocess=0.7.0=pyhd3eb1b0_2 283 | pulseaudio=14.0=h7f54b18_8 284 | pure_eval=0.2.2=pyhd3eb1b0_0 285 | py-opencv=4.6.0=py38h7f3c49e_3 286 | pyasn1=0.4.8=pypi_0 287 | pyasn1-modules=0.2.8=pypi_0 288 | pycocotools=2.0.4=pypi_0 289 | pycodestyle=2.8.0=pyhd8ed1ab_0 290 | pycparser=2.21=pyhd8ed1ab_0 291 | pydocstyle=6.3.0=pyhd8ed1ab_0 292 | pydot=1.4.2=pypi_0 293 | pyflakes=2.4.0=pyhd8ed1ab_0 294 | pygments=2.14.0=pyhd8ed1ab_0 295 | pylint=2.7.2=py38h578d9bd_0 296 | pyls-spyder=0.4.0=pyhd8ed1ab_0 297 | pyopenssl=23.0.0=pyhd8ed1ab_0 298 | pyparsing=3.0.9=pyhd8ed1ab_0 299 | pypng=0.0.19=pypi_0 300 | pyqt=5.15.7=py38h7492b6b_0 301 | pyqt5-sip=12.11.0=py38hfa26641_0 302 | pyqtwebengine=5.15.7=py38h7492b6b_0 303 | pyquaternion=0.9.9=pypi_0 304 | pyrsistent=0.19.3=py38h1de0b5d_0 305 | pysocks=1.7.1=pyha2e5f31_6 306 | python=3.8.13=h582c2e5_0_cpython 307 | python-dateutil=2.8.2=pyhd3eb1b0_0 308 | python-fastjsonschema=2.16.2=pyhd8ed1ab_0 309 | python-lsp-black=1.2.1=pyhd8ed1ab_0 310 | python-lsp-jsonrpc=1.0.0=pyhd8ed1ab_0 311 | python-lsp-server=1.5.0=py38h06a4308_0 312 | python-slugify=8.0.0=pyhd8ed1ab_0 313 | python_abi=3.8=2_cp38 314 | pytils=0.4.1=pypi_0 315 | pytoolconfig=1.2.5=pyhd8ed1ab_0 316 | pytz=2022.7.1=pyhd8ed1ab_0 317 | pywavelets=1.4.1=pypi_0 318 | pyxdg=0.28=pyhd8ed1ab_0 319 | pyyaml=6.0=py38h0a891b7_4 320 | pyzmq=23.2.0=py38h6a678d5_0 321 | qdarkstyle=3.0.3=pyhd8ed1ab_0 322 | qstylizer=0.2.2=pyhd8ed1ab_0 323 | qt-main=5.15.4=ha5833f6_2 324 | qt-webengine=5.15.4=hcbadb6c_3 325 | qtawesome=1.2.2=pyhd8ed1ab_0 326 | qtconsole=5.3.2=py38h06a4308_0 327 | qtpy=2.3.0=pyhd8ed1ab_0 328 | readline=8.1.2=h0f457ee_0 329 | regex=2022.8.17=pypi_0 330 | requests=2.28.2=pyhd8ed1ab_0 331 | requests-oauthlib=1.3.1=pypi_0 332 | retrying=1.3.4=pypi_0 333 | rhash=1.4.3=h166bdaf_0 334 | rope=1.7.0=pyhd8ed1ab_0 335 | ros-catkin=0.7.17=py38h950e882_5 336 | ros-conda-base=0.0.2=hcb32578_2 337 | ros-conda-mutex=1.0=melodic 338 | ros-cpp-common=0.6.12=py38h794f011_5 339 | ros-environment=1.2.1=py38h950e882_2 340 | ros-gencpp=0.6.2=py38h950e882_1 341 | ros-geneus=2.2.6=py38h950e882_1 342 | ros-genlisp=0.4.16=py38h950e882_1 343 | ros-genmsg=0.5.12=py38h950e882_1 344 | ros-gennodejs=2.0.1=py38h950e882_1 345 | ros-genpy=0.6.8=py38h950e882_1 346 | ros-message-generation=0.4.0=h950e882_1 347 | ros-message-runtime=0.4.12=he1b5a44_0 348 | ros-rosbuild=1.14.6=he1b5a44_0 349 | ros-rosconsole=1.13.10=hfdf2c5b_2 350 | ros-roscpp=1.14.3=py38h794f011_4 351 | ros-roscpp-serialization=0.6.12=he1b5a44_0 352 | ros-roscpp-traits=0.6.12=he1b5a44_0 353 | ros-rosgraph=1.14.3=py38h950e882_1 354 | ros-rosgraph-msgs=1.11.2=py38h950e882_1 355 | ros-roslib=1.14.6=py38h794f011_4 356 | ros-rospack=2.5.3=py38hd02d5f2_1 357 | ros-rospy=1.14.3=py38h950e882_1 358 | ros-rostime=0.6.12=h794f011_3 359 | ros-std-msgs=0.5.12=py38h950e882_1 360 | ros-xmlrpcpp=1.14.3=he1b5a44_0 361 | rosdep=0.22.1=pyhd8ed1ab_0 362 | rosdistro=0.8.3=py38h578d9bd_4 363 | rospkg=1.4.0=pyhd8ed1ab_0 364 | rsa=4.9=pypi_0 365 | rtree=1.0.1=py38h02d302b_1 366 | scikit-image=0.21.0=pypi_0 367 | scipy=1.10.0=pypi_0 368 | secretstorage=3.3.3=py38h578d9bd_1 369 | send2trash=1.8.0=pyhd3eb1b0_1 370 | setuptools=65.3.0=py38h578d9bd_0 371 | sip=6.6.2=py38hfa26641_0 372 | six=1.16.0=pyhd3eb1b0_1 373 | sniffio=1.2.0=py38h06a4308_1 374 | snowballstemmer=2.2.0=pyhd8ed1ab_0 375 | sortedcontainers=2.4.0=pyhd8ed1ab_0 376 | soupsieve=2.3.2.post1=pyhd8ed1ab_0 377 | sphinx=6.1.3=pyhd8ed1ab_0 378 | sphinxcontrib-applehelp=1.0.4=pyhd8ed1ab_0 379 | sphinxcontrib-devhelp=1.0.2=py_0 380 | sphinxcontrib-htmlhelp=2.0.1=pyhd8ed1ab_0 381 | sphinxcontrib-jsmath=1.0.1=py_0 382 | sphinxcontrib-qthelp=1.0.3=py_0 383 | sphinxcontrib-serializinghtml=1.1.5=pyhd8ed1ab_2 384 | spyder=5.3.3=py38h06a4308_0 385 | spyder-kernels=2.3.3=py38h06a4308_0 386 | sqlite=3.39.2=h4ff8645_1 387 | stack_data=0.2.0=pyhd3eb1b0_0 388 | svt-av1=1.2.1=h27087fc_0 389 | tabulate=0.8.10=pypi_0 390 | tenacity=8.2.2=pyhd8ed1ab_0 391 | tensorboard=2.10.0=pypi_0 392 | tensorboard-data-server=0.6.1=pypi_0 393 | tensorboard-plugin-wit=1.8.1=pypi_0 394 | termcolor=1.1.0=pypi_0 395 | terminado=0.17.1=py38h06a4308_0 396 | text-unidecode=1.3=py_0 397 | textdistance=4.5.0=pyhd8ed1ab_0 398 | three-merge=0.1.1=pyh9f0ad1d_0 399 | tifffile=2023.7.10=pypi_0 400 | tinycss2=1.2.1=pyhd8ed1ab_0 401 | tinyxml2=9.0.0=h9c3ff4c_2 402 | tk=8.6.12=h27826a3_0 403 | toml=0.10.2=pyhd8ed1ab_0 404 | tomli=2.0.1=pyhd8ed1ab_0 405 | tools=0.1.9=pypi_0 406 | torch=1.8.1+cu111=pypi_0 407 | torchaudio=0.8.1=pypi_0 408 | torchvision=0.9.1+cu111=pypi_0 409 | tornado=6.1=py38h27cfd23_0 410 | traitlets=5.1.1=pyhd3eb1b0_0 411 | typing-extensions=4.7.1=pypi_0 412 | typing_extensions=4.4.0=pyha770c72_0 413 | ujson=5.7.0=py38h8dc9893_0 414 | unicodedata2=15.0.0=py38h0a891b7_0 415 | unidecode=1.3.6=pyhd8ed1ab_0 416 | urllib3=1.26.14=pyhd8ed1ab_0 417 | watchdog=2.2.1=py38h578d9bd_0 418 | wcwidth=0.2.5=pyhd3eb1b0_0 419 | webencodings=0.5.1=py_1 420 | websocket-client=0.58.0=py38h06a4308_4 421 | werkzeug=2.2.3=pypi_0 422 | whatthepatch=1.0.4=pyhd8ed1ab_0 423 | wheel=0.37.1=pyhd8ed1ab_0 424 | widgetsnbextension=4.0.9=pypi_0 425 | wrapt=1.12.1=py38h497a2fe_3 426 | wurlitzer=3.0.3=pyhd8ed1ab_0 427 | x264=1!164.3095=h166bdaf_2 428 | x265=3.5=h924138e_3 429 | xcb-util=0.4.0=h166bdaf_0 430 | xcb-util-image=0.4.0=h166bdaf_0 431 | xcb-util-keysyms=0.4.0=h166bdaf_0 432 | xcb-util-renderutil=0.3.9=h166bdaf_0 433 | xcb-util-wm=0.4.1=h166bdaf_0 434 | xorg-fixesproto=5.0=h7f98852_1002 435 | xorg-inputproto=2.3.2=h7f98852_1002 436 | xorg-kbproto=1.0.7=h7f98852_1002 437 | xorg-libice=1.0.10=h7f98852_0 438 | xorg-libsm=1.2.3=hd9c2040_1000 439 | xorg-libx11=1.7.2=h7f98852_0 440 | xorg-libxau=1.0.9=h7f98852_0 441 | xorg-libxdmcp=1.1.3=h7f98852_0 442 | xorg-libxext=1.3.4=h7f98852_1 443 | xorg-libxfixes=5.0.3=h7f98852_1004 444 | xorg-libxi=1.7.10=h7f98852_0 445 | xorg-libxrender=0.9.10=h7f98852_1003 446 | xorg-renderproto=0.11.1=h7f98852_1002 447 | xorg-xextproto=7.3.0=h7f98852_1002 448 | xorg-xproto=7.0.31=h7f98852_1007 449 | xz=5.2.6=h166bdaf_0 450 | yacs=0.1.8=pypi_0 451 | yaml=0.2.5=h7f98852_2 452 | yapf=0.32.0=pyhd8ed1ab_0 453 | yarl=1.9.2=pypi_0 454 | zeromq=4.3.4=h2531618_0 455 | zipp=3.12.0=pyhd8ed1ab_0 456 | zlib=1.2.12=h166bdaf_2 457 | zstd=1.5.2=h6239696_4 458 | -------------------------------------------------------------------------------- /data_generation/Worlds/metrics2.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 1 5 | 6 | 7 | 8 | 9 | 0 0 1 10 | 100 100 11 | 12 | 13 | 14 | 15 | 16 | 100 17 | 50 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 10 29 | 30 | 31 | 0 32 | 33 | 34 | 0 0 1 35 | 100 100 36 | 37 | 38 | 39 | 43 | 44 | 45 | 0 46 | 0 47 | 1 48 | 0 49 | 50 | 51 | 0 0 -9.8 52 | 6e-06 2.3e-05 -4.2e-05 53 | 54 | 55 | 0.001 56 | 1 57 | 1000 58 | 59 | 60 | 0.4 0.4 0.4 1 61 | 0.7 0.7 0.7 1 62 | 1 63 | 64 | 65 | EARTH_WGS84 66 | 0 67 | 0 68 | 0 69 | 0 70 | 71 | 72 | 0 0 2 0 -0 0 73 | 0.5 0.5 0.5 1 74 | 0.1 0.1 0.1 1 75 | 76 | 20 77 | 0.5 78 | 0.01 79 | 0.001 80 | 81 | 0 82 | 0 0 -1 83 | 84 | 0 85 | 0 86 | 0 87 | 88 | 89 | 90 | -0.2 0 0.87 0 -0 0 91 | 92 | 93 | 0.1 94 | 95 | 96 | 97 | 98 | 0.073 0.276 0.072 99 | 100 | 101 | 10 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 121 | 122 | 123 | 124 | model://kinect/meshes/kinect.dae 125 | 126 | 127 | 128 | 129 | 20 130 | 131 | 1.0472 132 | 133 | 640 134 | 480 135 | R8G8B8 136 | 137 | 138 | 0.02 139 | 3 140 | 141 | 142 | 143 | 0.2 144 | 1 145 | 0.0 146 | kinect1 147 | color/image_raw 148 | color/camera_info 149 | depth/image_raw 150 | color/camera_info 151 | depth/points 152 | kinect1_optical_link 153 | 0.03 154 | 3 155 | 0 156 | 0 157 | 0 158 | 0 159 | 0 160 | 0 161 | 0 162 | 0 163 | 0 164 | 0 165 | 166 | 167 | 0 168 | 0 169 | 0 170 | 171 | 1 172 | 0 173 | 174 | 175 | 1 176 | 177 | 0 0 0 0 -0 0 178 | 179 | 0 0.04617 0.38 0 -0 0 180 | 35.4321 181 | 182 | 1.73083 183 | 2.01582e-11 184 | 2.78594e-13 185 | 5.32936 186 | 4.32952e-13 187 | 3.81098 188 | 189 | 190 | 191 | 0 0 0 0 -0 0 192 | 193 | 194 | 1 1 1 195 | model://table1/meshes/table1.dae 196 | 197 | 198 | 10 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 0 0 0 0 -0 0 214 | 215 | 216 | 1 1 1 217 | model://table1/meshes/table1.dae 218 | 219 | 220 | 221 | 0 222 | 0 223 | 0 224 | 225 | -0.475 0 0 0 -0 0 226 | 227 | 228 | 1 229 | 230 | 231 | 232 | 233 | 0.001 0.001 0.001 234 | file://left_gear/meshes/left_gear.dae 235 | 236 | 237 | 10 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 0.001 0.001 0.001 255 | file://left_gear/meshes/left_gear.dae 256 | 257 | 258 | 259 | 0 260 | 0 261 | 0 262 | 263 | 0 0 0.84 0 -0 0 264 | 265 | 266 | 1 267 | 268 | 269 | 270 | 271 | 0.001 0.001 0.001 272 | model://bottom_casing/meshes/bottom_casing.dae 273 | 274 | 275 | 10 276 | 277 | 278 | 279 | 280 | 281 | 282 | 283 | 284 | 285 | 286 | 287 | 288 | 289 | 290 | 291 | 292 | 0.001 0.001 0.001 293 | model://bottom_casing/meshes/bottom_casing.dae 294 | 295 | 296 | 297 | 0 298 | 0 299 | 0 300 | 301 | 0.03 0.03 0.84 0 -0 0 302 | 303 | 304 | 1 305 | 306 | 307 | 308 | 309 | 0.001 0.001 0.001 310 | model://right_gear/meshes/right_gear.dae 311 | 312 | 313 | 10 314 | 315 | 316 | 317 | 318 | 319 | 320 | 321 | 322 | 323 | 324 | 325 | 326 | 327 | 328 | 329 | 330 | 0.001 0.001 0.001 331 | model://right_gear/meshes/right_gear.dae 332 | 333 | 334 | 335 | 0 336 | 0 337 | 0 338 | 339 | 0.06 -0.06 0.84 0 -0 0 340 | 341 | 342 | 1 343 | 344 | 345 | 346 | 347 | 0.001 0.001 0.001 348 | model://top_casing/meshes/top_casing.dae 349 | 350 | 351 | 10 352 | 353 | 354 | 355 | 356 | 357 | 358 | 359 | 360 | 361 | 362 | 363 | 364 | 365 | 366 | 367 | 368 | 0.001 0.001 0.001 369 | model://top_casing/meshes/top_casing.dae 370 | 371 | 372 | 373 | 0 374 | 0 375 | 0 376 | 377 | -0.06 -0.06 0.84 0 -0 0 378 | 379 | 380 | 381 | 1.34127 0.085069 2.06153 -0 0.765799 3.10901 382 | orbit 383 | perspective 384 | 385 | 386 | 387 | 388 | 161 338000000 389 | 164 53345965 390 | 1652676708 207285980 391 | 161338 392 | 393 | -0.007997 0.03 0.84 0 -0 0 394 | 1 1 1 395 | 396 | -0.007997 0.03 0.84 0 -0 0 397 | 0 0 0 0 -0 0 398 | 0 0 0 0 -0 0 399 | 0 0 0 0 -0 0 400 | 401 | 402 | 403 | 0 0 0 0 -0 0 404 | 1 1 1 405 | 406 | 0 0 0 0 -0 0 407 | 0 0 0 0 -0 0 408 | 0 0 0 0 -0 0 409 | 0 0 0 0 -0 0 410 | 411 | 412 | 413 | -0.2 0 0.87 0 -0 0 414 | 1 1 1 415 | 416 | -0.2 0 0.87 0 -0 0 417 | 0 0 0 0 -0 0 418 | 0 0 0 0 -0 0 419 | 0 0 0 0 -0 0 420 | 421 | 422 | 423 | 0.002668 0.002471 0.825125 0 -0 0 424 | 1 1 1 425 | 426 | 0.002668 0.002471 0.825125 0 -0 0 427 | 0 0 0 0 -0 0 428 | 0 0 0 0 -0 0 429 | 0 0 0 0 -0 0 430 | 431 | 432 | 433 | 0.04523 -0.005755 0.84 0 -0 0 434 | 1 1 1 435 | 436 | 0.04523 -0.005755 0.84 0 -0 0 437 | 0 0 0 0 -0 0 438 | 0 0 0 0 -0 0 439 | 0 0 0 0 -0 0 440 | 441 | 442 | 443 | -0.475 0 0 0 -0 0 444 | 1 1 1 445 | 446 | -0.475 0 0 0 -0 0 447 | 0 0 0 0 -0 0 448 | 0 0 0 0 -0 0 449 | 0 0 0 0 -0 0 450 | 451 | 452 | 453 | -0.004849 -0.033747 0.828307 0 -0 0 454 | 1 1 1 455 | 456 | -0.004849 -0.033747 0.828307 0 -0 0 457 | 0 0 0 0 -0 0 458 | 0 0 0 0 -0 0 459 | 0 0 0 0 -0 0 460 | 461 | 462 | 463 | 0 0 2 0 -0 0 464 | 465 | 466 | 467 | 468 | -------------------------------------------------------------------------------- /data_generation/Worlds/fidget_world.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 1 1 1 1 6 | 0.8 0.8 0.8 1 7 | true 8 | 9 | 0 0 -9.8 10 | 6e-06 2.3e-05 -4.2e-05 11 | 12 | 13 | 0.001 14 | 1 15 | 1000 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 3D View 24 | false 25 | docked 26 | 27 | 28 | ogre2 29 | scene 30 | 0.4 0.4 0.4 31 | 0.8 0.8 0.8 32 | -6 0 6 0 0.5 0 33 | 34 | 0.25 35 | 25000 36 | 37 | 38 | 39 | 40 | 41 | 42 | floating 43 | 5 44 | 5 45 | false 46 | 47 | 48 | 49 | 50 | false 51 | 5 52 | 5 53 | floating 54 | false 55 | 56 | 57 | 58 | 59 | false 60 | 5 61 | 5 62 | floating 63 | false 64 | 65 | 66 | 67 | 68 | false 69 | 5 70 | 5 71 | floating 72 | false 73 | 74 | 75 | 76 | 77 | false 78 | 5 79 | 5 80 | floating 81 | false 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | false 91 | 5 92 | 5 93 | floating 94 | false 95 | 96 | 97 | 98 | 99 | false 100 | 5 101 | 5 102 | floating 103 | false 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | false 114 | 5 115 | 5 116 | floating 117 | false 118 | 119 | 120 | 121 | 122 | 123 | 124 | World control 125 | false 126 | false 127 | 72 128 | 121 129 | 1 130 | 131 | floating 132 | 133 | 134 | 135 | 136 | 137 | 138 | true 139 | true 140 | true 141 | true 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | semantic/colored_map 150 | 151 | 152 | 153 | 154 | panoptic/colored_map 155 | 156 | 157 | 158 | 159 | semantic/labels_map 160 | 161 | 162 | 163 | 164 | 165 | World stats 166 | false 167 | false 168 | 110 169 | 290 170 | 1 171 | 172 | floating 173 | 174 | 175 | 176 | 177 | 178 | 179 | true 180 | true 181 | true 182 | true 183 | 184 | 185 | 186 | 187 | 188 | false 189 | 0 190 | 0 191 | 250 192 | 50 193 | floating 194 | false 195 | #666666 196 | 197 | 198 | 199 | 200 | 201 | 202 | false 203 | 250 204 | 0 205 | 150 206 | 50 207 | floating 208 | false 209 | #666666 210 | 211 | 212 | 213 | 214 | 215 | 216 | false 217 | 0 218 | 50 219 | 250 220 | 50 221 | floating 222 | false 223 | #777777 224 | 225 | 226 | 227 | false 228 | 229 | 230 | 231 | 232 | 233 | false 234 | 250 235 | 50 236 | 50 237 | 50 238 | floating 239 | false 240 | #777777 241 | 242 | 243 | 244 | 245 | 246 | 247 | false 248 | 300 249 | 50 250 | 50 251 | 50 252 | floating 253 | false 254 | #777777 255 | 256 | 257 | 258 | true 259 | true 260 | 4000000 261 | 262 | 263 | 264 | false 265 | 266 | 267 | 268 | 269 | 270 | docked_collapsed 271 | 272 | 273 | 274 | 275 | 276 | 277 | docked_collapsed 278 | 279 | 280 | 281 | 282 | 283 | 284 | docked_collapsed 285 | 286 | 287 | 288 | false 289 | 290 | 291 | 292 | 293 | 294 | 295 | 0 0 2 0 -0 0 296 | 0.5 0.5 0.5 1 297 | 0.1 0.1 0.1 1 298 | 299 | 20 300 | 0.5 301 | 0.01 302 | 0.001 303 | 304 | 0 305 | 0 0 -1 306 | 307 | 308 | 309 | true 310 | 311 | 312 | 313 | 314 | 0 0 1 315 | 100 100 316 | 317 | 318 | 319 | 320 | 321 | 322 | 323 | 324 | 325 | 326 | 327 | 328 | 329 | 0 0 1 330 | 100 100 331 | 332 | 333 | 334 | 0.8 0.8 0.8 1 335 | 0.8 0.8 0.8 1 336 | 0.8 0.8 0.8 1 337 | 338 | 339 | 0 0 0 0 -0 0 340 | 341 | 0 0 0 0 -0 0 342 | 1 343 | 344 | 1 345 | 0 346 | 0 347 | 1 348 | 0 349 | 1 350 | 351 | 352 | false 353 | 354 | 0 0 0 0 -0 0 355 | false 356 | 357 | 358 | 359 | model://Fidget/bottom_casing 360 | bottom_casing 361 | 0 0 0.84 0 3.14 0 362 | 363 | 364 | 365 | model://Fidget/top_casing 366 | top_casing 367 | 0.03 0.03 0.84 0 0 0 368 | 369 | 370 | 371 | model://Fidget/left_gear 372 | left_gear 373 | 0.06 -0.06 0.84 0 0 374 | 375 | 376 | 377 | model://Fidget/right_gear 378 | right_gear 379 | -0.06 -0.06 0.84 0 0 0 380 | 381 | 382 | 383 | model://table1 384 | table1 385 | -0.475 0 0 0 0 0 386 | 387 | 388 | 389 | 390 | 0 0 0.5 0 0 3.14 391 | 392 | 0.05 0.05 0.05 0 0 0 393 | 394 | 0.1 395 | 396 | 0.000166667 397 | 0.000166667 398 | 0.000166667 399 | 400 | 401 | 402 | 403 | 404 | 0.1 0.1 0.1 405 | 406 | 407 | 408 | 409 | 410 | 411 | 0.1 0.1 0.1 412 | 413 | 414 | 415 | 416 | 417 | 1.047 418 | 419 | 320 420 | 240 421 | 422 | 423 | 0.1 424 | 100 425 | 426 | 427 | 1 428 | 30 429 | true 430 | camera 431 | 432 | 433 | 434 | 435 | 436 | 60 437 | rgbd 438 | 439 | 1.0472 440 | 441 | 442 | 443 | 554.25469 444 | 554.25469 445 | 446 | 320.5 447 | 448 | 240.5 449 | 0 450 | 451 | 452 | 453 | 0.0 454 | 0.0 455 | 0.0 456 | 0.0 457 | 0.0 458 |
0.5 0.5
459 | 460 | 461 | 640 462 | 480 463 | R8G8B8 464 | 465 | 466 | 0.01 467 | 300 468 | 469 | 470 | 471 | 0.1 472 | 10 473 | 474 | 475 | 476 | gaussian 477 | 0 478 | 0.007 479 | 480 | 481 | 482 | 483 | 484 | 0 0 10 0 -0 0 485 | true 486 | 1 487 | -0.5 0.1 -0.9 488 | 0.8 0.8 0.8 1 489 | 0.2 0.2 0.2 1 490 | 491 | 1000 492 | 0.01 493 | 0.90000000000000002 494 | 0.001 495 | 496 | 497 | 0 498 | 0 499 | 0 500 | 501 | 502 | 503 | 504 | -------------------------------------------------------------------------------- /render_icp_data.py: -------------------------------------------------------------------------------- 1 | #%% Importing libraries 2 | import sys 3 | import numpy as np 4 | import open3d as o3d 5 | import rospy 6 | import tf 7 | from cv_bridge import CvBridge, CvBridgeError 8 | from gazebo_msgs.msg import LinkState as stateGZ 9 | from gazebo_msgs.srv import GetModelState as getStateGZ 10 | from gazebo_msgs.srv import SetLinkState as setStateGZ 11 | from geometry_msgs.msg import (Point, Pose, PoseArray, PoseStamped, Quaternion, 12 | Twist, Vector3) 13 | from rospy.exceptions import ROSException 14 | from scipy.spatial.transform import Rotation as R 15 | from sensor_msgs.msg import CameraInfo, Image, PointCloud2 16 | import copy 17 | import time 18 | import cv2 19 | import scipy.io as sio 20 | import os 21 | import argparse 22 | import json 23 | from numpy import save 24 | 25 | 26 | 27 | # cd ~/tf2_tools && source devel/setup.bash 28 | # rosrun tf2_tools echo2.py base_link camera_depth_optical_frame 29 | 30 | # python3 render_icp_data.py -m bottom_casing left_gear right_gear top_casing -d fidget -s 1 31 | # python3 render_icp_data.py -m Nema17 sun_gear housing carrier cover -d Nema17_reducer -s 1 32 | # python3 render_icp_data.py -mcase cover gear_0 gear_1 gear_1_g_testing gear_carrier -d Planetary_reducer -s 1 33 | 34 | #%% Pass the models to be rendered as arguments via argument parser 35 | parser = argparse.ArgumentParser(description = 'parse some parameters') 36 | parser.add_argument('-m','--models', nargs='+', help="Enter the names of the models seperated by a space",required=True) 37 | parser.add_argument('-d','--dataset',type=str, help="enter dataset name",required=True) 38 | parser.add_argument('-s','--stage' ,type=int, help="enter the stage of assembly",required=True) 39 | #args = parser.parse_args('-m bottom_casing left_gear right_gear top_casing -d fidget_dataset -s 1'.split()) 40 | args = parser.parse_args() 41 | 42 | n_models = len(args.models) 43 | dataset_name = args.dataset 44 | stage_number = str(args.stage) 45 | 46 | #%% Create the directories if they don't already exist 47 | if not os.path.exists(dataset_name+'_dataset/stage_'+stage_number+'/rgb'): 48 | os.makedirs(dataset_name+'_dataset/stage_'+stage_number+'/rgb') 49 | if not os.path.exists(dataset_name+'_dataset/stage_'+stage_number+'/depth'): 50 | os.makedirs(dataset_name+'_dataset/stage_'+stage_number+'/depth') 51 | if not os.path.exists(dataset_name+'_dataset/stage_'+stage_number+'/seg_maps'): 52 | os.makedirs(dataset_name+'_dataset/stage_'+stage_number+'/seg_maps') 53 | if not os.path.exists(dataset_name+'_dataset/stage_'+stage_number+'/masks'): 54 | os.makedirs(dataset_name+'_dataset/stage_'+stage_number+'/masks') 55 | if not os.path.exists(dataset_name+'_dataset/model_pointcloud'): 56 | os.makedirs(dataset_name+'_dataset/model_pointcloud') 57 | if not os.path.exists(dataset_name+'_dataset/model_meshes'): 58 | os.makedirs(dataset_name+'_dataset/model_meshes') 59 | 60 | print('Generating data for '+str(n_models)+' selected models') 61 | #%% Initialize ros 62 | rospy.init_node('data_render_gazebo', anonymous = True) 63 | rate = rospy.Rate(0.5) 64 | bridge = CvBridge() 65 | cam_info_msg = rospy.wait_for_message('camera/color/camera_info', CameraInfo, timeout = 2) 66 | dpt_cam_info_msg = rospy.wait_for_message('camera/depth/camera_info', CameraInfo, timeout = 2) 67 | 68 | 69 | #%% Set camera pose in gazebo 70 | def set_cam_state_gazebo(camPos, camTwist): 71 | # Set cam state in gazebo 72 | camstate = stateGZ('robot::base_link', camPos, camTwist, 'world' ) 73 | print('Transforming camera to pose : '+str(sample_num)) 74 | try: 75 | gzclient = rospy.ServiceProxy('gazebo/set_link_state', setStateGZ) 76 | resp = gzclient(camstate) 77 | 78 | except Exception as inst: 79 | print('Error in gazebo/set_link_state service request: ' + str(inst) ) 80 | 81 | #%% Calculate parameters for current loop for camera state setting 82 | def calc_params(phi,theta,dist): 83 | theta_rad = np.deg2rad(theta) 84 | phi_rad = np.deg2rad(phi) 85 | X = dist*np.cos(phi_rad)*np.cos(theta_rad) 86 | Y = dist*np.cos(phi_rad)*np.sin(theta_rad) 87 | Z = np.abs(dist*np.sin(phi_rad)) + 0.84 88 | 89 | cam_euler = R.from_euler('xyz',[0,phi,theta+180], degrees=True) 90 | cam_quat = cam_euler.as_quat() 91 | 92 | camPos = Pose(position= Point(x=X, y=Y, z=Z), 93 | orientation= Quaternion(x=cam_quat[0], y=cam_quat[1] , z=cam_quat[2], w=cam_quat[3])) 94 | camTwist = Twist(linear= Vector3(x=0, y=0, z=0) , 95 | angular= Vector3(x=0, y=0, z=0)) 96 | 97 | return camPos,camTwist, X, Y,Z, cam_euler 98 | 99 | #%% inplane rotation of the kinect 100 | 101 | #%% Function to convert between Image types for depth images 102 | def convert_types(img, orig_min, orig_max, tgt_min, tgt_max, tgt_type): 103 | 104 | #info = np.finfo(img.dtype) # Get the information of the incoming image type 105 | # normalize the data to 0 - 1 106 | img_out = img / (orig_max-orig_min) # Normalize by input range 107 | img_out = (tgt_max - tgt_min) * img_out # Now scale by the output range 108 | img_out = img_out.astype(tgt_type) 109 | 110 | #cv2.imshow("Window", img) 111 | return img_out 112 | 113 | #%% Avoid duplicates 114 | def check_dup(): 115 | rgb_duplicate = True 116 | while rgb_duplicate: 117 | print('Subscribing to rgb topics...') 118 | img_msg = rospy.wait_for_message('/camera/color/image_raw', Image, timeout = 3) 119 | cv_image = bridge.imgmsg_to_cv2(img_msg, desired_encoding='bgr8') 120 | #cv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY) 121 | 122 | if sample_num > 0: 123 | #No point checking for the sample 0 124 | previous_im = cv2.imread(dataset_name+'_dataset/stage_'+stage_number+'/rgb/'+str(sample_num-1)+'.png', -1) 125 | rgb_duplicate = abs(np.mean(cv_image - previous_im)) < 0.5 # Mean of all pixels shouldn't be this small if it's two different images 126 | print('rgb diff: '+str(np.mean(cv_image - previous_im))) 127 | print(rgb_duplicate) 128 | if rgb_duplicate: 129 | #Try setting state again. Sometimes gazebo trips out as well. 130 | set_cam_state_gazebo(camPos, camTwist) 131 | 132 | else: 133 | rgb_duplicate = False 134 | 135 | depth_duplicate = True 136 | while depth_duplicate: 137 | print('Subscribing to depth topics...') 138 | depthImg_msg = rospy.wait_for_message('/camera/depth/image_raw', Image, timeout = 3 ) 139 | cv_depthImage = bridge.imgmsg_to_cv2(depthImg_msg, desired_encoding='passthrough') 140 | if sample_num > 0: 141 | previous_im = cv2.imread(dataset_name+'_dataset/stage_'+stage_number+'/depth/'+str(sample_num-1)+'.png', -1) 142 | depth_duplicate = abs(np.nanmean(cv_depthImage - previous_im))< 50 # Mean of all pixels shouldn't be this small if it's two different images 143 | print('depth diff: '+str(np.nanmean(cv_depthImage - previous_im)))# - previous_im))) 144 | print(depth_duplicate) 145 | if depth_duplicate: 146 | #Try setting state again. Sometimes gazebo trips out as well. 147 | set_cam_state_gazebo(camPos, camTwist) 148 | else: 149 | depth_duplicate = False 150 | 151 | return cv_image, cv_depthImage 152 | 153 | #%% Get Camera Extrinsics 154 | def get_camera_extrinsics(phi,theta, dist): 155 | 156 | _,_,X,Y,Z,_ = calc_params(phi,theta,dist) 157 | 158 | cam_euler = R.from_euler('xyz',[0,phi,theta+180], degrees=True) 159 | cam_world_R = cam_euler.as_matrix() 160 | cam_world_t = np.array([X,Y,Z]).reshape(3,1) 161 | cam_world_T = np.hstack((cam_world_R,cam_world_t)) 162 | cam_world_T = np.vstack((cam_world_T, [0,0,0,1])) 163 | 164 | return cam_world_T 165 | 166 | #%% get object states and save pose 167 | def get_object_states(n_models): 168 | resp=[] 169 | #Get object state 170 | try: 171 | rospy.wait_for_service('gazebo/get_model_state') 172 | client = rospy.ServiceProxy('gazebo/get_model_state', getStateGZ) 173 | for i in range(n_models): 174 | #print(args.models[i]) 175 | #print(client(args.models[i], 'world')) 176 | resp.append( client(args.models[i], 'world')) 177 | except Exception as inst: 178 | print('Error in gazebo/get_link_state service request: ' + str(inst) ) 179 | return resp 180 | 181 | #%% True Object pose in world frame obtained from Gazebo Service 182 | def get_object2cam_pose(resp, n_models, cam_world_T): 183 | 184 | obj_cam_T = np.zeros(( n_models, 4, 4)) # Transformation Mats for 10 object classes 185 | obj_cam = np.zeros(( n_models,4, 4)) 186 | obj_world = np.zeros((n_models, 4, 4)) 187 | 188 | 189 | for i in range(0 , n_models): 190 | obj_pos = np.array([resp[i].pose.position.x, resp[i].pose.position.y,resp[i].pose.position.z]).reshape(3,1) 191 | obj_or = [resp[i].pose.orientation.x, resp[i].pose.orientation.y, resp[i].pose.orientation.z, resp[i].pose.orientation.w] 192 | obj_or = (R.from_quat(obj_or)).as_matrix() 193 | obj_world_T = np.concatenate((obj_or, obj_pos), axis = 1) 194 | 195 | 196 | # Transformation from object2world to object2cam for GT label poses 197 | #obj_cam_T = np.dot(obj_world_T, np.linalg.inv(cam_world_T) ) 198 | obj_world_T = np.vstack(( obj_world_T, [0,0,0,1] )) 199 | 200 | obj_world[i,:,:] = obj_world_T 201 | 202 | obj_cam_T[i, :, :] = np.dot( np.linalg.inv(cam_world_T), obj_world_T )#[:3,:] 203 | obj_cam[i,:,:] = np.dot( np.linalg.inv(cam_world_T), obj_world_T ) 204 | #print(i,np.dot( np.linalg.inv(cam_world_T), obj_world_T )) 205 | 206 | #print(obj_cam_T) 207 | gt_dict = { 'poses':obj_cam_T[:3,:,:] } #returns [ R T , i] 208 | return gt_dict, obj_cam_T, obj_cam, obj_world 209 | 210 | #%% Load the meshes of all objects convert them to point clouds 211 | # combine and return the pointclouds of all meshes in a dictionary 212 | def mesh2pcld(n_models): 213 | 214 | all_points = {} 215 | all_pclds = {} 216 | mesh_path = dataset_name+'_dataset/model_meshes/' 217 | pcld_path = dataset_name+'_dataset/model_pointcloud/' 218 | 219 | for i in range (0,n_models): 220 | 221 | model = str(args.models[i]) 222 | if not os.path.exists(pcld_path + model +'.ply'): 223 | 224 | 225 | mesh = o3d.io.read_triangle_mesh(mesh_path + str(args.models[i])+'.ply') 226 | pcld = mesh.sample_points_poisson_disk(number_of_points=30000) 227 | all_pclds[model] = pcld 228 | o3d.io.write_point_cloud(pcld_path + model +'.ply', pcld ) 229 | all_points[model] = np.asarray(pcld.points)#, dtype= np.float32) 230 | 231 | else: 232 | print( model +" pointcloud file already exists..") 233 | pcld = o3d.io.read_point_cloud (pcld_path + model + '.ply') 234 | all_pclds[model] = pcld 235 | all_points[model] = np.asarray(pcld.points) 236 | 237 | return all_points, all_pclds 238 | 239 | #%% Function to fill empty spaces in point cloud project 240 | def cv_im_fill(bin_im): 241 | im_floodfill = bin_im.copy() 242 | # Mask used to flood filling. 243 | # Notice the size needs to be 2 pixels smaller than the image. 244 | h, w = bin_im.shape[:2] 245 | mask = np.zeros((h+2, w+2), np.uint8) 246 | # Floodfill from point (0, 0) 247 | cv2.floodFill(im_floodfill, mask, (0,0), 255); 248 | # Invert floodfilled image 249 | im_floodfill_inv = cv2.bitwise_not(im_floodfill) 250 | # Combine the two images to get the foreground. 251 | im_out = bin_im | im_floodfill_inv 252 | return im_out 253 | 254 | def get_pcd_order(all_pclds,obj_world, models_list,dpt_opt_world_T): 255 | 256 | pclds_centers =[] 257 | for i in range(0, len(models_list)) : 258 | this_pcld = all_pclds[str(models_list[i])] 259 | transformation_in_opt_frame = np.dot(np.linalg.inv(dpt_opt_world_T), obj_world[i,:,:]) 260 | pcd_in_opt_frame = copy.deepcopy(this_pcld).transform(transformation_in_opt_frame) 261 | pclds_centers.append(-pcd_in_opt_frame.get_center()[2]) 262 | 263 | pclds_from_farthest_2_closest = np.argsort(pclds_centers) 264 | 265 | return pclds_from_farthest_2_closest 266 | 267 | #%% Transform the pointclouds to binary mask 268 | def pcl_2_binary_mask(obj_cam_T,n_models, all_points): 269 | 270 | #Projection Matrix / Camera instrinsics 271 | cam_P = np.array(cam_info_msg.P).reshape(3,4) 272 | cam_P = np.vstack((cam_P , [0,0,0,1])) 273 | dpt_cam_P = np.array(dpt_cam_info_msg.P).reshape(3,4) 274 | dpt_cam_P = np.vstack((dpt_cam_P , [0,0,0,1])) 275 | 276 | 277 | # Empty arrays 278 | bin_mask = np.zeros((cam_info_msg.height, cam_info_msg.width), dtype= np.uint8) 279 | seg_mask = np.zeros((cam_info_msg.height, cam_info_msg.width), dtype= np.uint8) 280 | mask_list = [] 281 | pixels_list = [] 282 | 283 | 284 | # get the tranformation matrix for depth optical_frame wrt to base_link frame 285 | dpt_opt_cam_T = rospy.wait_for_message('/Cam_2_World', PoseStamped ) 286 | T_pos = np.array([dpt_opt_cam_T.pose.position.x, dpt_opt_cam_T.pose.position.y, dpt_opt_cam_T.pose.position.z]) 287 | T_pos = T_pos.reshape(3,1) 288 | T_or = [dpt_opt_cam_T.pose.orientation.x, dpt_opt_cam_T.pose.orientation.y, dpt_opt_cam_T.pose.orientation.z, dpt_opt_cam_T.pose.orientation.w] 289 | T_or = (R.from_quat(T_or)).as_matrix() 290 | dpt_opt_cam_T = np.concatenate((T_or, T_pos), axis = 1) 291 | dpt_opt_cam_T = np.vstack((dpt_opt_cam_T, [0,0,0,1] )) 292 | 293 | # get the tranformation matrix for depth_optical_frame wrt to world_frame 294 | dpt_opt_world_T = np.dot(cam_world_T, dpt_opt_cam_T) 295 | 296 | 297 | # get pclds ordered in farthest to closest distance 298 | pcd_order = get_pcd_order(all_pclds,obj_world, args.models,dpt_opt_world_T) 299 | 300 | for i in pcd_order: 301 | print(str(args.models[i])) 302 | print(i) 303 | 304 | #1. Get the pose of this_object in optical frame 305 | thisObj_world_T = obj_world[i,:,:] 306 | thisObj_opt_T = np.dot(np.linalg.inv(dpt_opt_world_T), thisObj_world_T) 307 | 308 | #1. Transform the pointcloud of this_object to optical frame 309 | this_pcld = all_pclds[str(args.models[i])] 310 | cloud_optical = copy.deepcopy(this_pcld).transform(thisObj_opt_T) 311 | cloud_optical = np.asarray(cloud_optical.points).transpose() 312 | cloud_optical = np.vstack((cloud_optical, np.ones((1,cloud_optical.shape[1])) )) 313 | 314 | 315 | # perspective projection into image-plane 316 | x,y,z,w = np.dot( dpt_cam_P,cloud_optical).astype(np.float32) #This is the projection step 317 | x = x / z 318 | y = y / z 319 | 320 | 321 | #clips out all the points projected out of image height and width 322 | clipping = np.logical_and( np.logical_and(x>=0, x<=cam_info_msg.width) , 323 | np.logical_and(y>=0, y<=cam_info_msg.height) ) 324 | x = x[np.where(clipping)] 325 | y = y[np.where(clipping)] 326 | 327 | 328 | #Leave the background black 329 | pixels = np.vstack((x,y)).transpose() 330 | pixels = np.array(pixels, dtype=np.uint16) 331 | pixels_list.append([pixels]) 332 | 333 | this_mask = np.zeros((cam_info_msg.height, cam_info_msg.width), dtype= np.uint8) 334 | 335 | for point in pixels: 336 | this_mask[point[1]-1, point[0]-1] = 255 337 | 338 | this_mask = cv_im_fill(this_mask) 339 | 340 | this_mask[this_mask.nonzero()] = 1.05*np.ceil(255*(i+1)/n_models) 341 | r,c = this_mask.nonzero() 342 | 343 | bin_mask[this_mask.nonzero()] = 0 344 | bin_mask += this_mask 345 | 346 | seg_mask[this_mask.nonzero()] = i+1 347 | 348 | 349 | return bin_mask, seg_mask , dpt_opt_world_T 350 | 351 | #%% Main program 352 | 353 | # convert the meshes to pointclouds 354 | print('Generating the pointclouds for '+str(n_models)+ ' models. hold on , This could take few minutes ...') 355 | all_points, all_pclds = mesh2pcld(n_models) 356 | print(all_points.keys()) 357 | sample_num = 0 358 | #0.15,0.5,0.125 359 | #0.25,0.6,0.0625 360 | for dist in np.arange(0.25,0.6,0.0625): 361 | for phi in range(35,70,15): 362 | for theta in range(0, 360, 15): 363 | 364 | camPos,camTwist,_,_,_,_ = calc_params(phi,theta,dist) 365 | set_cam_state_gazebo(camPos, camTwist) 366 | 367 | 368 | while not rospy.is_shutdown(): 369 | print('Subscribing to camera topics...') 370 | try: 371 | cv_image, cv_depthImage = check_dup() 372 | break 373 | except ROSException as e: 374 | print('Timeout occured in subscribing.Trying again...') 375 | continue 376 | 377 | #cv_depthImage = convert_types(cv_depthImage,0,3, 0,65535, np.uint16) ## 0 - 3m is the input range of kinect depth 378 | print('Writing Images') 379 | cv2.imwrite(dataset_name+'_dataset/stage_'+stage_number+'/rgb/'+str(sample_num)+'.png', cv_image) 380 | cv2.imwrite(dataset_name+'_dataset/stage_'+stage_number+'/depth/'+str(sample_num)+'.png',cv_depthImage) 381 | 382 | try: 383 | resp = get_object_states(n_models) 384 | except Exception as inst: 385 | print('Error in gazebo/get_link_state service request: ' + str(inst) ) 386 | 387 | cam_world_T = get_camera_extrinsics(phi,theta,dist) 388 | gt_dict,obj_cam_T,obj_cam, obj_world = get_object2cam_pose(resp, n_models,cam_world_T) 389 | 390 | 391 | # save binary mask and segmentation map 392 | bin_mask, seg_mask, dpt_opt_world_T = pcl_2_binary_mask(obj_cam_T, n_models, all_points) 393 | 394 | print('Writing binary mask images') 395 | cv2.imwrite(dataset_name+'_dataset/stage_'+stage_number+'/masks/'+str(sample_num)+'.png',bin_mask) 396 | 397 | print('Saving segment maps as numpy files') 398 | save(dataset_name+'_dataset/stage_'+stage_number+'/seg_maps/'+str(sample_num)+'_seg_map.npy', seg_mask) 399 | 400 | # save scene_gt and scene_camera 401 | print('Writing scene_gt,scene_w_gt,scene_camera json files', '\n'*5) 402 | 403 | with open(dataset_name+'_dataset/stage_'+stage_number+"/scene_gt.json", "a") as outfile: 404 | gt_all= {} 405 | obj_list =[] 406 | for i in range(0,n_models): 407 | obj_data = { 408 | "obj_id": str(i), 409 | "cam_R_m2c": np.ravel(np.reshape(obj_cam[i][0:3,0:3],(1,9))).tolist(), 410 | "cam_t_m2c": (obj_cam[i][0:3,3]*1000).tolist(), 411 | } 412 | obj_list.append(obj_data) 413 | gt_all[str(sample_num)] = obj_list 414 | json.dump(gt_all, outfile) 415 | outfile.write("\n") # Add newline cause Py JSON does not 416 | 417 | with open(dataset_name+'_dataset/stage_'+stage_number+"/scene_w_gt.json", "a") as outfile: 418 | gt_w_all= {} 419 | obj_list =[] 420 | for i in range(0,n_models): 421 | obj_data = { 422 | "obj_id": str(i), 423 | "cam_R_m2c": np.ravel(np.reshape(obj_world[i][0:3,0:3],(1,9))).tolist(), 424 | "cam_t_m2c": (obj_world[i][0:3,3]*1000).tolist() 425 | } 426 | obj_list.append(obj_data) 427 | gt_w_all[str(sample_num)] = obj_list 428 | json.dump(gt_w_all, outfile) 429 | outfile.write("\n") # Add newline cause Py JSON does not 430 | 431 | with open(dataset_name+'_dataset/stage_'+stage_number+"/scene_camera.json", "a") as outfile: 432 | scene_cam = {} 433 | obj_data = { 434 | "dpt_cam_K": np.array(dpt_cam_info_msg.K).tolist(), 435 | "cam_K" : np.array(cam_info_msg.K).tolist() , 436 | "depth_scale" : 1.0, 437 | "dptopt_cam_R_w2c":np.ravel(np.reshape(dpt_opt_world_T[0:3,0:3],(1,9))).tolist() , 438 | "dptopt_cam_t_w2c": (dpt_opt_world_T[0:3,3]*1000).tolist(), 439 | "cam_R_w2c":np.ravel(np.reshape(cam_world_T[0:3,0:3],(1,9))).tolist() , 440 | "cam_t_w2c": (cam_world_T[0:3,3]*1000).tolist(), 441 | } 442 | 443 | #this_cam.append(obj_data) 444 | scene_cam[str(sample_num)] = obj_data 445 | json.dump(scene_cam, outfile) 446 | outfile.write("\n") # Add newline cause Py JSON does not 447 | sample_num += 1 448 | 449 | 450 | --------------------------------------------------------------------------------