def loadImuData(self):
print "Reading IMU data ({0})".format(self.dataset.topic)
# prepare progess bar
iProgress = sm.Progress2(self.dataset.numMessages())
iProgress.sample()
Rgyro = np.eye(
3) * self.gyroUncertaintyDiscrete * self.gyroUncertaintyDiscrete
Raccel = np.eye(
3) * self.accelUncertaintyDiscrete * self.accelUncertaintyDiscrete
# Now read the imu measurements.
imu = []
for timestamp, omega, alpha in self.dataset:
timestamp = acv.Time(timestamp.toSec())
imu.append(
self.ImuMeasurement(timestamp, omega, alpha, Rgyro, Raccel))
iProgress.sample()
self.imuData = imu
if len(self.imuData) > 1:
print "\r Read %d imu readings over %.1f seconds " % (
len(imu), imu[-1].stamp.toSec() - imu[0].stamp.toSec())
else:
sm.logFatal(
"Could not find any IMU messages. Please check the dataset.")
sys.exit(-1)
def extractCornersFromDataset(dataset, detector, multithreading=False, numProcesses=None, clearImages=True, noTransformation=False):
print "Extracting calibration target corners"
targetObservations = []
numImages = dataset.numMessages()
# prepare progess bar
iProgress = sm.Progress2(numImages)
iProgress.sample()
if multithreading:
if not numProcesses:
numProcesses = max(1,multiprocessing.cpu_count()-1)
try:
manager = multiprocessing.Manager()
resultq = manager.Queue()
manager2 = multiprocessing.Manager()
taskq = manager2.Queue()
for idx, (timestamp, image) in enumerate(dataset.readDataset()):
taskq.put( (idx, timestamp, image) )
plist=list()
for pidx in range(0, numProcesses):
detector_copy = copy.copy(detector)
p = multiprocessing.Process(target=multicoreExtractionWrapper, args=(detector_copy, taskq, resultq, clearImages, noTransformation, ))
p.start()
plist.append(p)
#wait for results
last_done=0
while 1:
if all([not p.is_alive() for p in plist]):
time.sleep(0.1)
break
done = numImages-taskq.qsize()
sys.stdout.flush()
if (done-last_done) > 0:
iProgress.sample(done-last_done)
last_done = done
time.sleep(0.5)
resultq.put('STOP')
except Exception, e:
raise RuntimeError("Exception during multithreaded extraction: {0}".format(e))
#get result sorted by time (=idx)
if resultq.qsize() > 1:
targetObservations = [[]]*(resultq.qsize()-1)
for lidx, data in enumerate(iter(resultq.get, 'STOP')):
obs=data[0]; time_idx = data[1]
targetObservations[lidx] = (time_idx, obs)
targetObservations = list(zip(*sorted(targetObservations, key=lambda tup: tup[0]))[1])
else:
targetObservations=[]
def addAccelerometerErrorTerms(self,
problem,
poseSplineDv,
gravityExpression,
mSigma=0.0,
accelNoiseScale=1.0):
print
print "Adding accelerometer error terms ({0})".format(
self.dataset.topic)
#progress bar
iProgress = sm.Progress2(len(self.imuData))
iProgress.sample()
# AccelerometerError(measurement, invR, C_b_w, acceleration_w, bias, g_w)
w = 1.0 / accelNoiseScale
accelErrors = []
num_skipped = 0
if mSigma > 0.0:
mest = aopt.HuberMEstimator(math.sqrt(mSigma))
else:
mest = aopt.NoMEstimator()
for im in self.imuData:
tk = im.stamp.toSec()
if tk > poseSplineDv.spline().t_min() and tk < poseSplineDv.spline(
).t_max():
C_b_w = poseSplineDv.orientation(tk).inverse()
ak = poseSplineDv.linearAcceleration(tk)
bk = self.accelBiasDv.toEuclideanExpression(tk, 0)
aerr = ket.AccelerometerError(im.alpha, im.alphaInvR * w,
C_b_w, ak, bk, gravityExpression)
aerr.setMEstimatorPolicy(mest)
accelErrors.append(aerr)
problem.addErrorTerm(aerr)
else:
num_skipped = num_skipped + 1
#update progress bar
iProgress.sample()
print "\r Added {0} of {1} accelerometer error terms (skipped {2} out-of-bounds measurements)".format(
len(self.imuData) - num_skipped, len(self.imuData), num_skipped)
self.accelErrors = accelErrors
def addGyroscopeErrorTerms(self,
problem,
poseSplineDv,
mSigma=0.0,
gyroNoiseScale=1.0):
print
print "Adding gyroscope error terms ({0})".format(self.dataset.topic)
#progress bar
iProgress = sm.Progress2(len(self.imuData))
iProgress.sample()
num_skipped = 0
gyroErrors = []
w = 1.0 / gyroNoiseScale
if mSigma > 0.0:
mest = aopt.HuberMEstimator(math.sqrt(mSigma))
else:
mest = aopt.NoMEstimator()
for im in self.imuData:
tk = im.stamp.toSec()
if tk > poseSplineDv.spline().t_min() and tk < poseSplineDv.spline(
).t_max():
# GyroscopeError(measurement, invR, angularVelocity, bias)
wk = poseSplineDv.angularVelocityBodyFrame(tk)
bk = self.gyroBiasDv.toEuclideanExpression(tk, 0)
gerr = ket.GyroscopeError(im.omega, im.omegaInvR * w, wk, bk)
#gerr.setMEstimatorPolicy(mest)
gyroErrors.append(gerr)
problem.addErrorTerm(gerr)
else:
num_skipped = num_skipped + 1
#update progress bar
iProgress.sample()
print "\r Added {0} of {1} gyroscope error terms (skipped {2} out-of-bounds measurements)".format(
len(self.imuData) - num_skipped, len(self.imuData), num_skipped)
self.gyroErrors = gyroErrors
def addCameraErrorTerms(self,
problem,
poseSplineDv,
T_cN_b,
blakeZissermanDf=0.0,
timeOffsetPadding=0.0):
print
print "Adding camera error terms ({0})".format(self.dataset.topic)
#progress bar
iProgress = sm.Progress2(len(self.targetObservations))
iProgress.sample()
allReprojectionErrors = list()
error_t = self.camera.reprojectionErrorType
for obs in self.targetObservations:
# Build a transformation expression for the time.
frameTime = self.cameraTimeToImuTimeDv.toExpression() + obs.time(
).toSec() + self.timeshiftCamToImuPrior
frameTimeScalar = frameTime.toScalar()
#as we are applying an initial time shift outside the optimization so
#we need to make sure that we dont add data outside the spline definition
if frameTimeScalar <= poseSplineDv.spline().t_min(
) or frameTimeScalar >= poseSplineDv.spline().t_max():
continue
T_w_b = poseSplineDv.transformationAtTime(frameTime,
timeOffsetPadding,
timeOffsetPadding)
T_b_w = T_w_b.inverse()
#calibration target coords to camera N coords
#T_b_w: from world to imu coords
#T_cN_b: from imu to camera N coords
T_c_w = T_cN_b * T_b_w
#get the image and target points corresponding to the frame
imageCornerPoints = np.array(obs.getCornersImageFrame()).T
targetCornerPoints = np.array(obs.getCornersTargetFrame()).T
#setup an aslam frame (handles the distortion)
frame = self.camera.frameType()
frame.setGeometry(self.camera.geometry)
#corner uncertainty
R = np.eye(2) * self.cornerUncertainty * self.cornerUncertainty
invR = np.linalg.inv(R)
for pidx in range(0, imageCornerPoints.shape[1]):
#add all image points
k = self.camera.keypointType()
k.setMeasurement(imageCornerPoints[:, pidx])
k.setInverseMeasurementCovariance(invR)
frame.addKeypoint(k)
reprojectionErrors = list()
for pidx in range(0, imageCornerPoints.shape[1]):
#add all target points
targetPoint = np.insert(targetCornerPoints.transpose()[pidx],
3, 1)
p = T_c_w * aopt.HomogeneousExpression(targetPoint)
#build and append the error term
rerr = error_t(frame, pidx, p)
#add blake-zisserman m-estimator
if blakeZissermanDf > 0.0:
mest = aopt.BlakeZissermanMEstimator(blakeZissermanDf)
rerr.setMEstimatorPolicy(mest)
problem.addErrorTerm(rerr)
reprojectionErrors.append(rerr)
allReprojectionErrors.append(reprojectionErrors)
#update progress bar
iProgress.sample()
print "\r Added {0} camera error terms ".format(
len(self.targetObservations))
self.allReprojectionErrors = allReprojectionErrors
def extractCornersFromDataset(dataset,
detector,
multithreading=False,
numProcesses=None,
clearImages=True,
noTransformation=False):
print("Extracting calibration target corners")
targetObservations = []
numImages = dataset.numImages()
# prepare progess bar
iProgress = sm.Progress2(numImages)
iProgress.sample()
if multithreading:
if not numProcesses:
numProcesses = max(1, multiprocessing.cpu_count() - 1)
try:
manager = multiprocessing.Manager()
resultq = manager.Queue()
manager2 = multiprocessing.Manager()
taskq = manager2.Queue()
for idx, (timestamp, image) in enumerate(dataset.readDataset()):
taskq.put((idx, timestamp, image))
plist = list()
for pidx in range(0, numProcesses):
detector_copy = copy.copy(detector)
p = multiprocessing.Process(target=multicoreExtractionWrapper,
args=(
detector_copy,
taskq,
resultq,
clearImages,
noTransformation,
))
p.start()
plist.append(p)
#wait for results
last_done = 0
while 1:
if all([not p.is_alive() for p in plist]):
time.sleep(0.1)
break
done = numImages - taskq.qsize()
sys.stdout.flush()
if (done - last_done) > 0:
iProgress.sample(done - last_done)
last_done = done
time.sleep(0.5)
resultq.put('STOP')
except Exception as e:
raise RuntimeError(
"Exception during multithreaded extraction: {0}".format(e))
#get result sorted by time (=idx)
if resultq.qsize() > 1:
targetObservations = [[]] * (resultq.qsize() - 1)
for lidx, data in enumerate(iter(resultq.get, 'STOP')):
obs = data[0]
time_idx = data[1]
targetObservations[lidx] = (time_idx, obs)
targetObservations = list(
zip(*sorted(targetObservations, key=lambda tup: tup[0])))[1]
else:
targetObservations = []
#single threaded implementation
else:
for timestamp, image in dataset.readDataset():
if noTransformation:
success, observation = detector.findTargetNoTransformation(
timestamp, np.array(image))
else:
success, observation = detector.findTarget(
timestamp, np.array(image))
if clearImages:
observation.clearImage()
if success == 1:
targetObservations.append(observation)
iProgress.sample()
if len(targetObservations) == 0:
print("\r")
sm.logFatal(
"No corners could be extracted for camera {0}! Check the calibration target configuration and dataset."
.format(dataset.topic))
else:
print(
"\r Extracted corners for %d images (of %d images) "
% (len(targetObservations), numImages))
#close all opencv windows that might be open
cv2.destroyAllWindows()
return targetObservations