def testInitPose(self):
bsp = bsplines.BSplinePose(4,sm.RotationVector())
# Create two random transformations.
T_n_0 = bsp.curveValueToTransformation(numpy.random.random(6))
T_n_1 = bsp.curveValueToTransformation(numpy.random.random(6))
# Initialize the curve.
bsp.initPoseSpline(0.0,1.0,T_n_0, T_n_1)
# Check the values.
self.assertEqual(bsp.t_min(),0.0);
self.assertEqual(bsp.t_max(),1.0);
curve_T_n_0 = bsp.transformation(0.0);
self.assertMatricesEqual(T_n_0, curve_T_n_0, 1e-9,"T_n_0")
curve_T_n_1 = bsp.transformation(1.0);
self.assertMatricesEqual(T_n_1, curve_T_n_1, 1e-9,"T_n_1")
tend = 2.0
# Extend the segment.
T_n_25 = bsp.curveValueToTransformation(numpy.random.random(6))
bsp.addPoseSegment(tend, T_n_25);
# Check the values.
self.assertEqual(bsp.t_min(),0.0);
self.assertEqual(bsp.t_max(),tend);
curve_T_n_0 = bsp.transformation(0.0);
self.assertMatricesEqual(T_n_0, curve_T_n_0, 1e-6, "T_n_0")
curve_T_n_1 = bsp.transformation(1.0);
self.assertMatricesEqual(T_n_1, curve_T_n_1, 1e-4, "T_n_1")
curve_T_n_25 = bsp.transformation(tend);
self.assertMatricesEqual(T_n_25, curve_T_n_25, 1e-4, "T_n_25")
def __generateInitialSpline(self, splineOrder, timeOffsetPadding, numberOfKnots = None, framerate = None):
poseSpline = bsplines.BSplinePose(splineOrder, sm.RotationVector())
# Get the observation times.
times = np.array([observation.time().toSec() for observation in self.__observations ])
# get the pose values of the initial transformations at observation time
curve = np.matrix([ poseSpline.transformationToCurveValue( observation.T_t_c().T() ) for observation in self.__observations]).T
# make sure all values are well defined
if np.isnan(curve).any():
raise RuntimeError("Nans in curve values")
sys.exit(0)
# Add 2 seconds on either end to allow the spline to slide during optimization
times = np.hstack((times[0] - (timeOffsetPadding * 2.0), times, times[-1] + (timeOffsetPadding * 2.0)))
curve = np.hstack((curve[:,0], curve, curve[:,-1]))
self.__ensureContinuousRotationVectors(curve)
seconds = times[-1] - times[0]
# fixed number of knots
if (numberOfKnots is not None):
knots = numberOfKnots
# otherwise with framerate estimate
else:
knots = int(round(seconds * framerate/3))
print
print "Initializing a pose spline with %d knots (%f knots per second over %f seconds)" % ( knots, 100, seconds)
poseSpline.initPoseSplineSparse(times, curve, knots, 1e-4)
return poseSpline
def testCurveToTransformation(self):
rvs = (sm.RotationVector(), sm.EulerAnglesZYX(), sm.EulerRodriguez())
for r in rvs:
bsp = bsplines.BSplinePose(4,r)
# Build a random, valid transformation.
T1 = bsp.curveValueToTransformation(numpy.random.random(6))
p = bsp.transformationToCurveValue(T1)
T2 = bsp.curveValueToTransformation(p)
self.assertMatricesEqual(T1, T2, 1e-9,"Checking the invertiblity of the transformation to curve values:")
def initPoseSplineFromCamera(self,
splineOrder=6,
poseKnotsPerSecond=100,
timeOffsetPadding=0.02):
T_c_b = self.T_extrinsic.T()
pose = bsplines.BSplinePose(splineOrder, sm.RotationVector())
# Get the checkerboard times.
times = np.array([
obs.time().toSec() + self.timeshiftCamToImuPrior
for obs in self.targetObservations
])
curve = np.matrix([
pose.transformationToCurveValue(np.dot(obs.T_t_c().T(), T_c_b))
for obs in self.targetObservations
]).T
if np.isnan(curve).any():
raise RuntimeError("Nans in curve values")
sys.exit(0)
# Add 2 seconds on either end to allow the spline to slide during optimization
times = np.hstack((times[0] - (timeOffsetPadding * 2.0), times,
times[-1] + (timeOffsetPadding * 2.0)))
curve = np.hstack((curve[:, 0], curve, curve[:, -1]))
# Make sure the rotation vector doesn't flip
for i in range(1, curve.shape[1]):
previousRotationVector = curve[3:6, i - 1]
r = curve[3:6, i]
angle = np.linalg.norm(r)
axis = r / angle
best_r = r
best_dist = np.linalg.norm(best_r - previousRotationVector)
for s in range(-3, 4):
aa = axis * (angle + math.pi * 2.0 * s)
dist = np.linalg.norm(aa - previousRotationVector)
if dist < best_dist:
best_r = aa
best_dist = dist
curve[3:6, i] = best_r
seconds = times[-1] - times[0]
knots = int(round(seconds * poseKnotsPerSecond))
print
print "Initializing a pose spline with %d knots (%f knots per second over %f seconds)" % (
knots, poseKnotsPerSecond, seconds)
pose.initPoseSplineSparse(times, curve, knots, 1e-4)
return pose
def testInversePose2(self):
rvs = (sm.RotationVector(), sm.EulerAnglesZYX(), sm.EulerRodriguez())
for r in rvs:
bsp = bsplines.BSplinePose(4,r)
# Create two random transformations.
T_n_0 = bsp.curveValueToTransformation(numpy.random.random(6))
T_n_1 = bsp.curveValueToTransformation(numpy.random.random(6))
# Initialize the curve.
bsp.initPoseSpline(0.0,1.0,T_n_0, T_n_1)
for t in numpy.arange(0.0,1.0,0.1):
T = bsp.transformation(t)
invT,J,C = bsp.inverseTransformationAndJacobian(t)
one = numpy.dot(T,invT)
self.assertMatricesEqual(one,numpy.eye(4),1e-14,"T * inv(T)")
def __init__(self,
order=4,
target_point=[0., 0, 0],
ctrl_point_num=10,
time=10.,
random_range=[[0, 0, 0.], [1., 1., 1.]]):
self.target_point = np.array(target_point)
self.bsp = bsplines.BSplinePose(4, sm.RotationVector())
self.ctrl_point_num = ctrl_point_num
random_range = np.array(random_range) + self.target_point
self.curve = self.TargetOrientedPose(ctrl_point_num, random_range)
self.time = time
times = np.linspace(0, time, ctrl_point_num)
curve_in = np.array(self.curve)
curve_in[3:] *= -1
self.bsp.initPoseSplineSparse(times, curve_in,
int(ctrl_point_num * 3.), 1e-5)
def testInverseOrientationJacobian(self):
rvs = (sm.RotationVector(), sm.EulerAnglesZYX(), sm.EulerRodriguez())
for r in rvs:
for order in range(2,7):
bsp = bsplines.BSplinePose(order,r)
T_n_0 = bsp.curveValueToTransformation(numpy.random.random(6))
T_n_1 = bsp.curveValueToTransformation(numpy.random.random(6))
# Initialize the curve.
bsp.initPoseSpline(0.0,1.0,T_n_0, T_n_1)
for t in numpy.linspace(bsp.t_min(), bsp.t_max(), 4):
# Create a random homogeneous vector
v = numpy.random.random(3)
CJI = bsp.inverseOrientationAndJacobian(t);
#print "TJI: %s" % (TJI)
je = nd.Jacobian(lambda c: bsp.setLocalCoefficientVector(t,c) or numpy.dot(bsp.inverseOrientation(t), v))
estJ = je(bsp.localCoefficientVector(t))
JT = CJI[1]
J = numpy.dot(sm.crossMx(numpy.dot(CJI[0],v)), JT)
self.assertMatricesEqual(J, estJ, 1e-8,"C_n_0")
def stereoCalibrate(camL_geometry,
camH_geometry,
obslist,
distortionActive=False,
baseline=None):
#####################################################
## find initial guess as median of all pnp solutions
#####################################################
if baseline is None:
r = []
t = []
for obsL, obsH in obslist:
#if we have observations for both camss
if obsL is not None and obsH is not None:
success, T_L = camL_geometry.geometry.estimateTransformation(
obsL)
success, T_H = camH_geometry.geometry.estimateTransformation(
obsH)
baseline = T_H.inverse() * T_L
t.append(baseline.t())
rv = sm.RotationVector()
r.append(rv.rotationMatrixToParameters(baseline.C()))
r_median = np.median(np.asmatrix(r), axis=0).flatten().T
R_median = rv.parametersToRotationMatrix(r_median)
t_median = np.median(np.asmatrix(t), axis=0).flatten().T
baseline_HL = sm.Transformation(sm.rt2Transform(R_median, t_median))
else:
baseline_HL = baseline
#verbose output
if sm.getLoggingLevel() == sm.LoggingLevel.Debug:
dL = camL_geometry.geometry.projection().distortion().getParameters(
).flatten()
pL = camL_geometry.geometry.projection().getParameters().flatten()
dH = camH_geometry.geometry.projection().distortion().getParameters(
).flatten()
pH = camH_geometry.geometry.projection().getParameters().flatten()
sm.logDebug("initial guess for stereo calib: {0}".format(
baseline_HL.T()))
sm.logDebug("initial guess for intrinsics camL: {0}".format(pL))
sm.logDebug("initial guess for intrinsics camH: {0}".format(pH))
sm.logDebug("initial guess for distortion camL: {0}".format(dL))
sm.logDebug("initial guess for distortion camH: {0}".format(dH))
############################################
## solve the bundle adjustment
############################################
problem = aopt.OptimizationProblem()
#baseline design variable
baseline_dv = addPoseDesignVariable(problem, baseline_HL)
#target pose dv for all target views (=T_camL_w)
target_pose_dvs = list()
for obsL, obsH in obslist:
if obsL is not None: #use camL if we have an obs for this one
success, T_t_cL = camL_geometry.geometry.estimateTransformation(
obsL)
else:
success, T_t_cH = camH_geometry.geometry.estimateTransformation(
obsH)
T_t_cL = T_t_cH * baseline_HL #apply baseline for the second camera
target_pose_dv = addPoseDesignVariable(problem, T_t_cL)
target_pose_dvs.append(target_pose_dv)
#add camera dvs
camL_geometry.setDvActiveStatus(True, distortionActive, False)
camH_geometry.setDvActiveStatus(True, distortionActive, False)
problem.addDesignVariable(camL_geometry.dv.distortionDesignVariable())
problem.addDesignVariable(camL_geometry.dv.projectionDesignVariable())
problem.addDesignVariable(camL_geometry.dv.shutterDesignVariable())
problem.addDesignVariable(camH_geometry.dv.distortionDesignVariable())
problem.addDesignVariable(camH_geometry.dv.projectionDesignVariable())
problem.addDesignVariable(camH_geometry.dv.shutterDesignVariable())
############################################
## add error terms
############################################
#corner uncertainty
# \todo pass in the detector uncertainty somehow.
cornerUncertainty = 1.0
R = np.eye(2) * cornerUncertainty * cornerUncertainty
invR = np.linalg.inv(R)
#Add reprojection error terms for both cameras
reprojectionErrors0 = []
reprojectionErrors1 = []
for cidx, cam in enumerate([camL_geometry, camH_geometry]):
sm.logDebug(
"stereoCalibration: adding camera error terms for {0} calibration targets"
.format(len(obslist)))
#get the image and target points corresponding to the frame
target = cam.ctarget.detector.target()
#add error terms for all observations
for view_id, obstuple in enumerate(obslist):
#add error terms if we have an observation for this cam
obs = obstuple[cidx]
if obs is not None:
T_cam_w = target_pose_dvs[view_id].toExpression().inverse()
#add the baseline for the second camera
if cidx != 0:
T_cam_w = baseline_dv.toExpression() * T_cam_w
for i in range(0, target.size()):
p_target = aopt.HomogeneousExpression(
sm.toHomogeneous(target.point(i)))
valid, y = obs.imagePoint(i)
if valid:
# Create an error term.
rerr = cam.model.reprojectionError(
y, invR, T_cam_w * p_target, cam.dv)
rerr.idx = i
problem.addErrorTerm(rerr)
if cidx == 0:
reprojectionErrors0.append(rerr)
else:
reprojectionErrors1.append(rerr)
sm.logDebug("stereoCalibrate: added {0} camera error terms".format(
len(reprojectionErrors0) + len(reprojectionErrors1)))
############################################
## solve
############################################
options = aopt.Optimizer2Options()
options.verbose = True if sm.getLoggingLevel(
) == sm.LoggingLevel.Debug else False
options.nThreads = 4
options.convergenceDeltaX = 1e-3
options.convergenceDeltaJ = 1
options.maxIterations = 200
options.trustRegionPolicy = aopt.LevenbergMarquardtTrustRegionPolicy(10)
optimizer = aopt.Optimizer2(options)
optimizer.setProblem(problem)
#verbose output
if sm.getLoggingLevel() == sm.LoggingLevel.Debug:
sm.logDebug("Before optimization:")
e2 = np.array([e.evaluateError() for e in reprojectionErrors0])
sm.logDebug(
" Reprojection error squarred (camL): mean {0}, median {1}, std: {2}"
.format(np.mean(e2), np.median(e2), np.std(e2)))
e2 = np.array([e.evaluateError() for e in reprojectionErrors1])
sm.logDebug(
" Reprojection error squarred (camH): mean {0}, median {1}, std: {2}"
.format(np.mean(e2), np.median(e2), np.std(e2)))
sm.logDebug("baseline={0}".format(
baseline_dv.toTransformationMatrix()))
try:
retval = optimizer.optimize()
if retval.linearSolverFailure:
sm.logError("stereoCalibrate: Optimization failed!")
success = not retval.linearSolverFailure
except:
sm.logError("stereoCalibrate: Optimization failed!")
success = False
if sm.getLoggingLevel() == sm.LoggingLevel.Debug:
sm.logDebug("After optimization:")
e2 = np.array([e.evaluateError() for e in reprojectionErrors0])
sm.logDebug(
" Reprojection error squarred (camL): mean {0}, median {1}, std: {2}"
.format(np.mean(e2), np.median(e2), np.std(e2)))
e2 = np.array([e.evaluateError() for e in reprojectionErrors1])
sm.logDebug(
" Reprojection error squarred (camH): mean {0}, median {1}, std: {2}"
.format(np.mean(e2), np.median(e2), np.std(e2)))
#verbose output
if sm.getLoggingLevel() == sm.LoggingLevel.Debug:
dL = camL_geometry.geometry.projection().distortion().getParameters(
).flatten()
pL = camL_geometry.geometry.projection().getParameters().flatten()
dH = camH_geometry.geometry.projection().distortion().getParameters(
).flatten()
pH = camH_geometry.geometry.projection().getParameters().flatten()
sm.logDebug("guess for intrinsics camL: {0}".format(pL))
sm.logDebug("guess for intrinsics camH: {0}".format(pH))
sm.logDebug("guess for distortion camL: {0}".format(dL))
sm.logDebug("guess for distortion camH: {0}".format(dH))
if success:
baseline_HL = sm.Transformation(baseline_dv.toTransformationMatrix())
return success, baseline_HL
else:
#return the initial guess if we fail
return success, baseline_HL
import sm
import bsplines
import numpy as np
from scipy.spatial.transform import Rotation as R
if __name__ == "__main__":
bsp = bsplines.BSplinePose(4, sm.RotationVector())
curve = np.array([[0.1, 0.2, 0.3, 0.4, 0.5, 0.6], [0.1, 0, 0, 0, 0, 0.1],
[0.2, 0, 0, 0, 0, 0.1]]).T
bsp.initPoseSplineSparse(np.array([0., 1, 2]), curve, 5, 1e-5)
r0 = R.from_dcm(bsp.orientation(0.0)).as_rotvec()
r0_ = R.from_dcm(bsp.curveValueToTransformation(
curve.T[0])[:3, :3]).as_rotvec()
t0_ = bsp.curveValueToTransformation(curve.T[0])[:3, 3]
print(r0)
print(t0_)
