[QUESTION] Teaser++ FPFH using own dataset

[ericahellscythe]

Have you read the documentation?

• [x] Yes

======================================================================================================= I was trying out the FPFH example and the example worked fine. However, when I swapped the example ply file with my own RGBD images, the result was terrible.

I converted my RGBD image into a point cloud and visualized it by publishing the results in RVIZ because o3d visualization just doesn't seem to work. I am fairly new to this and any help is appreciated. Thanks!

Distributor ID: Ubuntu Description: Ubuntu 20.04.5 LTS Release: 20.04 Codename: focal

IMAGE FILE: https://drive.google.com/drive/folders/1VNJI0wbwzcMvmBuVFPw63cGltdu71odC?usp=share_link

RESULT I got: https://drive.google.com/drive/folders/1XXAzopOnYBt9T_Yu0rfzn4pkLB4Ayq3j?usp=share_link

CODE:

import open3d as o3d
import teaserpp_python
import numpy as np 
import time
import copy
import os
from helpers import *
import rospy
from sensor_msgs.msg import PointCloud2
from open3d_ros_helper import open3d_ros_helper as orh

VOXEL_SIZE = 0.003
VISUALIZE = True
pc_pub = rospy.Publisher("/pointcloudImg", PointCloud2, queue_size=2)
pc_target_pub = rospy.Publisher("/pointcloudImg2", PointCloud2, queue_size=2)

def display_point_cloud(pc1, pc2, transformation):
    src_temp = copy.deepcopy(pc1)
    dst_temp = copy.deepcopy(pc2)
    if transformation is True:
        src_temp.transform(transformation)
    ros_pc = orh.o3dpc_to_rospc(src_temp, frame_id='rgb_camera_link')
    target_ros_pc = orh.o3dpc_to_rospc(dst_temp, frame_id='rgb_camera_link')
    print('publishing')
    for i in range(2):
        pc_pub.publish(ros_pc)
        pc_target_pub.publish(target_ros_pc)    
        time.sleep(1)
    print('done\n')

# Load and visualize two point clouds from 3DMatch dataset
# A_pcd_raw = o3d.io.read_point_cloud('/home/arm-orin-01/backup/src/TEASER-plusplus/examples/teaser_python_fpfh_icp/data/cloud_bin_0.ply')
# B_pcd_raw = o3d.io.read_point_cloud('/home/arm-orin-01/backup/src/TEASER-plusplus/examples/teaser_python_fpfh_icp/data/cloud_bin_4.ply')

rospy.init_node("Nat_Testing_Site", anonymous=True)
intrinsic_value = o3d.camera.PinholeCameraIntrinsic(width=1280, height=720,
                                                    fx = 929.4201124928686,
                                                    fy = 929.4157492052153,
                                                    cx = 956.414801791948,
                                                    cy = 548.4053450544782)

rgb_img_directory = '/home/arm-orin-01/backup/src/nat_ws/images/rgb'
depth_img_directory = '/home/arm-orin-01/backup/src/nat_ws/images/depth'
first = True
for i in range(len(os.listdir(rgb_img_directory))):
    print('Phase 1')
    color_img = os.path.join(rgb_img_directory, os.listdir(rgb_img_directory)[i])
    depth_img = os.path.join(depth_img_directory, os.listdir(depth_img_directory)[i])
    print(f"{color_img=}") if os.path.isfile(color_img) else print('something went wrong')
    print(f"{depth_img=}") if os.path.isfile(depth_img) else print('something went wrong')

    #Convert the rgb and depth image into a o3d point cloud
    color_raw = o3d.io.read_image(color_img)
    depth_raw = o3d.io.read_image(depth_img)
    color = o3d.geometry.Image(np.array(np.asarray(color_raw)[:, :, :3]).astype('uint8'))
    rgbd_image = o3d.geometry.RGBDImage.create_from_color_and_depth(color, depth_raw, convert_rgb_to_intensity=False)
    pcd = o3d.geometry.PointCloud.create_from_rgbd_image(rgbd_image, intrinsic_value)
    """
    Check if it is the first loop
    If it is, make the first pcd the cumulative one so that 
    the following pcds will be the one to merge with the cumulative pcd
    """
    if first:
        first = False
        B_pcd_raw = pcd
        print('cumulative_pcd: ', B_pcd_raw, '\n')
        continue
    else:
        A_pcd_raw = pcd
        print('next_pcd: ', A_pcd_raw, '\n')
        break

# if VISUALIZE:
#     o3d.visualization.draw_geometries([A_pcd_raw,B_pcd_raw]) # plot A and B 

# voxel downsample both clouds
A_pcd = A_pcd_raw.voxel_down_sample(voxel_size=VOXEL_SIZE)
B_pcd = B_pcd_raw.voxel_down_sample(voxel_size=VOXEL_SIZE)
print(A_pcd)
print(B_pcd)
# if VISUALIZE:
    # o3d.visualization.draw_geometries([A_pcd,B_pcd]) # plot downsampled A and B 

A_xyz = pcd2xyz(A_pcd) # np array of size 3 by N
B_xyz = pcd2xyz(B_pcd) # np array of size 3 by M

# extract FPFH features
print('extracting features')
A_feats = extract_fpfh(A_pcd,VOXEL_SIZE)
B_feats = extract_fpfh(B_pcd,VOXEL_SIZE)

# establish correspondences by nearest neighbour search in feature space
print('correspondences')
corrs_A, corrs_B = find_correspondences(
    A_feats, B_feats, mutual_filter=True)
A_corr = A_xyz[:,corrs_A] # np array of size 3 by num_corrs
B_corr = B_xyz[:,corrs_B] # np array of size 3 by num_corrs

num_corrs = A_corr.shape[1]
print(f'FPFH generates {num_corrs} putative correspondences.')

# visualize the point clouds together with feature correspondences
print('lines')
points = np.concatenate((A_corr.T,B_corr.T),axis=0)
lines = []
for i in range(num_corrs):
    lines.append([i,i+num_corrs])
colors = [[0, 1, 0] for i in range(len(lines))] # lines are shown in green
line_set = o3d.geometry.LineSet(
    points=o3d.utility.Vector3dVector(points),
    lines=o3d.utility.Vector2iVector(lines),
)
line_set.colors = o3d.utility.Vector3dVector(colors)
# o3d.visualization.draw_geometries([A_pcd,B_pcd,line_set])

# robust global registration using TEASER++
NOISE_BOUND = VOXEL_SIZE
teaser_solver = get_teaser_solver(NOISE_BOUND)
teaser_solver.solve(A_corr,B_corr)
solution = teaser_solver.getSolution()
R_teaser = solution.rotation
t_teaser = solution.translation
T_teaser = Rt2T(R_teaser,t_teaser)

# Visualize the registration results
A_pcd_T_teaser = copy.deepcopy(A_pcd).transform(T_teaser)
# o3d.visualization.draw_geometries([A_pcd_T_teaser,B_pcd])

# local refinement using ICP
icp_sol = o3d.pipelines.registration.registration_icp(
    A_pcd, B_pcd, NOISE_BOUND, T_teaser,
    o3d.pipelines.registration.TransformationEstimationPointToPoint(),
    o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=1000))
T_icp = icp_sol.transformation
print(T_icp)

# visualize the registration after ICP refinement
A_pcd_T_icp = copy.deepcopy(A_pcd).transform(T_icp)
# o3d.visualization.draw_geometries([A_pcd_T_icp,B_pcd])
display_point_cloud(A_pcd_T_icp,B_pcd, False)