Question: RANSAC and ICP repeat on condition

[rukie]

I'm using a Velodyne HDL 64 and I know my images scene to scene will be small translations. For the most part, my transformation matrix will be close to 1 0 0 0 0 1 0 0 0 0 1 0 0 0 0 1

Occasionally, I'll end up with a transformation matrix that'll rotate an image by 90 degrees. Would you have any suggestions on how to auto-attempt this again? Should I check the values (0,0), (1,1), (2,2), (3,3) as being >0.99? Or would a math function on the whole matrix possibly be quicker? Just looking for a second opinion I guess. Maybe there's another approach I missed?

I'll be checking the values from edit: pose_graph.nodes[point_id].pose edit:

Thanks again for the great tool!

[syncle]

Hi @rukie. Can you share code snippet or data so that I can regenerate your issue? Presumably this issue may be related to #276.

[rukie]

Currently I have a vehicle roof mounted HDL 64. It only scans the forward 180 degrees. I'd say about 95% of the time my RANSAC and ICP settings work as expected.

#!/usr/bin/python3
# src/Python/Tutorial/Advanced/multiway_registration.py
import numpy as np
import sys
sys.path.append("/share/CFD/ProjectTerraScan/libraries/Open3D-master/build/lib") # Linux build location for Open3D
from py3d import *

def main(in_dir,out_dir='./'):
    '''
    in_dir  <- directory where the xyz files are located
    out_dir     <- directory to save results
    '''
###
    set_verbosity_level(VerbosityLevel.Debug)
    if in_dir[-1] != '/':
        in_dir+='/'
    if out_dir[-1] != '/':
        out_dir +='/'
