Update 'Home'

Home.md

@@ -2,8 +2,10 @@ Welcome to the Wiki.
 ```python
 #!/usr/bin/env python
 
+#!/usr/bin/env python
+
 import rospy
-from sensor_msgs.msg import Image, PointCloud2, PointField
+from sensor_msgs.msg import Image, PointCloud2, PointField, CameraInfo
 from cv_bridge import CvBridge
 import numpy as np
 import open3d as o3d
@@ -17,6 +19,7 @@ class ImageAndPointCloudProcessor:
         self.bridge = CvBridge()
         self.image_sub = rospy.Subscriber('/camera/color/image_raw', Image, self.image_callback)
         self.pointcloud_sub = rospy.Subscriber('/camera/depth/color/points', PointCloud2, self.pointcloud_callback)
+        self.camera_info_sub = rospy.Subscriber('/camera/color/camera_info', CameraInfo, self.camera_info_callback)
         self.pointcloud_pub = rospy.Publisher('/custom/pointcloud', PointCloud2, queue_size=10)
         self.box = [0, 0, 640, 480]  # default box
         self.camera_intrinsics = None
@@ -35,12 +38,12 @@ class ImageAndPointCloudProcessor:
         # Process cropped image here
         # ...
 
+    def camera_info_callback(self, msg):
+        self.camera_intrinsics = np.array(msg.K).reshape((3, 3))
+
     def pointcloud_callback(self, msg):
         if self.camera_intrinsics is None:
-            self.camera_intrinsics = np.array([msg.height, msg.width, 1])
-            for p in msg.fields:
-                if p.name == 'x':
-                    self.camera_intrinsics[2] = p.count
+            return
 
         try:
             pointcloud = o3d.geometry.PointCloud()
@@ -71,7 +74,7 @@ class ImageAndPointCloudProcessor:
         # Publish red pointcloud for the plane
         header = Header()
         header.stamp = rospy.Time.now()
-        header.frame_id = "camera_depth_optical_frame"
+        header.frame_id = msg.header.frame_id
         fields = [PointField('x', 0, PointField.FLOAT32, 1),
                   PointField('y', 4, PointField.FLOAT32, 1),
                   PointField('z', 8, PointField.FLOAT32, 1),
@@ -80,6 +83,8 @@ class ImageAndPointCloudProcessor:
         # Create a list of points representing the plane
         plane_points = []
         for p in inlier_cloud.points:
+            u = (p[0] - self.camera_intrinsics[0, 2]) / self.camera_intrinsics[0, 0]
+            v = (p[1] - self.camera_intrinsics[1, 2]) / self.camera_intrinsics[1, 1]
             plane_points.append([p[0], p[1], p[2], np.float32(0xff0000)])
         
         # Create and publish the pointcloud message