py_src/mapping.py
def convert2world_frame(lidar_scan,lidar_pose,head_angles):
    '''
    This takes in the lidar frame reading and computes the world frame readings
    Ideally the z axis should not change
    '''

    #Pose from lidar to head
    #print("Pose is ",lidar_pose)
    lid2head_pose = np.hstack((np.eye(3),np.array([0,0,0.15]).reshape(3,1)))
    lid2head_pose = np.vstack((lid2head_pose,np.array([0,0,0,1]).reshape(1,4)))

    #Pose from head to body
    yaw,pit = head_angles
    rot_yaw = np.array([[np.cos(yaw),-np.sin(yaw),0],[np.sin(yaw),np.cos(yaw),0],[0,0,1]])#.astype(float)
    rot_pit = np.array([[np.cos(pit),0,np.sin(pit)],[0,1,0],[-np.sin(yaw),0,np.cos(yaw)]])#.astype(float)
    head2body_rot = rot_yaw @ rot_pit
    head2body_pose = np.hstack((head2body_rot,np.array([0,0,0.33]).reshape(3,1)))
    head2body_pose = np.vstack((head2body_pose,np.array([0,0,0,1]).reshape(1,4)))

    #Pose from body to world

    yaw = lidar_pose[2]
    body2world_rot = np.array([[np.cos(yaw),-np.sin(yaw),0],[np.sin(yaw),np.cos(yaw),0],[0,0,1]])#.astype(float)
    body2world_pose = np.hstack((body2world_rot,np.array([lidar_pose[0],lidar_pose[1],0.93]).reshape(3,1)))
    body2world_pose = np.vstack((body2world_pose,np.array([0,0,0,1]).reshape(1,4)))
      

    body_pose = head2body_pose @ lid2head_pose @ lidar_scan
    
    ###### Filtering Out Ground Points ######
    body_pose_T = body_pose.T
    body_pose_corrected = body_pose_T[np.where(body_pose[2,:] > 0)]
    body_pose_corrected = body_pose_corrected.T
    
    return body2world_pose @ body_pose_corrected

    This takes in the lidar frame reading and computes the world frame readings
    Ideally the z axis should not change
    
This takes in the lidar frame reading and computes the world frame readings
Ideally the z axis should not change
This takes in the lidar frame reading and computes the world frame readings Ideally the z axis should not change
def convert2world_frame(lidar_scan,lidar_pose,head_angles):
    lid2head_pose = np.hstack((np.eye(3),np.array([0,0,0.15]).reshape(3,1)))
    lid2head_pose = np.vstack((lid2head_pose,np.array([0,0,0,1]).reshape(1,4)))
    yaw,pit = head_angles
    rot_yaw = np.array([[np.cos(yaw),-np.sin(yaw),0],[np.sin(yaw),np.cos(yaw),0],[0,0,1]])
    rot_pit = np.array([[np.cos(pit),0,np.sin(pit)],[0,1,0],[-np.sin(yaw),0,np.cos(yaw)]])
    head2body_rot = rot_yaw @ rot_pit
    head2body_pose = np.hstack((head2body_rot,np.array([0,0,0.33]).reshape(3,1)))
    head2body_pose = np.vstack((head2body_pose,np.array([0,0,0,1]).reshape(1,4)))
    yaw = lidar_pose[2]
    body2world_rot = np.array([[np.cos(yaw),-np.sin(yaw),0],[np.sin(yaw),np.cos(yaw),0],[0,0,1]])
    body2world_pose = np.hstack((body2world_rot,np.array([lidar_pose[0],lidar_pose[1],0.93]).reshape(3,1)))
    body2world_pose = np.vstack((body2world_pose,np.array([0,0,0,1]).reshape(1,4)))
    body_pose = head2body_pose @ lid2head_pose @ lidar_scan
    body_pose_T = body_pose.T
    body_pose_corrected = body_pose_T[np.where(body_pose[2,:] > 0)]
    body_pose_corrected = body_pose_corrected.T
    return body2world_pose @ body_pose_corrected
def convert2world_frame ( lidar_scan , lidar_pose , head_angles ) : lid2head_pose = np . hstack ( ( np . eye ( 3 ) , np . array ( [ 0 , 0 , 0.15 ] ) . reshape ( 3 , 1 ) ) ) lid2head_pose = np . vstack ( ( lid2head_pose , np . array ( [ 0 , 0 , 0 , 1 ] ) . reshape ( 1 , 4 ) ) ) yaw , pit = head_angles rot_yaw = np . array ( [ [ np . cos ( yaw ) , - np . sin ( yaw ) , 0 ] , [ np . sin ( yaw ) , np . cos ( yaw ) , 0 ] , [ 0 , 0 , 1 ] ] ) rot_pit = np . array ( [ [ np . cos ( pit ) , 0 , np . sin ( pit ) ] , [ 0 , 1 , 0 ] , [ - np . sin ( yaw ) , 0 , np . cos ( yaw ) ] ] ) head2body_rot = rot_yaw @ rot_pit head2body_pose = np . hstack ( ( head2body_rot , np . array ( [ 0 , 0 , 0.33 ] ) . reshape ( 3 , 1 ) ) ) head2body_pose = np . vstack ( ( head2body_pose , np . array ( [ 0 , 0 , 0 , 1 ] ) . reshape ( 1 , 4 ) ) ) yaw = lidar_pose [ 2 ] body2world_rot = np . array ( [ [ np . cos ( yaw ) , - np . sin ( yaw ) , 0 ] , [ np . sin ( yaw ) , np . cos ( yaw ) , 0 ] , [ 0 , 0 , 1 ] ] ) body2world_pose = np . hstack ( ( body2world_rot , np . array ( [ lidar_pose [ 0 ] , lidar_pose [ 1 ] , 0.93 ] ) . reshape ( 3 , 1 ) ) ) body2world_pose = np . vstack ( ( body2world_pose , np . array ( [ 0 , 0 , 0 , 1 ] ) . reshape ( 1 , 4 ) ) ) body_pose = head2body_pose @ lid2head_pose @ lidar_scan body_pose_T = body_pose . T body_pose_corrected = body_pose_T [ np . where ( body_pose [ 2 , : ] > 0 ) ] body_pose_corrected = body_pose_corrected . T return body2world_pose @ body_pose_corrected
This takes in the lidar frame reading and computes the world frame readings
Ideally the z axis should not change
This takes in the lidar frame reading and computes the world frame readings Ideally the z axis should not change
'''
    This takes in the lidar frame reading and computes the world frame readings
    Ideally the z axis should not change
    ''' #Pose from lidar to head #print("Pose is ",lidar_pose) #Pose from head to body #.astype(float) #.astype(float) #Pose from body to world #.astype(float) ###### Filtering Out Ground Points ######
lidar_scan
lidar_pose
head_angles