def create_planar_bundle(noise=.1):
NUM_CAMERAS = 4
NUM_POINTS = 12
POINT_CLOUD_RADIUS = 5.
MISSING_FRAC = 0 # fraction of measurements that are missing
OUTLIER_FRAC = 0 # fraction of measurements that are outliers
OUTLIER_RANGE = 10. # bounds of uniform distribution from which outliers are sampled
CAUCHY_PARAM = .05 # determines the width of the robustifier
# Setup some cameras
np.random.seed(1111) # for repeatability
K = np.array([[ 2., 0., -.5 ],
[ .05, 3., .1 ],
[ 0., 0., 1. ]])
# Random rotations; zero translation
Rs = [ SO3.exp(np.random.rand(3)*.1) for i in range(NUM_CAMERAS) ]
ts = np.zeros((NUM_CAMERAS, 3))
# Sample 3D points
r = POINT_CLOUD_RADIUS
pts = np.random.uniform(-r, r, (NUM_POINTS, 3))
pts[:,2] += 10 # ensure points are not close to focal plane
# Compute ideal projections and add noise
msm = np.array([[ bundle.project(K, R, t, pt) for pt in pts ]
for (R,t) in zip(Rs,ts) ])
msm += np.random.randn(*msm.shape) * noise
# Mark some measurements as missing
np.random.seed(4309) # for repeatability
msm_mask = np.ones(msm.shape[:2], bool)
nmissing = int(MISSING_FRAC * NUM_POINTS)
if nmissing > 0:
for i in range(NUM_CAMERAS):
missing_inds = np.random.permutation(NUM_POINTS)[:nmissing]
msm_mask[i, missing_inds] = False
# Generate some outliers by replacing measurements with random data
np.random.seed(101) # for repeatability
outlier_mask = np.zeros((NUM_CAMERAS, NUM_POINTS), bool)
noutliers = int(OUTLIER_FRAC * NUM_POINTS)
if noutliers > 0:
for i in range(NUM_CAMERAS):
outlier_inds = np.random.permutation(NUM_POINTS)[:noutliers]
outlier_mask[i,outlier_inds] = True
for j in outlier_inds:
msm[i,j] = np.random.uniform(-OUTLIER_RANGE, OUTLIER_RANGE, 2)
# Create the bundle
b = bundle.Bundle.FromArrays(K, Rs, ts, pts, msm, msm_mask)
# Attach robustifier
b.sensor_model = sensor_model.GaussianModel(.1)
#b.sensor_model = sensor_model.CauchyModel(CAUCHY_PARAM)
# Store this for visualization later
b.outlier_mask = outlier_mask
return b
def create_planar_bundle(noise=.1):
NUM_CAMERAS = 4
NUM_POINTS = 12
POINT_CLOUD_RADIUS = 5.
MISSING_FRAC = 0 # fraction of measurements that are missing
OUTLIER_FRAC = 0 # fraction of measurements that are outliers
OUTLIER_RANGE = 10. # bounds of uniform distribution from which outliers are sampled
CAUCHY_PARAM = .05 # determines the width of the robustifier
# Setup some cameras
np.random.seed(1111) # for repeatability
K = np.array([[2., 0., -.5], [.05, 3., .1], [0., 0., 1.]])
# Random rotations; zero translation
Rs = [SO3.exp(np.random.rand(3) * .1) for i in range(NUM_CAMERAS)]
ts = np.zeros((NUM_CAMERAS, 3))
# Sample 3D points
r = POINT_CLOUD_RADIUS
pts = np.random.uniform(-r, r, (NUM_POINTS, 3))
pts[:, 2] += 10 # ensure points are not close to focal plane
# Compute ideal projections and add noise
msm = np.array([[bundle.project(K, R, t, pt) for pt in pts]
for (R, t) in zip(Rs, ts)])
msm += np.random.randn(*msm.shape) * noise
# Mark some measurements as missing
np.random.seed(4309) # for repeatability
msm_mask = np.ones(msm.shape[:2], bool)
nmissing = int(MISSING_FRAC * NUM_POINTS)
if nmissing > 0:
for i in range(NUM_CAMERAS):
missing_inds = np.random.permutation(NUM_POINTS)[:nmissing]
msm_mask[i, missing_inds] = False
# Generate some outliers by replacing measurements with random data
np.random.seed(101) # for repeatability
outlier_mask = np.zeros((NUM_CAMERAS, NUM_POINTS), bool)
noutliers = int(OUTLIER_FRAC * NUM_POINTS)
if noutliers > 0:
for i in range(NUM_CAMERAS):
outlier_inds = np.random.permutation(NUM_POINTS)[:noutliers]
outlier_mask[i, outlier_inds] = True
for j in outlier_inds:
msm[i, j] = np.random.uniform(-OUTLIER_RANGE, OUTLIER_RANGE, 2)
# Create the bundle
b = bundle.Bundle.FromArrays(K, Rs, ts, pts, msm, msm_mask)
# Attach robustifier
b.sensor_model = sensor_model.GaussianModel(.1)
#b.sensor_model = sensor_model.CauchyModel(CAUCHY_PARAM)
# Store this for visualization later
b.outlier_mask = outlier_mask
return b
def generate_measurements(K, R, t, pts, noise):
assert pts.ndim == 2, 'pts.shape = '+str(pts.shape)
assert pts.shape[1] == 3, 'pts.shape = '+str(pts.shape)
return np.array([ bundle.project(K, R, t, x) + np.random.randn(2)*noise
for x in pts ])
def generate_measurements(K, R, t, pts, noise):
assert pts.ndim == 2, 'pts.shape = ' + str(pts.shape)
assert pts.shape[1] == 3, 'pts.shape = ' + str(pts.shape)
return np.array(
[bundle.project(K, R, t, x) + np.random.randn(2) * noise for x in pts])