Fix IMU example
							parent
							
								
									853d327725
								
							
						
					
					
						commit
						b30a7685db
					
				| 
						 | 
				
			
			@ -69,7 +69,7 @@ class ImuFactorExample(PreintegrationExample):
 | 
			
		|||
            # get measurements and add them to PIM
 | 
			
		||||
            measuredOmega = self.runner.measuredAngularVelocity(t)
 | 
			
		||||
            measuredAcc = self.runner.measuredSpecificForce(t)
 | 
			
		||||
            pim.integrateMeasurement(measuredAcc, measuredOmega, self.dt, H9, H9)
 | 
			
		||||
            pim.integrateMeasurement(measuredAcc, measuredOmega, self.dt)
 | 
			
		||||
            
 | 
			
		||||
            # Plot IMU many times
 | 
			
		||||
            if k % 10 == 0:
 | 
			
		||||
| 
						 | 
				
			
			
 | 
			
		|||
| 
						 | 
				
			
			@ -101,7 +101,7 @@ class PreintegrationExample(object):
 | 
			
		|||
        actualPose = self.scenario.pose(t)
 | 
			
		||||
        plotPose3(POSES_FIG, actualPose, 0.3)
 | 
			
		||||
        t = actualPose.translation()
 | 
			
		||||
        self.maxDim = max([abs(t.x()), abs(t.y()), abs(t.z()), self.maxDim])
 | 
			
		||||
        self.maxDim = max([abs(t[0]), abs(t[1]), abs(t[2]), self.maxDim])
 | 
			
		||||
        ax = plt.gca()
 | 
			
		||||
        ax.set_xlim3d(-self.maxDim, self.maxDim)
 | 
			
		||||
        ax.set_ylim3d(-self.maxDim, self.maxDim)
 | 
			
		||||
| 
						 | 
				
			
			
 | 
			
		|||
| 
						 | 
				
			
			@ -33,7 +33,7 @@ def plot3DPoints(fignum, values, linespec, marginals=None):
 | 
			
		|||
def plotPose3OnAxes(ax, pose, axisLength=0.1):
 | 
			
		||||
    # get rotation and translation (center)
 | 
			
		||||
    gRp = pose.rotation().matrix()  # rotation from pose to global
 | 
			
		||||
    C = pose.translation().vector()
 | 
			
		||||
    C = pose.translation()
 | 
			
		||||
 | 
			
		||||
    # draw the camera axes
 | 
			
		||||
    xAxis = C + gRp[:, 0] * axisLength
 | 
			
		||||
| 
						 | 
				
			
			
 | 
			
		|||
		Loading…
	
		Reference in New Issue