(self, color, depth, pose_vins)
| 114 | self.frame_poses = [] |
| 115 | |
| 116 | def callback(self, color, depth, pose_vins): |
| 117 | update_pose = False |
| 118 | bridge = CvBridge() |
| 119 | color_image = bridge.imgmsg_to_cv2(color, desired_encoding="passthrough") |
| 120 | depth_image = bridge.imgmsg_to_cv2(depth, desired_encoding="passthrough") |
| 121 | q = pose_vins.pose.pose.orientation |
| 122 | dcm = Rotation.from_quat(np.array([q.x, q.y, q.z, q.w])).as_matrix() |
| 123 | trans = pose_vins.pose.pose.position |
| 124 | trans = np.array([trans.x, trans.y, trans.z]) |
| 125 | pose = np.eye(4) |
| 126 | pose[:3, :3] = dcm |
| 127 | pose[:3, 3] = trans |
| 128 | depth_image = depth_image * 0.001 |
| 129 | if self.max_distance > 0: |
| 130 | depth_image[(depth_image > self.max_distance)] = 0 |
| 131 | color_image = color_image / 256 |
| 132 | |
| 133 | tracked_frame = RGBDFrame(self.idx, color_image, depth_image, K=self.intrinsic, offset=self.offset, |
| 134 | ref_pose=pose) |
| 135 | self.mapping_step(self.idx, tracked_frame, update_pose) |
| 136 | self.idx += 1 |
| 137 | print("idx: ", self.idx) |
| 138 | |
| 139 | def mapping_step(self, frame_id, tracked_frame, update_pose): |
| 140 | ###################### |
nothing calls this directly
no test coverage detected