Needle/tools 2D projection

[kschwan]

Hi,

I noticed the challenge deadline was moved and so we're aiming to make a submission 🙂

We're probably taking a machine learning approach to needle detection and I was wondering if there is an easy way to get the needle/robot points projected to 2D (also taking occlusions into account) to act as ground truth labels for training / automatically generate training data?

I noticed the example script for projecting the needle points, but that only projects some points on a circular arc, using the needle pose, and not all the points/pixels.

I hope my question makes sense.

Best,
Kim, SDU, Denmark

[adnanmunawar]

Hi Kim, great to hear from you and looking forward to your participation :). I am copying Juan @jabarragann who created the script for his opinion.

[jabarragann]

Hi Kim,

I have another script for semi-automatically producing a segmentation mask for the needle. The output of this script is a binary image where only the pixels corresponding to the needle will be white and the rest of the image black. Is this what you are asking for?

[kschwan]

Hi Juan,

Yes! :)

And, if possible, for the tool shaft / end-effector too

[kschwan]

@jabarragann Hi Juan, were you going to share the script you mentioned for producing a segmentation mask? It would be a nice help :)

[jabarragann]

Hi Kim,

I apologize for the delay with this script. This was coded that I wasn't planning to upload so I had to clean it significantly before sending it to you.

The script generates only segmentation masks for the needle based on a thresholding algorithm. It is not perfect, but it should give you a good segmentation mask when only the needle is in the image. Probably, it won't work as well when having both the tool-shaft in the image since it has a very similar color to the needle.

from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
import cv2
import rospy
import numpy as np
import ambf_client as Client
from ambf_client import Client
from pathlib import Path
import datetime

np.set_printoptions(precision=3, suppress=True)


class ImageSaver:
    def __init__(self):
        """This class will not work unless you initialize a ROS topic

        rospy.init_node("image_listener")
        """
        self.bridge = CvBridge()
        self.left_cam_subs = rospy.Subscriber("/ambf/env/cameras/cameraL/ImageData", Image, self.left_callback)
        self.right_cam_subs = rospy.Subscriber("/ambf/env/cameras/cameraR/ImageData", Image, self.right_callback)
        self.left_frame = np.zeros((640, 480, 3))
        self.left_ts = None
        self.right_frame = np.zeros((640, 480, 3))
        self.right_ts = None

        # Wait a until subscribers and publishers are ready
        rospy.sleep(0.6)

    def get_current_frame(self, camera_selector: str) -> np.ndarray:
        if camera_selector == "left":
            return self.left_frame
        elif camera_selector == "right":
            return self.right_frame
        else:
            raise ValueError("camera selector should be either 'left' or 'right'")

    def left_callback(self, msg):
        try:
            cv2_img = self.bridge.imgmsg_to_cv2(msg, "bgr8")
            self.left_frame = cv2_img
            self.left_ts = msg.header.stamp
        except CvBridgeError as e:
            print(e)

    def right_callback(self, msg):
        try:
            cv2_img = self.bridge.imgmsg_to_cv2(msg, "bgr8")
            self.right_frame = cv2_img
            self.right_ts = msg.header.stamp
        except CvBridgeError as e:
            print(e)

    def save_frame(self, camera_selector: str, path: Path):
        if camera_selector not in ["left", "right"]:
            ValueError("camera selector error")

        img = self.left_frame if camera_selector == "left" else self.right_frame
        ts = self.left_ts if camera_selector == "left" else self.right_ts
        name = camera_selector + "_frame" + ".jpeg"
        # Save frame
        cv2.imwrite(str(path / name), img)  ## Opencv does not work with pathlib


def save_image(path, l_img, l_seg, r_img, r_seg):
    # generate timestamp
    ts = datetime.datetime.now().strftime("%Y%m%d%H%M%S")
    cv2.imwrite(str(path / f"{ts}_l_img.jpeg"), l_img)
    cv2.imwrite(str(path / f"{ts}_r_img.jpeg"), r_img)
    cv2.imwrite(str(path / f"{ts}_l_seg.jpeg"), l_seg)
    cv2.imwrite(str(path / f"{ts}_r_seg.jpeg"), r_seg)


