Implimentation on depth camera point cloud

[Kaivalya192]

Hi,,

How can i implement this neugraspnet to my depth camera rgbd feed with custom segmentation for objects?

Because evaluation code is for dataset evaluation .

how can i run this model on real world evaluation as shown in YouTube video by you..

Thank you.

[sjauhri]

Hi @Kaivalya192

You can have a look at the example script grasp_generator.py https://github.com/pearl-robot-lab/neugraspnet/blob/main/neugraspnet_ros/scripts/grasp_generator.py

You may add object segmentation to the image and thus segment the pointcloud for grasping a specific object

[Kaivalya192]

Hi i have successfully implemented on Intel realsense depth camera for prediction.

first i am sharing code and output i am getting..

Code

import argparse
import numpy as np
import open3d as o3d
import pyrealsense2 as rs # type: ignore
from types import SimpleNamespace

from neugraspnet.src.experiments.detection_implicit import NeuGraspImplicit
from neugraspnet.src.utils.perception import TSDFVolume
from neugraspnet.src.utils.transform import Transform

class FakeGripper:
    def __init__(self):
        self.max_opening_width = 0.08
        self.finger_depth = 0.05

class FakeSim:
    def __init__(self):
        self.gripper = FakeGripper()

def get_aligned_realsense_frames():
    pipeline = rs.pipeline()
    config = rs.config()

    config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
    config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)

    align = rs.align(rs.stream.color)
    profile = pipeline.start(config)

    # Warm-up
    for _ in range(10):
        pipeline.wait_for_frames()

    frames = pipeline.wait_for_frames()
    aligned = align.process(frames)

    color_frame = aligned.get_color_frame()
    depth_frame = aligned.get_depth_frame()

    depth_scale = profile.get_device().first_depth_sensor().get_depth_scale()

    color = np.asanyarray(color_frame.get_data())
    depth = np.asanyarray(depth_frame.get_data()).astype(np.float32) * depth_scale

    pipeline.stop()

    fx, fy = 615.37701416, 615.37701416
    cx, cy = 313.68743896, 259.01800537

    return color, depth, fx, fy, cx, cy

def integrate_tsdf(depth, fx, fy, cx, cy, size=0.3, resolution=64, down_sample_pc=0.0075):
    from neugraspnet.src.utils.perception import camera_on_sphere
    from neugraspnet.src.utils.transform import Transform

    tsdf = TSDFVolume(size, resolution)

    class Intrinsics:
        def __init__(self, width, height, fx, fy, cx, cy):
            self.width = width
            self.height = height
            self.fx = fx
            self.fy = fy
            self.cx = cx
            self.cy = cy

    intr = Intrinsics(640, 480, fx, fy, cx, cy)

    center = Transform.identity()
    extrinsic = camera_on_sphere(center, 0.6, np.pi / 6, np.pi / 4)

    tsdf.integrate(depth, intr, extrinsic)
    pc = tsdf.get_cloud()
    pc_down = pc.voxel_down_sample(voxel_size=down_sample_pc)

    print(f"[INFO] TSDF voxel count: {len(pc.points)}")
    return tsdf, pc

def draw_grasp_as_frame(pose: np.ndarray, width: float = 0.05, scale: float = 0.05):
    """Creates a coordinate frame or box representing a grasp."""
    frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=scale)
    T = np.eye(4)
    T[:3, :] = pose
    frame.transform(T)
    return frame

def create_grasp_mesh(grasp, score,finger_depth):
    from neugraspnet.src.utils.visual import grasp2mesh
    mesh = grasp2mesh(grasp,score, finger_depth=finger_depth)
    return mesh

def trimesh_to_open3d(tri_mesh):
    mesh = o3d.geometry.TriangleMesh()
    mesh.vertices = o3d.utility.Vector3dVector(tri_mesh.vertices)
    mesh.triangles = o3d.utility.Vector3iVector(tri_mesh.faces)
    if hasattr(tri_mesh.visual, 'vertex_colors') and len(tri_mesh.visual.vertex_colors) > 0:
        mesh.vertex_colors = o3d.utility.Vector3dVector(tri_mesh.visual.vertex_colors[:, :3] / 255.0)
    mesh.compute_vertex_normals()
    return mesh

