Detection issue on custom YCB objects

Muhayyuddin commented 6 days ago

Hi, I tried with NVIDIA 3080 ti GPU, it didnot complain about the GPU memory issue anymore, thanks for highlighting about GPU memory. However, now when i try to detect the ycb objects (banana) the detecting didnot works. i followed the following steps. 1) Put the banana.obj file in the demo_data directory. 2) when i run the foundationpose_ros_multi.py it shows the newly added object in the list (image below ) Screenshot from 2024-11-09 17-56-33

3) it shows the window with the segmentation, i select banana and press enter Screenshot from 2024-11-09 17-51-50 then it show the window but without any detection as below

any idea to fix the detection issue ? regards

ammar-n-abbas commented 6 days ago

HI @Muhayyuddin

Can you show the terminal output?

It seems that you have to resize the mesh to meters, it might be in mm.

You may use this

mesh = trimesh.load('path/to/your/mesh/file.stl')
mesh.apply_scale(0.001)
mesh.export('path/to/save/resized_mesh.stl')

then replace the mesh file with the exported one.

Muhayyuddin commented 6 days ago

Hi @Ammar, Thank you for your reply. I tried by resizing the mesh as well but still not detecting the objects, below is the terminal output ''' [register()] Welcome [register()] poses:(252, 4, 4) [register()] after viewpoint, add_errs min:-1.0 [predict()] ob_in_cams:(252, 4, 4) [predict()] self.cfg.use_normal:False [predict()] trans_normalizer:[0.019999999552965164, 0.019999999552965164, 0.05000000074505806], rot_normalizer:0.3490658503988659 [predict()] making cropped data [make_crop_data_batch()] Welcome make_crop_data_batch [make_crop_data_batch()] make tf_to_crops done [make_crop_data_batch()] render done [make_crop_data_batch()] warp done [make_crop_data_batch()] pose batch data done [predict()] forward start [predict()] forward done [predict()] making cropped data [make_crop_data_batch()] Welcome make_crop_data_batch [make_crop_data_batch()] make tf_to_crops done [make_crop_data_batch()] render done [make_crop_data_batch()] warp done [make_crop_data_batch()] pose batch data done [predict()] forward start [predict()] forward done [predict()] making cropped data [make_crop_data_batch()] Welcome make_crop_data_batch [make_crop_data_batch()] make tf_to_crops done [make_crop_data_batch()] render done [make_crop_data_batch()] warp done [make_crop_data_batch()] pose batch data done [predict()] forward start [predict()] forward done [predict()] making cropped data [make_crop_data_batch()] Welcome make_crop_data_batch [make_crop_data_batch()] make tf_to_crops done [make_crop_data_batch()] render done [make_crop_data_batch()] warp done [make_crop_data_batch()] pose batch data done [predict()] forward start [predict()] forward done [predict()] ob_in_cams:(252, 4, 4) [predict()] self.cfg.use_normal:False [predict()] making cropped data [make_crop_data_batch()] Welcome make_crop_data_batch [make_crop_data_batch()] make tf_to_crops done [make_crop_data_batch()] render done [make_crop_data_batch()] pose batch data done

[predict()] forward done [register()] final, add_errs min:-1.0 [register()] sort ids:tensor([ 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 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, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63, 64, 65, 66, 67, 68, 69, 70, 71, 72, 73, 74, 75, 76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, 90, 91, 92, 93, 94, 95, 96, 97, 98, 99, 100, 101, 102, 103, 104, 105, 106, 107, 108, 109, 110, 111, 112, 113, 114, 115, 116, 117, 118, 119, 120, 121, 122, 123, 124, 125, 126, 127, 128, 129, 130, 131, 132, 133, 134, 135, 136, 137, 138, 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, 161, 162, 163, 164, 165, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 181, 182, 183, 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, 198, 199, 200, 201, 202, 203, 204, 205, 206, 207, 208, 209, 210, 211, 212, 213, 214, 215, 216, 217, 218, 219, 220, 221, 222, 223, 224, 225, 226, 227, 228, 229, 230, 231, 232, 233, 234, 235, 236, 237, 238, 239, 240, 241, 242, 243, 244, 245, 246, 247, 248, 249, 250, 251]) [register()] sorted scores:tensor([145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125]) [register()] Welcome [register()] poses:(252, 4, 4) [register()] after viewpoint, add_errs min:-1.0 [predict()] ob_in_cams:(252, 4, 4) [predict()] self.cfg.use_normal:False [predict()] trans_normalizer:[0.019999999552965164, 0.019999999552965164, 0.05000000074505806], rot_normalizer:0.3490658503988659 [predict()] making cropped data [make_crop_data_batch()] Welcome make_crop_data_batch [make_crop_data_batch()] make tf_to_crops done [make_crop_data_batch()] render done [make_crop_data_batch()] warp done [make_crop_data_batch()] pose batch data done [predict()] forward start [predict()] forward done [predict()] making cropped data [make_crop_data_batch()] Welcome make_crop_data_batch [make_crop_data_batch()] make tf_to_crops done [make_crop_data_batch()] render done [make_crop_data_batch()] warp done [make_crop_data_batch()] pose batch data done [predict()] forward start [predict()] forward done [predict()] making cropped data [make_crop_data_batch()] Welcome make_crop_data_batch [make_crop_data_batch()] make tf_to_crops done [make_crop_data_batch()] render done [make_crop_data_batch()] warp done [make_crop_data_batch()] pose batch data done [predict()] forward start [predict()] forward done [predict()] making cropped data [make_crop_data_batch()] Welcome make_crop_data_batch [make_crop_data_batch()] make tf_to_crops done [make_crop_data_batch()] render done [make_crop_data_batch()] warp done [make_crop_data_batch()] pose batch data done [predict()] forward start [predict()] forward done [predict()] ob_in_cams:(252, 4, 4) [predict()] self.cfg.use_normal:False [predict()] making cropped data [make_crop_data_batch()] Welcome make_crop_data_batch [make_crop_data_batch()] make tf_to_crops done [make_crop_data_batch()] render done [make_crop_data_batch()] pose batch data done

[predict()] forward done [register()] final, add_errs min:-1.0 [register()] sort ids:tensor([ 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 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, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63, 64, 65, 66, 67, 68, 69, 70, 71, 72, 73, 74, 75, 76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, 90, 91, 92, 93, 94, 95, 96, 97, 98, 99, 100, 101, 102, 103, 104, 105, 106, 107, 108, 109, 110, 111, 112, 113, 114, 115, 116, 117, 118, 119, 120, 121, 122, 123, 124, 125, 126, 127, 128, 129, 130, 131, 132, 133, 134, 135, 136, 137, 138, 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, 161, 162, 163, 164, 165, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 181, 182, 183, 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, 198, 199, 200, 201, 202, 203, 204, 205, 206, 207, 208, 209, 210, 211, 212, 213, 214, 215, 216, 217, 218, 219, 220, 221, 222, 223, 224, 225, 226, 227, 228, 229, 230, 231, 232, 233, 234, 235, 236, 237, 238, 239, 240, 241, 242, 243, 244, 245, 246, 247, 248, 249, 250, 251]) [register()] sorted scores:tensor([145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125, 145.3125]) [INFO] [1731228243.058595468] [pose_estimation_node]: Object 1 selected. [reset_object()] self.diameter:0.2976207394250458, vox_size:0.01488103697125229 [reset_object()] self.pts:torch.Size([527, 3]) [reset_object()] reset done [make_rotation_grid()] cam_in_obs:(42, 4, 4) [make_rotation_grid()] rot_grid:(252, 4, 4) num original candidates = 252 num of pose after clustering: 252 [make_rotation_grid()] after cluster, rot_grid:(252, 4, 4) [make_rotation_grid()] self.rot_grid: torch.Size([252, 4, 4]) [register()] Welcome [register()] poses:(252, 4, 4) [register()] after viewpoint, add_errs min:-1.0 [predict()] ob_in_cams:(252, 4, 4) [predict()] self.cfg.use_normal:False [predict()] trans_normalizer:[0.019999999552965164, 0.019999999552965164, 0.05000000074505806], rot_normalizer:0.3490658503988659 [predict()] making cropped data [make_crop_data_batch()] Welcome make_crop_data_batch [make_crop_data_batch()] make tf_to_crops done [make_crop_data_batch()] render done [make_crop_data_batch()] warp done [make_crop_data_batch()] pose batch data done [predict()] forward start [predict()] forward done [predict()] making cropped data [make_crop_data_batch()] Welcome make_crop_data_batch [make_crop_data_batch()] make tf_to_crops done [make_crop_data_batch()] render done [make_crop_data_batch()] warp done [make_crop_data_batch()] pose batch data done [predict()] forward start [predict()] forward done [predict()] making cropped data [make_crop_data_batch()] Welcome make_crop_data_batch [make_crop_data_batch()] make tf_to_crops done [make_crop_data_batch()] render done [make_crop_data_batch()] warp done [make_crop_data_batch()] pose batch data done [predict()] forward start [predict()] forward done [predict()] making cropped data [make_crop_data_batch()] Welcome make_crop_data_batch [make_crop_data_batch()] make tf_to_crops done [make_crop_data_batch()] render done [make_crop_data_batch()] warp done [make_crop_data_batch()] pose batch data done [predict()] forward start [predict()] forward done [predict()] ob_in_cams:(252, 4, 4) [predict()] self.cfg.use_normal:False [predict()] making cropped data [make_crop_data_batch()] Welcome make_crop_data_batch [make_crop_data_batch()] make tf_to_crops done [make_crop_data_batch()] render done

''' regards Muhayyuddin

