def load_relative(self, img_set, img_num): # load and format the images, depending on number of channels img_type = 'RGB' if self.input_channels == 6 else 'L' img_1 = load_image(self.images[img_set][img_num], type=img_type, resolution=self.input_resolution) img_2 = load_image(self.images[img_set][img_num + 1], type=img_type, resolution=self.input_resolution) if self.input_channels == 2: # 2 channels # 0 R = img_1 grayscale # 1 G = img_2 grayscale img = Image.merge("LA", (img_1, img_2)) elif self.input_channels == 3: # 3 channels # 0 R = img_1 grayscale # 1 G = img_2 grayscale # 2 B = zero img = Image.merge("RGB", (img_1, img_2, Image.new('L', img_1.size, (0)))) elif self.input_channels == 6: # 6 channels # 0 = img_1 R # 1 = img_1 G # 2 = img_1 B # 3 = img_2 R # 4 = img_2 G # 5 = img_2 B img = np.concatenate((np.asarray(img_1), np.asarray(img_2)), axis=2) else: raise Exception('invalid in_channels {:d}'.format( self.input_channels)) # apply image transform if self.transform is not None: img = self.transform(img) else: img = torch.from_numpy(img.transpose((2, 0, 1))) # calc pose deltas: # - global translation (if not predicting orientations) # - relative translation/rotation (if predicting orientations) pose_1 = self.poses[img_set][img_num] pose_2 = self.poses[img_set][img_num + 1] translation = [pose_2[n] - pose_1[n] for n in range(3)] # global translation if self.predict_orientations: quat_1 = Quaternion(pose_1[6], pose_1[3], pose_1[4], pose_1[5]) quat_2 = Quaternion(pose_2[6], pose_2[3], pose_2[4], pose_2[5]) quat_delta = quat_1.inverse * quat_2 # relative rotation translation = quat_1.rotate(translation) # relative translation pose_delta = translation + [ quat_delta.x, quat_delta.y, quat_delta.z, quat_delta.w ] if quat_delta == False: print('warning: zero quaternion, relative rotation ({:d})'. format(idx)) else: pose_delta = translation # scale/normalize output if self.scale_outputs: pose_delta = scale(pose_delta, self.output_range) if self.normalize_outputs: pose_delta = normalize_std(pose_delta, self.output_mean, self.output_std) #print('idx {:04d} {:s}'.format(idx, str(pose_delta))) return img, torch.Tensor(pose_delta)
class BoatData: qx_180 = Quaternion(axis=[1, 0, 0], angle=np.pi) qz_90p = Quaternion(axis=[0, 0, 1], angle=np.pi / 2) q_rot = qz_90p * qx_180 def __init__(self): self.position = [0.0, 0.0, 0.0] self.velocity = [0.0, 0.0, 0.0] self.acceleration = [0.0, 0.0, 0.0] self.velocity_rot = [0.0, 0.0, 0.0] self.acceleration_rot = [0.0, 0.0, 0.0] self.bodyAxis= [0.0, 0.0, 0.0] #self.RotMatrix = RPYToRot(0.0, 0.0, 0.0) self.quat_orientation = Quaternion(w=1.0,x=0.0,y=0.0,z=0.0) self.euler = [0.0, 0.0, 0.0] self.angular_velo = [0.0, 0.0, 0.0] self.angular_velo_rot = [0.0, 0.0, 0.0] self.accelMedianX = np.array([0.0, 0.0, 0.0, 0.0, 0.0]) self.accelMedianY = np.array([0.0, 0.0, 0.0, 0.0, 0.0]) self.accelMedianZ = np.array([0.0, 0.0, 0.0, 0.0, 0.0]) self.veloMedianX = np.array([0.0, 0.0, 0.0, 0.0, 0.0]) self.veloMedianY = np.array([0.0, 0.0, 0.0, 0.0, 0.0]) self.veloMedianZ = np.array([0.0, 0.0, 0.0, 0.0, 0.0]) self.filteredAccelValue=[0.0, 0.0, 0.0] self.filteredVeloValue = [0.0, 0.0, 0.0] self.camera_position=[0.0, 0.0, 0.0] self.camera_velocity= [0.0, 0.0, 0.0] self.camera_orientation = Quaternion(w=1.0,x=0.0,y=0.0,z=0.0) self.euler_camera = [0.0, 0.0, 0.0] #Publisher self.position_publish = rospy.Publisher('/mavros/local_position/pose_NED2', PoseStamped, queue_size=1) self.velocity_publish = rospy.Publisher('/mavros/local_position/velocity_localNED2', TwistStamped, queue_size=1) self.accel_publish = rospy.Publisher('/mavros/imu/data_NED2', Imu, queue_size=1) self.angular_velocity_publish = rospy.Publisher('/mavros/local_position/velocity_bodyNED2', TwistStamped, queue_size=1) #self.current_axis_pub = rospy.Publisher('/hippocampus/current_axis', HippocampusControl, # queue_size=1) # Subscriber self.axis_publish = rospy.Publisher('/hippocampus/currentaxis', HippocampusCurrentaxis, queue_size=1) rospy.Subscriber("/mavros/local_position/pose", PoseStamped, self.poseCallback) rospy.Subscriber("/mavros/local_position/velocity_body", TwistStamped, self.angularVeloCallback) rospy.Subscriber("/mavros/local_position/velocity_local", TwistStamped, self.veloCallback) rospy.Subscriber("/mavros/imu/data", Imu, self.imuCallback) rospy.Subscriber("/mavros/local_position/pose_NED", PoseStamped, self.cameraPoseCallback) rospy.Subscriber("/estimated_twist", TwistStamped, self.cameraVeloCallback) def poseCallback(self, msg): #print("POSE CALLBACK", msg.pose.position.x) #print("POSE CALLBACK2", self.position) tmpQuat = Quaternion(w=msg.pose.orientation.w, x=msg.pose.orientation.x, y=msg.pose.orientation.y, z=msg.pose.orientation.z) orientation_quat_ned = self.q_rot * (tmpQuat * self.qx_180) euler_orientation = euler_from_quaternion(np.array([orientation_quat_ned.x,orientation_quat_ned.y,orientation_quat_ned.z,orientation_quat_ned.w])) self.quat_orientation = orientation_quat_ned #Orientation in NED as Quaternion self.euler = euler_orientation #Orientation in NED as Euler #Generate Random Errors if desired randx = random.uniform(-1.,1.) *0.1 *0 randy = random.uniform(-1.,1.) *0.1 *0 randz = random.uniform(-1.,1.) *0.1 *0 randdistance=np.linalg.norm(np.array([randx,randy,randz])) #print("RandomErrors :",randx,randy,randz,randdistance) self.position = self.q_rot.rotate(np.array([msg.pose.position.x + randx, msg.pose.position.y + randy, msg.pose.position.z + randz])) #Publish Transformed Position and Orientation to ROS NED = PoseStamped() NED.header = msg.header #TIMESTAMP? NED.header.frame_id = 'global_tank' NED.pose.position.x = self.position[0] NED.pose.position.y = self.position[1] NED.pose.position.z = self.position[2] current_axis = self.getBodyAxisQuat() NED.pose.orientation.w = self.quat_orientation.w NED.pose.orientation.x = self.quat_orientation.x NED.pose.orientation.y = self.quat_orientation.y NED.pose.orientation.z = self.quat_orientation.z currAxis = HippocampusCurrentaxis() currAxis.frame_stamp = rospy.Time.now() currAxis.axis_x = current_axis[0] currAxis.axis_y = current_axis[1] currAxis.axis_z = current_axis[2] self.axis_publish.publish(currAxis) self.position_publish.publish(NED) #current_axis = self.normalize(self.quat_orientation.rotate(np.array([1, 0, 0]))) #hcc = HippocampusControl() #hcc.frame_stamp = rospy.Time.now() #hcc.thrust = 0.0 #hcc.roll_effort = current_axis[0] #hcc.pitch_effort =current_axis[1] #hcc.yaw_effort = current_axis[2] #self.current_axis_pub.publish(hcc) def veloCallback(self,velo): self.velocity[0] = velo.twist.linear.x self.velocity[1] = velo.twist.linear.y self.velocity[2] = velo.twist.linear.z randx = random.uniform(-1.,1.) * 0.1*0 randy = random.uniform(-1.,1.) * 0.1*0 randz = random.uniform(-1.,1.) * 0.1*0 randdistance = np.linalg.norm(np.array([randx, randy, randz])) #print("RandomErrors Velo:", randx, randy, randz, randdistance) self.velocity_rot = self.q_rot.rotate(np.array([velo.twist.linear.x+randx, velo.twist.linear.y+randy, velo.twist.linear.z+randz])) #print("Velocity", self.velocity) self.veloMedianX = np.roll(self.veloMedianX, 1) self.veloMedianY = np.roll(self.veloMedianY, 1) self.veloMedianZ = np.roll(self.veloMedianZ, 1) self.veloMedianX[0] = self.velocity_rot[0] self.veloMedianY[1] = self.velocity_rot[1] self.veloMedianZ[2] = self.velocity_rot[2] self.filteredVeloValue[0] = (np.sum(self.veloMedianX) / len(self.veloMedianX)) self.filteredVeloValue[1] = (np.sum(self.veloMedianY) / len(self.veloMedianY)) self.filteredVeloValue[2] = (np.sum(self.veloMedianZ) / len(self.veloMedianZ)) veloNED =TwistStamped() veloNED.header = velo.header #TIMESTAMP? veloNED.header.frame_id = 'global_tank' veloNED.twist.linear.x = self.filteredVeloValue[0] veloNED.twist.linear.y = self.filteredVeloValue[1] veloNED.twist.linear.z = self.filteredVeloValue[2] self.velocity_publish.publish(veloNED) def imuCallback(self, imu): self.acceleration[0] = imu.linear_acceleration.x self.acceleration[1] = imu.linear_acceleration.y self.acceleration[2] = imu.linear_acceleration.z #print("Accel", self.acceleration) accel_temp = np.array([-imu.linear_acceleration.x, imu.linear_acceleration.y, imu.linear_acceleration.z]) #print("AccelRaw",accel_temp) accel_temp_trans = self.getQuaternionOrientation().rotate(accel_temp) accel_temp_trans[0]= -accel_temp_trans[0] accel_temp_trans[1]= -accel_temp_trans[1] accel_temp_trans[2] = -(accel_temp_trans[2] - 9.81) self.acceleration_rot = accel_temp_trans self.accelMedianX = np.roll(self.accelMedianX, 1) self.accelMedianY = np.roll(self.accelMedianY, 1) self.accelMedianZ = np.roll(self.accelMedianZ, 1) self.accelMedianX[0] = accel_temp_trans[0] self.accelMedianY[1] = accel_temp_trans[1] self.accelMedianZ[2] = accel_temp_trans[2] self.filteredAccelValue[0]= (np.sum(self.accelMedianX) / len(self.accelMedianX)) self.filteredAccelValue[1] = (np.sum(self.accelMedianY) / len(self.accelMedianY)) self.filteredAccelValue[2] = (np.sum(self.accelMedianZ) / len(self.accelMedianZ)) # print(imu.linear_acceleration.x) #print("X",self.accelMedianX) #print("X Filtered", self.filteredAccelValue[0]) accelNED=Imu() accelNED.header=imu.header accelNED.linear_acceleration.x=self.filteredAccelValue[0] accelNED.linear_acceleration.y = self.filteredAccelValue[1] accelNED.linear_acceleration.z = self.filteredAccelValue[2] self.accel_publish.publish(accelNED) def angularVeloCallback(self, ang_velo): self.angular_velo[0] = ang_velo.twist.angular.x self.angular_velo[1] = ang_velo.twist.angular.y self.angular_velo[2] = ang_velo.twist.angular.z tmp_angular_velo_rot = np.array([ang_velo.twist.angular.x, ang_velo.twist.angular.y, ang_velo.twist.angular.z]) self.angular_velo_rot = self.qx_180.rotate(tmp_angular_velo_rot) ang_veloNED = TwistStamped() ang_veloNED.header = ang_velo.header # TIMESTAMP? ang_veloNED.header.frame_id = 'global_tank' ang_veloNED.twist.angular.x= self.angular_velo_rot[0] ang_veloNED.twist.angular.y = self.angular_velo_rot[1] ang_veloNED.twist.angular.z = self.angular_velo_rot[2] self.angular_velocity_publish.publish(ang_veloNED) def cameraPoseCallback(self, camera_pose): self.camera_position[0] = camera_pose.pose.position.x self.camera_position[1] = camera_pose.pose.position.y self.camera_position[2] = camera_pose.pose.position.z self.camera_orientation = Quaternion(w=camera_pose.pose.orientation.w, x=camera_pose.pose.orientation.x, y=camera_pose.pose.orientation.y, z=camera_pose.pose.orientation.z) self.euler_camera = euler_from_quaternion(np.array([camera_pose.pose.orientation.x,camera_pose.pose.orientation.y, camera_pose.pose.orientation.z,camera_pose.pose.orientation.w])) def cameraVeloCallback(self, camera_velocity): self.camera_velocity[0]= camera_velocity.twist.linear.x self.camera_velocity[1]= camera_velocity.twist.linear.y self.camera_velocity[2] = camera_velocity.twist.linear.z def normalize(self, v): norm = np.linalg.norm(v) if norm == 0: return v return v / norm def getFilteredVelo(self): return self.filteredVeloValue def getFilteredAccel(self): return self.filteredAccelValue def getPosition(self): return self.position def getVelocity(self): return self.velocity def getAcceleration(self): return self.acceleration def getVelocityRotated(self): return self.velocity_rot def getAccelerationRotated(self): return self.acceleration_rot def getAngularVelocityRotated(self): return self.angular_velo_rot def getQuaternionOrientation(self): return self.quat_orientation def getQuaternionOrientationCamera(self): return self.camera_orientation def getEulerOrientation(self): return self.euler def getEulerOrientationCamera(self): return self.euler_camera def getBodyAxisQuat(self): return self.normalize(self.quat_orientation.rotate(np.array([1, 0, 0]))) def getBodyAxisQuatCamera(self): return self.normalize(self.camera_orientation.rotate(np.array([1, 0, 0]))) def getPositionCamera(self): return self.camera_position def getVelocityCamera(self): return self.camera_velocity
def sampled_kvectors_spherical_coordinates(self, NA_in, NA_out, NumSample, kd): #sample multiple planewaves at different angle to do simulation as a focused beam # return a list of planewave direction vectors Kd # and the corresponding scale factor if using uniform sampling # NA: numberical aperture of the lens which planewaves are focused from # NumSample: number of samples(planewaves) # kd: center planewave of the focused beam #allocatgge space for the field and initialize it to zero start3 = time.time() CenterKd = self.k #defualt planewave coming in perpendicular to the surface kd = kd / np.linalg.norm(kd) #normalize the new planewave r = np.sqrt( CenterKd[0]**2 + CenterKd[1]**2 + CenterKd[2]**2 ) #radiance of the hemisphere where the k vectors are sampled from if ( kd[0] == CenterKd[0] and kd[1] == CenterKd[1] and kd[2] == CenterKd[2] ): #if new planewave is at the same direction as the default plane wave rotateAxis = CenterKd #set rotation axis as defualt k vector RoAngle = 0 #set rotation axis as 0 degrees else: #if new plane wave is at different direction as the defualt planewave, rotation is needed rotateAxis = np.cross( CenterKd, kd ) #find a axis which is perpendicular to both vectors to be rotation axis RoAngle = math.asin(kd[2] / r) #calculate the rotation angle beamRotate = Quaternion( axis=rotateAxis, angle=RoAngle) #create a quaternion for rotation Kd = np.zeros((3, NumSample)) #initialize the planewave list # scaleFactor = np.zeros(NumSample) #initialize a list of scalefactors which are used to scale down the amplitude of planewaves later on along latitude domain #convert the axis from Cartesian to Spherical pha = math.acos(CenterKd[2] / r) #calculate polar angle pha from Z coordinate phaM = math.asin(NA_out / np.real( self.n)) #calculate sample range of pha from numerical aperture inZ = np.cos(pha) #lower boundary of sampling along Z axis outZ = np.cos(phaM) #upper boundary of sampling along Z axis rangeZ = np.abs(inZ) - np.abs(outZ) #sampling range along Z axis # phaStep = phaM / NumSample #set longitudinal sample resolution as maximal pha divided by number of samples # thetaStep = thetaM / NumSample #set latitudinal sample resolution as maximal theta divided by number of samples ###following is uniform sampling # for i in range(NumSample): #sample along longitudinal (pha) domain # for j in range(NumSample): #sample along latitudinal (theta) domain # KdR = r #sample hemisphere radiance will be all the same as r # KdTheta = theta + thetaStep * j #sample theta at each step in the sample range # KdPha = pha + phaStep * i #sample theta at each step in the sample range # Kd[0,j,i] = KdR * np.cos(KdTheta) * np.sin(KdPha) #convert coordinates from spherical to Cartesian # Kd[1,j,i] = KdR * np.sin(KdTheta) * np.sin(KdPha) # Kd[2,j,i] = KdR * np.cos(KdPha) # Kd[:,j,i] = beamRotate.rotate(Kd[:,j,i]) #rotate k vectors by the quaternion generated # scaleFactor[j,i] = np.sin(KdPha) #calculate the scalefactors by the current polar angle pha # ###here comes Monte Carlo Sampling for i in range(NumSample): KdR = r #the r coordinate of the vector under spherical system KdTheta = random.random( ) * 2 * np.pi #get a random value for theta coordinate under spherical system KdZ = random.random( ) * rangeZ + inZ #get a random value for Z coordinate under cartesian system KdPha = math.acos(KdZ) #convert it back to spherical system Kd[0, i] = KdR * np.cos(KdTheta) * np.sin( KdPha) #convert coordinates from spherical to Cartesian Kd[1, i] = KdR * np.sin(KdTheta) * np.sin( KdPha ) #the reason why we sample Z at cartesian is that we want the vectors to distribute randomly on that direction Kd[2, i] = KdR * np.cos( KdPha ) #if we sample it on phi domain, they will be denser towards center Kd[:, i] = beamRotate.rotate( Kd[:, i]) #rotate k vectors by the quaternion generated # Kd = np.reshape(Kd, ((3, NumSample ** 2))) # scaleFactor = np.reshape(scaleFactor, ((NumSample ** 2))) #reshape list of k vectors and scalefactors to an one dimentional list end3 = time.time() print("sample planewaves: " + str(end3 - start3) + " s\n") return Kd
class Pose: """SE(3) rigid transform class that allows compounding of 6-DOF poses and provides common transformations that are commonly seen in geometric problems. """ def __init__(self, wxyz=np.float64([1, 0, 0, 0]), tvec=np.float64([0, 0, 0])): """Initialize a Pose with Quaternion and 3D Position Parameters ---------- wxyz: np.float64 or Quaternion or torch.FloatTensor, (default: np.float64([1,0,0,0])) Quaternion/Rotation (wxyz) tvec: np.float64 or torch.FloatTensor, (default: np.float64([0,0,0])) Translation (xyz) """ if isinstance(wxyz, torch.FloatTensor): wxyz = wxyz.numpy() if isinstance(tvec, torch.FloatTensor): tvec = tvec.numpy() assert isinstance(wxyz, (np.ndarray, Quaternion)) assert isinstance(tvec, np.ndarray) self.quat = Quaternion(wxyz) self.tvec = tvec def __repr__(self): formatter = {'float_kind': lambda x: '%.2f' % x} tvec_str = np.array2string(self.tvec, formatter=formatter) return 'wxyz: {}, tvec: ({})'.format(self.quat, tvec_str) def copy(self): """Return a copy of this pose object. Returns ---------- result: Pose Copied pose object. """ return self.__class__(Quaternion(self.quat), self.tvec.copy()) def __mul__(self, other): """Left-multiply Pose with another Pose or 3D-Points. Parameters ---------- other: Pose or np.ndarray 1. Pose: Identical to oplus operation. (i.e. self_pose * other_pose) 2. ndarray: transform [N x 3] point set (i.e. X' = self_pose * X) Returns ---------- result: Pose or np.ndarray Transformed pose or point cloud """ if isinstance(other, Pose): assert isinstance(other, self.__class__) t = self.quat.rotate(other.tvec) + self.tvec q = self.quat * other.quat return self.__class__(q, t) elif isinstance(other, BoundingBox3D): return BoundingBox3D(self * other.pose, other.sizes) else: assert other.shape[-1] == 3, 'Point cloud is not 3-dimensional' X = np.hstack([other, np.ones((len(other), 1))]).T return (np.dot(self.matrix, X).T)[:, :3] def __rmul__(self, other): raise NotImplementedError('Right multiply not implemented yet!') def inverse(self): """Returns a new Pose that corresponds to the inverse of this one. Returns ---------- result: Pose Inverted pose """ qinv = self.quat.inverse return self.__class__(qinv, qinv.rotate(-self.tvec)) @property def matrix(self): """Returns a 4x4 homogeneous matrix of the form [R t; 0 1] Returns ---------- result: np.ndarray 4x4 homogeneous matrix """ result = self.quat.transformation_matrix result[:3, 3] = self.tvec return result @property def rotation_matrix(self): """Returns the 3x3 rotation matrix (R) Returns ---------- result: np.ndarray 3x3 rotation matrix """ result = self.quat.transformation_matrix return result[:3, :3] @property def rotation(self): """Return the rotation component of the pose as a Quaternion object. Returns ---------- self.quat: Quaternion Rotation component of the Pose object. """ return self.quat @property def translation(self): """Return the translation component of the pose as a np.ndarray. Returns ---------- self.tvec: np.ndarray Translation component of the Pose object. """ return self.tvec @classmethod def from_matrix(cls, transformation_matrix): """Initialize pose from 4x4 transformation matrix Parameters ---------- transformation_matrix: np.ndarray 4x4 containing rotation/translation Returns ------- Pose """ return cls(wxyz=Quaternion(matrix=transformation_matrix[:3, :3]), tvec=np.float64(transformation_matrix[:3, 3])) @classmethod def from_pose_proto(cls, pose_proto): """Initialize pose from 4x4 transformation matrix Parameters ---------- pose_proto: Pose_pb2 Pose as defined in proto/geometry.proto Returns ------- Pose """ rotation = np.float64([ pose_proto.rotation.qw, pose_proto.rotation.qx, pose_proto.rotation.qy, pose_proto.rotation.qz, ]) translation = np.float64([ pose_proto.translation.x, pose_proto.translation.y, pose_proto.translation.z, ]) return cls(wxyz=rotation, tvec=translation) def to_proto(self): """Convert Pose into pb object. Returns ------- pose_0S: Pose_pb2 Pose as defined in proto/geometry.proto """ pose_0S = geometry_pb2.Pose() pose_0S.rotation.qw = self.quat.elements[0] pose_0S.rotation.qx = self.quat.elements[1] pose_0S.rotation.qy = self.quat.elements[2] pose_0S.rotation.qz = self.quat.elements[3] pose_0S.translation.x = self.tvec[0] pose_0S.translation.y = self.tvec[1] pose_0S.translation.z = self.tvec[2] return pose_0S def __eq__(self, other): return self.quat == other.quat and (self.tvec == other.tvec).all()
def sampled_kvectors_spherical_coordinates(self, NA, NumSample, kd): #sample multiple planewaves at different angle to do simulation as a focused beam # return a list of planewave direction vectors Kd # and the corresponding scale factor # NA: numberical aperture of the lens which planewaves are focused from # NumSample: number of samples(planewaves) along longitudinal and latitudinal axis # kd: center planewave of the focused beam #allocatgge space for the field and initialize it to zero start3 = time.time() CenterKd = [ 0, 0, -1 ] #defualt planewave coming in perpendicular to the surface kd = kd / np.linalg.norm(kd) #normalize the new planewave r = np.sqrt( CenterKd[0]**2 + CenterKd[1]**2 + CenterKd[2]**2 ) #radiance of the hemisphere where the k vectors are sampled from if ( kd[0] == CenterKd[0] and kd[1] == CenterKd[1] and kd[2] == CenterKd[2] ): #if new planewave is at the same direction as the default plane wave rotateAxis = CenterKd #set rotation axis as defualt k vector RoAngle = 0 #set rotation axis as 0 degrees else: #if new plane wave is at different direction as the defualt planewave, rotation is needed rotateAxis = np.cross( CenterKd, kd ) #find a axis which is perpendicular to both vectors to be rotation axis RoAngle = math.asin(kd[2] / r) #calculate the rotation angle beamRotate = Quaternion( axis=rotateAxis, angle=RoAngle) #create a quaternion for rotation Kd = np.zeros( (3, NumSample, NumSample)) #initialize the planewave list scaleFactor = np.zeros( (NumSample, NumSample) ) #initialize a list of scalefactors which are used to scale down the amplitude of planewaves later on along latitude domain #convert the axis from Cartesian to Spherical if (CenterKd[0] == 0 or CenterKd[1] == 0): #if the defualt planewave is at the direction of Z axis theta = 0 #set azimuthal angle theta as 0 else: theta = math.atan( CenterKd[1] / CenterKd[0]) #if not calculate theta from X and Y coordinates pha = math.acos(CenterKd[2] / r) #calculate polar angle pha from Z coordinate phaM = math.asin( NA / np.real(n)) #calculate sample range of pha from numerical aperture thetaM = 2 * np.pi #set sample range of theta as 2pi phaStep = phaM / NumSample #set longitudinal sample resolution as maximal pha divided by number of samples thetaStep = thetaM / NumSample #set latitudinal sample resolution as maximal theta divided by number of samples for i in range(NumSample): #sample along longitudinal (pha) domain for j in range( NumSample): #sample along latitudinal (theta) domain KdR = r #sample hemisphere radiance will be all the same as r KdTheta = theta + thetaStep * j #sample theta at each step in the sample range KdPha = pha + phaStep * i #sample theta at each step in the sample range Kd[0, j, i] = KdR * np.cos(KdTheta) * np.sin( KdPha) #convert coordinates from spherical to Cartesian Kd[1, j, i] = KdR * np.sin(KdTheta) * np.sin(KdPha) Kd[2, j, i] = KdR * np.cos(KdPha) Kd[:, j, i] = beamRotate.rotate( Kd[:, j, i]) #rotate k vectors by the quaternion generated scaleFactor[j, i] = np.sin( KdPha ) #calculate the scalefactors by the current polar angle pha Kd = np.reshape(Kd, ((3, NumSample**2))) scaleFactor = np.reshape( scaleFactor, ((NumSample**2)) ) #reshape list of k vectors and scalefactors to an one dimentional list end3 = time.time() print("sample planewaves: " + str(end3 - start3) + " s\n") return Kd, scaleFactor
def set_force_signal(self, target_pose): force_signal = self.desired_force_torque.copy() target_q = Quaternion(np.roll(target_pose[3:], -1)) force_signal[:3] = target_q.rotate(force_signal[:3]) self.force_control_model.set_goals(force=force_signal)
import numpy as np from pyquaternion import Quaternion import xml.etree.ElementTree as ET #root = ET.parse("/root/Desktop/studying/flyBattle/code/data.xml").getroot() #rocket = root.find('Rocket') #d = rocket.find('x') #d = float(d.text) #print((d)) #a = d + 60 #print(a) #itemlist = xmldoc.getElementsByTagName('item') #print(len(itemlist)) #print(itemlist[0].attributes['name'].value) x = 5 y = 5 z = 5 v = np.array([1, 1, 1]) q1 = Quaternion(axis=[0, 0, 1], angle=np.arctan2(y, x)) qv = q1.rotate([0, 1, 0]) q2 = Quaternion(axis=qv, angle=np.arctan2(z, np.sqrt(x**2 + y**2))) q3 = q2.conjugate * q1 print(q3.rotate(v)) #print(qv)
class Pose(object): """ Pose defined by three dimensional translation and rotation The translation is representated by a three dimensional row vector. The rotation is represented by a quaternion. The pose itself is defined in coordinate system which is defined in frame_id (default world) Methods ------- from_transformation_matrix(matrix, frame_id='',normalize_rotation=False) Creates an instance of Pose from a transformation matrix from_posestamped_msg(msg) Initialize Pose from ROS posestamped message from_pose_msg(msg, frame_id='') Initialize Pose from ROS pose message normalize_rotation() Creates an instance of Pose with normalized quaternion is_rotation_normalized() Return True if quaternion is normalized rotation_matrix() Returns the rotation matrix(3x3 numpy array) transformation_matrix() Returns the transformation matrix in homogenous coordinates (4x4 numpy array) inverse(new_frame_id) Creates an instance which contains the inverse of this pose translate(translation) Translate pose rotate(rotation) Rotate pose to_posestamped_msg() Creates an instance of the ROS message PoseStamped from Pose """ def __init__(self, translation, rotation, frame_id='world', normalize_rotation=False): """ Initialization of the pose class For the internal quaternion representation and also to initialize the the class the library pyquaternion is used : https://kieranwynn.github.io/pyquaternion/ Parameters ---------- translation : convertable to numpy array of shape (3,) translation or position the pose describes rotation : pyquaternion.Quaternion rotation the pose describes as Quaternion frame_id : str (optinal defaul = 'world') name of the coordinate system the pose is defined normalize_rotation : bool (optinal default = False) If true quaternion normalization is performed in the initialization process Returns ------ pose : Pose An instance of class Pose Raises ------ TypeError : type mismatch If tranlation vector could not converted to a numpy array of shape (3,) with d type floating and the quaternion is not of type Quaternion. If frame_id is not of type str If normalize_rotation is not of type bool ValueError If the quaternion initalization from translation matrix went wrong Examples -------- Create a Pose by translation and quaternion and perform normalization >>> quaternion = Quaternion(matrix=np.eye(3)) >>> translation = np.array([1.0,1.0,1.0]) >>> p1 = Pose(translation, quaternion, _, True) """ # translation try: self.__translation = np.fromiter(translation, float) if self.__translation.shape[0] != 3: raise ValueError('provided translation is not' ' convertabel to a numpy vector of size (3,)') except (TypeError, ValueError) as exc: raise exc # rotation if isinstance(rotation, Quaternion): self.__quaternion = Quaternion(rotation.q.copy()) else: raise TypeError('rotation is not of the expected' ' type Quaternion') if not isinstance(frame_id, str): raise TypeError('frame_id is not of expected type str') self.__frame_id = frame_id if not isinstance(normalize_rotation, bool): raise TypeError('normalize_rotation is not of expected type bool') if normalize_rotation is True: self._normalize_rotation() self.__translation.flags.writeable = False self.__quaternion.q.flags.writeable = False @classmethod def identity(cls, frame_id='world'): """ Creates a instance of Pose from idenity matrix Parameters ---------- frame_id : str (optional defaul = 'world') name of the coordinate system the pose is defined Returns ------ pose : Pose An instance of class Pose """ translation = [0.0, 0.0, 0.0] quaternion = Quaternion([1.0, 0.0, 0.0, 0.0]) return cls(translation, quaternion, frame_id) @classmethod def from_transformation_matrix(cls, matrix, frame_id='world', normalize_rotation=False): """ Initialization of the pose class from transformation matrix Parameters ---------- matrix : numpy.ndarray((4,4),np.dtype=floating) A transformation matrix in homogenous coordinates frame_id : str (defaul = 'world') name of the coordinate system the pose is defined normalize_rotation : bool (optional default = False) If true quaternion normalization is performed in the initialization process Returns ------ pose : Pose An instance of class Pose Raises ------ TypeError : type mismatch If tranformation matrix is not a 4x4 numpy array with dtype floating Examples -------- Create a Pose instance from transformation matrix and set frame_id >>> transformation_matrix = np.eye(4) >>> p0 = Pose.from_transformation_matrix(transformation_matrix, "global") """ if isinstance(matrix, np.ndarray): if (len(matrix.shape) != 2 or matrix.shape[0] != 4 or matrix.shape[1] != 4): raise ValueError('matrix is not a 4x4 numpy array') if not issubclass(matrix.dtype.type, np.floating): raise TypeError('matrix is not a' 'dtype is no floating type') transformation_matrix = matrix.copy() translation = transformation_matrix[:-1, 3] try: quaternion = Quaternion(matrix=transformation_matrix) except ValueError as exc: raise ValueError( 'quaternion initialization went wrong') from exc else: raise TypeError('matrix is not of type numpy array') return cls(translation, quaternion, frame_id, normalize_rotation) @classmethod def from_posestamped_msg(cls, msg): """ Initialize Pose from ROS posestamped message Parameters ---------- msg : PoseStamped from ROS geometry_msgs posestamped message Returns ------- pose : Pose Instance of Pose generated from PoseStamped message Raises ------ TypeError If msg is not of type PoseStamped """ if not isinstance(msg, geometry_msgs.PoseStamped): raise TypeError('msg is not of expected type PoseStamped') translation = np.fromiter( [msg.pose.position.x, msg.pose.position.y, msg.pose.position.z], float) quaternion = Quaternion([ msg.pose.orientation.w, msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z ]) frame_id = msg.header.frame_id if not frame_id: frame_id = 'world' return cls(translation, quaternion, frame_id) @classmethod def from_pose_msg(cls, msg, frame_id='world'): """ Initialize Pose from ros geometry_msgs/Pose Parameters ---------- msg : Pose from geometry_msgs pose message Returns ------- pose : Pose Instance of Pose from pose message Raises ------ TypeError If msg is not of type Pose """ if not isinstance(msg, geometry_msgs.Pose): raise TypeError('msg is not of type ros geometry_msgs/Pose') translation = np.fromiter( [msg.position.x, msg.position.y, msg.position.z], float) quaternion = Quaternion([ msg.orientation.w, msg.orientation.x, msg.orientation.y, msg.orientation.z ]) return cls(translation, quaternion, frame_id) @property def frame_id(self): """ frame_id : str (readonly) Id of the coordinate system / frame """ return self.__frame_id @property def translation(self): """ translation : numpy.array((3,) dtype=floating) (readonly) numpy row array of size 3 which describes the translation """ return self.__translation @property def quaternion(self): """ quaternion: Quaternion(pyquaternion lib) (readonly) Quaternion which describes the rotation """ return self.__quaternion def normalize_rotation(self): """ Creates an instance of Pose with normalized quaternion Returns ------ pose : Pose Instance of Pose with normalized quaternion / rotation """ if np.isclose(np.linalg.norm(self.__quaternion.elements), 1.0): return self else: return Pose(self.__translation, self.__quaternion, normalize_rotation=True) def _normalize_rotation(self): self.__quaternion._fast_normalise() if self.__quaternion[0] < 0: self.__quaternion = -self.__quaternion def is_rotation_normalized(self): """ Returns True is rotation is normalized Returns ------- result : bool True is rotation is normalized else False """ if np.isclose(np.linalg.norm(self.__quaternion.elements), 1.0): return True else: return False def rotation_matrix(self): """ Returns the rotation matrix (3x3 numpy array) Returns ------ rotation_matrix : np.ndarray A 3x3 numpy array with dtype float which represents the rotation matrix """ return self.__quaternion.rotation_matrix def transformation_matrix(self): """ Return the transformation martix in homogenous coordinates (4x4 numpy array) Returns ------ transformation_matrix : np.ndarray A 4x4 numpy array with dtype float which represents the transformation matrix in homogenous coordinates """ transformation_matrix = self.__quaternion.transformation_matrix transformation_matrix[:-1, -1] = self.__translation return transformation_matrix def inverse(self, new_frame_id): """ Creates an instance which contains the inverse of this pose Parameters ---------- new_frame_id : str convertable name of the coordinate system in which pose is now defined Returns ------- inv_pose : Pose pose which is the inverse of self Raises ------ TypeError : type mismatch If new_frame_id is not of type str """ q_inv = self.__quaternion.inverse t_inv = q_inv.rotate(-self.translation) return self.__class__(t_inv, q_inv, new_frame_id) def translate(self, translation): """ Translate pose Parameters ---------- translation : Iterable translation which defines the change in x,y,z Returns ------- translated_pose : Pose pose which is translated by translate vector Raises ------ TypeError : type mismatch If translation is not convertable to np.ndarray ValueError If translation is not of size 3 (x,y,z) """ try: translation = np.fromiter(translation, float) if translation.shape[0] != 3: raise ValueError('provided translation is not' ' convertabel to a numpy vector of size (3,)') except (TypeError, ValueError) as exc: raise exc new_t = self.__translation + translation return type(self)(new_t, self.__quaternion, self.__frame_id) def rotate(self, rotation): """ Rotate pose Parameters ---------- rotation: (Quaternion or np.ndarray) defines rotation by Quaternion or 3x3 rotation matrix Returns ------- rotated_pose : Pose pose which is rotated by rotation Raises ------ TypeError : type mismatch If rotation is not one of expected types 3x3 np.ndarray or pyquaternion.Quaternion ValueError If initialization of Quaternion is not possible """ is_quaternion = isinstance(rotation, Quaternion) is_rotation_matrix = (isinstance(rotation, np.ndarray) and rotation.shape == (3, 3)) if is_quaternion: new_q = self.__quaternion * rotation elif is_rotation_matrix: try: new_q = self.__quaternion * Quaternion(matrix=rotation) except ValueError as exc: raise ValueError( 'quaternion initialization went wrong') from exc else: raise TypeError('rotation is not one of expected types ' 'Quaternion or np.ndarray (3x3)') return type(self)(self.__translation, new_q, self.__frame_id) def to_posestamped_msg(self): """ Creates an instance of the ROS message PoseStamped Returns ------ Instance of ROS message PoseStamped (seq and time are not set) docs.ros.org/kinetic/api/geometry_msgs/html/msg/PoseStamped.html """ pose_stamped = geometry_msgs.PoseStamped() pose_stamped.header.frame_id = self.__frame_id pose_stamped.pose.position.x = self.__translation[0] pose_stamped.pose.position.y = self.__translation[1] pose_stamped.pose.position.z = self.__translation[2] pose_stamped.pose.orientation.w = self.__quaternion[0] pose_stamped.pose.orientation.x = self.__quaternion[1] pose_stamped.pose.orientation.y = self.__quaternion[2] pose_stamped.pose.orientation.z = self.__quaternion[3] return pose_stamped def to_pose_msg(self): """ Creates an instance of the ROS message Pose Returns ------ Instance of ROS message Pose geometry_msgs/Pose """ pose = geometry_msgs.Pose() pose.position.x = self.__translation[0] pose.position.y = self.__translation[1] pose.position.z = self.__translation[2] pose.orientation.w = self.__quaternion[0] pose.orientation.x = self.__quaternion[1] pose.orientation.y = self.__quaternion[2] pose.orientation.z = self.__quaternion[3] return pose def __str__(self): axes = tuple(['w', 'x', 'y', 'z']) translation_str = '\n'.join([ 'translation.' + axes[i + 1] + ' : ' + str(value) for i, value in enumerate(self.__translation) ]) quaternion_str = '\n'.join([ 'quaternion.' + axes[i] + ' : ' + str(value) for i, value in enumerate(self.__quaternion) ]) return ('Pose:\n' + translation_str + '\n' + quaternion_str + '\nframe_id : ' + self.__frame_id) def __repr__(self): return self.__str__() def __eq__(self, other): r_tol = 1.0e-6 a_tol = 1.0e-7 if not isinstance(other, self.__class__): return False if id(other) == id(self): return True if not np.allclose( self.__translation, other.translation, rtol=r_tol, atol=a_tol): return False if self.__quaternion != other.quaternion: return False return True def __ne__(self, other): return not self.__eq__(other) def __mul__(self, other): if isinstance(other, self.__class__): new_q = self.__quaternion * other.quaternion new_t = (self.__translation + self.__quaternion.rotate(other.translation)) return self.__class__(new_t, new_q, self.frame_id) elif (isinstance(other, np.ndarray) and issubclass(other.dtype.type, np.floating)): if other.shape == (3, ): new_t = (self.__translation + self.__quaternion.rotate(other)) return new_t elif other.shape == (3, 1): new_t = (self.__translation + self.__quaternion.rotate(other)) return np.expand_dims(new_t, axis=1) elif other.shape == (4, ): new_t = np.ones(other.shape) new_t[0:3] = (self.__translation * other[-1] + self.__quaternion.rotate(other[:3])) return new_t elif other.shape == (4, 1): new_t = np.ones(other.shape) t3 = (self.__translation * other[-1] + self.__quaternion.rotate(other[:3])) new_t[0:3] = np.expand_dims(t3, axis=1) return new_t elif other.shape == (4, 4): product = np.matmul(self.transformation_matrix(), other) return self.from_transformation_matrix(product, self.__frame_id) else: TypeError('vector is not of shape (3,), (3,1),' ' (4,), (4,1) or matrix (4,4)') else: TypeError('other is not of expected type Pose,' ' 3 or 4 dimensional numpy vector or' ' 4x4 transformation matrix dtype floating') def __rmul__(self, other): return np.dot(other, self.transformation_matrix()) __array_priority__ = 10000
def main(): global publish_magnetic_data global show_magnetic_field # load json and create model json_file = open( '/home/letrend/workspace/roboy3/src/ball_in_socket_estimator/python/' + model_name + '.json', 'r') loaded_model_json = json_file.read() json_file.close() model = model_from_json(loaded_model_json) # load weights into new model model.load_weights( "/home/letrend/workspace/roboy3/src/ball_in_socket_estimator/python/" + model_name + ".h5") #_checkpoint print("Loaded model from disk") model.summary() rospy.init_node('replay') listener = tf.TransformListener() broadcaster = tf.TransformBroadcaster() magneticSensor_pub = rospy.Publisher('roboy/middleware/MagneticSensor', MagneticSensor, queue_size=1) joint_state_pub = rospy.Publisher('external_joint_states', sensor_msgs.msg.JointState, queue_size=1) joint_targets_pub = rospy.Publisher('joint_targets', sensor_msgs.msg.JointState, queue_size=1) visualization_pub = rospy.Publisher('visualization_marker', visualization_msgs.msg.Marker, queue_size=100) rospy.loginfo('loading data') dataset = pandas.read_csv("/home/letrend/workspace/roboy3/" + data_name + "_data0.log", delim_whitespace=True, header=1) dataset = dataset.values[1:len(dataset) - 1, 0:] print('%d values' % (len(dataset))) dataset = dataset[abs(dataset[:, 12]) <= 0.7, :] dataset = dataset[abs(dataset[:, 13]) <= 0.7, :] dataset = dataset[abs(dataset[:, 14]) <= 1.5, :] euler_set = np.array(dataset[:, 12:15]) # mean_euler = euler_set.mean(axis=0) # std_euler = euler_set.std(axis=0) # euler_set = (euler_set - mean_euler) / std_euler print('max euler ' + str(np.amax(euler_set))) print('min euler ' + str(np.amin(euler_set))) sensors_set = np.array([ dataset[:, 0], dataset[:, 1], dataset[:, 2], dataset[:, 3], dataset[:, 4], dataset[:, 5], dataset[:, 6], dataset[:, 7], dataset[:, 8], dataset[:, 9], dataset[:, 10], dataset[:, 11] ]) sensors_set = np.transpose(sensors_set) # mean_sensor = sensors_set.mean(axis=0) # std_sensor = sensors_set.std(axis=0) # sensors_set = (sensors_set - mean_sensor) / std_sensor # sensors_set = wrangle.mean_zero(pandas.DataFrame(sensors_set)).values sample = 0 samples = len(euler_set) t = 0 rate = rospy.Rate(1) error = 0 stride = 1 print('model predicts') euler_predict = model.predict(sensors_set) mse = numpy.linalg.norm(euler_predict - euler_set) / len(euler_predict) print('mse: ' + str(mse)) msg = sensor_msgs.msg.JointState() msg.header = std_msgs.msg.Header() msg.name = ['head_axis0', 'head_axis1', 'head_axis2'] msg.velocity = [0, 0, 0] msg.effort = [0, 0, 0] for i in range(0, len(euler_predict), stride): if rospy.is_shutdown(): return # rospy.loginfo_throttle(1, (euler_predict[i,0],euler_predict[i,1],euler_predict[i,2])) # error = error+numpy.linalg.norm(euler_predict[i,i:i+stride]-euler_set[i,i:i+stride]) # euler_p = euler_predict[i,:]*std_euler+mean_euler euler_predictED = [ euler_predict[i, 0], euler_predict[i, 1], euler_predict[i, 2] ] euler_truth = euler_set[i, :] broadcaster.sendTransform( (0, 0, 0), euler_to_quaternion( euler_predictED[0], euler_predictED[1], euler_predictED[2]), #euler_p[0],euler_p[1],euler_p[2]), rospy.Time.now(), "predict", "world") broadcaster.sendTransform( (0, 0, 0), euler_to_quaternion(euler_truth[0], euler_truth[1], euler_truth[2]), rospy.Time.now(), "ground_truth", "world") msg.header.stamp = rospy.Time.now() msg.position = euler_predictED joint_state_pub.publish(msg) msg.position = euler_truth joint_targets_pub.publish(msg) if show_magnetic_field: msg2 = visualization_msgs.msg.Marker() msg2.type = msg2.SPHERE msg2.id = i msg2.color.r = 1 msg2.color.a = 1 msg2.action = msg2.ADD msg2.header.seq = i msg2.header.frame_id = "world" msg2.header.stamp = rospy.Time.now() msg2.lifetime = rospy.Duration(0) msg2.scale.x = 0.01 msg2.scale.y = 0.01 msg2.scale.z = 0.01 msg2.pose.orientation.w = 1 msg2.ns = "magnetic_field_sensor0" sens = numpy.array([s[3], s[4], s[5]]) q_w = Quaternion(q) sens_w = q_w.rotate(sens) * 0.01 msg2.pose.position.x = sens_w[0] msg2.pose.position.y = sens_w[1] msg2.pose.position.z = sens_w[2] i = i + 1 visualization_pub.publish(msg2) if publish_magnetic_data: msg = MagneticSensor() msg.id = 5 msg.sensor_id = [0, 1, 2] msg.x = [sensors_set[i, 0], sensors_set[i, 3]] #,sensors_set[i,6] msg.y = [sensors_set[i, 1], sensors_set[i, 4]] #,sensors_set[i,7]] msg.z = [sensors_set[i, 2], sensors_set[i, 5]] #,sensors_set[i,8]] magneticSensor_pub.publish(msg) t0 = rospy.Time.now() error = numpy.linalg.norm(euler_truth - euler_predictED) * 180 / math.pi print("%d/%d\t\t%.3f%% error %f\n" "truth : %.3f %.3f %.3f\n" "predicted : %.3f %.3f %.3f" % (t, samples, (t / float(samples)) * 100.0, error, euler_truth[0], euler_truth[1], euler_truth[2], euler_predictED[0], euler_predictED[1], euler_predictED[2])) t = t + stride if error > 1: rate.sleep() error = error / samples print("mean squared error: %f" % (error)) # Signal handler rospy.spin()
ax.plot([vec[0]], [vec[1]], [vec[2]], marker=marker, label=label) target = [-0.00319718, -0.37576394, 0.45137436, -0.4986701138808597, 0.49256585342539383, 0.5200487598517108, -0.4881150324852347] target = [-0.00319718, -0.37576394, 0.45137436, -0.0021967960033876664, -0.6684438218043409, 0.7437414872148852, -0.0051605594964759876] init = [-0.02702883, -0.35275209, 0.46842613, -0.0021968 , -0.66844382, 0.74374149, -0.00516056] steps = 300 init_pose = init[:3] final_pose = target[:3] target_q = Quaternion(np.roll(target[3:], 1)) p2 = np.zeros(3) p1 = target_q.rotate(np.array(init_pose) - final_pose) traj = get_conical_helix_trajectory(p1, p2, steps) traj2 = get_conical_helix_trajectory(init_pose, final_pose, steps) traj = np.apply_along_axis(target_q.rotate,1,traj) traj = traj + final_pose fig = plt.figure() ax = fig.add_subplot(111, projection='3d') col = np.arange(steps) # ax.scatter(traj2[:,0], traj2[:,1], traj2[:,2], s=15,c=col, marker='.') ax.scatter(traj[:,0], traj[:,1], traj[:,2], s=15,c=col, marker='.') # plot(ax, p1, label='p1') # plot(ax, p2, label='p2') plot(ax, init_pose, label='initial')
def transformToEarthFrame(self, vector, q_): """ onvert coordinates w.r.t drone -> global ("headless") """ q = Quaternion(q_) return q.rotate(vector)
class BBox3D: """ Class for 3D Bounding Boxes (3-orthotope). """ def __init__(self, x, y, z, length=1, width=1, height=1, rw=1, rx=0, ry=0, rz=0, q=None, euler_angles=None, is_center=True): """ Constructor for 3D bounding box (3-orthotope). It takes either the center of the 3D bounding box or the back-bottom-left corner, the width, height and length of the box, and quaternion values to indicate the rotation. Parameters ---------- x : float X axis coordinate of 3D bounding box. Can be either center of bounding box or back-bottom-left corner. y : float Y axis coordinate of 3D bounding box. Can be either center of bounding box or back-bottom-left corner. z : float Z axis coordinate of 3D bounding box. Can be either center of bounding box or back-bottom-left corner. length : float, optional The length of the box (the default is 1). width : float, optional The width of the box (the default is 1). height : float, optional The height of the box (the default is 1). rw : float, optional The real part of the rotation quaternion (the default is 1). rx : int, optional The first element of the quaternion vector (the default is 0). ry : int, optional The second element of the quaternion vector (the default is 0). rz : int, optional The third element of the quaternion vector (the default is 0). euler_angles : list or ndarray of float, optional Sequence of euler angles in `[x, y, z]` rotation order (the default is None). is_center : bool, optional Flag to indicate if the provided coordinate is the center of the box (the default is True). """ if is_center: self._cx, self._cy, self._cz = x, y, z self._c = np.array([x, y, z]) else: self._cx = x + length / 2 self._cy = y + width / 2 self._cz = z + height / 2 self._c = np.array([self._cx, self._cy, self._cz]) self._w, self._h, self._l = width, height, length if euler_angles: # we need to apply y, z and x rotations in order # http://www.euclideanspace.com/maths/geometry/rotations/euler/index.htm self._q = Quaternion(axis=[0, 1, 0], angle=euler_angles[1]) * \ Quaternion(axis=[0, 0, 1], angle=euler_angles[2]) * \ Quaternion(axis=[1, 0, 0], angle=euler_angles[0]) elif q is not None: self._q = Quaternion(q) else: self._q = Quaternion(rw, rx, ry, rz) @property def center(self): """ Attribute to access center coordinates of box. Returns ------- ndarray of float 3-dimensional vector representing (x, y, z) coordinates of the box. """ return self._c @center.setter def center(self, c): """ Setter for center coordinates of the box. Parameters ---------- c : list or ndarray of float Center coordinates in (x, y, z) format. Raises ------ ValueError If `c` is not a vector/list of length 3. """ if len(c) != 3: raise ValueError("Center coordinates should be a vector of size 3") self._c = c def __valid_scalar(self, x): if not np.isscalar(x): raise ValueError("Value should be a scalar") else: # x is a scalar so we check for numeric type if not isinstance(x, (np.float, np.int)): raise TypeError("Value needs to be either a float or an int") return x @property def cx(self): """ Get the X coordinate of center. Returns ------- float X coordinate of center """ return self._cx @cx.setter def cx(self, x): """Setter for X coordinate of center. Parameters ---------- x : float """ self._cx = self.__valid_scalar(x) @property def cy(self): """ Get the Y coordinate of center. Returns ------- float Y coordinate of center """ return self._cy @cy.setter def cy(self, x): self._cy = self.__valid_scalar(x) @property def cz(self): """ Get the Z coordinate of center. Returns ------- float Z coordinate of center """ return self._cz @cz.setter def cz(self, x): self._cz = self.__valid_scalar(x) @property def q(self): """ Get the the rotation quaternion. Returns ------- ndarray of float Quaternion values in (w, x, y, z) form. """ return np.hstack((self._q.real, self._q.imaginary)) @q.setter def q(self, q): if not isinstance(q, (list, tuple, np.ndarray, Quaternion)): raise TypeError( "Value shoud be either list, numpy array or Quaterion") if isinstance(q, (list, tuple, np.ndarray)) and len(q) != 4: raise ValueError("Quaternion input should be a vector of size 4") self._q = Quaternion(q) @property def quaternion(self): """ Get the the rotation quaternion. Returns ------- ndarray of float Quaternion values in (w, x, y, z) form. """ return self.q @quaternion.setter def quaternion(self, q): self.q = q @property def l(self): """ Get the length of the box. Returns ------- float Length of the box. """ return self._l @l.setter def l(self, x): self._l = self.__valid_scalar(x) @property def length(self): """ Get the length of the box. Returns ------- float Length of the box. """ return self._l @length.setter def length(self, x): self.l = x @property def w(self): """ Get the width of the box. Returns ------- float Width of the box. """ return self._w @w.setter def w(self, x): self._w = self.__valid_scalar(x) @property def width(self): """ Get the width of the box. Returns ------- float Width of the box. """ return self._w @width.setter def width(self, x): self.w = x @property def h(self): """ Get the height of the box. Returns ------- float Height of the box. """ return self._h @h.setter def h(self, x): self._h = self.__valid_scalar(x) @property def height(self): """ Get the height of the box. Returns ------- float Height of the box. """ return self._h @height.setter def height(self, x): self.h = x def __transform(self, x): """Rotate and translate the point to world coordinates""" y = self._c + self._q.rotate(x) return y @property def p1(self): p = np.array([-self._l / 2, -self._w / 2, -self._h / 2]) p = self.__transform(p) return p @property def p2(self): p = np.array([self._l / 2, -self._w / 2, -self._h / 2]) p = self.__transform(p) return p @property def p3(self): p = np.array([self._l / 2, self._w / 2, -self._h / 2]) p = self.__transform(p) return p @property def p4(self): p = np.array([-self._l / 2, self._w / 2, -self._h / 2]) p = self.__transform(p) return p @property def p5(self): p = np.array([-self._l / 2, -self._w / 2, self._h / 2]) p = self.__transform(p) return p @property def p6(self): p = np.array([self._l / 2, -self._w / 2, self._h / 2]) p = self.__transform(p) return p @property def p7(self): p = np.array([self._l / 2, self._w / 2, self._h / 2]) p = self.__transform(p) return p @property def p8(self): p = np.array([-self._l / 2, self._w / 2, self._h / 2]) p = self.__transform(p) return p @property def p(self): """ Attribute to access ndarray of all corners of box in order. Returns ------- ndarray of float All corners of the bounding box in order. """ x = np.vstack([ self.p1, self.p2, self.p3, self.p4, self.p5, self.p6, self.p7, self.p8 ]) return x def __repr__(self): return "BBox3D(x={cx},y={cy},z={cz}), length={l},width={w},height={h}, q=({rw}, {rx}, {ry}, {rz}))".format( cx=self._cx, cy=self._cy, cz=self._cz, l=self._l, w=self._w, h=self._h, rw=self._q.real, rx=self._q.imaginary[0], ry=self._q.imaginary[1], rz=self._q.imaginary[2])
class gameObject(object): def __init__(self, id_prefix, pos=sc([0, 0, 0]), mass=100000.0): global id_count if type(pos) == sc: self.position = pos else: self.position = sc(pos) self.inventory = items.inventory() self.identifier = id_prefix + "-" + "%06i" % id_count self.heading = Quaternion() id_count += 1 global objects objects.append(self) logging.info("New object: " + str(self)) def __str__(self): return self.identifier + ", " + str(self.position) def remove(self): global objects try: objects.remove(self) logging.info("object removed: " + str(self)) except: logging.exception("unable to remove self from objects") def getPosition(self): return self.position.getPosition() def getDistanceTo(self, target): return abs(target.position - self.position) def getAngleTo(self, target, inDeg=False): #Move target position to ships origo lpos = target.position - self.position #Calculate the heading vector heading_vect = self.heading.rotate([0.0, 0.0, 1.0]) dot = np.vdot(heading_vect, lpos.getPosition()) angle = np.arccos(dot / abs(lpos)) if inDeg: angle *= 180 / np.pi return angle def getSphericalCoordinateTo(self, target, inDeg=False): #Move target position to ships origo lpos = target.position - self.position #Move the target to local coordinate ltarget = self.heading.rotate(lpos.getPosition()) x, y, z = ltarget distance = abs(lpos) inclination = np.arccos(x / distance) - np.pi / 2 azimuth = np.arctan2(y, z) if inDeg: inclination *= 180 / np.pi azimuth *= 180 / np.pi return distance, azimuth, inclination def getHeading(self): return self.heading.rotate([0.0, 0.0, 1.0]) def get_mass(self): return self.inventory.get_mass()
def load_directory(self, path): pose_list_filename = os.path.join(path, "images.txt") if not os.path.isfile(pose_list_filename): return False pose_file = open(pose_list_filename, 'r') dir_poses = [] dir_images = [] while (True): pose_line = pose_file.readline() if not pose_line: break if pose_line[0] == '#': continue # IMAGE_ID, QW, QX, QY, QZ, TX, TY, TZ, CAMERA_ID, NAME pose_tokens = pose_line.rstrip().split(' ') if len(pose_tokens) != 10: print('invalid pose: ' + pose_line) break # parse tokens image_id = int(pose_tokens[0]) camera_id = int(pose_tokens[8]) image_name = pose_tokens[9] # https://colmap.github.io/format.html#images-txt R = (float(pose_tokens[1]), float(pose_tokens[2]), float(pose_tokens[3]), float(pose_tokens[4])) t = (float(pose_tokens[5]), float(pose_tokens[6]), float(pose_tokens[7])) # get the camera center from -R^t * T q = Quaternion(w=R[0], x=R[1], y=R[2], z=R[3]).inverse t = np.negative(q.rotate(np.array([t[0], t[1], t[2]]))).tolist() # (this is the same as transposing the quaternion's rotation matrix) #m = Quaternion(w=R[0], x=R[1], y=R[2], z=R[3]).rotation_matrix.transpose() #t = np.negative(np.dot(m, np.array([t[0], t[1], t[2]]))).tolist() # store metadata dir_poses.append(t + [q.x, q.y, q.z, q.w]) dir_images.append(os.path.join(self.root_dir, image_name)) # skip points2D line pose_file.readline() pose_file.close() self.poses.append(dir_poses) self.images.append(dir_images) self.num_images += len(dir_images) if self.relative_pose: self.num_images -= 1 print('({:s}) found {:04d} images under {:s}'.format( self.type, len(dir_images), path)) return True
def add_pose_estimation(bag): v_z = Quaternion(0, 0, 0, 1) # orientation of the imu frame imu_frame = Quaternion(-0.0198769629216, 0.974536168168, -0.218396560265, -0.0467665023439) x_vec = np.array([1, 0, 0]) time = [] previous_pcl = pcl.PointCloud() current_pcl = pcl.PointCloud() est_pose_bool = False pose_msg = PoseWithCovarianceStamped() pose_msg.header.frame_id = 'base_footprint' previous_pose = np.array([]) previous_ori = [] x = [] y = [] prev_tr = np.array([]) cov = [0] * 36 scan_time = [] ros_time = [] f_pose = 20 # Hz queued_pcl_list = [] dt = 0 print 'Extracting poses using ICP' for topic, msg, t in bag.read_messages( topics=['/imu0/data', '/scan_corrected'], start_time=rospy.Time(bag.get_start_time()) + rospy.Duration(0)): # if topic == '/imu0/data': q = Quaternion(msg.orientation.w, msg.orientation.x, msg.orientation.y, msg.orientation.z) v_z_ = q * imu_frame * v_z * imu_frame.conjugate * q.conjugate imu_rot = q if np.arccos(v_z_[3]) - math.pi > -.05: est_pose_bool = True else: est_pose_bool = False elif topic == "/scan_corrected" and est_pose_bool: previous_pcl.from_array(current_pcl.to_array()) current_pcl.from_array(to_pcl_array(msg)) if dt != 0: if dt > 1: queued_pcl_list = [] else: queued_pcl_list.append(previous_pcl) if len(queued_pcl_list ) > 1: #queue size = 2 less than 1521*2*3 points queued_pcl_list.pop(0) previous_pcl_array = np.concatenate( ([queue_.to_array() for queue_ in queued_pcl_list]), axis=0) previous_pcl.from_array(previous_pcl_array) if not time: time.append(msg.header.stamp.to_sec()) prev_tr = np.zeros((1, 3)) scan_time.append(msg.header.stamp.to_sec()) ros_time.append(t.to_sec()) yaw = [0] if previous_pcl.size is not 0: if len(scan_time) == 1: init_q = get_yaw_quaternion(imu_rot) prev_q = Quaternion(1, 0, 0, 0) q_array = [prev_q] icp = current_pcl.make_IterativeClosestPoint() # Final = icp.align() converged, transf, transf_current_pcl, fitness = icp.icp( current_pcl, previous_pcl) # check if the rotation matrix is valid (i.e. det==1) if not np.isclose(np.linalg.det(transf[0:3, 0:3]), 1.0): converged = False if converged: time.append(msg.header.stamp.to_sec()) dt = time[-1] - time[-2] if 0 < dt <= 0.5: #if more than 10 scans (i.e. .5s) have been dropped, do save the pose (same pose as previous) # get covariance matrix pose_orientation, pose_translation = matrix4_to_rot_trans( imu_rot, transf, mode='2d') #pose_orientation, pose_translation, cov = get_cov(transf, current_pcl, transf_current_pcl) if np.linalg.norm( pose_translation ) / dt * 3.6 < 40: # the speed between two scans should not be greater than 40 kph prev_tr = np.append(prev_tr, [ prev_q.rotate(pose_translation) + prev_tr[-1] ], axis=0) prev_q = init_q.conjugate * pose_orientation q_array.append(prev_q) scan_time.append(msg.header.stamp.to_sec()) ros_time.append(t.to_sec()) else: dt = 0 print '%i poses were calculated' % len(scan_time) print 'Interpolate the poses (orientation, position)' interp_time = np.linspace( scan_time[0], scan_time[-1], np.round((scan_time[-1] - scan_time[0]) * f_pose)) tr = np.zeros((interp_time.size, 3)) interp_ros_time = np.linspace(ros_time[0], ros_time[-1], interp_time.size) ori = get_interpolated_quat(interp_time, scan_time, q_array) tr[:, 0] = np.interp(interp_time, scan_time, prev_tr[:, 0]) tr[:, 1] = np.interp(interp_time, scan_time, prev_tr[:, 1]) print 'Change coordinates to footprint_frame' # rotate from laser_frame to footprint frame laser_rd = -(90 + 9) * math.pi / 180 X = np.cos(laser_rd) * tr[:, 1] - np.sin(laser_rd) * tr[:, 0] Y = np.sin(laser_rd) * tr[:, 1] + np.cos(laser_rd) * tr[:, 0] tr[:, 1] = X tr[:, 0] = Y # rotate the orientation '''rot_ = Quaternion(axis=[0, 0, 1], radians=laser_rd) ori = [rot_*o for o in ori]''' print ori plt.plot(X, Y) plt.axis('equal') plt.show() print 'Writing the poses in the /scan_pose topic' write_pose_to_bag(bag, interp_time, interp_ros_time, ori, tr) print 'Re-indexing the bag' for done in bag.reindex(): pass bag.close() print 'Done'
class Em: """The atom of the MR simulation""" def __init__(self, magnetization, position, gyromagnetic_ratio, shielding_constant, equilibrium_magnetization): """Initializes an em Params: magnetization: (numpy 3-vector) initial magnetization position: (numpy 3-vector) intial position gyromagnetic_ratio: (float) the gyromagnetic ratio of the nucleus shielding_constant: (nonnegative float) shielding constant from chemical environment equilibrium_magnetization: (positive float) longitudinal magnetization in thermal equilibrium """ # Check params if not (magnetization.dtype == np.float64 and magnetization.shape == (3, )): raise TypeError("magnetization must be a numpy 3-vector") if not (position.dtype == np.float64 and position.shape == (3, )): raise TypeError("position must be a numpy 3-vector") if not isinstance(gyromagnetic_ratio, float): raise TypeError("gyromagnetic_ratio must be a float") if not (isinstance(shielding_constant, float) and shielding_constant >= 0.0): raise TypeError("shielding constant must be a nonnegative float") if not (isinstance(equilibrium_magnetization, float) and equilibrium_magnetization > 0.0): raise TypeError( "equilibrium_magnetization must be a positive float") # Set data attributes self.mu = magnetization self.r = position self.gamma = gyromagnetic_ratio self.sigma = shielding_constant self.mu0 = equilibrium_magnetization self.flip_quaternion = Quaternion( 1) # Rotation quaterion for flip from excitation def set_shielding_constant(self, new_shielding_constant): """Updates shielding constant Params: new_shielding_constant: (nonnegative float) new shielding constant """ # Check params if not (isinstance(new_shielding_constant, float) and new_shielding_constant >= 0.0): raise TypeError( "new_shielding_constant must be a nonnegative float") # Update data attribute self.sigma = new_shielding_constant def diffuse(self, diffusion_coefficients, delta_t): """Updates position according to diffusion process Params: diffusion_coefficients: (numpy 3-vector of nonnegative floats) diffusion coefficients in 3 spatial directions at current position delta_t: (positive float) time step """ # Check params if not (diffusion_coefficients.shape == (3, ) and diffusion_coefficients.dtype == np.float64 and all([item >= 0.0 for item in diffusion_coefficients])): raise TypeError( "diffusion_coefficients must be a numpy 3-vector of nonnegative floats" ) if not (isinstance(delta_t, float) and delta_t > 0.0): raise TypeError("delta_t must be a positive float") # Model diffusion for ax_no in range(3): if (diffusion_coefficients[ax_no] > 0.0): self.r[ax_no] = self.r[ax_no] + np.random.normal( 0.0, np.sqrt(2 * diffusion_coefficients[ax_no] * delta_t)) def precess_and_relax(self, T1, T2, Bz, delta_t): """Updates magnetization assuming free precession Params: T1: (positive float) T1 relaxation time T2: (positive float) T2 relaxation time Bz: (float) longitudinal field delta_t: (positive float) time step """ # Check params if not (isinstance(T1, float) and T1 > 0.0): raise TypeError("T1 must be a positive float") if not (isinstance(T2, float) and T2 > 0.0): raise TypeError("T2 must be a positive float") if not isinstance(Bz, float): raise TypeError("Bz must be a float") if not (isinstance(delta_t, float) and delta_t > 0.0): raise TypeError("delta_t must be a positive float") # Rotate magnetization Bz = (1 - self.sigma) * Bz # effective field due to electron shielding m = (self.mu[0] + 1j * self.mu[1]) * np.exp(-delta_t / T2 - 1j * self.gamma * Bz * delta_t) self.mu[0] = m.real self.mu[1] = m.imag self.mu[2] = self.mu0 + (self.mu[2] - self.mu0) * np.exp(-delta_t / T1) def update_flip_quaternion(self, B1x, B1y, Bz, omega_rf, delta_t): """Updates flip quaternion assuming excitation Params: B1x: (float) RF pulse modulation in x direction in rotating frame B1y: (float) RF pulse modulation in y direction in rotating frame Bz: (float) field in z direction in lab frame omega_rf: (positive float) angular carrier frequency of RF pulse delta_t: (positive float) time step """ # Check params if not isinstance(B1x, float): raise TypeError("B1x must be a float") if not isinstance(B1y, float): raise TypeError("B1y must be a float") if not isinstance(Bz, float): raise TypeError("Bz must be a float") if not (isinstance(omega_rf, float) and omega_rf > 0.0): raise TypeError("omega_rf must be a positive float") if not (isinstance(delta_t, float) and delta_t > 0.0): raise TypeError("delta_t must be a positive float") # Compute update to flip quaternion Brot = np.array([B1x, B1y, Bz]) Brot = Brot * (1 - self.sigma ) # effective field due to electron shielding Beff = Brot - np.array([0.0, 0.0, omega_rf / self.gamma ]) # effective field in rotating frame Beff_norm = np.linalg.norm(Beff) flip_axis = Beff / Beff_norm flip_angle = -self.gamma * Beff_norm * delta_t self.flip_quaternion = self.flip_quaternion * Quaternion( axis=flip_axis, angle=flip_angle) def flip(self, omega_rf, pulse_duration): """Updates magnetization at end of excitation pulse and resets flip quaternion Params: omega_rf: (positive float) angular carrier frequency of RF pulse pulse_duration: (positive float) duration of RF pulse """ # Check params if not (isinstance(omega_rf, float) and omega_rf > 0.0): raise TypeError("omega_rf must be a positive float") if not (isinstance(pulse_duration, float) and pulse_duration > 0.0): raise TypeError("pulse_duration must be a positive float") # Apply flip quaternion and reset self.flip_quaternion = self.flip_quaternion * Quaternion( axis=[0, 0, 1], angle=-omega_rf * pulse_duration) self.mu = self.flip_quaternion.rotate(self.mu) self.flip_quaternion = Quaternion(1)
class BoundingBox3D: """ Class for 3D Bounding Boxes (3-orthotope). It takes either the center of the 3D bounding box or the \ back-bottom-left corner, the width, height and length \ of the box, and quaternion values to indicate the rotation. Args: x (:py:class:`float`): X axis coordinate of 3D bounding box. \ Can be either center of bounding box or back-bottom-left corner. y (:py:class:`float`): Y axis coordinate of 3D bounding box. \ Can be either center of bounding box or back-bottom-left corner. z (:py:class:`float`): Z axis coordinate of 3D bounding box. \ Can be either center of bounding box or back-bottom-left corner. length (:py:class:`float`, optional): The length of the box (default is 1). This corresponds to the dimension along the x-axis. width (:py:class:`float`, optional): The width of the box (default is 1). This corresponds to the dimension along the y-axis. height (:py:class:`float`, optional): The height of the box (default is 1). This corresponds to the dimension along the z-axis. rw (:py:class:`float`, optional): The real part of the rotation quaternion \ (default is 1). rx (:py:class:`int`, optional): The first element of the quaternion vector \ (default is 0). ry (:py:class:`int`, optional): The second element of the quaternion vector \ (default is 0). rz (:py:class:`int`, optional): The third element of the quaternion vector \ (default is 0). euler_angles (:py:class:`list` or :py:class:`ndarray` of float, optional): \ Sequence of euler angles in `[x, y, z]` rotation order \ (the default is None). is_center (`bool`, optional): Flag to indicate if the provided coordinate \ is the center of the box (the default is True). """ def __init__( self, x, y, z, length=1, width=1, height=1, rw=1, rx=0, ry=0, rz=0, q=None, euler_angles=None, is_center=True, ): if is_center: self._cx, self._cy, self._cz = x, y, z self._c = np.array([x, y, z]) else: self._cx = x + length / 2 self._cy = y + width / 2 self._cz = z + height / 2 self._c = np.array([self._cx, self._cy, self._cz]) self._w, self._h, self._l = width, height, length if euler_angles is not None: # we need to apply y, z and x rotations in order # http://www.euclideanspace.com/maths/geometry/rotations/euler/index.htm self._q = (Quaternion(axis=[0, 1, 0], angle=euler_angles[1]) * Quaternion(axis=[0, 0, 1], angle=euler_angles[2]) * Quaternion(axis=[1, 0, 0], angle=euler_angles[0])) elif q is not None: self._q = Quaternion(q) else: self._q = Quaternion(rw, rx, ry, rz) @classmethod def from_center_dimension_euler(cls, center, dimension, euler_angles=None): """Factory function to create BoundingBox3D from center, dimension, and euler arrays. Can pass in either np.arrays or Python lists. Args: center (list): list of length 3 dimension (list): list of length 3 euler_angles (list, optional): list of length 3. Defaults to None. Returns: BoundingBox3D: a new 3D bounding box object """ x, y, z = center length, width, height = dimension return BoundingBox3D( x=x, y=y, z=z, length=length, width=width, height=height, euler_angles=euler_angles, ) @classmethod def from_xyzxyz(cls, xyz1, xyz2): x1, y1, z1 = xyz1 x2, y2, z2 = xyz2 length, width, height = abs(x2 - x1), abs(y2 - y1), abs(z2 - z1) min_x = min(x1, x2) min_y = min(y1, y2) min_z = min(z1, z2) c_x = min_x + length // 2 c_y = min_y + width // 2 c_z = min_z + height // 2 return BoundingBox3D(x=c_x, y=c_y, z=c_z, length=length, width=width, height=height) @property def center(self): """ Attribute to access center coordinates of box in (x, y, z) format. Can be set to :py:class:`list` or :py:class:`ndarray` of float. Returns: :py:class:`ndarray` of float: 3-dimensional vector representing \ (x, y, z) coordinates of the box. Raises: ValueError: If `c` is not a vector/list of length 3. """ return self._c @center.setter def center(self, c): if len(c) != 3: raise ValueError("Center coordinates should be a vector of size 3") self._c = c def __valid_scalar(self, x): if not np.isscalar(x): raise ValueError("Value should be a scalar") else: # x is a scalar so we check for numeric type if not isinstance(x, (float, int)): raise TypeError("Value needs to be either a float or an int") return x @property def cx(self): """ :py:class:`float`: X coordinate of center. """ return self._cx @cx.setter def cx(self, x): self._cx = self.__valid_scalar(x) @property def cy(self): """ :py:class:`float`: Y coordinate of center. """ return self._cy @cy.setter def cy(self, x): self._cy = self.__valid_scalar(x) @property def cz(self): """ :py:class:`float`: Z coordinate of center. """ return self._cz @cz.setter def cz(self, x): self._cz = self.__valid_scalar(x) @property def q(self): """ Syntactic sugar for the rotation quaternion of the box. Returns :py:class:`ndarray` of float: Quaternion values in (w, x, y, z) form. """ return np.hstack((self._q.real, self._q.imaginary)) @q.setter def q(self, q): if not isinstance(q, (list, tuple, np.ndarray, Quaternion)): raise TypeError( "Value shoud be either list, numpy array or Quaterion") if isinstance(q, (list, tuple, np.ndarray)) and len(q) != 4: raise ValueError("Quaternion input should be a vector of size 4") self._q = Quaternion(q) @property def quaternion(self): """ The rotation quaternion. Returns: :py:class:`ndarray` of float: Quaternion values in (w, x, y, z) form. """ return self.q @quaternion.setter def quaternion(self, q): self.q = q @property def length(self): """ :py:class:`float`: Length of the box. """ return self._l @length.setter def length(self, x): self._l = self.__valid_scalar(x) @property def width(self): """ :py:class:`float`: The width of the box. """ return self._w @width.setter def width(self, x): self._w = self.__valid_scalar(x) @property def height(self): """ :py:class:`float`: The height of the box. """ return self._h @height.setter def height(self, x): self._h = self.__valid_scalar(x) def __transform(self, x): """ Rotate and translate the point to world coordinates. """ y = self._c + self._q.rotate(x) return y @property def p1(self): """ :py:class:`float`: Back-left-bottom point. """ p = np.array([-self._l / 2, -self._w / 2, -self._h / 2]) p = self.__transform(p) return p @property def p2(self): """ :py:class:`float`: Back-right-bottom point. """ p = np.array([-self._l / 2, self._w / 2, -self._h / 2]) p = self.__transform(p) return p @property def p3(self): """ :py:class:`float`: Front-right-bottom point. """ p = np.array([self._l / 2, self._w / 2, -self._h / 2]) p = self.__transform(p) return p @property def p4(self): """ :py:class:`float`: Front-left-bottom point. """ p = np.array([self._l / 2, -self._w / 2, -self._h / 2]) p = self.__transform(p) return p @property def p5(self): """ :py:class:`float`: Back-left-top point. """ p = np.array([-self._l / 2, -self._w / 2, self._h / 2]) p = self.__transform(p) return p @property def p6(self): """ :py:class:`float`: Back-right-top point. """ p = np.array([-self._l / 2, self._w / 2, self._h / 2]) p = self.__transform(p) return p @property def p7(self): """ :py:class:`float`: Front-right-top point. """ p = np.array([self._l / 2, self._w / 2, self._h / 2]) p = self.__transform(p) return p @property def p8(self): """ :py:class:`float`: Front-left-top point. """ p = np.array([self._l / 2, -self._w / 2, self._h / 2]) p = self.__transform(p) return p @property def p(self): """ Attribute to access ndarray of all corners of box in order. Returns: :py:class:`ndarray` of float: All corners of the bounding box in order. The order goes bottom->top and clockwise starting from the bottom-left point. """ x = np.vstack([ self.p1, self.p2, self.p3, self.p4, self.p5, self.p6, self.p7, self.p8 ]) return x def __repr__(self): template = ( "BoundingBox3D(x={cx}, y={cy}, z={cz}), length={l}, width={w}, height={h}, " "q=({rw}, {rx}, {ry}, {rz}))") return template.format( cx=self._cx, cy=self._cy, cz=self._cz, l=self._l, w=self._w, h=self._h, rw=self._q.real, rx=self._q.imaginary[0], ry=self._q.imaginary[1], rz=self._q.imaginary[2], ) def copy(self): return deepcopy(self) @property def triangle_vertices(self): """Get triangle vertices to use when plotting a triangular mesh Returns: np.array: Triangular vertices of the cube """ # Triangle vertex connections (for triangle meshes) i = [7, 0, 0, 0, 4, 4, 6, 6, 4, 0, 3, 2] j = [3, 4, 1, 2, 5, 6, 5, 2, 0, 1, 6, 3] k = [0, 7, 2, 3, 6, 7, 1, 1, 5, 5, 7, 6] triangle_vertices = np.vstack([i, j, k]) return triangle_vertices @property def edges(self): """Get the edge connections for the cube Returns: np.array: list of edges of the cube """ # Create the edges edges = np.array([ [0, 1], [0, 3], [0, 4], [1, 2], [1, 5], [2, 3], [2, 6], [3, 7], [4, 5], [4, 7], [5, 6], [6, 7], ]) return edges
def get_groundtruth_scores(constel_files, groundtruth_file): """Finds the distances between scenes based on positions and orientations information of frames. Estimates a scene position with distances to objects in it. Arguments: constel_files {list} -- List of strings representing the name of all the constellation txt files. groundtruth_file {string} -- String containing the path to the groundtruth txt file which has timestamps and poses Returns: 2d numpy array -- Array of size nb_frames x nb_frames with the distance between two estimated scene positions. """ # Get the timestamps of all the frames timestamps = np.array( [float(file.split('/')[-1].strip('.txt')) for file in constel_files]) distances_frame = [] # Get the average z positions (distances) of all the frames for constel_file in constel_files: z = 0. with open(constel_file) as f: point_from_objs = [line.split() for line in f] for line in point_from_objs: z += float(line[-1]) if len(point_from_objs) > 0: z = z / len(point_from_objs) distances_frame.append(z) # Load all the poses poses_non_sync = np.loadtxt(groundtruth_file) # Get the poses closest to the timestamps of the frames poses_frames = [] i = 0 for time in timestamps: # Go through the times of the groundtruth data until we get a higher value than the one we have while (time > poses_non_sync[i][0]): i += 1 # When we found the limit, choose the limit value or the one before depending on which is closest if (poses_non_sync[i][0] - time) > (time - poses_non_sync[i - 1][0]): poses_frames.append(poses_non_sync[i - 1][1:]) else: poses_frames.append(poses_non_sync[i][1:]) poses_frames = np.array(poses_frames) # Estimate points in front of the cone based on poses and distances points_in_front_cone = np.zeros((len(poses_frames), 3)) for i in range(len(poses_frames)): pose = poses_frames[i] vec_front_cam = np.array([0., 0., distances_frame[i]]) q = Quaternion([pose[-1], pose[3], pose[4], pose[5]]) v_front_ori = q.rotate(vec_front_cam) points_in_front_cone[i] = pose[:3] + v_front_ori cones_distances = np.zeros((len(poses_frames), len(poses_frames))) for i in range(len(poses_frames)): for j in range(i, len(poses_frames)): pos1 = points_in_front_cone[i] pos2 = points_in_front_cone[j] dist = math.sqrt(sum((pos1 - pos2)**2)) # Filter the values based on manually imposed rules defined in the filter_dist_freiburg3 function dist = filter_dist_freiburg3(poses_frames[i], poses_frames[j], pos1, pos2, dist) cones_distances[i, j] = dist cones_distances[j, i] = dist # Filter ignored neighbors for i in range(settings.loop_closure_neighbors_ignored + 1): np.fill_diagonal(cones_distances[i:], np.inf) np.fill_diagonal(cones_distances[:, i:], np.inf) # Put the scores between 0(= high distance) and 1 (= distance of 0) gt_scores = 1 - (cones_distances / np.max(cones_distances[cones_distances < 10000])) return gt_scores, cones_distances
def doublette(whichTrees, plots=True, output=True): ############################ depth_limit = 8. cam_distance_threshold = 2. frame_size = 30 match_found = 0 non_match_found = 0 first_img_found = 0 ############################ fx, fy, cx, cy = get_cam_intrinsics() drone_cam_translation = np.array([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 1., 0.46], [0., 0., 0., 1.]]) print('fx, fy, cx, cy...............', fx, fy, cx, cy) rotmat_x = build_rot_mat(-.5*np.pi, 'x') rotmat_y = build_rot_mat(-.5*np.pi, 'y') while (match_found < 1 and non_match_found < 1): print() print('finding initial camera position with visible tree..........................') print() while first_img_found < 1: '''pick a random image number for snapshot1''' if whichTrees == 'random': random_tree_kind = np.random.randint(len(trees)) random_tree_kind_str = trees[random_tree_kind] random_tree = np.random.randint(num_tree_folders) random_tree_path = DataPath + random_tree_kind_str + '/' + random_tree_kind_str + '_High' + str(random_tree) + '/' if whichTrees == 'debug': random_tree_kind = 0 random_tree_kind_str = trees[random_tree_kind] random_tree = random_tree_debug random_tree_path = DataPath + random_tree_kind_str + '/' + random_tree_kind_str + '_High' + str(random_tree) + '/' if whichTrees == 'debug_tejaswi': random_tree_kind = 0 random_tree_kind_str = trees[random_tree_kind] random_tree = 1 random_tree_path = DataPath + random_tree_kind_str + '/' + random_tree_kind_str + '_High' + str(random_tree) + '/' if whichTrees == 'Cherry': random_tree_kind = 0 random_tree_kind_str = trees[random_tree_kind] random_tree = np.random.randint(num_tree_folders) random_tree_path = DataPath + random_tree_kind_str + '/' + random_tree_kind_str + '_High' + str(random_tree) + '/' if whichTrees == 'KoreanStewartia': random_tree_kind = 1 random_tree_kind_str = trees[random_tree_kind] random_tree = np.random.randint(num_tree_folders) random_tree_path = DataPath + random_tree_kind_str + '/' + random_tree_kind_str + '_High' + str(random_tree) + '/' '''find total number of frames in respective folder''' data_table = np.asarray(pd.read_table(random_tree_path + 'poses.txt' , sep='\s', header=0, index_col=False, engine='python')) num_frames = np.max(data_table[:,0]) abc = np.squeeze(np.array(np.where(data_table[:,7] == 1))) mean_x = np.mean(data_table[abc[0]:abc[-1],1]) mean_y = np.mean(data_table[abc[0]:abc[-1],2]) '''find random initial frame''' snapshot1_index = np.random.randint(num_frames) if whichTrees == 'debug': snapshot1_index = snapshot1_index_debug '''load respective depth img''' file_index1 = file_index(snapshot1_index) depthImg1 = convert_pfm(random_tree_path + file_index1 + 'pl.pfm') '''delete too far away points''' depthImg1[depthImg1 > depth_limit] = 0 '''check if there is a tree''' if np.array(np.nonzero(depthImg1[frame_size:-frame_size,frame_size:-frame_size])).shape[1] > 10: first_img_found = 1 if output: print('num_frames...............', num_frames) print('random_tree_kind.........', random_tree_kind) print('random_tree_kind_str.....', random_tree_kind_str) print('random_tree..............', random_tree) print('random_tree_path.........', random_tree_path) print('index1...................', snapshot1_index) print(random_tree_path + file_index1 + 'pl.pfm') print() print('=======================================================> initial frame found') print() '''load the segmentation''' segmentation1 = imageio.imread(random_tree_path + file_index1 + 'seg.ppm') segmentation1 = np.round(np.mean(segmentation1, 2) / np.max(np.asarray(np.reshape(np.mean(segmentation1,2), (-1)))), 1) '''delete depth image parts without tree''' # depthImg1[.7 < segmentation1 and segmentation1 < .9] = 0 '''choose random point on tree''' rand_row, rand_col = 0, 0 while np.absolute(cy - rand_row) > (cy - frame_size) or np.absolute(cx - rand_col) > (cx - frame_size): random_spot1 = np.random.randint(0, np.array(np.nonzero(depthImg1)).shape[1]) rand_row, rand_col = np.array(np.nonzero(depthImg1))[:,random_spot1] if whichTrees == 'debug': rand_row, rand_col = rand_row_debug, rand_col_debug '''label point''' point_label = segmentation1[rand_row, rand_col] '''get 1st cam position and quaternions''' cam_position1 = get_cam_position(snapshot1_index, random_tree_path) qx1 = cam_position1[3] qy1 = cam_position1[4] qz1 = cam_position1[5] if qz1 == 0.: qz1 = 1e-04 qw1 = cam_position1[6] quaternion_cam1 = Quaternion(qw1, qx1, qy1, qz1) angle1 = -2 * np.arccos(cam_position1[-1]) rotmat_z1 = build_rot_mat(angle1, 'z') lens1_position = np.dot(rotmat_z1, np.dot(rotmat_x, np.dot(rotmat_y, drone_cam_translation[:3,3])) * np.array([-1.,1., 1.])) + np.transpose(cam_position1[:3]) x1, y1, z1 = lens1_position[:3] if output: print('rand_row, rand_col.......', rand_row, rand_col) print('label 1st point..........', point_label) print('x1, y1, z1..........', np.round(x1, 2), np.round(y1, 2), np.round(z1, 2)) print() print('=======================================================> 1st frame fully loaded') print() '''find 2nd camera position in proximity''' cam_distance = 100. loop_cam2_search = 0 while cam_distance > cam_distance_threshold: '''choose random and different 2nd point''' snapshot2_index = snapshot1_index while snapshot2_index == snapshot1_index: snapshot2_index = np.random.randint(num_frames) if whichTrees == 'debug': snapshot2_index = snapshot2_index_debug file_index2 = file_index(snapshot2_index) '''get 2nd cam position and quaternion''' cam_position2 = get_cam_position(snapshot2_index, random_tree_path) qx2 = cam_position2[3] qy2 = cam_position2[4] qz2 = cam_position2[5] if qz2 == 0.: qz2 = 1e-04 qw2 = cam_position2[6] quaternion_cam2 = Quaternion(qw2, qx2, qy2, qz2) angle2 = -2 * np.arccos(cam_position2[-1]) rotmat_z2 = build_rot_mat(angle2, 'z') lens2_position = np.dot(rotmat_z2, np.dot(rotmat_x, np.dot(rotmat_y, drone_cam_translation[:3,3])) * np.array([-1.,1., 1.])) + np.transpose(cam_position2[:3]) x2, y2, z2 = lens2_position[:3] '''check if relative distance under threshold''' cam_distance = dist_calc(np.array([x1, y1, z1]), np.array([x2, y2, z2])) if output: print('snapshot2_index..........', snapshot2_index) print('x2, y2, z2...............', np.round(x2, 2), np.round(y2, 2), np.round(z2, 2)) print('cam_distance.............', np.round(cam_distance, 2)) print(random_tree_path + file_index2 + 'pl.pfm') print() print('=======================================================> 2nd frame found') print() '''Compute transformtation''' angle_delta = angle2 - angle1 translation = np.transpose(lens2_position - lens1_position) '''compute sigma1 -> point coordinates''' cam1_2_point_distance = depthImg1[rand_row, rand_col] d1 = cam1_2_point_distance '''translate row, col to cam-coords''' u1, v1 = rand_col, 2 * cy - rand_row alpha1 = (u1 - cx) * d1 / fx beta1 = (v1 - cy) * d1 / fy gamma1 = d1 sigma1 = np.transpose(np.array([alpha1, beta1, gamma1, 1.])) sigma1_in_worldcoords = np.dot(rotmat_x, np.dot(rotmat_y, sigma1[:3])) * np.array([-1., 1., 1.]) # sigma1_in_worldcoords = np.dot(rotmat_x, np.dot(rotmat_y, sigma1[:3])) * np.array([1., 1., -1.]) # delte y rotation real_point = np.dot(rotmat_z1, sigma1_in_worldcoords) + np.transpose(lens1_position) lens2_2realpoint = real_point - lens2_position '''bis hier stimmts!!!!!!!!''' ######################################################################################################################################################## ######################################################################################################################################################## ######################################################################################################################################################## '''should be correct up to this point!!!''' ''' sigma_1 = point in camera1 coordinate system: x: horizontal, y: vertical, z: 'depth' ''' ''' translation = lens2_position - lens1_position ''' '''rotate pc1 (sigma1) around x by pi/2 ''' sigma1_x_rotated = np.dot(build_rot_mat(.5*np.pi, 'x'), sigma1[:3]) '''flip z-axis''' sigma1_x_rotated_flipped_z = np.transpose(np.array([sigma1_x_rotated[0], sigma1_x_rotated[1], -1*sigma1_x_rotated[2]])) inv_rotation_angle1 = np.linalg.inv(build_rot_mat(angle1, 'z')) Pw_notranslation = np.dot(inv_rotation_angle1, sigma1_x_rotated_flipped_z) Pw = Pw_notranslation - translation rotation_angle2 = build_rot_mat(angle2, 'z') PwCam2 = np.dot(rotation_angle2, Pw) PwCam2_flipped = np.array([PwCam2[0], PwCam2[1], -1*PwCam2[2]]) PwCam2_flipped_back_rotated = np.dot(np.linalg.inv(build_rot_mat(.5*np.pi, 'x')), PwCam2_flipped) print() print('sigma1_x_rotated') print(sigma1_x_rotated) print() print('sigma1_x_rotated_flipped_z') print(sigma1_x_rotated_flipped_z) print() print('inv_rotation_angle1') print(inv_rotation_angle1) print() print('Pw_notranslation') print(Pw_notranslation) print() print('Pw') print(Pw) print() print('PwCam2') print(PwCam2) print() print('PwCam2_flipped_back_rotated') print(PwCam2_flipped_back_rotated) rotated_translation = np.dot(build_rot_mat(1*(angle2), 'z'), translation) print('rotated_translation') print(rotated_translation) print('quaternion_cam2.rotate(translation)') print(quaternion_cam2.rotate(translation)) sigma2 = np.dot(build_rot_mat(1*(angle2 - angle1), 'y'), sigma1[:3])# - np.dot(build_rot_mat(angle2, 'z'), translation) sigma2 = quaternion_cam1.inverse.rotate(sigma1_in_worldcoords) # alpha2 = sigma2[0] + rotated_translation[1] # beta2 = sigma2[1] + rotated_translation[2] #always correct # gamma2 = sigma2[2] - rotated_translation[0] alpha2 = PwCam2_flipped_back_rotated[0] beta2 = PwCam2_flipped_back_rotated[1] gamma2 = PwCam2_flipped_back_rotated[2] d2 = gamma2 u2 = (alpha2 * fx / d2) + cx v2 = (beta2 * fy / d2) + cy col2 = u2 row2 = 2 * cy - v2 sigma22 = np.transpose(np.array([alpha2, beta2, gamma2, 1.])) sigma2_in_worldcoords = np.dot(rotmat_x, np.dot(rotmat_y, sigma22[:3])) * np.array([-1., 1., 1.]) real_point2 = np.dot(rotmat_z2, sigma2_in_worldcoords) + np.transpose(lens2_position) ptCam = sigma22[:3] # further processing # Compute bounding box in pixel coordinates bboxRange = np.array([ [ptCam[0]-voxelGridPatchRadius*voxelSize, ptCam[0]+voxelGridPatchRadius*voxelSize], [ptCam[1]-voxelGridPatchRadius*voxelSize, ptCam[1]+voxelGridPatchRadius*voxelSize], [ptCam[2]-voxelGridPatchRadius*voxelSize, ptCam[2]+voxelGridPatchRadius*voxelSize]]) bboxCorners = np.array([ [bboxRange[0,0],bboxRange[0,0],bboxRange[0,0],bboxRange[0,0],bboxRange[0,1],bboxRange[0,1],bboxRange[0,1],bboxRange[0,1]], [bboxRange[1,0],bboxRange[1,0],bboxRange[1,1],bboxRange[1,1],bboxRange[1,0],bboxRange[1,0],bboxRange[1,1],bboxRange[1,1]], [bboxRange[2,0],bboxRange[2,1],bboxRange[2,0],bboxRange[2,1],bboxRange[2,0],bboxRange[2,1],bboxRange[2,0],bboxRange[2,1]]]) bboxRange = np.reshape(bboxRange,(3,2)) bboxCorners = np.reshape(bboxCorners,(3,8)) p1_bboxCornersCam = bboxCorners if output: print('translation..............', np.round(translation, 2)) print('angle1...................', np.round(angle1, 2)) print('angle2...................', np.round(angle2, 2)) print('angle delta..............', np.round(angle_delta, 2)) print() print('u1, v1...................', u1, v1) print('sigma1...................', np.round(sigma1, 2)) print('sigma1_in_worldcoords....', np.round(sigma1_in_worldcoords, 2)) print('real_point...............', np.round(real_point, 2)) print() print('real_point2..............', np.round(real_point2, 2)) print() print('point dist...............', np.round(dist_calc(real_point, real_point2), 2)) print() print('alpha2, beta2, gamma2....', np.round(np.array([alpha2, beta2, gamma2]), 2)) print('sigma2...................', np.round(sigma2, 2)) print('sigma2_in_worldcoords....', np.round(sigma2_in_worldcoords, 2)) print('lens2_2realpoint.........', np.round(lens2_2realpoint, 2)) print('u2, v2...................', np.round(u2), np.round(v2)) print('row2, col2...............', np.round(row2), np.round(col2)) print() print('=======================================================> transformation done') print() if plots: '''load respective depth img''' depthImg2 = convert_pfm(random_tree_path + file_index2 + 'pl.pfm') depthImg2[depthImg2 > depth_limit] = 0 real_point_fromcam2 = np.dot(build_rot_mat(angle2, 'z'), sigma2_in_worldcoords) + np.transpose(lens2_position) test_mat1 = build_rot_mat(angle1, 'z') test_mat2 = build_rot_mat(angle2, 'z') # test_mat1 = build_trafo_mat(x1, y1, z1, angle1) # test_mat2 = build_trafo_mat(x2, y2, z2, angle2) optical_axis1 = np.dot(test_mat1[0:3,0:3],np.transpose(np.array([gamma1, 0., 0.]))) + np.transpose(lens1_position) optical_axis2 = np.dot(test_mat2[0:3,0:3],np.transpose(np.array([gamma2, 0., 0.]))) + np.transpose(lens2_position) optical_axis1_x = np.linspace(optical_axis1[0], lens1_position[0], 10) optical_axis1_y = np.linspace(optical_axis1[1], lens1_position[1], 10) optical_axis1_z = np.linspace(optical_axis1[2], lens1_position[2], 10) optical_axis2_x = np.linspace(optical_axis2[0], lens2_position[0], 10) optical_axis2_y = np.linspace(optical_axis2[1], lens2_position[1], 10) optical_axis2_z = np.linspace(optical_axis2[2], lens2_position[2], 10) point2cam1_x = np.linspace(real_point[0], lens1_position[0], 10) point2cam1_y = np.linspace(real_point[1], lens1_position[1], 10) point2cam1_z = np.linspace(real_point[2], lens1_position[2], 10) point2cam2_x = np.linspace(real_point[0], lens2_position[0], 10) point2cam2_y = np.linspace(real_point[1], lens2_position[1], 10) point2cam2_z = np.linspace(real_point[2], lens2_position[2], 10) plt.figure() plt.plot(data_table[:,1], data_table[:,2], c='g', label='drone flight') plt.scatter(x1, y1, c='r', label='cam_position1') plt.scatter(x2, y2, c='b', label='cam_position2') plt.scatter(x2, y2, c='b', label='cam_position2') plt.plot(optical_axis1_x, optical_axis1_y, c='k', label='center1') plt.plot(point2cam1_x, point2cam1_y, c='r', label='sight1') plt.plot(point2cam2_x, point2cam2_y, c='b', label='sight2') plt.plot(optical_axis2_x, optical_axis2_y, c='k', label='center2') plt.scatter(real_point[0], real_point[1], c='r', label='real_point1') plt.grid() plt.legend() fig = plt.figure() ax = fig.add_subplot(111, projection='3d') ax.scatter(x1, y1, z1, c='r', label='cam_position1') ax.scatter(x2, y2, z2, c='b', label='cam_position2') ax.scatter(bboxCorners[0,:] + real_point[0], bboxCorners[1,:] + real_point[1], bboxCorners[2,:] + real_point[2], c='g', label='corners') ax.plot(data_table[:,1], data_table[:,2], data_table[:,3], c='y', label='drone flight') # ax.plot(trans_vec_x, trans_vec_y, trans_vec_z, c='m', label='translation') ax.plot(optical_axis1_x, optical_axis1_y, optical_axis1_z, c='k', label='center1') ax.plot(point2cam1_x, point2cam1_y, point2cam1_z, c='r', label='sight1') ax.plot(point2cam2_x, point2cam2_y, point2cam2_z, c='b', label='sight2') ax.plot(optical_axis2_x, optical_axis2_y, optical_axis2_z, c='k', label='center2') ax.scatter(real_point[0], real_point[1], real_point[2], c='g', label='real_point1') ax.scatter(real_point2[0], real_point2[1], real_point2[2], c='b', label='real_point2') # ax.scatter(real_point_fromcam2[0], real_point_fromcam2[1], real_point_fromcam2[2], c='g', label='real_point2') for p in range(-60, 0): if p == -10: ax.scatter(mean_x, mean_y, p/10., c='k', label='tree') else: ax.scatter(mean_x, mean_y, p/10., c='k') ax.legend() ax.set_xlabel('X Label') ax.set_ylabel('Y Label') ax.set_zlabel('Z Label') # ax.set_xlim(-10, 10.) # ax.set_ylim(-10, 10.) # ax.set_zlim(-10., 0.) testimage1 = imageio.imread(random_tree_path + file_index1 + '.ppm') testimage2 = imageio.imread(random_tree_path + file_index2 + '.ppm') plt.figure(figsize=(10,10)) plt.subplot(221) plt.imshow(depthImg1) # plt.scatter(rand_col, rand_row, c="r") plt.scatter(rand_col, rand_row, 80, 'w', 'x') #plt.scatter(np.array(np.nonzero(depthImg1))[1,:],np.array(np.nonzero(depthImg1))[0,:]) plt.title('considered point:' + str(point_label)) plt.grid() plt.subplot(222) plt.imshow(depthImg2) plt.title('corresponding view point') plt.scatter(col2, row2, 80, 'w', 'x') # plt.scatter(u2, v2, 80, 'y', 'x') # plt.scatter(pixX, 2 * cy - pixY, 80, 'r', 'x') # plt.scatter(pixX, pixY, 80, 'r', 'x') plt.grid() plt.subplot(223) plt.imshow(testimage1) plt.title('image 1') plt.scatter(rand_col, rand_row, 80, 'r', 'x') plt.grid() plt.subplot(224) plt.imshow(testimage2) plt.title('image 2') plt.scatter(col2, row2, 80, 'r', 'x') # plt.scatter(u2, v2, 80, 'y', 'x') # plt.scatter(pixX, 2 * cy - pixY, 80, 'r', 'x') # plt.scatter(pixX, pixY, 80, 'r', 'x') plt.grid() plt.show() ################ match_found = 1 non_match_found = 1
inital = [ -0.0012867963980835793, -0.35302556809186536, 0.4085084665890285, -0.0498093375695251, -0.6912765989420319, 0.7208648458793617, 0.0030931571809757674 ] target = [ -0.003197180674706364, -0.37576393689660575, 0.45137435818768146, -0.0021967960033876664, -0.6684438218043409, 0.7437414872148852, -0.005160559496475987 ] target_q = Quat(np.roll(target[3:], 1)) steps = 300 p2 = np.zeros(3) p1 = target_q.rotate(np.array(inital[:3]) - target[:3]) traj = get_conical_helix_trajectory(p1, p2, steps, 3.0) traj = np.apply_along_axis(target_q.rotate, 1, traj) traj = traj + target[:3] rospy.init_node('path_node') path = Path() h = Header() h.stamp = rospy.Time.now() h.frame_id = "base_link" path.header = h for t in traj: p = PoseStamped()
def plotObject(self, name, startTime, y0, color=0, sampleRateMod=4, rotation=False, line=False, checkpoints=0, trim=False, boxsize=5): Xsamples = y0[0::self.sampleRate] if(trim): Xsamples = self.trimPositions(Xsamples,boxsize) colors = np.zeros((Xsamples.shape[0],3)) i = 0 while(i < colors.shape[0]): if(color == 0): colors[i,0] = i / colors.shape[0] colors[i,1] = 0 colors[i,2] = 0 elif(color == 1): colors[i,0] = 0 colors[i,1] = i / colors.shape[0] colors[i,2] = 0 elif(color == 2): colors[i,0] = 0 colors[i,1] = 0 colors[i,2] = i / colors.shape[0] i += 1 #plot the sampled position points with a color gradient #self.ax.scatter(Xsamples[:,0], Xsamples[:,1], Xsamples[:,2], #label='X, every {0} ticks'.format(self.sampleRate), color=colors) if(line): self.ax.plot(Xsamples[:,0], Xsamples[:,1], Xsamples[:,2], color=colors[colors.shape[0]-1]) else: self.ax.scatter(Xsamples[:,0], Xsamples[:,1], Xsamples[:,2], color=colors) if(checkpoints > 0): #pick out points to mark as checkpoints csampleRate = int(Xsamples.shape[0] / checkpoints) checkPointSamples = Xsamples[0::csampleRate] self.ax.scatter(checkPointSamples[:,0], checkPointSamples[:,1], checkPointSamples[:,2], color=(0.,0.,0.)) if(rotation): Z_line = np.ones((y0.shape[0],3)) Y_line = np.ones((y0.shape[0],3)) X_line = np.ones((y0.shape[0],3)) r = y0[:,3:7] i = 0 while(i < y0.shape[0]): tq = Quaternion(r[i]) trZ = tq.rotate(np.array([0.,0.,1.])) trY = tq.rotate(np.array([0.,1.,0.])) trX = tq.rotate(np.array([1.,0.,0.])) Z_line[i] = y0[i,0:3] + trZ Y_line[i] = y0[i,0:3] + trY X_line[i] = y0[i,0:3] + trX i += 1 f = X_line.shape[0] objectStartPosition = y0[0,0:3] lineStartPosition = X_line[0] axisStart = np.vstack((objectStartPosition,lineStartPosition)) objectEndPosition = y0[y0.shape[0]-1,0:3] lineEndPoint = X_line[f-1] axisEnd = np.vstack((objectEndPosition,lineEndPoint)) self.ax.plot(X_line[:,0], X_line[:,1], X_line[:,2], label='X+rot*[1,0,0] X', color=(1,0,0)) self.ax.scatter(X_line[0,0], X_line[0,1], X_line[0,2], color='r', marker='o') #circle = start self.ax.scatter(X_line[f-1:f,0], X_line[f-1:f,1], X_line[f-1:f,2], color='r', marker='^') #triangle = end self.ax.plot(axisStart[:,0],axisStart[:,1],axisStart[:,2], color = (1.,1.,.4)) #line between the first position and the first X,Y,or Z marker self.ax.plot(axisEnd[:,0],axisEnd[:,1],axisEnd[:,2], color = 'k') #line between last position and the last X,Y,or Z marker f = Y_line.shape[0] objectStartPosition = y0[0,0:3] lineStartPosition = Y_line[0] axisStart = np.vstack((objectStartPosition,lineStartPosition)) objectEndPosition = y0[y0.shape[0]-1,0:3] lineEndPoint = Y_line[f-1] axisEnd = np.vstack((objectEndPosition,lineEndPoint)) self.ax.plot(Y_line[:,0], Y_line[:,1], Y_line[:,2], label='X+rot*[0,1,0] Y', color=(0,1,0)) self.ax.scatter(Y_line[0,0], Y_line[0,1], Y_line[0,2], color='r', marker='o') #circle = start self.ax.scatter(Y_line[f-1:f,0], Y_line[f-1:f,1], Y_line[f-1:f,2], color='r', marker='^') #triangle = end self.ax.plot(axisStart[:,0],axisStart[:,1],axisStart[:,2], color = (1.,1.,.4)) #line between the first position and the first X,Y,or Z marker self.ax.plot(axisEnd[:,0],axisEnd[:,1],axisEnd[:,2], color = 'k') #line between last position and the last X,Y,or Z marker f = Z_line.shape[0] objectStartPosition = y0[0,0:3] lineStartPosition = Z_line[0] axisStart = np.vstack((objectStartPosition,lineStartPosition)) objectEndPosition = y0[y0.shape[0]-1,0:3] lineEndPoint = Z_line[f-1] axisEnd = np.vstack((objectEndPosition,lineEndPoint)) self.ax.plot(Z_line[:,0], Z_line[:,1], Z_line[:,2], label='X+rot*[0,0,1] Z', color=(0,0,1)) self.ax.scatter(Z_line[0,0], Z_line[0,1], Z_line[0,2], color='r', marker='o') #circle = start self.ax.scatter(Z_line[f-1:f,0], Z_line[f-1:f,1], Z_line[f-1:f,2], color='r', marker='^') #triangle = end self.ax.plot(axisStart[:,0],axisStart[:,1],axisStart[:,2], color = (1.,1.,.4)) #line between the first position and the first X,Y,or Z marker self.ax.plot(axisEnd[:,0],axisEnd[:,1],axisEnd[:,2], color = 'k')
def doublette(whichTrees): ############################ depth_limit = 8. cam_distance_threshold = 3.5 point_dist_threshold = 1.5 gamma_tolerance = .2 frame_size = voxelGridPatchRadius max_iter = 1 plots = True output = True match_found = 0 non_match_found = 0 first_img_found = 0 ############################ fx, fy, cx, cy = get_cam_intrinsics() drone_cam_translation = np.array([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 1., .46], [0., 0., 0., 1.]]) # print('fx, fy, cx, cy...............', fx, fy, cx, cy) print() print( 'finding initial camera position with visible tree..........................' ) print() while match_found < 1: gamma_loop = 0 frustum_loop = 0 bbox_loop = 0 first_img_found = 0 while first_img_found < 1: '''pick a random image number for snapshot1''' if whichTrees == 'random': random_tree_kind = np.random.randint(len(trees)) random_tree_kind_str = trees[random_tree_kind] random_tree = np.random.randint(num_tree_folders) random_tree_path = DataPath + random_tree_kind_str + '/' + random_tree_kind_str + '_High' + str( random_tree) + '/' if whichTrees == 'debug': random_tree_kind = 0 random_tree_kind_str = trees[random_tree_kind] random_tree = random_tree_debug random_tree_path = DataPath + random_tree_kind_str + '/' + random_tree_kind_str + '_High' + str( random_tree) + '/' if whichTrees == 'debug_tejaswi': random_tree_kind = 0 random_tree_kind_str = trees[random_tree_kind] random_tree = 1 random_tree_path = DataPath + random_tree_kind_str + '/' + random_tree_kind_str + '_High' + str( random_tree) + '/' if whichTrees == 'Cherry': random_tree_kind = 0 random_tree_kind_str = trees[random_tree_kind] random_tree = np.random.randint(1, num_tree_folders) random_tree_path = DataPath + random_tree_kind_str + '/' + random_tree_kind_str + '_High' + str( random_tree) + '/' if whichTrees == 'KoreanStewartia': random_tree_kind = 1 random_tree_kind_str = trees[random_tree_kind] random_tree = np.random.randint(1, num_tree_folders) random_tree_path = DataPath + random_tree_kind_str + '/' + random_tree_kind_str + '_High' + str( random_tree) + '/' '''find total number of frames in respective folder''' data_table = np.asarray( pd.read_table(random_tree_path + 'poses.txt', sep='\s', header=0, index_col=False, engine='python')) num_frames = np.max(data_table[:, 0]) abc = np.squeeze(np.array(np.where(data_table[:, 7] == 1))) mean_x = np.mean(data_table[abc[0]:abc[-1], 1]) mean_y = np.mean(data_table[abc[0]:abc[-1], 2]) drone_trajectory_z = data_table[:, 3] * -1 drone_trajectory_x = (data_table[:, 1] - mean_x) drone_trajectory_y = (data_table[:, 2] - mean_y) drone_trajectory = np.array( [drone_trajectory_x, drone_trajectory_y, drone_trajectory_z]) drone_trajectory_x = drone_trajectory[0, :] drone_trajectory_y = drone_trajectory[1, :] drone_trajectory_z = drone_trajectory[2, :] '''find random initial frame''' snapshot1_index = np.random.randint(num_frames) if whichTrees == 'debug': snapshot1_index = snapshot1_index_debug '''load respective depth img''' file_index1 = file_index(snapshot1_index) depthImg1_raw = convert_pfm(random_tree_path + file_index1 + 'pl.pfm') '''delete too far away points''' depthImg1 = depthImg1_raw depthImg1[depthImg1_raw > depth_limit] = 0 '''check if there is a tree''' if np.array( np.nonzero( depthImg1[frame_size:-frame_size, frame_size:-frame_size])).shape[1] > 1000: first_img_found = 1 '''choose random point on tree''' rand_row, rand_col = 0, 0 while np.absolute(cy - rand_row) > ( cy - frame_size) or np.absolute(cx - rand_col) > ( cx - frame_size): random_spot1 = np.random.randint( 0, np.array(np.nonzero(depthImg1)).shape[1]) rand_row, rand_col = np.array( np.nonzero(depthImg1))[:, random_spot1] if whichTrees == 'debug': rand_row, rand_col = rand_row_debug, rand_col_debug '''get 1st cam position and quaternions''' cam_position1 = get_cam_position(snapshot1_index, random_tree_path) qx1 = cam_position1[3] qy1 = cam_position1[4] qz1 = cam_position1[5] if qz1 == 0.: qz1 = 1e-04 qw1 = cam_position1[6] quaternion_cam1 = Quaternion(qw1, qx1, qy1, qz1) x1, y1, z1 = cam_position1[:3] x1 = x1 - mean_x y1 = y1 - mean_y z1 = -1 * z1 drone1position = np.transpose(np.array([x1, y1, z1])) lens1position = drone1position + quaternion_cam1.rotate( np.array([drone_cam_translation[2, 3], 0, 0])) match_loop = 0 if gamma_loop > max_iter or frustum_loop > max_iter or bbox_loop > max_iter: break while match_found < 1: if gamma_loop > max_iter or frustum_loop > max_iter or bbox_loop > max_iter: break '''find 2nd camera position in proximity''' cam_distance = 100. match_loop += 1 while cam_distance > cam_distance_threshold: '''choose random and different 2nd point''' snapshot2_index = snapshot1_index while snapshot2_index == snapshot1_index: snapshot2_index = np.random.randint(num_frames) if whichTrees == 'debug': snapshot2_index = snapshot2_index_debug file_index2 = file_index(snapshot2_index) '''get 2nd cam position and quaternion''' cam_position2 = get_cam_position(snapshot2_index, random_tree_path) qx2 = cam_position2[3] qy2 = cam_position2[4] qz2 = cam_position2[5] if qz2 == 0.: qz2 = 1e-04 qw2 = cam_position2[6] quaternion_cam2 = Quaternion(qw2, qx2, qy2, qz2) x2, y2, z2 = cam_position2[:3] x2 = x2 - mean_x y2 = y2 - mean_y z2 = -1 * z2 drone2position = np.transpose(np.array([x2, y2, z2])) lens2position = drone2position + quaternion_cam2.rotate( np.array([drone_cam_translation[2, 3], 0, 0])) '''load respective depth img''' depthImg2_raw = convert_pfm(random_tree_path + file_index2 + 'pl.pfm') depthImg2 = depthImg2_raw depthImg2[depthImg2_raw > depth_limit] = 0 # '''load the segmentation''' # segmentation2 = imageio.imread(random_tree_path + file_index2 + 'seg.ppm') # segmentation2 = np.round(np.mean(segmentation2, 2) / np.max(np.asarray(np.reshape(np.mean(segmentation2,2), (-1)))), 1) '''check if relative distance under threshold''' cam_distance = dist_calc(lens1position, lens2position) if gamma_loop > max_iter or frustum_loop > max_iter or bbox_loop > max_iter: break '''Compute transformtation''' translation = np.transpose( np.array([x1, y1, z1]) - np.array([x2, y2, z2])) '''compute sigma1 -> point coordinates''' cam1_2_point_distance = depthImg1[rand_row, rand_col] d1 = cam1_2_point_distance '''translate row, col to cam-coords''' u1, v1 = rand_col, 2 * cy - rand_row alpha1 = (u1 - cx) * d1 / fx beta1 = (v1 - cy) * d1 / fy gamma1 = d1 sigma1 = np.transpose(np.array([alpha1, beta1, gamma1, 1.])) PointRealWorld = drone1position + quaternion_cam1.rotate( np.array([gamma1 + .46, -alpha1, beta1])) # lens2point1 = PointRealWorld - lens1position # lens2point2 = PointRealWorld - lens2position lens2point1 = PointRealWorld - drone1position lens2point2 = PointRealWorld - drone2position lens2point1_vector = np.array( [[lens1position[0], PointRealWorld[0]], [lens1position[1], PointRealWorld[1]], [lens1position[2], PointRealWorld[2]]]) lens2point2_vector = np.array( [[lens2position[0], PointRealWorld[0]], [lens2position[1], PointRealWorld[1]], [lens2position[2], PointRealWorld[2]]]) newSigma = quaternion_cam2.inverse.rotate(lens2point2) alpha2 = -1 * newSigma[1] beta2 = newSigma[2] gamma2 = newSigma[0] - .46 sigma2 = np.transpose(np.array([alpha2, beta2, gamma2, 1.])) PointRealWorld2 = lens2position + quaternion_cam2.rotate( np.array([gamma2, -alpha2, beta2])) d2 = gamma2 u2 = (alpha2 * fx / d2) + cx v2 = (beta2 * fy / d2) + cy if np.abs(alpha2 * fx / d2) + frame_size > cx or np.abs( beta2 * fy / d2) + frame_size > cy: print('error2: not in camera frustum') frustum_loop += 1 continue # if np.abs(alpha2 * fx / d2) + frame_size < cx and np.abs(beta2 * fy / d2) + frame_size < cy: # match_found = 1 col2 = int(np.round(u2)) row2 = int(np.round(2 * cy - v2)) # point_label2 = segmentation2[row2, col2] if np.abs(depthImg2[row2, col2] - gamma2) > gamma_tolerance: gamma_loop += 1 print('error: depth does not match gamma') continue p1_bboxCornersCam = bbox_corners(sigma1[:3]) p2_bboxCornersCam = bbox_corners(sigma2[:3]) p1_bboxCorners_test, p2_bboxCorners_test = [], [] for j in range(8): p1_bboxCorners_test.append( lens1position + quaternion_cam1.rotate( np.array([ p1_bboxCornersCam[2, j], -p1_bboxCornersCam[0, j], p1_bboxCornersCam[1, j] ]))) p2_bboxCorners_test.append( lens2position + quaternion_cam2.rotate( np.array([ p2_bboxCornersCam[2, j], -p2_bboxCornersCam[0, j], p2_bboxCornersCam[1, j] ]))) p1_bboxCorners_test = np.asarray(p1_bboxCorners_test) p2_bboxCorners_test = np.asarray(p2_bboxCorners_test) u_BBox1 = np.round((p1_bboxCornersCam[0, :] * fx / p1_bboxCornersCam[2, :]) + cx) v_BBox1 = 2 * cy - np.round( (p1_bboxCornersCam[1, :] * fy / p1_bboxCornersCam[2, :]) + cy) bboxPixX1 = np.array([ np.int(u1 - max(np.abs(u1 - u_BBox1))), np.int(u1 + max(np.abs(u1 - u_BBox1))) ]) # bboxPixY1 = np.array([v1-max(np.abs(v1-v_BBox1)), v1+max(np.abs(v1-v_BBox1))]) bboxPixY1 = np.array([ np.int(rand_row - max(np.abs(rand_row - v_BBox1))), np.int(rand_row + max(np.abs(rand_row - v_BBox1))) ]) u_BBox2 = np.round((p2_bboxCornersCam[0, :] * fx / p2_bboxCornersCam[2, :]) + cx) v_BBox2 = 2 * cy - np.round( (p2_bboxCornersCam[1, :] * fy / p2_bboxCornersCam[2, :]) + cy) bboxPixX2 = np.array([ np.int(u2 - max(np.abs(u2 - u_BBox2))), np.int(u2 + max(np.abs(u2 - u_BBox2))) ]) # bboxPixY2 = np.array([v2-max(np.abs(v2-v_BBox2)), v2+max(np.abs(v2-v_BBox2))]) bboxPixY2 = np.array([ np.int(row2 - max(np.abs(row2 - v_BBox2))), np.int(row2 + max(np.abs(row2 - v_BBox2))) ]) if np.any(bboxPixX1 <= 0) or np.any(bboxPixX1 > 2 * cx) or np.any( bboxPixY1 <= 0) or np.any(bboxPixY1 > 2 * cy): match_found = 0 bbox_loop += 1 print('error1: not in bbox frustum') continue if np.any(bboxPixX2 <= 0) or np.any(bboxPixX2 > 2 * cx) or np.any( bboxPixY2 <= 0) or np.any(bboxPixY2 > 2 * cy): print('error2: not in bbox frustum') match_found = 0 bbox_loop += 1 continue p1_bboxRangePixels = np.array([[bboxPixX1], [bboxPixY1]]) p2_bboxRangePixels = np.array([[bboxPixX2], [bboxPixY2]]) match_found = 1 if gamma_loop > max_iter or frustum_loop > max_iter or bbox_loop > max_iter: break if gamma_loop > max_iter or frustum_loop > max_iter or bbox_loop > max_iter: continue '''load the segmentation''' segmentation1 = imageio.imread(random_tree_path + file_index1 + 'seg.ppm') segmentation1 = np.round( np.mean(segmentation1, 2) / np.max(np.asarray(np.reshape(np.mean(segmentation1, 2), (-1)))), 1) segmentation2 = imageio.imread(random_tree_path + file_index2 + 'seg.ppm') segmentation2 = np.round( np.mean(segmentation2, 2) / np.max(np.asarray(np.reshape(np.mean(segmentation2, 2), (-1)))), 1) '''label point''' point_label1 = segmentation1[rand_row, rand_col] point_label2 = segmentation2[row2, col2] p1 = { 'bboxCornersCam': p1_bboxCornersCam, 'bboxRangePixels': p1_bboxRangePixels, 'framePath': random_tree_path + file_index1, 'pixelCoords': np.array([rand_col, rand_row]), 'camCoords': sigma1[:3], 'label': point_label1, 'depthIm': depthImg1_raw } p2 = { 'bboxCornersCam': p2_bboxCornersCam, 'bboxRangePixels': p2_bboxRangePixels, 'framePath': random_tree_path + file_index2, 'pixelCoords': np.array([col2, row2]), 'camCoords': sigma2[:3], 'label': point_label2, 'depthIm': depthImg2_raw } while non_match_found < 1: point2point_dist = 0. while point2point_dist < point_dist_threshold: '''find random initial frame''' snapshot3_index = np.random.randint(num_frames) '''load respective depth img''' file_index3 = file_index(snapshot3_index) depthImg3_raw = convert_pfm(random_tree_path + file_index3 + 'pl.pfm') depthImg3 = depthImg3_raw '''delete too far away points''' depthImg3[depthImg3_raw > depth_limit] = 0 '''check if there is a tree''' if np.array( np.nonzero( depthImg3[frame_size:-frame_size, frame_size:-frame_size])).shape[1] > 10: random_spot3 = np.random.randint( 0, np.array( np.nonzero( depthImg3[frame_size:-frame_size, frame_size:-frame_size])).shape[1]) row3, col3 = np.array(np.nonzero(depthImg3))[:, random_spot3] '''get 1st cam position and quaternions''' cam_position3 = get_cam_position(snapshot3_index, random_tree_path) qx3 = cam_position3[3] qy3 = cam_position3[4] qz3 = cam_position3[5] if qz3 == 0.: qz3 = 1e-04 qw3 = cam_position3[6] quaternion_cam3 = Quaternion(qw3, qx3, qy3, qz3) x3, y3, z3 = cam_position3[:3] x3 = x3 - mean_x y3 = y3 - mean_y z3 = -1 * z3 drone3position = np.transpose(np.array([x3, y3, z3])) lens3position = drone3position + quaternion_cam3.rotate( np.array([drone_cam_translation[2, 3], 0, 0])) d3 = depthImg3[row3, col3] '''translate row, col to cam-coords''' u3, v3 = col3, 2 * cy - row3 alpha3 = (u3 - cx) * d3 / fx beta3 = (v3 - cy) * d3 / fy gamma3 = d3 sigma3 = np.transpose(np.array([alpha3, beta3, gamma3, 1.])) PointRealWorld3 = drone3position + quaternion_cam3.rotate( np.array([gamma3 + .46, -alpha3, beta3])) lens2point3 = PointRealWorld3 - drone3position lens2point3_vector = np.array( [[lens3position[0], PointRealWorld3[0]], [lens3position[1], PointRealWorld3[1]], [lens3position[2], PointRealWorld3[2]]]) point2point_dist = dist_calc(PointRealWorld3, PointRealWorld) p3_bboxCornersCam = bbox_corners(sigma3[:3]) p3_bboxCorners_test = [] for j in range(8): p3_bboxCorners_test.append( lens3position + quaternion_cam3.rotate( np.array([ p3_bboxCornersCam[2, j], -p3_bboxCornersCam[0, j], p3_bboxCornersCam[1, j] ]))) p3_bboxCorners_test = np.asarray(p3_bboxCorners_test) u_BBox3 = np.round((p3_bboxCornersCam[0, :] * fx / p3_bboxCornersCam[2, :]) + cx) v_BBox3 = 2 * cy - np.round((p3_bboxCornersCam[1, :] * fy / p3_bboxCornersCam[2, :]) + cy) bboxPixX3 = np.array([ col3 - max(np.abs(col3 - u_BBox3)), col3 + max(np.abs(col3 - u_BBox3)) ]) bboxPixY3 = np.array([ row3 - max(np.abs(row3 - v_BBox3)), row3 + max(np.abs(row3 - v_BBox3)) ]) p3_bboxRangePixels = np.array([[bboxPixX3], [bboxPixY3]]) lowBoundX = int(np.min(p3_bboxRangePixels)) highBoundX = int(np.max(p3_bboxRangePixels)) + 1 lowBoundY = int(np.min(p3_bboxRangePixels)) highBoundY = int(np.max(p3_bboxRangePixels)) + 1 if lowBoundX < 1 or lowBoundY < 1 or highBoundX > 2 * cx or highBoundY > 2 * cy: match_found = 0 print('error: bounds not in frame') continue if np.any(bboxPixX3 <= 0) or np.any( bboxPixX3 > 2 * cx) or np.any( bboxPixY3 <= 0) or np.any(bboxPixY3 > 2 * cy): match_found = 0 print('error: point 3 not in bbox frustum') continue else: non_match_found = 1 print('3rd one fully loaded!') '''load the segmentation''' segmentation3 = imageio.imread(random_tree_path + file_index3 + 'seg.ppm') segmentation3 = np.round( np.mean(segmentation3, 2) / np.max(np.asarray(np.reshape(np.mean(segmentation3, 2), (-1)))), 1) p3_bboxCorners_test = [] for j in range(8): p3_bboxCorners_test.append(lens3position + quaternion_cam3.rotate( np.array([ p3_bboxCornersCam[2, j], -p3_bboxCornersCam[0, j], p3_bboxCornersCam[1, j] ]))) p3_bboxCorners_test = np.asarray(p3_bboxCorners_test) '''label point''' point_label3 = segmentation3[row3, col3] p3 = { 'bboxCornersCam': p3_bboxCornersCam, 'bboxRangePixels': p3_bboxRangePixels, 'framePath': random_tree_path + file_index3, 'pixelCoords': np.array([col3, row3]), 'camCoords': sigma3[:3], 'label': point_label3, 'depthIm': depthImg3_raw } p1_voxelGridTDF = getPatchData(p1, voxelGridPatchRadius, voxelSize, voxelMargin) print() print('1st TDF ==========> success!') print(p1_voxelGridTDF.ndim) # p2_voxelGridTDF = getPatchData(p2,voxelGridPatchRadius,voxelSize,voxelMargin) # print() # print('2nd TDF ==========> success!') # p3_voxelGridTDF = getPatchData(p3,voxelGridPatchRadius,voxelSize,voxelMargin) # print() # print('3rd TDF ==========> success!') if output: # print('num_frames...............', num_frames) # print('random_tree_kind.........', random_tree_kind) # print('random_tree_kind_str.....', random_tree_kind_str) # print('random_tree..............', random_tree) # print('random_tree_path.........', random_tree_path) # print('index1...................', snapshot1_index) print(random_tree_path + file_index1 + 'pl.pfm') # print('rand_row, rand_col.......', rand_row, rand_col) # print('label 1st point..........', point_label1) print('lens1position............', np.round(lens1position, 2)) print() print( '=======================================================> initial frame found' ) print() print(random_tree_path + file_index2 + 'pl.pfm') print('lens2position............', np.round(lens2position, 2)) print('cam_distance.............', np.round(cam_distance, 2)) print() print( '================================================> 2nd frame found' ) print() print(random_tree_path + file_index1 + 'pl.pfm') print() print('col1, row1...............', rand_col, rand_row) print('col2, row2...............', col2, row2) print('col3, row3...............', col3, row3) print() print('sigma1:', np.round(sigma1[:3], 1)) print('sigma2:', np.round(sigma2[:3], 1)) print('sigma3:', np.round(sigma3[:3], 1)) print() print('PointRealWorld1') print(np.round(PointRealWorld, 1)) print('PointRealWorld2') print(np.round(PointRealWorld2, 1)) print('PointRealWorld3') print(np.round(PointRealWorld3, 1)) # print('num_frames...............', num_frames) # print('random_tree_kind.........', random_tree_kind) # print('random_tree_kind_str.....', random_tree_kind_str) # print('random_tree..............', random_tree) # print('random_tree_path.........', random_tree_path) # print('index1...................', snapshot1_index) print() print( '=======================================================> third frame found' ) print() if plots: # '''load respective depth img''' # depthImg2 = convert_pfm(random_tree_path + file_index2 + 'pl.pfm') # depthImg2[depthImg2 > depth_limit] = 0 print('depth-------', depthImg2[row2, col2]) fig = plt.figure() ax = fig.add_subplot(111, projection='3d') ax.scatter(x1, y1, z1, c='b') ax.scatter(lens1position[0], lens1position[1], lens1position[2], c='b', label='inital cam') ax.scatter(x2, y2, z2, c='g') ax.scatter(lens2position[0], lens2position[1], lens2position[2], c='g', label='match cam') ax.scatter(x3, y3, z3, c='r') ax.scatter(lens3position[0], lens3position[1], lens3position[2], c='r', label='non-match cam') ax.scatter(PointRealWorld[0], PointRealWorld[1], PointRealWorld[2], c='b', s=10, label='point1') ax.scatter(PointRealWorld2[0], PointRealWorld2[1], PointRealWorld2[2], c='g', s=10, label='point2') ax.scatter(PointRealWorld3[0], PointRealWorld3[1], PointRealWorld3[2], c='r', s=10, label='point3') ax.scatter(p1_bboxCorners_test[:, 0], p1_bboxCorners_test[:, 1], p1_bboxCorners_test[:, 2], c='b', s=10, label='bbox1') ax.scatter(p2_bboxCorners_test[:, 0], p2_bboxCorners_test[:, 1], p2_bboxCorners_test[:, 2], c='g', s=10, label='bbox2') ax.scatter(p3_bboxCorners_test[:, 0], p3_bboxCorners_test[:, 1], p3_bboxCorners_test[:, 2], c='r', s=10, label='bbox3') ax.plot(lens2point1_vector[0], lens2point1_vector[1], lens2point1_vector[2], c='b', label='sight1') ax.plot(lens2point2_vector[0], lens2point2_vector[1], lens2point2_vector[2], c='g', label='sight2') ax.plot(lens2point3_vector[0], lens2point3_vector[1], lens2point3_vector[2], c='r', label='sight3') ax.plot(drone_trajectory_x, drone_trajectory_y, drone_trajectory_z, c='y', label='drone flight') ax.plot(np.zeros(2), np.zeros(2), np.linspace(0, 8, 2), c='k', label='tree') ax.legend() ax.set_xlabel('X Label') ax.set_ylabel('Y Label') ax.set_zlabel('Z Label') ax.set_xlim(-10, 10) ax.set_ylim(-10, 10) ax.set_zlim(0, 10) testimage1 = imageio.imread(random_tree_path + file_index1 + '.ppm') print('-------------------------------------------') print('-------------------------------------------') print('-------------------------------------------') print('-------------------------------------------') print('-------------------------------------------') print('-------------------------------------------') print('-------------------------------------------') print(testimage1.shape) print('-------------------------------------------') print('-------------------------------------------') print('-------------------------------------------') print('-------------------------------------------') print('-------------------------------------------') print('-------------------------------------------') print('-------------------------------------------') print('-------------------------------------------') print('-------------------------------------------') testimage2 = imageio.imread(random_tree_path + file_index2 + '.ppm') testimage3 = imageio.imread(random_tree_path + file_index3 + '.ppm') plt.figure(figsize=(10, 10)) plt.subplot(231) plt.imshow(depthImg1) # plt.scatter(rand_col, rand_row, c="r") plt.scatter(rand_col, rand_row, 80, 'w', 'x') #plt.scatter(np.array(np.nonzero(depthImg1))[1,:],np.array(np.nonzero(depthImg1))[0,:]) plt.title('considered point:' + str(point_label1)) plt.grid() plt.subplot(232) plt.imshow(depthImg2) plt.title('corresponding view point') plt.scatter(col2, row2, 80, 'w', 'x') # # plt.scatter(u2, v2, 80, 'y', 'x') # # plt.scatter(pixX, 2 * cy - pixY, 80, 'r', 'x') # # plt.scatter(pixX, pixY, 80, 'r', 'x') plt.grid() plt.subplot(233) plt.imshow(depthImg3) plt.title('non-match') plt.scatter(col3, row3, 80, 'w', 'x') # # plt.scatter(u2, v2, 80, 'y', 'x') # # plt.scatter(pixX, 2 * cy - pixY, 80, 'r', 'x') # # plt.scatter(pixX, pixY, 80, 'r', 'x') plt.grid() plt.subplot(234) plt.imshow(testimage1) plt.title('image 1') plt.scatter(rand_col, rand_row, 80, 'r', 'x') plt.grid() plt.subplot(235) plt.imshow(testimage2) plt.title('image 2') plt.scatter(col2, row2, 80, 'r', 'x') # # plt.scatter(u2, v2, 80, 'y', 'x') # # plt.scatter(pixX, 2 * cy - pixY, 80, 'r', 'x') # # plt.scatter(pixX, pixY, 80, 'r', 'x') plt.grid() plt.subplot(236) plt.imshow(testimage3) plt.title('image 2') plt.scatter(col3, row3, 80, 'r', 'x') # # plt.scatter(u2, v2, 80, 'y', 'x') # # plt.scatter(pixX, 2 * cy - pixY, 80, 'r', 'x') # # plt.scatter(pixX, pixY, 80, 'r', 'x') plt.grid() plt.show() return p1_voxelGridTDF #, p2_voxelGridTDF, p3_voxelGridTDF
class CoordinateSystems: def __init__(self): #World Coordinate system vectors self.ball_world = None #Position, Velocities in world frame self.car_world = None #Position, Velocities in world frame self.q_car_to_world = None #Unit Quaternion to change car frame to world frame self.R_car_to_world = None #Rigid Transformation matrix to change car frame to world frame #Car Coordinate system vectors self.ball_car = None #Position, Velocities to ball in car coordinates self.q_car_to_ball = None #Unit Quaternion pointing to ball from car frame self.q_world_to_car = None #Unit Quaternion to change from world frame to car frame self.R_world_to_car = None #Rigid Transformation matrix to change world frame to car frame self.heading = None def update(self, car, ball): #Rcar_to_world r = -1*car.roll #rotation around roll axis to get car to world frame p = -1*car.pitch #rotation around pitch axis to get car to world frame y = car.yaw #rotation about the world z axis to get the car to the world frame self.Rx = np.matrix([[1, 0, 0], [0, math.cos(r), -1*math.sin(r)], [0, math.sin(r), math.cos(r)]]) self.Ry = np.matrix([[math.cos(p), 0, math.sin(p)], [0, 1, 0], [-1*math.sin(p), 0, math.cos(p)]]) self.Rz = np.matrix([[math.cos(y), -1*math.sin(y), 0], [math.sin(y), math.cos(y), 0], [0, 0, 1]]) #Order of rotations from car to world is z then y then x self.Rinter = np.matmul(self.Rz, self.Ry) self.Rcar_to_world = np.matmul(self.Rinter, self.Rx) #Rworld_to_car r = car.roll #rotation around roll axis to get world to car frame p = car.pitch #rotation around pitch axis to get world to car frame y = -1*car.yaw #rotation about the world z axis to get world to car frame self.Rx = np.matrix([[1, 0, 0], [0, math.cos(r), -1*math.sin(r)], [0, math.sin(r), math.cos(r)]]) self.Ry = np.matrix([[math.cos(p), 0, math.sin(p)], [0, 1, 0], [-1*math.sin(p), 0, math.cos(p)]]) self.Rz = np.matrix([[math.cos(y), -1*math.sin(y), 0], [math.sin(y), math.cos(y), 0], [0, 0, 1]]) #Order of rotations from world to car is x then y then z self.Rinter = np.matmul(self.Rx, self.Ry) self.Rworld_to_car = np.matmul(self.Rinter, self.Rz) #Rworld_to_ball ux = np.array([1.,0.,0.]) uy = np.array([0.,1.,0.]) uz = np.array([0.,0.,1.]) Pb = np.array([ball.x, ball.y, ball.z]) Pc = np.array([car.x, car.y, car.z]) #negate z because z axis for car is pointed downwards Pbc = np.subtract(Pb, Pc) #Get vector to ball from car in car coordinates xyz = np.cross(ux, Pbc) #xyz of quaternion is rotation between Pbc and unit x vector w = math.sqrt(1 * ((Pbc.item(0) ** 2) + (Pbc.item(1) ** 2) + (Pbc.item(2) ** 2))) + np.dot(ux, Pbc) #scalr of quaternion qbcworld = Quaternion(w = w, x = xyz.item(0), y = xyz.item(1), z = xyz.item(2)) #Quaternions for rotations between frames self.Qcar_to_world = Quaternion(matrix = self.Rcar_to_world).normalised.normalised self.Qworld_to_car = Quaternion(matrix = self.Rworld_to_car).normalised.normalised self.Qworld_to_ball = Quaternion(matrix = qbcworld.rotation_matrix).normalised.normalised #Random rotation to try to point to r = 1 p = 0 y = 1 self.Rx = np.matrix([[1, 0, 0], [0, math.cos(r), -1*math.sin(r)], [0, math.sin(r), math.cos(r)]]) self.Ry = np.matrix([[math.cos(p), 0, math.sin(p)], [0, 1, 0], [-1*math.sin(p), 0, math.cos(p)]]) self.Rz = np.matrix([[math.cos(y), -1*math.sin(y), 0], [math.sin(y), math.cos(y), 0], [0, 0, 1]]) #Order of rotations from world to car is x then y then z self.Rinter = np.matmul(self.Rx, self.Ry) self.Rworld_to_point = np.matmul(self.Rinter, self.Rz) self.Qworld_to_point = Quaternion(matrix = self.Rworld_to_point).normalised.normalised # print('r', r, 'p', p, 'y', y, 'q', self.Qtocar) ux_world = np.array([1.,0.,0.]) uy_world = np.array([0.,1.,0.]) uz_world = np.array([0.,0.,1.]) self.ux_world = np.array([1.,0.,0.]) self.uy_world = np.array([0.,1.,0.]) self.uz_world = np.array([0.,0.,1.]) self.headingx = self.Qworld_to_car.normalised.rotate(ux_world) self.headingy = self.Qworld_to_car.normalised.rotate(uy_world) self.headingz = self.Qworld_to_car.normalised.rotate(uz_world) #get point vectors for car and ball Pb_world = np.array([ball.x, ball.y, ball.z]) #multiply by negative one to keep axis orientations consistent between car and world Pc_world = np.array([car.x, car.y, car.z]) self.Pbc_world = np.subtract(Pb_world, Pc_world) #Get vector to ball from car in world coordinate system but origin at car center self.Pbc_world_normalised = self.Pbc_world/np.linalg.norm(self.Pbc_world, axis = 0) self.Pbc_world_magnitude = np.linalg.norm(self.Pbc_world, axis=0) xyz = np.cross(self.headingx, self.Pbc_world/np.linalg.norm(self.Pbc_world, axis=0)) #xyz of quaternion is rotation between the UNIT Pbc vector and normalised x vector w = math.sqrt(1 * ((self.Pbc_world.item(0) ** 2) + (self.Pbc_world.item(1) ** 2) + (self.Pbc_world.item(2) ** 2))) + np.dot(ux_world, self.Pbc_world) #scalr of quaternion self.qbcworld = Quaternion(w = w, x = xyz.item(0), y = xyz.item(1), z = xyz.item(2)) # eulerAngles = toEulerAngle(qbcworld.normalised) #convert quaternion to euler angles # print("Pbc", self.Pbc) #Get quaternion pointing to ball in car coordinates self.Pbc_car = self.Qworld_to_car.normalised.rotate(self.Pbc_world) self.P0_world = np.array([1,0,0]) self.P0_car = self.Qworld_to_car.normalised.rotate(self.P0_world) self.P1_car = np.array([1,0,0]) self.P1_world = self.Qcar_to_world.normalised.rotate(self.P1_car) vector = self.Qcar_to_world.rotate(self.Pbc_car) # print("Pbc_world", self.Pbc_world, 'Pbc_car', self.Pbc_car, 'Pbc_world2', vector, 'r:', car.roll, 'p:', car.pitch, 'y:', car.yaw) #Rotational Velocity conversion from world to car coordinates wx_world = np.array([car.wx, 0, 0]) wy_world = np.array([0, car.wy, 0]) wz_world = np.array([0, 0, car.wz]) self.wx_car = self.toCarCoordinates(wx_world) self.wy_car = self.toCarCoordinates(wy_world) self.wz_car = self.toCarCoordinates(wz_world) self.w_car = self.wx_car + self.wy_car + self.wz_car # print('wx_car:', self.wx_car, 'wy_car', self.wy_car, 'wz_car', self.wz_car) def getDesiredQuaternion(self, vector):#vector is the 3d point vector to where we want the car to face in world coordinates / attitude is the current attitude of the car [roll, pitch, yaw] #Get quaternion that rotates world coordinates to car Coordinates xyz = np.cross(self.ux_world, vector/np.linalg.norm(vector, axis=0)) #xyz of quaternion is rotation between the UNIT Pbc vector and normalised x vector w = math.sqrt(1 * ((vector.item(0) ** 2) + (vector.item(1) ** 2) + (vector.item(2) ** 2))) + np.dot(self.ux_world, vector) #scalr of quaternion desired = Quaternion(w = w, x = xyz.item(0), y = xyz.item(1), z = xyz.item(2)) return desired #Convert vector to Car Coordinates #Convert new vector in car coordinates to a quaternion def getVectorToBall_world(self): vector = self.Qcar_to_world.rotate(self.Pbc_car) return vector def toWorldCoordinates(self, vector): vec = self.Qcar_to_world.rotate(vector) return vec def toCarCoordinates(self, vector): #vector is the 3d point vector to rotate to car coordintaes #get world to car quaternion vec = self.Qworld_to_car.rotate(vector) return vec def createQuaternion_world_at_car(self, point): #convert point into quaternion #Rworld_to_ball ux = np.array([1.,0.,0.]) uy = np.array([0.,1.,0.]) uz = np.array([0.,0.,1.]) Ppoint = np.array([point.item(0), point.item(1), point.item(2)]) #Pcar = np.array([car.item(0), car.item(1), car.item(2)]) #negate z because z axis for car is pointed downwards P = Ppoint #np.subtract(Ppoint, Pcar) #Get vector to ball from car in car coordinates xyz = np.cross(ux, P) #xyz of quaternion is rotation between Pbc and unit x vector w = math.sqrt(1 * ((P.item(0) ** 2) + (P.item(1) ** 2) + (P.item(2) ** 2))) + np.dot(ux, P) #scalr of quaternion Qworld_to_point = Quaternion(w = w, x = xyz.item(0), y = xyz.item(1), z = xyz.item(2)).normalised #return quaternion return Qworld_to_point
Pcl_ = pcl.PointCloud() data = np.empty((0, 3), dtype=np.float64) i = 0 # for the pedestrian dataset /imu/data and /scan # for bike data set /imu0/data and /scan_corrected for topic, msg, t in bag.read_messages( topics=["/imu/data", "/scan"], start_time=rospy.Time(bag.get_start_time()) + rospy.Duration(240)): if topic == "/imu/data": curr_ori = Quaternion(msg.orientation.w, msg.orientation.x, msg.orientation.y, msg.orientation.z) '''acc = curr_ori.conjugate.rotate([msg.linear_acceleration.x, msg.linear_acceleration.y, msg.linear_acceleration.z]) x.append(acc[0]*dt+x[-1]) y.append(acc[1]*dt+y[-1]) z.append((acc[2] - 9.8)*dt+z[-1])''' v_z_ = curr_ori.rotate(v_z) if np.arccos(v_z_[2]) - math.pi > -.04: plot_scan = True else: plot_scan = False else: #if plot_scan: #data = np.concatenate((data, to_pcl_array(msg, curr_ori)), axis=0) data = to_pcl_array(msg, curr_ori) '''filter_ = Pcl_.make_statistical_outlier_filter() filter_.set_mean_k(50) filter_.set_std_dev_mul_thresh(1.0) Pcl_ = filter_.filter()'''
def updateRealtimeVis(quat, idStr, ax): if idStr == 'head': headPos = quat.rotate([0, 0, 1]) ind = np.linspace(0, 1, 11) x = [headPos[0] * i for i in ind] y = [headPos[1] * i for i in ind] z = [headPos[2] * i for i in ind] ax.plot(x, y, z) return quat euler = transformations.euler_from_quaternion( [quat[1], quat[2], quat[3], quat[0]]) print(euler) if idStr == 'rightArm': ans = ANGLE_MAP_R[str(rad2Bucket(euler[2]))][str(rad2Bucket( euler[1]))][str(rad2Bucket(euler[0]))] if ans['shoulderX'] is not None: elbowRelativeEuler = [ deg2rad(ans['shoulderX']), deg2rad(ans['shoulderZ']), deg2rad(ans['shoulderY']) ] elbowRelativeQuat = transformations.quaternion_from_euler( elbowRelativeEuler[0], elbowRelativeEuler[1], elbowRelativeEuler[2]) elbowRelativeQuat = Quaternion(elbowRelativeQuat[3], elbowRelativeQuat[0], elbowRelativeQuat[1], elbowRelativeQuat[2]) elbowPos = elbowRelativeQuat.rotate([0, 0, -1]) wristRelativePos = quat.rotate([0, 0, -1]) ind = np.linspace(0, 1, 11) x = [0 + elbowPos[0] * i for i in ind ] + [0 + elbowPos[0] + wristRelativePos[0] * i for i in ind] y = [-1 + elbowPos[1] * i for i in ind ] + [-1 + elbowPos[1] + wristRelativePos[1] * i for i in ind] z = [0 + elbowPos[2] * i for i in ind ] + [0 + elbowPos[2] + wristRelativePos[2] * i for i in ind] ax.plot(x, y, z) return elbowRelativeQuat if idStr == 'leftArm': #print(euler) ans = ANGLE_MAP_L[str(rad2Bucket(euler[0]))][str(rad2Bucket( euler[2]))][str(rad2Bucket(euler[1]))] #print(ans) if ans['shoulderX'] is not None: elbowRelativeEuler = [ deg2rad(ans['shoulderX']), deg2rad(ans['shoulderZ']), deg2rad(ans['shoulderY']) ] elbowRelativeQuat = transformations.quaternion_from_euler( elbowRelativeEuler[0], elbowRelativeEuler[1], elbowRelativeEuler[2]) elbowRelativeQuat = Quaternion(elbowRelativeQuat[3], elbowRelativeQuat[0], elbowRelativeQuat[1], elbowRelativeQuat[2]) elbowPos = elbowRelativeQuat.rotate([0, 0, -1]) wristRelativePos = quat.rotate([0, 0, -1]) ind = np.linspace(0, 1, 11) x = [0 + elbowPos[0] * i for i in ind ] + [0 + elbowPos[0] + wristRelativePos[0] * i for i in ind] y = [1 + elbowPos[1] * i for i in ind ] + [1 + elbowPos[1] + wristRelativePos[1] * i for i in ind] z = [0 + elbowPos[2] * i for i in ind ] + [0 + elbowPos[2] + wristRelativePos[2] * i for i in ind] ax.plot(x, y, z) return elbowRelativeQuat
# This program demonstrates various properties and applications of quaternions. import numpy from pyquaternion import Quaternion my_quaternion = Quaternion(1, -2, 3, -4) my_inverse = my_quaternion.inverse numpy.set_printoptions(suppress=True) # Suppress insignificant values for clarity u = numpy.array([0, -3/5, 4/5]) # Unit vector v = numpy.array([0, 3/5, -4/5]) # Unit vector u_prime = my_quaternion.rotate(u) u_undo = my_inverse.rotate(u_prime) v_prime = my_quaternion.rotate(v) v_undo = my_inverse.rotate(v_prime) print(v_prime) print(v_undo) print(u_prime) print(u_undo)
color[0][j] = 255 color[1][j] = 0 color[2][j] = 255 j+=1 for i in range(number_of_samples): motor_pos = values[()]['motor_position'][i] j = 0 for select in args.select: # print(motor_pos) sensor_positions[i][j] = np.array(ball.config['sensor_pos'][select]) quat = Quaternion(axis=[1, 0, 0], degrees=-motor_pos) sensor_positions[i][j] = quat.rotate(sensor_positions[i][j]) angle = ball.config['sensor_angle'][select][2] sensor_quat = Quaternion(axis=[0, 0, 1], degrees=-angle) # sv = np.array([0,0,0]) sv = values[()]['sensor_values'][i][select] sv = sensor_quat.rotate(sv) sv = quat.rotate(sv) if select>=14: # align sensors from other side of the pcb quat2 = Quaternion(axis=[1, 0, 0], degrees=8) sv = quat2.rotate(sv) quat2 = Quaternion(axis=[0, 1, 0], degrees=90) sv = quat2.rotate(sv) sensor_positions[i][j] = quat2.rotate(sensor_positions[i][j]) sensor_values[i][j] = sv
dt = pi w_x = 0.5 w_z = 0.5 w_y = 0.5 w_norm = sqrt(w_x**2 + w_y**2 + w_z**2) alpha = dt * w_norm / 2 q0 = cos(alpha) q1 = sin(alpha) * w_x / w_norm q2 = sin(alpha) * w_y / w_norm q3 = sin(alpha) * w_z / w_norm rot = Quaternion(q0, q1, q2, q3) v = array([1, 0, 0]) v_prime = rot.rotate(v) print(v_prime) def quaternion_rotation(sample_rate, w_x, w_y, w_z, v_x, v_y, v_z, n): w_norm = np.sqrt(w_x[n]**2 + w_y[n]**2 + w_z[n]**2) alpha = w_norm / (sample_rate * 2) q0 = np.cos(alpha) q1 = np.sin(alpha) * w_x[n] / w_norm q2 = np.sin(alpha) * w_y[n] / w_norm q3 = np.sin(alpha) * w_z[n] / w_norm v = np.array([v_x[n], v_y[n], v_z[n]]) q = Quaternion(q0, q1, q2, q3) v_prime = q.rotate(v)
def extract_direction_dataset1(q): #q is quaternion v0 = [1, 0, 0] q = Quaternion([q[3], q[2], q[1], q[0]]) return q.rotate(v0)
def generate_propagation_parameters(self): ''' --------------------------------------------- generate_propagation_parameters() --------------------------------------------- This method finds the parameters to pass to the opencl kernel to correctly define the sampling grid. ''' # Find the unit normal vector to the plane. unit_normal = (1 / np.linalg.norm(self.normal_vector)) * self.normal_vector # Define the quaternion required to rotate the parallel vector by an angle of pi/2. quaternion = Quaternion( axis=[unit_normal[0], unit_normal[1], unit_normal[2]], angle=tau / 4) # Rotate the parallel vector to get a new vector which is perpendicular to the normal and the parallel vector. self.vector_2 = quaternion.rotate(self.parallel_vector) # Find the norm of these vectors. parallel_vector_norm = np.linalg.norm(self.parallel_vector) vector_2_norm = np.linalg.norm(self.vector_2) # Find the unit vectors. unit_parallel_vector = (1 / parallel_vector_norm) * self.parallel_vector unit_vector_2 = (1 / vector_2_norm) * self.vector_2 # Compute the angle between these vectors to check that is equal to pi/2. angle_1 = int( np.round( np.rad2deg( np.arccos(np.dot(unit_parallel_vector, unit_vector_2))), 1)) # Compute the angle between each of these vectors and the normal to ensure that they parameterise the plane perpendicular to the nromal. angle_2 = int( np.round( np.rad2deg( np.arccos( np.dot(self.parallel_vector, self.normal_vector) / (parallel_vector_norm * vector_2_norm))), 1)) angle_3 = int( np.round( np.rad2deg( np.arccos( np.dot(self.vector_2, self.normal_vector) / (parallel_vector_norm * vector_2_norm))), 1)) # Throw error if they are not. if np.bitwise_or(angle_1 != 90, np.bitwise_or(int(angle_2) != 90, int(angle_3) != 90)): raise Exception( 'Could not parameterise the requested sampling grid correctly!' ) self.x_lim = np.float32(self.N_x / 2) self.y_lim = np.float32(self.N_y / 2) self.vx1 = np.float32(self.parallel_vector[0]) self.vy1 = np.float32(self.parallel_vector[1]) self.vz1 = np.float32(self.parallel_vector[2]) self.x0 = np.float32(self.origin[0]) self.y0 = np.float32(self.origin[1]) self.z0 = np.float32(self.origin[2]) self.vx2 = np.float32(np.round(self.vector_2[0], 4)) self.vy2 = np.float32(np.round(self.vector_2[1], 4)) self.vz2 = np.float32(np.round(self.vector_2[2], 4))