def drawParticles2d(self, V, frame_id='origin', name='particles', lname='yaw', c=['b'], lc=['r'], ll=0.05):
LD = []
if len(V) < 1:
print 'drawParticles2d -- no data? skip'
return
d,N = V[0].shape
l = len(V)
for v in V:
Dv = np.vstack((v[0:2,:], np.zeros((1,N)))).T
LD.append(Dv)
publish_cloud(name, LD, c=c, frame_id=frame_id)
if d > 2:
count = -1
DV = np.zeros((d,l*N))
DT = np.zeros((d,l*N))
for v in V:
tt = np.zeros((d,N))
for i in range(0,N):
t = Pose.from_rigid_transform(0,RigidTransform(xyzw=Quaternion.from_roll_pitch_yaw(0,0,v[2,i]).to_xyzw(), tvec=[v[0,i], v[1,i], 0]))
pert = Pose.from_rigid_transform(0,RigidTransform(tvec=[ll,0,0]))
tv = (t.oplus(pert)).tvec
tt[0:2,i] = tv[0:2]
count +=1
DT[0:d,(count*N):((count+1)*N)] = tt
Dv = np.vstack((v[0:2,:], np.zeros((1,N))))
DV[0:d,(count*N):((count+1)*N)] = Dv
publish_line_segments(lname, DV.T, DT.T, c=lc[0], frame_id=frame_id)
def imu_handler(self, channel, data):
self.imumsg = ins_t.decode(data)
#lRb, self.prtObj.yaw = self.prtObj.propNavIMU(0.01, np.array(self.imumsg.gyro)[:,np.newaxis], np.array(self.imumsg.accel)[:,np.newaxis], yaw=self.prtObj.yaw)
self.prtObj.upStateIMU_mod(0.01, np.array(self.imumsg.gyro)[:,np.newaxis], np.array(self.imumsg.accel)[:,np.newaxis])
lRb = self.prtObj.lRb()
#self.prtObj.a_prev[:,0] = self.imumsg.accel
if self.viconmsg:
p = Pose.from_rigid_transform(0, RigidTransform(tvec=self.viconmsg.pos))
# print 'acc', self.imumsg.accel
# bRa = self.prtObj.bRa(acc=np.matrix(self.imumsg.accel).T)
# lRb = self.prtObj.lRb(np.matrix(self.imumsg.accel).T)
bRl = lRb.T
rt = RigidTransform.from_Rt(lRb , self.viconmsg.pos) # bRl
#print( "R, P, Y, calc_Y: %.3f, %.3f, %.3f, %.3f" % (rt.to_roll_pitch_yaw_x_y_z()[0]*180/np.pi, rt.to_roll_pitch_yaw_x_y_z()[1]*180/np.pi, rt.to_roll_pitch_yaw_x_y_z()[2]*180/np.pi, self.prtObj.yaw*180/np.pi) )
o = Pose.from_rigid_transform(1, rt)
# plot accelerometer estimated orientation of IMU
publish_pose_list('Localpose', [p], frame_id='origin')
publish_pose_list('IMUpose', [o], frame_id='origin')
#publish_cloud('particle_imu_max', self.imu_max.T+self.viconmsg.pos, c='b', frame_id='origin')
#test magnetometer
lMag = self.prtObj.magUpdateYaw_mod(np.array(self.imumsg.mag)[:,np.newaxis])
#lMag = self.prtObj.magUpdateYaw_mod(np.array([[1.0],[0.0],[0.0]]))
#lMag /= np.linalg.norm(lMag)
#lMag = np.dot(bRl, np.array([[1.0],[0.0],[0.0]]))
publish_cloud('mag', np.array(self.imumsg.mag)+self.viconmsg.pos, c='b', frame_id='origin')
def imu_handler(self, channel, data):
self.imumsg = ins_t.decode(data)
# lRb, self.prtObj.yaw = self.prtObj.propNavIMU(0.01, np.array(self.imumsg.gyro)[:,np.newaxis], np.array(self.imumsg.accel)[:,np.newaxis], yaw=self.prtObj.yaw)
self.prtObj.upStateIMU_mod(
0.01, np.array(self.imumsg.gyro)[:, np.newaxis], np.array(self.imumsg.accel)[:, np.newaxis]
)
lRb = self.prtObj.lRb()
# self.prtObj.a_prev[:,0] = self.imumsg.accel
if self.viconmsg:
p = Pose.from_rigid_transform(0, RigidTransform(tvec=self.viconmsg.pos))
# print 'acc', self.imumsg.accel
# bRa = self.prtObj.bRa(acc=np.matrix(self.imumsg.accel).T)
# lRb = self.prtObj.lRb(np.matrix(self.imumsg.accel).T)
bRl = lRb.T
rt = RigidTransform.from_Rt(lRb, self.viconmsg.pos) # bRl
# print( "R, P, Y, calc_Y: %.3f, %.3f, %.3f, %.3f" % (rt.to_roll_pitch_yaw_x_y_z()[0]*180/np.pi, rt.to_roll_pitch_yaw_x_y_z()[1]*180/np.pi, rt.to_roll_pitch_yaw_x_y_z()[2]*180/np.pi, self.prtObj.yaw*180/np.pi) )
o = Pose.from_rigid_transform(1, rt)
# plot accelerometer estimated orientation of IMU
publish_pose_list("Localpose", [p], frame_id="origin")
publish_pose_list("IMUpose", [o], frame_id="origin")
# publish_cloud('particle_imu_max', self.imu_max.T+self.viconmsg.pos, c='b', frame_id='origin')
# test magnetometer
lMag = self.prtObj.magUpdateYaw_mod(np.array(self.imumsg.mag)[:, np.newaxis])
# lMag = self.prtObj.magUpdateYaw_mod(np.array([[1.0],[0.0],[0.0]]))
# lMag /= np.linalg.norm(lMag)
# lMag = np.dot(bRl, np.array([[1.0],[0.0],[0.0]]))
publish_cloud("mag", np.array(self.imumsg.mag) + self.viconmsg.pos, c="b", frame_id="origin")
def my_handler(self, channel, data):
global uts
firstpass = False
if uts < 1:
firstpass = True
msg = ins_t.decode(data)
dt = (msg.utime - uts)*1e-6
uts = msg.utime
if firstpass:
return
bRi = self.prtObj.bRi
acc = np.array(msg.accel)[:,np.newaxis]
gyr = np.array(msg.gyro)[:,np.newaxis]
acc = np.dot(bRi, acc)
gyr = np.dot(bRi, gyr)
self.prtObj.yaw = self.prtObj.propNavIMU(dt, gyr, acc, self.prtObj.yaw)
self.prtObj.a_prev[:,0:1] = acc
aRb = self.prtObj.bRa(acc=np.array(msg.accel)[:,np.newaxis]).T
rt = RigidTransform.from_Rt( aRb , [0,0,0])
o = Pose.from_rigid_transform(1, rt)
publish_pose_list('aRb', [o], frame_id='origin')
lRb = self.prtObj.lRb()
rt2 = RigidTransform.from_Rt( lRb , [0,0,0])
o2 = Pose.from_rigid_transform(2, rt2)
publish_pose_list('lRb', [o2], frame_id='origin')
def my_handler(self, channel, data):
global uts
firstpass = False
if uts < 1:
firstpass = True
msg = ins_t.decode(data)
dt = (msg.utime - uts) * 1e-6
uts = msg.utime
if firstpass:
return
bRi = self.prtObj.bRi
acc = np.array(msg.accel)[:, np.newaxis]
gyr = np.array(msg.gyro)[:, np.newaxis]
acc = np.dot(bRi, acc)
gyr = np.dot(bRi, gyr)
self.prtObj.yaw = self.prtObj.propNavIMU(dt, gyr, acc, self.prtObj.yaw)
self.prtObj.a_prev[:, 0:1] = acc
aRb = self.prtObj.bRa(acc=np.array(msg.accel)[:, np.newaxis]).T
rt = RigidTransform.from_Rt(aRb, [0, 0, 0])
o = Pose.from_rigid_transform(1, rt)
publish_pose_list('aRb', [o], frame_id='origin')
lRb = self.prtObj.lRb()
rt2 = RigidTransform.from_Rt(lRb, [0, 0, 0])
o2 = Pose.from_rigid_transform(2, rt2)
publish_pose_list('lRb', [o2], frame_id='origin')
def drawParticles2d(self,
V,
frame_id='origin',
name='particles',
lname='yaw',
c=['b'],
lc=['r'],
ll=0.05):
LD = []
if len(V) < 1:
print 'drawParticles2d -- no data? skip'
return
d, N = V[0].shape
l = len(V)
for v in V:
Dv = np.vstack((v[0:2, :], np.zeros((1, N)))).T
LD.append(Dv)
publish_cloud(name, LD, c=c, frame_id=frame_id)
if d > 2:
count = -1
DV = np.zeros((d, l * N))
DT = np.zeros((d, l * N))
for v in V:
tt = np.zeros((d, N))
for i in range(0, N):
t = Pose.from_rigid_transform(
0,
RigidTransform(xyzw=Quaternion.from_roll_pitch_yaw(
0, 0, v[2, i]).to_xyzw(),
tvec=[v[0, i], v[1, i], 0]))
pert = Pose.from_rigid_transform(
0, RigidTransform(tvec=[ll, 0, 0]))
tv = (t.oplus(pert)).tvec
tt[0:2, i] = tv[0:2]
count += 1
DT[0:d, (count * N):((count + 1) * N)] = tt
Dv = np.vstack((v[0:2, :], np.zeros((1, N))))
DV[0:d, (count * N):((count + 1) * N)] = Dv
publish_line_segments(lname,
DV.T,
DT.T,
c=lc[0],
frame_id=frame_id)
def vicon_handler(self,channel, data):
self.viconmsg = body_t.decode(data)
self.drawParticles(offset=self.viconmsg.pos)
# plot true orientation of IMU via vicon
vicon_orient = self.viconmsg.orientation
w = vicon_orient[0]
x = vicon_orient[1]
y = vicon_orient[2]
z = vicon_orient[3]
# quaternion to rotation matrix
wRv = np.zeros([3,3],dtype=np.double)
wRv[0,0] = 1 - 2*y**2 - 2*z**2
wRv[0,1] = 2*x*y - 2*z*w
wRv[0,2] = 2*x*z + 2*y*w
wRv[1,0] = 2*x*y + 2*z*w
wRv[1,1] = 1 - 2*x**2 - 2*z**2
wRv[1,2] = 2*y*z - 2*x*w
wRv[2,0] = 2*x*z - 2*y*w
wRv[2,1] = 2*y*z + 2*x*w
wRv[2,2] = 1 - 2*x**2 - 2*y**2
# rotational transformation between microstrain and vicon (collocated)
vRm = np.zeros([3,3],dtype=np.double)
vRm[0,0] = 0.747293477674224
vRm[0,1] = 0.663765523047521
vRm[0,2] = 0.031109301487093
vRm[1,0] = 0.663949387400485
vRm[1,1] = -0.747757418253684
vRm[1,2] = 0.005482190903960
vRm[2,0] = 0.026901100276478
vRm[2,1] = 0.016558196158918
vRm[2,2] = -0.999500953948458
wRm = np.dot(wRv, vRm)
self.vicon_lRb = wRm
p = Pose.from_rigid_transform(2, RigidTransform.from_Rt(wRm,self.viconmsg.pos))
publish_pose_list('VICONpose', [p], frame_id='origin')
#drawing true max as point in new image and in local frame
if self.imumsg != None:
im2 = np.zeros([180,360])
PV = np.array([[30],[30],[4]]) - np.array(self.viconmsg.pos)[:,np.newaxis]
self.vicon_max = PV/(np.linalg.norm(PV))
publish_cloud('particle_vicon_max', self.vicon_max.T+self.viconmsg.pos, c='g', frame_id='origin')
# lRb_val = self.prtObj.lRb()
# bRl_val = lRb_val.T
# dot_prod = np.dot(bRl_val, self.vicon_max) # use IMU local-to-body transform
dot_prod = np.dot(self.vicon_lRb.T, self.vicon_max) # use vicon local-to-body transform
az_el = self.prtObj.peRb_pt(dot_prod)
if az_el[0] < 0:
az_el[0] = az_el[0]+2*np.pi
# print az_el*180/np.pi
self.gen_heatmap(az_el[0], az_el[1])
cv2.circle(im2, (int(az_el[0]*180/np.pi),int(az_el[1]*180/np.pi)), 5, (255,0,0))
cv2.imshow('img2', im2)
cv2.waitKey(1)
def vicon_handler(self, channel, data):
self.viconmsg = body_t.decode(data)
self.drawParticles(offset=self.viconmsg.pos)
# plot true orientation of IMU via vicon
vicon_orient = self.viconmsg.orientation
w = vicon_orient[0]
x = vicon_orient[1]
y = vicon_orient[2]
z = vicon_orient[3]
# quaternion to rotation matrix
wRv = np.zeros([3, 3], dtype=np.double)
wRv[0, 0] = 1 - 2 * y ** 2 - 2 * z ** 2
wRv[0, 1] = 2 * x * y - 2 * z * w
wRv[0, 2] = 2 * x * z + 2 * y * w
wRv[1, 0] = 2 * x * y + 2 * z * w
wRv[1, 1] = 1 - 2 * x ** 2 - 2 * z ** 2
wRv[1, 2] = 2 * y * z - 2 * x * w
wRv[2, 0] = 2 * x * z - 2 * y * w
wRv[2, 1] = 2 * y * z + 2 * x * w
wRv[2, 2] = 1 - 2 * x ** 2 - 2 * y ** 2
# rotational transformation between microstrain and vicon (collocated)
vRm = np.zeros([3, 3], dtype=np.double)
vRm[0, 0] = 0.747293477674224
vRm[0, 1] = 0.663765523047521
vRm[0, 2] = 0.031109301487093
vRm[1, 0] = 0.663949387400485
vRm[1, 1] = -0.747757418253684
vRm[1, 2] = 0.005482190903960
vRm[2, 0] = 0.026901100276478
vRm[2, 1] = 0.016558196158918
vRm[2, 2] = -0.999500953948458
wRm = np.dot(wRv, vRm)
self.vicon_lRb = wRm
p = Pose.from_rigid_transform(2, RigidTransform.from_Rt(wRm, self.viconmsg.pos))
publish_pose_list("VICONpose", [p], frame_id="origin")
# drawing true max as point in new image and in local frame
if self.imumsg != None:
im2 = np.zeros([180, 360])
PV = np.array([[30], [30], [4]]) - np.array(self.viconmsg.pos)[:, np.newaxis]
self.vicon_max = PV / (np.linalg.norm(PV))
publish_cloud("particle_vicon_max", self.vicon_max.T + self.viconmsg.pos, c="g", frame_id="origin")
# lRb_val = self.prtObj.lRb()
# bRl_val = lRb_val.T
# dot_prod = np.dot(bRl_val, self.vicon_max) # use IMU local-to-body transform
dot_prod = np.dot(self.vicon_lRb.T, self.vicon_max) # use vicon local-to-body transform
az_el = self.prtObj.peRb_pt(dot_prod)
if az_el[0] < 0:
az_el[0] = az_el[0] + 2 * np.pi
# print az_el*180/np.pi
self.gen_heatmap(az_el[0], az_el[1])
cv2.circle(im2, (int(az_el[0] * 180 / np.pi), int(az_el[1] * 180 / np.pi)), 5, (255, 0, 0))
cv2.imshow("img2", im2)
cv2.waitKey(1)