def segment_needle(frame):
    # Convert white pixels into black pixels
    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
    ret, thresh = cv2.threshold(gray, 220, 255, cv2.THRESH_BINARY)
    frame[thresh == 255] = 0

    # Threshold the image
    frame_HSV = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
    # frame_threshold = cv2.inRange(frame_HSV, (0,84,72), (180,254,197))
    frame_threshold = cv2.inRange(frame_HSV, (0, 0, 27), (180, 84, 255))
    # Dilatation et erosion
    kernel = np.ones((15, 15), np.uint8)
    img_dilation = cv2.dilate(frame_threshold, kernel, iterations=1)
    img_erode = cv2.erode(img_dilation, kernel, iterations=1)
    # clean all noise after dilatation and erosion
    img_erode = cv2.medianBlur(img_erode, 7)
    # Back to RGB
    img_erode = cv2.cvtColor(img_erode, cv2.COLOR_GRAY2BGR)

    return img_erode


def main():
    image_subs = ImageSaver()
    w_name = "disp_final"
    cv2.namedWindow(w_name, cv2.WINDOW_NORMAL)

    print(f"Data collection util cmds")
    print(f"Press 's' to save new images")
    print(f"Press 'q' to quit")

    p = Path("/home/juan1995/research_juan/needle_dataset_/d7")
    if not p.exists():
        p.mkdir(parents=True)

    count = 0
    while True:
        frame_l = image_subs.get_current_frame("left")
        frame_r = image_subs.get_current_frame("right")
        segmented_l = segment_needle(frame_l)
        segmented_r = segment_needle(frame_r)

        final = np.hstack((frame_l, segmented_l))

        cv2.imshow(w_name, final)
        k = cv2.waitKey(3) & 0xFF

        if k == ord("q"):
            break
        elif k == ord("s"):
            print(f"image {count} saved ...")
            save_image(p, frame_l, segmented_l, frame_r, segmented_r)
            count += 1

    cv2.destroyAllWindows()


if __name__ == "__main__":
    rospy.init_node("dataset_collection")
    c = Client("juanclient")
    c.connect()

    main()

[kschwan]

Thanks a lot! I'll try this. Alternatively we can load 3D models and project those onto the simulator's output image. I'll write here how it goes :)

[kschwan]

So, we decided we'd like to be able to generate nice ground truth data easily with needle-in-hand. For this I set up a "twin" 3D scene via open3d, where I load the .obj 3D models and place them according to the current state output by AMBF. Now, this is working nicely for the needle, but I would like to also place and render the tools/graspers. How do I output the state (6 DOF pose) of each link easily? Currently I see only baselink and toolyawlink. Alternatively, can I get the grasper closing angle?
@adnanmunawar

[kschwan]

Though now that leads to a new question: How do I get the toolgripper1link (and others) wrt. the world frame? The RigidBodyState represents its state wrt. its parent frame, but I'm not sure what that is. RigidBodyState.get_parent_name() gets me AttributeError: 'RigidBodyState' object has no attribute 'parent_name'. Can someone assist?

[kschwan]

Ah, it turns out toolgripper1link is already given in the World frame! I just had a bug in my script so that its state was not correctly mirrored in the twin scene.

[adnanmunawar]

Hi Kim, yes the passive: false flag shall publish the ROS msg for that specific object.

Just so that you are aware, it is fine to modify the assets/scripts in this repo for training but for the actual challenge, no modifications are allowed, (i.e. the assets/scripts will be restored to their original version).

[kschwan]

Thanks Adnan, I'm aware 🙂

I assume the cameras will be set to publish images during execution/evaluation though? Currently that's off by default.

[adnanmunawar]

Thanks for mentioning that. Yes, the images would be published by default and I will set them as such soon :)