def main(args):
    grasp_planner = NeuGraspImplicit(
        model_path=args.model,
        model_type=args.type,
        best=True,
        qual_th=0.5,
        force_detection=True,
        seen_pc_only=True,
        out_th=0.1,
        select_top=False,
        resolution=64,
        visualize=False,
        max_grasp_queries_at_once=40
    )

    color, depth, fx, fy, cx, cy = get_aligned_realsense_frames()

    tsdf, pc = integrate_tsdf(depth, fx, fy, cx, cy)

    if pc.is_empty():
        print("[ERROR] Empty point cloud! Adjust camera angle or depth.")
        return

    state = SimpleNamespace(tsdf=tsdf, pc=pc)
    grasps, scores, unseen_flags, _ = grasp_planner(state, sim=FakeSim())


    print(f"[INFO] Grasps found: {len(grasps)}")
    print(grasps[0].pose.as_matrix())
    print(f"[INFO] Average width: {np.mean([grasp.width for grasp in grasps])}")
    print(f"[INFO] Max width: {np.max([grasp.width for grasp in grasps])}")
    print(f"[INFO] Min width: {np.min([grasp.width for grasp in grasps])}")
    print(f"[INFO] Average score: {np.mean(scores)}")
    print(f"[INFO] Max score: {np.max(scores)}")
    print(f"[INFO] Min score: {np.min(scores)}")
    
    
    vis = o3d.visualization.Visualizer()
    vis.create_window("Grasp Visualization", width=1280, height=720)
    vis.add_geometry(pc)

    for grasp, score in zip(grasps, scores):
        g_trimesh = create_grasp_mesh(grasp, score=score, finger_depth=FakeSim().gripper.finger_depth)
        g_mesh_o3d = trimesh_to_open3d(g_trimesh)
        vis.add_geometry(g_mesh_o3d)

    vis.poll_events()
    vis.update_renderer()

    # Optional: Keep window alive
    while True:
        vis.poll_events()
        vis.update_renderer()



if __name__ == "__main__":
    parser = argparse.ArgumentParser()
    parser.add_argument("--model",default="./data/networks/neugraspnet_pile_efficient.pt", type=str)
    parser.add_argument("--type",default="neu_grasp_pn_deeper_efficient", type=str)
    args = parser.parse_args()
    main(args)

Output

Loading [neu_grasp_pn_deeper_efficient] model from ./data/networks/neugraspnet_pile_efficient.pt
[INFO] TSDF voxel count: 4320
/home/sastra/miniconda3/envs/neugraspnet/lib/python3.8/site-packages/torch/functional.py:478: 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:2894.)
  return _VF.meshgrid(tensors, **kwargs)  # type: ignore[attr-defined]
