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))