####

    initial_frame = 0 # Initial frame
    skip_frame = 2 # Number of frames to skip
    count_frame = 3 #
    end_frame = 45312
    # Calculate how many groups are needed
    # The small and large groups determine the window of files to create.
    # A large initial group will skip earlier frames.
    ngroupinit = int(163)
    ngrouplarge = int((end_frame/skip_frame)//count_frame)

    for ngroups in range(ngroupinit,ngrouplarge,1):
    #for ngroups in range(5,10,1):
        print("{} of {} ngroups completed.".format(ngroups, ngrouplarge))
        start_frame = initial_frame + ngroups*count_frame*skip_frame
        end_frame = start_frame + count_frame*skip_frame
        # Example start 550, skip frame 5, count frame 5
        # Start 550, 575, 600
        # End Frame, 575, 600, 625

        pcds = []

        normals_radius = 0.5
        normals_nn = 400
        crop_range_min = np.array([-15,0,-15])
        crop_range_max = np.array([-15,35,-15])

        for i in range(start_frame,end_frame,skip_frame):
            pcd = read_point_cloud(in_dir+"Frame_{:05d}.xyzrgb".format(i))
            #downpcd = voxel_down_sample(pcd, voxel_size = 0.25)
            pcds.append(pcd)
        #draw_geometries(pcds)

        pose_graph = PoseGraph()
        odometry = np.identity(4)
        pose_graph.nodes.append(PoseGraphNode(odometry))

        n_pcds = len(pcds)
        for source_id in range(n_pcds):
            source = pcds[source_id]
            estimate_normals(source, KDTreeSearchParamRadius(normals_radius))
            #estimate_normals(source_down, KDTreeSearchParamHybrid(
            #    normals_radius, normals_nn))
            orient_normals_towards_camera_location(source, np.array([0,0,12]))
            source_down = voxel_down_sample(source, 0.025)
            source_crop = crop_point_cloud(source_down, np.array([-15,0,-15]), np.array([15,35,15]))
            source_fpfh = compute_fpfh_feature(source_crop,
                KDTreeSearchParamHybrid(normals_radius, normals_nn))

            for target_id in range(source_id + 1, n_pcds):
                print("Frame {} and {} of {}".format(source_id, target_id, n_pcds))
                target = pcds[target_id]
                print("Estimate normal with search radius 0.1.")
                estimate_normals(target, KDTreeSearchParamRadius(normals_radius))
                #estimate_normals(target_down, KDTreeSearchParamHybrid(
                #    normals_radius, normals_nn))
                orient_normals_towards_camera_location(target, np.array([0,0,10]))            
                print("2. Downsample with a voxel size 0.05.")
                target_down = voxel_down_sample(target, 0.025)            
                print("X. Crop point cloud to work with closer information.")
                target_crop = crop_point_cloud(target_down, np.array([-15,0,-15]), np.array([15,35,15]))

                print("Compute FPFH feature with search radius 0.25")
                        #KDTreeSearchParamRadius(normals_radius))
                target_fpfh = compute_fpfh_feature(target_crop,
                        KDTreeSearchParamHybrid(normals_radius, normals_nn))
                        #KDTreeSearchParamRadius(normals_radius))

                print("5. RANSAC registration on downsampled point clouds.")
                print("   Since the downsampling voxel size is 0.05, we use a liberal")
                print("   distance threshold 0.075.")
                methodRANSAC = 1
                if methodRANSAC == 1:
                    (success_ransac, result_ransac) = register_point_cloud_fpfh(
                            source_crop, target_crop,
                            source_fpfh, target_fpfh)
                    if success_ransac:
                        print("RANSAC succeeded! \n")
                        init_transform = result_ransac.transformation
                        print(result_ransac)
                    else:
                        print("RANSAC Search failed \n")
                        init_transform = result_ransac
                else:
                    init_transform = np.identity(4)

                methodICP = 1
                if methodICP == 1:
                    print("Apply point-to-plane ICP")
                    icp_coarse = registration_icp(source_crop, target_crop, 0.6,
                            init_transform,
                            TransformationEstimationPointToPlane())
                    icp_fine = registration_icp(source_crop, target_crop, 0.04,
                            icp_coarse.transformation,
                            TransformationEstimationPointToPlane())
                    icp_fine = registration_colored_icp(source_crop, target_crop,
                        0.01, icp_coarse.transformation,
                    ICPConvergenceCriteria(relative_fitness = 1e-6,
                        relative_rmse = 1e-6, max_iteration = 5000))
                    transformation_icp = icp_fine.transformation
                    information_icp = get_information_matrix_from_point_clouds(
                            source, target, 0.01, icp_fine.transformation)
                    print(transformation_icp)
                if methodICP == 2:
                    icp_coarse = registration_icp(source_crop, target_crop, .1,
                            init_transform,
                            TransformationEstimationPointToPlane())
                    icp_fine = registration_icp(source_crop, target_crop, .01,
                            icp_coarse.transformation,
                            TransformationEstimationPointToPoint())
                    result_icp = registration_colored_icp(source_crop, target_crop,
                        0.025, icp_fine.transformation,
                        ICPConvergenceCriteria(relative_fitness = 1e-4,
                        relative_rmse = 1e-4, max_iteration = 75))
                    transformation_icp = result_icp.transformation
                    information_icp = get_information_matrix_from_point_clouds(
                            source, target, 0.05, result_icp.transformation)

                # draw_registration_result(source, target, np.identity(4))
                print("Build PoseGraph")
                if target_id == source_id + 1: # odometry case
                    odometry = np.dot(transformation_icp, odometry)
                    pose_graph.nodes.append(
                            PoseGraphNode(np.linalg.inv(odometry)))
                    pose_graph.edges.append(
                            PoseGraphEdge(source_id, target_id,
                            transformation_icp, information_icp, uncertain = False))
                else: # loop closure case
                    pose_graph.edges.append(
                            PoseGraphEdge(source_id, target_id,
                            transformation_icp, information_icp, uncertain = True))

        print("Optimizing PoseGraph ...")
        option = GlobalOptimizationOption(
                max_correspondence_distance = 0.01, #0.03 default
                edge_prune_threshold = 50.0, #5 before
                reference_node = 0)
        global_optimization(pose_graph,
                GlobalOptimizationLevenbergMarquardt(),
                GlobalOptimizationConvergenceCriteria(), option)

        print("Transform points and display")
        for point_id in range(n_pcds):
            print(pose_graph.nodes[point_id].pose)
            pcds[point_id].transform(pose_graph.nodes[point_id].pose)
        #draw_geometries(pcds)
        #print(type(pcds))
        #print(pcds[0])
        #    print(xyz_load)

        # Determine if pose graph is good, scene to scene
        for point_id in range(n_pcds):

        fgroup = open(out_dir+"Group_{:05d}.xyzrgb".format(ngroups), 'w')

        for i,target in enumerate(pcds):
            frame_names = list(range(start_frame,end_frame,skip_frame))

            xyz_load = np.asarray(pcds[i].points) # Load x,y,z points
            xnynzn_load = np.asarray(pcds[i].normals) # Load normals
            rgb_load = np.asarray(pcds[i].colors) # Load colors
            data_load = np.concatenate((xyz_load, xnynzn_load, rgb_load), axis=1)
            data_group = np.concatenate((xyz_load, rgb_load), axis=1)
            f = open(out_dir+"Frame{:05d}.csv".format(frame_names[i]), 'w')
            for points in data_load:
                strlist = [str(i) for i in points]
                f.write(','.join(strlist)+'\n')
            f.close()
            for points in data_group:
                strlist = [str(i) for i in points]
                fgroup.write(' '.join(strlist)+'\n')
        fgroup.close()

def register_point_cloud_fpfh(source, target,
        source_fpfh, target_fpfh):
    '''Inputs take 
    input cloud, output cloud, input feature, output feature
    maximum distance between features
    feature size, scaling of images (true/false),
    number of points to correlate, 

    max iterations, and max validation?
    '''
    method = 3
    if method == 1:
        return (False, np.identity(4))
    elif method == 2:
        result_ransac = registration_ransac_based_on_feature_matching(
            source, target, source_fpfh, target_fpfh, 20.0,
            TransformationEstimationPointToPoint(False), 4,
            [CorrespondenceCheckerBasedOnEdgeLength(0.5),
            #CorrespondenceCheckerBasedOnDistance(0.075)],
            CorrespondenceCheckerBasedOnDistance(.02)],
            RANSACConvergenceCriteria(6000000, 2000))
    elif method == 3: # Utilize normals here
        result_ransac = registration_ransac_based_on_feature_matching(
            source, target, source_fpfh, target_fpfh, 1.0,
            TransformationEstimationPointToPoint(False), 4,
            [CorrespondenceCheckerBasedOnEdgeLength(0.5),
             CorrespondenceCheckerBasedOnDistance(15.0),
             CorrespondenceCheckerBasedOnNormal(0.5)], 
             RANSACConvergenceCriteria(6000000, 2000))
    if (result_ransac.transformation.trace() == 4.0):
        return (False, np.identity(4))
    else:
        return (True, result_ransac)

if __name__ == "__main__":
    print(sys.argv)
    args=sys.argv[1:]

    if len(sys.argv) == 3:
        main(sys.argv[1], sys.argv[2])
    else: 
        print('Incorrect arguments \n')

[syncle]

I quickly went through the script. Overall, it looks good to me except for the several things need to be checked.

• edge_prune_threshold = 50.0: it is normalized. Only [0,1] scale will work.
• parameters: I don't know how the point cloud is distributed, or how it is dense enough.

Let me give some general idea. Depending on the dataset and configurations, local feature extraction and matching may not working. I think you can debug one by one: pick an easy pair, see/validate point cloud normal, feature extraction matching, and validate ICP. In this manner, you may notice the wrong step breaks the registration result, or find the better parameter configurations. If everything working fine with the pairs, the next thing to validate is global optimization. Visualize pose graph before and after optimization, and check whether the energy decreases.

[rukie]

It is definitely a sparse point cloud. See https://www.youtube.com/watch?v=09JjEQp9Nxk for an idea. It would be similar to this.

Thank's for catching the pruning threshold! I'll make an update there.

I've spent considerable time looking at normals and validating them in meshlab. I'm pretty confident it breaks in the RANSAC step, and of course ICP can't make up for that.

Generally my transforms are very small (see sample). [[ 1.00000000e+00 -5.84664492e-19 0.00000000e+00 0.00000000e+00] [ 2.82803125e-19 1.00000000e+00 0.00000000e+00 -2.71050543e-20] [ 1.08420217e-19 0.00000000e+00 1.00000000e+00 1.35525272e-20] [ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]] [[ 9.99998116e-01 1.73833634e-03 -8.63639802e-04 1.87099813e-03] [ -1.73772219e-03 9.99998237e-01 7.11358632e-04 1.75419109e-01] [ 8.64874860e-04 -7.09856525e-04 9.99999374e-01 7.79016245e-03] [ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]] [[ 9.99992591e-01 -3.84076138e-03 2.59039609e-04 8.06286387e-03] [ 3.84103031e-03 9.99992076e-01 -1.04579853e-03 3.59428156e-01] [ -2.55020894e-04 1.04678576e-03 9.99999420e-01 8.55598405e-03] [ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]]

Right now I'm working on processing 44,000 frames of data. I've been taking groups of 3-4 frames, combining them, and exporting new point clouds. Then I take those new point clouds and repeat the process. My 44,000 frames become 8,000 frames, my 8,000 become 800, and so on. Since I have so much data, I'm okay throwing some of it out the window.

I think I'm going to use something like the following to determine if I should throw it out.

        translation_failed = 0
        for point_id in range(n_pcds):
            if (pose_graph.nodes[point_id].pose[0,0]<0.98):
                if (pose_graph.nodes[point_id].pose[1,1]<0.98):
                    if (pose_graph.nodes[point_id].pose[2,2]<0.98):
                        translation_failed = 1
            if (pose_graph.nodes[point_id].pose[1,0]>0.1):
                translation_failed = 1
            if (pose_graph.nodes[point_id].pose[0,1]>0.1):
                translation_failed = 1

[syncle]

I think I'm going to use something like the following to determine if I should throw it out.

Yes, or simply you might try to use trace of the matrix. I found good discussions from https://stackoverflow.com/questions/34139586/how-to-check-if-an-eigenmatrix4f-is-close-to-identity-matrix.

I'm pretty confident it breaks in the RANSAC step, and of course ICP can't make up for that.

If the feature based matching keep failing due to the nature of the dataset, and the desirable pose is not far from the identity matrix, how about just using local registration method? You can skip global registration steps because local feature is not a good option and the point clouds just require minor pose alignment.

[rukie]

Since the lidar system scans radially, I get a set of half circles (I'm only scanning forward 180 degrees). Local alignment tends to drive for placing the semi-circles on-top of each other, rather than aligning features. (with iterative closest point) Using planes seems to alleviate some of this, but tends to be a little more blurry.

I've had my best luck with the approach above. I think ideally, I need to adjust the parameters based on cloud density. The features work excellent when close to buildings, but as I move into an open field I have more trouble. I can tune for larger open areas, but then issues arise in smaller (dense) clouds.

Imagine driving on an open country road in the middle of Nebraska, versus driving through wooded hills in Connecticut. That would probably be pretty representative of my current data set.

[rukie]

This is a (small) section of scanned terrain as viewed from above. This is 3 frames. There's a very clear radial effect from how the information is scanned. (color refers to intensity of returned). As the lasers scan further from the device, the space between annular rings increases. When terrain is particularly flat, it likes to align those more.

A single frame as seen from above:

Obviously there is more data, this is showing maybe 1/3. Trees/terrain visible in the distance. This is all I'm willing to share publicly however.

[syncle]

Thanks for sharing images @rukie. Without the sample data, my best recommendation is not using local features, increasing voxel down sampling size (to reduce non-uniform sampling issue), and apply local alignment algorithm.

Maybe there would be better solution I am missing. @qianyizh, can you comment something on @rukie's issue?

[rukie]

Is there an easy way I can tune parameters based on density of the cloud? Can I easily (and quickly) obtain size and average distance between points? Then I could voxelize based on that information, or crop based on that information. As my scans move around buildings, trees, on/off road the size of each cloud may vary from 20 meters wide to 300 meters wide. Sometimes it'll be dense up close, sometimes I'll have features in the distance.

This would be an example of more dense clouds (again, 3 frames through the same script):

and seen from above:

Note: This is maybe 20% of the total cloud.

[rukie]

For additional detail: each box is a group of 15 frames. You can see that the overall size of my clouds can vary significantly. I have 10 boxes (150 frames at 100,000 points each). The red/blue colors represent trees, and green is mostly ground.

[syncle]

Thanks @rukie. This depicts the challenge of real Lidar scans. It is quite interesting.

Is there an easy way I can tune parameters based on density of the cloud? Can I easily (and quickly) obtain size and average distance between points?

One naive idea I had is to extract features using multiple scales: for example, you may extract features using 1m, 3m, 5m radius (just example), when you do match, the features should be matched if they are extracted from same scale. In this way, the coarse region will naturally use the feature with larger radius, and vise versa for the dense regions. Again, this is my naive idea :)

[rukie]

So lets say I want to try RANSAC 3 times with 3 different scales, I can run RANSAC easily enough, but to access the RMSE (root mean squared error right?) Can I access that as result_ransac.inlier_rmse? Or result_ransac.rmse? Or should I evaluate with fitness instead?

(success_ransac_1, result_ransac_1) = register_point_cloud_fpfh(source_crop, target_crop, source_fpfh_1, target_fpfh_1)
`''

[syncle]

I think you might need to customize the function instead of iterating RANSAC for three times. You may extend register_point_cloud_fpfh. Consider matching the features of the same scale in the function. In this manner, you would not need to care about multiple result_ransacs. This idea not implemented though. I just proposing an idea here hoping this might be helpful. 😁

[rukie]

I'm not sure I follow. You're suggesting I edit the python function below? Or do you mean I should modify the c code on function:

RegistrationResult RegistrationRANSACBasedOnFeatureMatching(

Could you elaborate just a little more?

def register_point_cloud_fpfh(source, target, source_fpfh, target_fpfh):
    result_ransac = registration_ransac_based_on_feature_matching(
            source, target, source_fpfh, target_fpfh, 0.075,
            TransformationEstimationPointToPoint(False), 4,
            [CorrespondenceCheckerBasedOnEdgeLength(0.9),
            CorrespondenceCheckerBasedOnDistance(0.075),
            CorrespondenceCheckerBasedOnNormal(0.52359878)],
            RANSACConvergenceCriteria(4000000, 2000))
    if (result_ransac.transformation.trace() == 4.0):
        return (False, np.identity(4))
    else:
        return (True, result_ransac)

[syncle]

Oh, I was meant to say c++ code, as all the internal implementations are there. You can customize your RegistrationRANSACBasedOnFeatureMatching function.

[syncle]

Closing the issue due to inactivity. @rukie: Please feel free to reopen if needed.