Lab 3: Computer Vision For Robotics

Part 1: April Tag Detector

About implementing the april tag detector

For this part of the exercise, we heavily use OpenCV and the apriltag library since the task is to process the image and detect the apriltags.

Here is a breakdown of the code which explains what we did in order to build an apriltag detector:

First we need to initialize the node, and define a subscriber to the camera image topic, a publisher to publish the modified image with detected apriltags on it to a new topic, and an apriltag detector. We also need to define the service which was provided for us, for changing the LED colors based on the detection. 

       self.sub_image = rospy.Subscriber('/csc22938/camera_node/image/compressed',   CompressedImage, self.callback)

       self.image_pub = rospy.Publisher("/csc22938/camera_node/april_tags/compressed",

                                        CompressedImage)

       self.detector = apriltag.Detector()

       self.braodcaster = TransformBroadcaster()

       rospy.wait_for_service("/csc22938/led_emitter_node/set_pattern")

       self.changePattern = rospy.ServiceProxy("/csc22938/led_emitter_node/set_pattern", ChangePattern)

Then we do the main task In the subscriber callback function:

First, we preprocess the image using OpenCV, to prepare the image for giving it as an input to the apriltag detector:

           np_arr = np.frombuffer(data.data, np.uint8)

           img = cv2.imdecode(np_arr, 1)

           img = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)

Then we call the detector on the image. The output of this method is a list of detections, each of which indicates a detected apriltag. We can find the position of each apriltag and draw a rectangle around it and indicate its label by iterating through these detections and using detection attributes( corner, and center):

result = self.detector.detect(img)

for r in result:

               c1 = [int(x - 10) for x in r.corners[0]]

               c2 = [int(x + 10) for x in r.corners[2]]

               area = abs(c1[0]-c2[0])*abs(c1[1]-c2[1])

               if(area > max_area):

                   max_area = area

                   max_area_id = int(r.tag_id)

               cv2.rectangle(img, tuple(c1), tuple(c2), 255, 6)

               center = tuple([int(x) for x in r.center])

               img = cv2.putText(img, str(r.tag_id), center, cv2.FONT_HERSHEY_SIMPLEX, 1, 255, 2, cv2.LINE_AA)

Questions

• What does the april tag library return to you for determining its position?

When we call apriltag.detector.detect(), it will return a list of detection objects.

Each of these objects has attributes that contain information about the detected tag.

These attributes include tag_id, tag_family, corners, centers, etc. A detailed list of the attributes and their explanation is provided below (based on GitHub - duckietown/lib-dt-apriltags: Python bindings to the Apriltags library)

Figure 4: Effect of different values of kd

Part 3: Localization using Sensor Fusion

In this part of the lab, we worked heavily with the transformations to do the localization.

Transformations are used to represent the position and orientation of objects relative to each other in a robot system. These transformations allow for seamless coordination between various robot components and sensors, and enable accurate navigation and manipulation in complex environments.

In summary, in different parts of this exercise, we define and send transforms, to indicate the robot frame, world frame, april tag positions, and detections. Then we visualize the frames and the results in Rviz.

Part 3.1: Odometry Frame

We can send the odometry frame information with relation to the world frame as the parent, to visualize the robot moving around in the world.

In order to send the transrom, we first need to define a TransformStamped object, and then indicate its different attributes.  Here, odometry is the child, and world is the parent.

Translation is the component of a transform that describes the position of an object in three-dimensional space, typically expressed as a vector (x, y, z) indicating the position  in the x, y, and z directions. In this case, translation is the robot’s position which is calculated using odometry and kinematics in the deadreckoning package.

We also need to indicate another component of the transformation: rotation.

The rotation component of a transform describes how an object is rotated or oriented in three-dimensional space relative to a reference coordinate frame. In a transform, the rotation component can be represented using various methods such as Euler angles, rotation matrices, or quaternions. By specifying the rotation component of a transform, one can determine how an object is oriented in space and how it should be transformed to align with another object or coordinate frame.

We found out that the easier way to define orientation, is to use euler angles.

Euler angles are a set of three angles (denoted as roll, pitch, and yaw) that represent the rotation around each of the three axes in a fixed order.

We then convert euler angles to quaternions( a four-dimensional vector to indicate rotations) using a tf library built-in function and pass it to the constructor of the transform object.

Then we can send the transformation using a broadcaster as shown here:

#Part 3.1

       t = TransformStamped()

       t.header = odom.header

       t.header.stamp = rospy.Time.now()

       t.child_frame_id = "odometry"

       t.header.frame_id = "world"

       t.transform = Transform(

                   translation=Vector3(self.x, self.y, self.z), rotation=Quaternion(*self.q))

       self._tf_broadcaster.sendTransform(t)

Observations and Deliverables: We find out that as we circle the robot multiple times, the errors accumulate, and the odometry frame shown in rviz tends to drift from the robot’s real position in the world. The reason, as mentioned, is the errors caused by the imperfections of the real world in calculating the odometry, and accumulating all these errors over time.