ammar-n-abbas commented 6 days ago

check if the cam_K matrix is properly initialized and if you can see the pose of the object through ros2 topic list

Muhayyuddin commented 6 days ago

It seems the cam_K matrix is initialized below is the terminal output. No, with ros2 topic list it is not showing any pose topic

[__init__()] self.h5_file:
[__init__()] Using pretrained model from /home/muhayy/pose_ws/src/FoundationPoseROS2/FoundationPose/learning/training/../../weights/2023-10-28-18-33-37/model_best.pth
[__init__()] init done
[INFO] [1731232813.840266548] [pose_estimation_node]: Camera intrinsic matrix initialized: [[      429.2           0      423.88]
 [          0      428.64      239.62]
 [          0           0           1]]

0: 1024x1024 1 0, 1 1, 1 2, 1 3, 1 4, 1 5, 1 6, 1 7, 1 8, 1 9, 1 10, 1 11, 1 12, 1 13, 1 14, 1 15, 1 16, 1 17, 1 18, 1 19, 1 20, 1 21, 2446.7ms
Speed: 14.8ms preprocess, 2446.7ms inference, 0.4ms postprocess per image at shape (1, 3, 1024, 1024)
[INFO] [1731232821.460357101] [pose_estimation_node]: Object 0 selected.
[reset_object()] self.diameter:0.19783510038918775, vox_size:0.009891755019459388
[reset_object()] self.pts:torch.Size([312, 3])
[reset_object()] reset done
[make_rotation_grid()] cam_in_obs:(42, 4, 4)
[make_rotation_grid()] rot_grid:(252, 4, 4)
num original candidates = 252
num of pose after clustering: 252
[make_rotation_grid()] after cluster, rot_grid:(252, 4, 4)
[make_rotation_grid()] self.rot_grid: torch.Size([252, 4, 4])
[register()] Welcome
Module Utils 64702ec load on device 'cuda:0' took 0.17 ms  (cached)
[register()] poses:(252, 4, 4)
[register()] after viewpoint, add_errs min:-1.0
/opt/conda/envs/my/lib/python3.10/site-packages/torch/__init__.py:614: UserWarning: torch.set_default_tensor_type() is deprecated as of PyTorch 2.1, please use torch.set_default_dtype() and torch.set_default_device() as alternatives. (Triggered internally at ../torch/csrc/tensor/python_tensor.cpp:451.)
  _C._set_default_tensor_type(t)
