Esempio n. 1
0
    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('-----')
Esempio n. 2
0
    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('-----')
Esempio n. 3
0
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()
Esempio n. 4
0
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()