models.py 文件源码

def optimize_model_coefficients(self, inliers, model_coefficients):
        '''
        Recompute the model coefficients using the given inlier set and return them to the user.

        These are the coefficients of the model after refinement
        (e.g., after SVD)

        # Parameters
        inliers : list of int
            The data inliers supporting the model
        model_coefficients : coefficients array
            The initial guess for the model coefficients

        # Returns
        optimized_coefficients : coefficients array
            The resultant recomputed coefficients after non-linear optimization
        '''
        logger = logging.getLogger('pcl.sac.SampleConsensusModel.optimize_model_coefficients')
        if len(model_coefficients) != self.model_size:
            logger.error('Invalid number of model coefficients given (%lu).',
                         len(model_coefficients))
            return model_coefficients
        if len(inliers) <= self.sample_size:
            logger.error('Not enough inliers found to optimize model coefficients (%lu)!',
                         len(model_coefficients))
            return model_coefficients

        ###### Followings are implemetation of original PCL using PCA ######

        # covariance_matrix, xyz_centroid = compute_mean_and_covariance_matrix(self._input, inliers)
        # eigen_value, eigen_vector = np.linalg.eigh(covariance_matrix)
        # smallest = np.argmin(eigen_value)
        #
        # optimized_coefficients = eigen_vector[:, smallest].tolist()
        # optimized_coefficients.append(-np.dot(optimized_coefficients + [0], xyz_centroid))

        cloud = self._input.xyz
        if inliers is not None:
            cloud = cloud[inliers]

        # Use Least-Squares to fit the plane through all the given sample points
        # and find out its coefficients
        constant = -np.ones(len(cloud))
        optimized_coefficients, *_ = lstsq(cloud, constant)
        optimized_coefficients = optimized_coefficients.tolist()
        optimized_coefficients.append(1)

        if not self._is_model_valid(optimized_coefficients):
            logger.warning('Optimized coefficients invalid, returning original one')
            return model_coefficients

        return optimized_coefficients