[predict()] ob_in_cams:(252, 4, 4)
[predict()] self.cfg.use_normal:False
[predict()] trans_normalizer:[0.019999999552965164, 0.019999999552965164, 0.05000000074505806], rot_normalizer:0.3490658503988659
[predict()] making cropped data
[make_crop_data_batch()] Welcome make_crop_data_batch
[make_crop_data_batch()] make tf_to_crops done
[make_crop_data_batch()] render done
/opt/conda/envs/my/lib/python3.10/site-packages/torch/functional.py:504: UserWarning: torch.meshgrid: in an upcoming release, it will be required to pass the indexing argument. (Triggered internally at ../aten/src/ATen/native/TensorShape.cpp:3526.)
  return _VF.meshgrid(tensors, **kwargs)  # type: ignore[attr-defined]
[make_crop_data_batch()] warp done
[make_crop_data_batch()] pose batch data done
[predict()] forward start
[predict()] forward done
[predict()] making cropped data
[make_crop_data_batch()] Welcome make_crop_data_batch
[make_crop_data_batch()] make tf_to_crops done
[make_crop_data_batch()] render done
[make_crop_data_batch()] warp done
[make_crop_data_batch()] pose batch data done
[predict()] forward start
[predict()] forward done
[predict()] making cropped data
[make_crop_data_batch()] Welcome make_crop_data_batch
[make_crop_data_batch()] make tf_to_crops done
[make_crop_data_batch()] render done
[make_crop_data_batch()] warp done
[make_crop_data_batch()] pose batch data done
[predict()] forward start
[predict()] forward done
[predict()] making cropped data
[make_crop_data_batch()] Welcome make_crop_data_batch
[make_crop_data_batch()] make tf_to_crops done
[make_crop_data_batch()] render done
[make_crop_data_batch()] warp done
[make_crop_data_batch()] pose batch data done
[predict()] forward start
[predict()] forward done
[predict()] ob_in_cams:(252, 4, 4)
[predict()] self.cfg.use_normal:False
[predict()] making cropped data
[make_crop_data_batch()] Welcome make_crop_data_batch
[make_crop_data_batch()] make tf_to_crops done
[make_crop_data_batch()] render done
[make_crop_data_batch()] pose batch data done

