Exemple #1
0
 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)
Exemple #2
0
    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')
Exemple #3
0
    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")
Exemple #4
0
 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')
Exemple #5
0
 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')
Exemple #6
0
 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)
Exemple #7
0
    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)
Exemple #8
0
    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)