Part 3.2: Creating Static Landmark Frames

We can define the static frames for the apriltags using static transform broadcasters in static_transform_broadcaster.py file in our project.

Again, the parent frame is the world, and the child frame is the apriltag. We calculate the rotation based on how the robot is oriented in x,y and z axis with respect to the world frame( using the right hand rule), and then convert these euler angles to the quaternions.

We put sleep to make sure all the transformations are sent.

#!/usr/bin/env python3

import rospy

import tf

from geometry_msgs.msg import TransformStamped

from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster

# two almost similar sources I reference for this code:

# https://docs.ros.org/en/rolling/Tutorials/Intermediate/Tf2/Writing-A-Tf2-Static-Broadcaster-Py.html

# http://wiki.ros.org/tf2/Tutorials/Writing%20a%20tf2%20static%20broadcaster%20%28Python%29

PI = 3.1415

apriltag_list = [200, 201, 94, 93, 153, 133, 58, 62, 169, 162]

apriltag_name_list = ['200', '201', '94', '93',

                     '153', '133', '58', '62', '169', '162']

x_list = [0.17, 1.65, 1.65, 0.17, 1.75, 1.253, 0.574, 0.075, 0.574, 1.253]

y_list = [0.17, 0.17, 2.84, 2.84, 1.252, 1.755, 1.259, 1.755, 1.755, 1.253]

yaw = [PI/2 + PI/4, PI/2 + 3*PI/4, -PI/4, PI/2 - PI/4, 0, PI, 0, PI, PI/2, -PI/2 ]

pitch = [1.57]

if __name__ == '__main__':

   rospy.init_node('my_static_tf2_broadcaster')

   broadcaster = StaticTransformBroadcaster()

   i = 0

   for i_april in apriltag_list:

       t = TransformStamped()

       t.header.stamp = rospy.Time.now()

       t.header.frame_id = 'world'

       t.child_frame_id = apriltag_name_list[i]

       t.transform.translation.x = float(x_list[i])

       t.transform.translation.y = float(y_list[i])

       t.transform.translation.z = float(0)

       quat = tf.transformations.quaternion_from_euler(           float(-PI/2), float(0), yaw[i])

       t.transform.rotation.x = quat[0]

       t.transform.rotation.y = quat[1]

       t.transform.rotation.z = quat[2]

       t.transform.rotation.w = quat[3]

       broadcaster.sendTransform(t)

       # self.log(f"sent info for apriltag {apriltag_name_list[i]}")

       rospy.sleep(0.2)

       i+=1

   # rospy.spin()

We also define a service for teleportation. Here is the function that can update the robot’s position. ODO and ODOResponse are srv  defined in the costum_msgs package, which are used as service descriptions for enabling this service:

def Teleport(self, msg):

       odom = Odometry()

       self.x = msg.x

       self.y = msg.y

       self.z = msg.z

       odom.header.stamp = rospy.Time.now()  # Ideally, should be encoder time

       odom.header.frame_id = self.origin_frame

       self.yaw = msg.orientation

       self.q = tr.quaternion_from_euler(0, 0, msg.orientation)

       odom.pose.pose = Pose(Point(msg.x, msg.y, msg.z), Quaternion(*self.q))

       odom.child_frame_id = self.target_frame

       odom.twist.twist = Twist(Vector3(self.tv, 0.0, 0.0), Vector3(0.0, 0.0, self.rv))

       self.pub.publish(odom)

       return ODOResponse()

We can call the service by indicating the robot’s new position(x, y and z) and orientation in the z axis(yaw) as the parameters.

$rosservice call /csc22938/deadreckoning_node/teleport 0.0 0.0 0.0 0.0

Questions

• Where did your odometry seem to drift the most? Why would that be?

In the corners when the robot makes a turn, it seems that the odometry drifts the most.

The reason is that Kinematics formulas used to update the odometry are derived based on some assumptions which might not be true in the real world Wheels are assumed to be identical which might not be the case. It is also assumed that there is no slipping and skidding.  Rigid body rotation is another assumption, that might be violated in the real world. These reasons can cause the actual and the desired location to be different. For instance, when the robot turns, it might slip, or if the robot moves fast, it might skid.

Also, as the time passed, wes saw that the odometry drifted even more. The reason is that in deadreckoning updates the robot estimated position based on previous estimates, which results in the accumulation of errors.

• Did adding the landmarks make it easier to understand where and when the odometry drifted?

Yes, adding the landmarks made it easier to understand where and when the odometry drifted. By creating static frames for the apriltags and using them as landmarks, we could compare the robot’s perceived odometry with the location of the known landmarks. This way, we can update the robot position based on these known locations in case of drift and errors, and make our localization more accurate in case of drifting. In general, the landmarks provide a reference frame that the robot can use to correct for any errors in the odometry estimate, and improve localization.

Deliverable 3

Record a video on your computer showing RViz: displaying your camera feed, odometry frame and static apriltag frames as it completes one circle. You can drive using manual control or lane following.