I am using intel realsense 405 the following topics

 self.image_sub = self.create_subscription(Image, '/camera/camera/color/image_rect_raw', self.image_callback, 10)
        self.depth_sub = self.create_subscription(Image, '/camera/camera/aligned_depth_to_color/image_raw', self.depth_callback, 10)
        self.info_sub = self.create_subscription(CameraInfo, '/camera/camera/color/camera_info', self.camera_info_callback, 10)

ammar-n-abbas commented 6 days ago

can you subscribe to '/camera/camera/color/image_raw' instead of '/camera/camera/color/image_rect_raw'

ammar-n-abbas commented 6 days ago

Otherwise, I think the range of depth for RS D405 might be low for this application

Muhayyuddin commented 6 days ago

Ok, thank you for point out this I will try with other depth camera like zed, will update if its work :-)

ammar-n-abbas commented 5 days ago

can you subscribe to '/camera/camera/color/image_raw' instead of '/camera/camera/color/image_rect_raw'

This did not work as well?

Muhayyuddin commented 4 days ago

Yes changing the topic did not work by the way which RS cam you used for the demo? I will setup the zed camera an will try let see.

ammar-n-abbas commented 4 days ago

We used RS D435

Muhayyuddin commented 4 days ago

After digging into the code i figured out the issue is in the following part of the code the condition " if pose_est.scorer is None:" is always false. just to test I added a dummy flag that run the else part and then the if part in they way it correctly show the pose and visulize as well.

any idea how to fix this issue that " if pose_est.scorer is None: " should not return None? Thank you for your help.

Screenshot from 2024-11-12 12-11-36

 for idx, data in self.pose_estimations.items():
            pose_est = data['pose_est']
            obj_mask = data['mask']
            to_origin = data['to_origin']
            if pose_est.scorer is None:
                pose = pose_est.track_one(rgb=color, depth=depth, K=self.cam_K, iteration=args.track_refine_iter)
                center_pose = pose @ np.linalg.inv(to_origin)
                self.publish_pose_stamped(center_pose, f"object_{idx}_frame", f"/Current_OBJ_position_{idx+1}")
                visualization_image = self.visualize_pose(visualization_image, center_pose, idx)
            else:
                pose = pose_est.register(K=self.cam_K, rgb=color, depth=depth, ob_mask=obj_mask, iteration=args.est_refine_iter)
            self.i += 1

I tested it in this way with dummy flag

for idx, data in self.pose_estimations.items():
            pose_est = data['pose_est']
            obj_mask = data['mask']
            to_origin = data['to_origin']
            if self.x:
                pose = pose_est.track_one(rgb=color, depth=depth, K=self.cam_K, iteration=args.track_refine_iter)
                center_pose = pose @ np.linalg.inv(to_origin)
                self.publish_pose_stamped(center_pose, f"object_{idx}_frame", f"/Current_OBJ_position_{idx+1}")
                visualization_image = self.visualize_pose(visualization_image, center_pose, idx)
            else:
                pose = pose_est.register(K=self.cam_K, rgb=color, depth=depth, ob_mask=obj_mask, iteration=args.est_refine_iter)
                print(pose)
                self.x=True

ammar-n-abbas commented 2 days ago

pose_est.scorer will not be "None" once it registers (estimates) the pose on the first frame before starting to track. The above-modified case you mentioned will estimate the pose on each frame, which we do not want due to the computational load and mask requirement.

Check out this thread which mentions a possible solution: https://github.com/ammar-n-abbas/FoundationPoseROS2/issues/4#issuecomment-2474027752

ammar-n-abbas commented 1 day ago

There was a minor bug in the code which has been fixed now, apologies for that. The demo data rosbag screen recording has now been added in the Readme along with the terminal log.

Muhayyuddin commented 1 day ago

Ok I will try by pulling the recent changes and will let you know. Thanks

ammar-n-abbas / FoundationPoseROS2

Detection issue on custom YCB objects #5