[Warning]: Points are too few! Skipping this grasp...
[Warning]: Points are too few! Skipping this grasp...
[Warning]: Points are too few! Skipping this grasp...
[Warning]: Points are too few! Skipping this grasp...
[Warning]: Points are too few! Skipping this grasp...
[Warning]: Points are too few! Skipping this grasp...
[Warning]: Points are too few! Skipping this grasp...
[Warning]: Points are too few! Skipping this grasp...
[Warning]: Points are too few! Skipping this grasp...
[Warning]: Points are too few! Skipping this grasp...
[Warning]: Points are too few! Skipping this grasp...
[Warning]: Points are too few! Skipping this grasp...
[Warning]: Points are too few! Skipping this grasp...
[Warning]: Points are too few! Skipping this grasp...
[Warning]: Points are too few! Skipping this grasp...
[Warning]: Points are too few! Skipping this grasp...
[Warning]: Points are too few! Skipping this grasp...
[Warning]: Points are too few! Skipping this grasp...
[Warning]: Points are too few! Skipping this grasp...
[Warning]: Points are too few! Skipping this grasp...
[Warning]: Points are too few! Skipping this grasp...
[Warning]: Points are too few! Skipping this grasp...
[Warning]: Points are too few! Skipping this grasp...
[Warning]: Points are too few! Skipping this grasp...
[Warning]: Points are too few! Skipping this grasp...
[Warning]: Points are too few! Skipping this grasp...
[Warning]: Points are too few! Skipping this grasp...
[Warning]: Points are too few! Skipping this grasp...
[Warning]: Points are too few! Skipping this grasp...
[Warning]: Points are too few! Skipping this grasp...
[Warning]: Points are too few! Skipping this grasp...
[Warning]: Points are too few! Skipping this grasp...
[Warning]: Points are too few! Skipping this grasp...
[Warning]: Points are too few! Skipping this grasp...
[Warning]: Points are too few! Skipping this grasp...
[Warning]: Points are too few! Skipping this grasp...
[Warning]: Points are too few! Skipping this grasp...
[Warning]: Points are too few! Skipping this grasp...
[Warning]: Points are too few! Skipping this grasp...
[Warning]: Points are too few! Skipping this grasp...
[Warning]: Points are too few! Skipping this grasp...
[Warning]: Points are too few! Skipping this grasp...
[Warning]: Points are too few! Skipping this grasp...
[Warning]: Points are too few! Skipping this grasp...
[Warning]: Points are too few! Skipping this grasp...
[Warning]: Points are too few! Skipping this grasp...
[Warning]: Points are too few! Skipping this grasp...
[Warning]: Points are too few! Skipping this grasp...
[Warning]: Points are too few! Skipping this grasp...
[Warning]: Points are too few! Skipping this grasp...
[Warning]: Points are too few! Skipping this grasp...
[INFO] Grasps found: 11
[[ 0.66855836 -0.74164547  0.05469649  0.19324917]
 [-0.7405065  -0.67068642 -0.04277665  0.13421912]
 [ 0.0684093  -0.01190442 -0.99758631  0.28381264]
 [ 0.          0.          0.          1.        ]]
[INFO] Average width: -0.011800877004861831
[INFO] Max width: -0.01059749498963356
[INFO] Min width: -0.014080450683832168
[INFO] Average score: 7.26167309039738e-06
[INFO] Max score: 1.9539818822522648e-05
[INFO] Min score: 2.14664055420144e-06

Input Condition

Visualization

Output of grasp seems great

Question and doubts regarding this.

1. Why the width and score data is unrealistic

    my scene size is 0.3
    
    [INFO] Average width: -0.011800877004861831
    [INFO] Max width: -0.01059749498963356
    [INFO] Min width: -0.014080450683832168
    [INFO] Average score: 7.26167309039738e-06
    [INFO] Max score: 1.9539818822522648e-05
    [INFO] Min score: 2.14664055420144e-06

2. Doubt regarding setting camera extrinsic parameters

is this method for setting the camera extrinsic is correct?
extrinsic = camera_on_sphere(center, 0.6, np.pi / 6, np.pi / 4)

• i have object 60 cm apart from camera and 30 deg elevation for camera and i dont know what this third parameter stands for i have this np.pi / 4 value by trial and error.

because i don't have manipulator ,assuming fixed camera position for now.

3. What this warning means?

[Warning]: Points are too few! Skipping this grasp...
for your reference i have [INFO] TSDF voxel count: 4320

4. What should i improve in code?

5. What should i do for grasp affordance?

[Kaivalya192]

Hi @sjauhri ,

Gentle reminder waiting for your response.

[sjauhri]

Hi @Kaivalya192

1. Yes, the scores seem odd. Can you try setting the origin of the scene i.e. the extrinsic) to be 5cms below the table? This is the default setting in the dataset.
2. Instead of setting the camera intrinsics manually, I would recommend performing camera calibration (with AR Tags for example) for accurately estimating the extrinsic matrix.
3. This warning means that there were too few rendered points found in the 3d reconstructed pointcloud. This may or may not be an issue.
4. Looks like you're almost there. Some tweaks and the grasp detection should be good to go
5. The grasp affordance code has not been released but I could recommend using some language-based segmentation technique to get the affordance based on your use-case