def calculate_ls(self,iters, ransac_iters):
self.ls_beta = pose_estimate.least_square(self.X, self.Y)
self.ls_pose = tf.get_projection_matrix(self.ls_beta)
self.ls_position = self.ls_pose.dot(self.ls_position)
self.me_beta = pose_estimate.m_estimator(self.X, self.Y, iters)
self.me_pose = tf.get_projection_matrix(self.me_beta)
self.m_position = self.me_pose.dot(self.m_position)
init_beta = pose_estimate.ransac_ls(self.X,self.Y,ransac_iters,3)
self.ransac_beta = pose_estimate.m_estimator(self.X, self.Y, iters, init_beta)
self.ransac_pose = tf.get_projection_matrix(self.ransac_beta)
self.r_position = self.ransac_pose.dot(self.r_position)
self.ls_noise_beta = pose_estimate.least_square(self.X_n, self.Y_n)
self.ls_noise_pose = tf.get_projection_matrix(self.ls_noise_beta)
self.ls_position_noise = self.ls_noise_pose.dot(self.ls_position_noise)
self.me_noise_beta = pose_estimate.m_estimator(self.X_n, self.Y_n, iters)
self.me_noise_pose = tf.get_projection_matrix(self.me_noise_beta)
self.m_position_noise = self.me_noise_pose.dot(self.m_position_noise)
# init_beta = pose_estimate.ransac_ls(self.X,self.Y,50,3)
# self.ransac_noise_beta = pose_estimate.m_estimator(self.X_n, self.Y_n, iters, init_beta)
# self.ransac_noise_pose = tf.get_projection_matrix(self.ransac_noise_beta)
# self.r_position_noise = self.ransac_noise_pose.dot(self.r_position_noise)
bp = tf.pose_to_beta(self.me_pose)
self.kf_pose_ls.predict()
self.kf_pose_ls.update(bp.T)
print('-----')
print(ut.to_string(bp))
print(ut.to_string(self.kf_pose_ls.x.T))
print('-----')
def calculate_ls(self, iters, ransac_iters):
self.ls_beta = pose_estimate.least_square(self.X, self.Y)
self.ls_pose = tf.get_projection_matrix(self.ls_beta)
self.ls_position = self.ls_pose.dot(self.ls_position)
self.me_beta = pose_estimate.m_estimator(self.X, self.Y, iters)
self.me_pose = tf.get_projection_matrix(self.me_beta)
self.m_position = self.me_pose.dot(self.m_position)
init_beta = pose_estimate.ransac_ls(self.X, self.Y, ransac_iters, 3)
self.ransac_beta = pose_estimate.m_estimator(self.X, self.Y, iters,
init_beta)
self.ransac_pose = tf.get_projection_matrix(self.ransac_beta)
self.r_position = self.ransac_pose.dot(self.r_position)
self.ls_noise_beta = pose_estimate.least_square(self.X_n, self.Y_n)
self.ls_noise_pose = tf.get_projection_matrix(self.ls_noise_beta)
self.ls_position_noise = self.ls_noise_pose.dot(self.ls_position_noise)
self.me_noise_beta = pose_estimate.m_estimator(self.X_n, self.Y_n,
iters)
self.me_noise_pose = tf.get_projection_matrix(self.me_noise_beta)
self.m_position_noise = self.me_noise_pose.dot(self.m_position_noise)
# init_beta = pose_estimate.ransac_ls(self.X,self.Y,50,3)
# self.ransac_noise_beta = pose_estimate.m_estimator(self.X_n, self.Y_n, iters, init_beta)
# self.ransac_noise_pose = tf.get_projection_matrix(self.ransac_noise_beta)
# self.r_position_noise = self.ransac_noise_pose.dot(self.r_position_noise)
bp = tf.pose_to_beta(self.me_pose)
self.kf_pose_ls.predict()
self.kf_pose_ls.update(bp.T)
print('-----')
print(ut.to_string(bp))
print(ut.to_string(self.kf_pose_ls.x.T))
print('-----')
def run_esperiment_dummy():
# coefficients of the model
a1, a2, a3 = 0.1, -0.2, 4.0
# ground truth
A_gt = [a1, a2, a3]
#print 'A_gt = ', A_gt
# create a coordinate matrix
nx = np.linspace(-1, 1, 41)
ny = np.linspace(-1, 1, 41)
x, y = np.meshgrid(nx, ny)
# make the estimation
z = a1 * x + a2 * y + a3
# let's add some gaussian noise
z_noise = z + 0.1 * np.random.standard_normal(z.shape)
x_fl = x.flatten()
y_fl = y.flatten()
z_ones = np.ones([x.size, 1])
X = np.hstack((np.reshape(x_fl, ([len(x_fl), 1])),
np.reshape(y_fl, ([len(y_fl), 1])), z_ones))
Z = np.zeros(z_noise.shape)
Z[:] = z_noise
Z_fl = Z.flatten()
Z = np.reshape(Z_fl, ([len(Z_fl), 1]))
# create outliers
outlier_prop = 0.3
outlier_IND = np.random.permutation(x.size)
outlier_IND = outlier_IND[0:np.floor(x.size * outlier_prop)]
z_noise_outlier = np.zeros(z_noise.shape)
z_noise_outlier[:] = z_noise
z_noise_outlier_flt = z_noise_outlier.flatten()
z_noise_outlier_flt[outlier_IND] = z_noise_outlier_flt[
outlier_IND] + 10 * np.random.standard_normal(
z_noise_outlier_flt[outlier_IND].shape)
z_noise_outlier = np.reshape(z_noise_outlier_flt, z.shape)
# non-robust least squares estimation
Z = np.zeros(z_noise_outlier.shape)
Z[:] = z_noise_outlier
Z_fl = Z.flatten()
Z = np.reshape(Z_fl, ([len(Z_fl), 1]))
beta = pose_estimate.least_square(X, Z)
beta_m = pose_estimate.m_estimator(X, Z, 5)
beta_r = pose_estimate.ransac_ls(X, Z, 10, 5)
beta_r = pose_estimate.m_estimator(X, Z, 5, beta_r)
z_lsq_outlier = np.dot(X, beta)
z_lsq_outlier = np.reshape(z_lsq_outlier, z.shape)
z_m_outlier = np.dot(X, beta_m)
z_m_outlier = np.reshape(z_m_outlier, z.shape)
z_r_outlier = np.dot(X, beta_r)
z_r_outlier = np.reshape(z_r_outlier, z.shape)
lsq_non_robust_outlier = np.hstack(
(z, z_noise_outlier, z_lsq_outlier, z_m_outlier, z_r_outlier))
plt.figure()
plt.title('Non-robust estimate (corrupted by noise AND outliers)')
plt.imshow(lsq_non_robust_outlier)
plt.clim(z.min(), z.max())
plt.show()
def run_esperiment_dummy():
# coefficients of the model
a1, a2, a3 = 0.1, -0.2, 4.0
# ground truth
A_gt = [a1, a2, a3]
#print 'A_gt = ', A_gt
# create a coordinate matrix
nx = np.linspace(-1, 1, 41)
ny = np.linspace(-1, 1, 41)
x, y = np.meshgrid(nx, ny)
# make the estimation
z = a1*x + a2*y + a3
# let's add some gaussian noise
z_noise = z + 0.1*np.random.standard_normal(z.shape)
x_fl = x.flatten()
y_fl = y.flatten()
z_ones = np.ones([x.size,1])
X = np.hstack((np.reshape(x_fl, ([len(x_fl),1])), np.reshape(y_fl, ([len(y_fl),1])), z_ones))
Z = np.zeros(z_noise.shape)
Z[:] = z_noise
Z_fl = Z.flatten()
Z = np.reshape(Z_fl, ([len(Z_fl),1]))
# create outliers
outlier_prop = 0.3
outlier_IND = np.random.permutation(x.size)
outlier_IND = outlier_IND[0:np.floor(x.size * outlier_prop)]
z_noise_outlier = np.zeros(z_noise.shape)
z_noise_outlier[:] = z_noise
z_noise_outlier_flt = z_noise_outlier.flatten()
z_noise_outlier_flt[outlier_IND] = z_noise_outlier_flt[outlier_IND] + 10*np.random.standard_normal(z_noise_outlier_flt[outlier_IND].shape)
z_noise_outlier = np.reshape(z_noise_outlier_flt, z.shape)
# non-robust least squares estimation
Z = np.zeros(z_noise_outlier.shape)
Z[:] = z_noise_outlier
Z_fl = Z.flatten()
Z = np.reshape(Z_fl, ([len(Z_fl),1]))
beta = pose_estimate.least_square(X,Z)
beta_m = pose_estimate.m_estimator(X,Z,5)
beta_r = pose_estimate.ransac_ls(X,Z,10,5)
beta_r = pose_estimate.m_estimator(X,Z,5,beta_r)
z_lsq_outlier = np.dot(X, beta)
z_lsq_outlier = np.reshape(z_lsq_outlier, z.shape)
z_m_outlier = np.dot(X, beta_m)
z_m_outlier = np.reshape(z_m_outlier, z.shape)
z_r_outlier = np.dot(X, beta_r)
z_r_outlier = np.reshape(z_r_outlier, z.shape)
lsq_non_robust_outlier = np.hstack((z, z_noise_outlier, z_lsq_outlier, z_m_outlier, z_r_outlier))
plt.figure()
plt.title('Non-robust estimate (corrupted by noise AND outliers)')
plt.imshow(lsq_non_robust_outlier)
plt.clim(z.min(), z.max())
plt.show()
