def test_align_poses_along_straight_line_gauge(self): """Test if Align Pose3Pairs method can account for gauge ambiguity. Scenario: 3 object poses with gauge ambiguity (2x scale) world frame has poses rotated about z-axis (90 degree yaw) world and egovehicle frame translated by 11 meters w.r.t. each other """ Rz90 = Rot3.Rz(np.deg2rad(90)) # Create source poses (three objects o1, o2, o3 living in the egovehicle "e" frame) # Suppose they are 3d cuboids detected by an onboard sensor in the egovehicle frame eTo0 = Pose3(Rot3(), np.array([1, 0, 0])) eTo1 = Pose3(Rot3(), np.array([2, 0, 0])) eTo2 = Pose3(Rot3(), np.array([4, 0, 0])) eToi_list = [eTo0, eTo1, eTo2] # Create destination poses # (same three objects, but instead living in the world/city "w" frame) wTo0 = Pose3(Rz90, np.array([0, 12, 0])) wTo1 = Pose3(Rz90, np.array([0, 14, 0])) wTo2 = Pose3(Rz90, np.array([0, 18, 0])) wToi_list = [wTo0, wTo1, wTo2] we_pairs = Pose3Pairs(list(zip(wToi_list, eToi_list))) # Recover the transformation wSe (i.e. world_S_egovehicle) wSe = Similarity3.Align(we_pairs) for wToi, eToi in zip(wToi_list, eToi_list): self.gtsamAssertEquals(wToi, wSe.transformFrom(eToi))
def run(self): reached = 0 r = rospy.Rate(10) while not rospy.is_shutdown(): goal = self.goals[self.goalindex] self.sTg = Pose3(Rot3.Rz(goal[3]), Point3(goal[0], goal[1], goal[2])) R = quaternion_matrix([ self.sTcurr_ros.pose.orientation.x, self.sTcurr_ros.pose.orientation.y, self.sTcurr_ros.pose.orientation.z, self.sTcurr_ros.pose.orientation.w ])[0:3, 0:3] self.sTcurr = Pose3( Rot3(R), Point3(self.sTcurr_ros.pose.position.x, self.sTcurr_ros.pose.position.y, self.sTcurr_ros.pose.position.z)) currTg = self.sTcurr.between(self.sTg) u_pitch = self.kp_x * currTg.x() + self.kd_x * ( currTg.x() - self.lastT.x()) + min( max(-0.2, self.ki_x * self.ix), 0.2) u_roll = self.kp_y * currTg.y() + self.kd_y * (currTg.y() - self.lastT.y()) currTg_yaw = currTg.rotation().rpy()[2] u_yaw = self.kp_yaw * currTg_yaw + self.kd_yaw * (currTg_yaw - self.lastyaw) u_z = self.kp_x * currTg.z() + self.kd_z * (currTg.z() - self.lastT.z()) self.lastT = currTg self.lastyaw = currTg_yaw self.ix += currTg.x() self.iy += currTg.y() self.iz += currTg.z() self.iyaw += currTg_yaw self.move_cmd.linear.x = min(max(-1, u_pitch), 1) self.move_cmd.linear.y = min(max(-1, u_roll), 1) self.move_cmd.linear.z = min(max(-1, u_z), 1) self.move_cmd.angular.z = min(max(-1, u_yaw), 1) self.velPub.publish(self.move_cmd) reached = self.check(currTg) if (reached == 1): reached = 0 self.goalindex += 1 r.sleep()
def test_align_poses_scaled_squares(self): """Test if Align Pose3Pairs method can account for gauge ambiguity. Make sure a big and small square can be aligned. The u's represent a big square (10x10), and v's represents a small square (4x4). Scenario: 4 object poses with gauge ambiguity (2.5x scale) """ # 0, 90, 180, and 270 degrees yaw R0 = Rot3.Rz(np.deg2rad(0)) R90 = Rot3.Rz(np.deg2rad(90)) R180 = Rot3.Rz(np.deg2rad(180)) R270 = Rot3.Rz(np.deg2rad(270)) aTi0 = Pose3(R0, np.array([2, 3, 0])) aTi1 = Pose3(R90, np.array([12, 3, 0])) aTi2 = Pose3(R180, np.array([12, 13, 0])) aTi3 = Pose3(R270, np.array([2, 13, 0])) aTi_list = [aTi0, aTi1, aTi2, aTi3] bTi0 = Pose3(R0, np.array([4, 3, 0])) bTi1 = Pose3(R90, np.array([8, 3, 0])) bTi2 = Pose3(R180, np.array([8, 7, 0])) bTi3 = Pose3(R270, np.array([4, 7, 0])) bTi_list = [bTi0, bTi1, bTi2, bTi3] ab_pairs = Pose3Pairs(list(zip(aTi_list, bTi_list))) # Recover the transformation wSe (i.e. world_S_egovehicle) aSb = Similarity3.Align(ab_pairs) for aTi, bTi in zip(aTi_list, bTi_list): self.gtsamAssertEquals(aTi, aSb.transformFrom(bTi))