Adjusting algo for chessboard frame's direction

launch/main_3d_scanner.launch

@@ -111,11 +111,11 @@
             <param name="y_range_up" type="double" value="0.0" /> -->
             <param name="z_range_low" type="double" value="-0.5" />     
             <param name="z_range_up" type="double" value="0.5" />     
-            <param name="x_range_radius" type="double" value="0.15" />     
-            <param name="y_range_radius" type="double" value="0.15" />     
+            <param name="x_range_radius" type="double" value="0.2" />     
+            <param name="y_range_radius" type="double" value="0.2" />     
 
             <!-- segment plane -->
-            <param name="num_planes" type="int" value="1" />     
+            <param name="num_planes" type="int" value="0" />     
             <param name="plane_distance_threshold" type="double" value="0.01" />     
             <param name="plane_max_iterations" type="int" value="100" />     
 

src_main/n3_register_clouds_to_object.py

@@ -16,7 +16,7 @@
 sys.path.append(PYTHON_FILE_PATH + "../src_python")
 from lib_cloud_conversion_between_Open3D_and_ROS import convertCloudFromOpen3dToRos, convertCloudFromRosToOpen3d
 from lib_cloud_registration import drawTwoClouds, registerClouds, copyOpen3dCloud
-from lib_geo_trans_ros import rotx, roty, rotz
+from lib_geo_trans import rotx, roty, rotz
 
 VIEW_RES_BY_OPEN3D=True # This is difficult to set orientation. And has some bug.
 VIEW_RES_BY_RVIZ=~VIEW_RES_BY_OPEN3D
@@ -79,7 +79,7 @@ def popCloud(self):
         return self.cloud_buff.popleft()
 
     def rotateCloudForBetterViewing(self, cloud):
-        T=rotx(np.pi)
+        T=rotx(np.pi, matrix_len=4)
         cloud.transform(T)
 
 # ---------------------------- Main ----------------------------
@@ -112,7 +112,8 @@ def rotateCloudForBetterViewing(self, cloud):
                 copyOpen3dCloud(src=new_cloud_piece, dst=final_cloud)
             else:
                 final_cloud, transformation = registerClouds(
-                    src=new_cloud_piece, target=final_cloud, radius_base=0.002)
+                    src=new_cloud_piece, target=final_cloud, 
+                    radius_regi=0.010, radius_merge=0.005)
 
             # Update point cloud
             viewer.updateCloud(final_cloud)

src_python/lib_cam_calib.py

@@ -12,6 +12,23 @@
 from lib_geo_trans import *
 
 # ---------------------------------- Main functions
+
+
+# Generate a unique chessboard frame, by:
+#   1. Let chessboard z axis pointing to the camera
+#   2. I manually draw a black dot near the center of board.
+#       I will rotate the frame so that this dot's pos is (d_row=0.5, d_col=-0.5)
+def helperMakeUniqueChessboardFrame(img, R, p):
+
+    # -- 1. Make chessboard z axis pointing towards the camera
+    vec_cam_to_chess = np.array(p).squeeze(axis=1)
+    vec_chess_z = form_T(R, p).dot(np.array([[0],[0],[1],[0]])).squeeze(axis=1)[0:3]-np.array(p)
+    if vec_cam_to_chess.dot(vec_chess_z)>0:
+        R=R*rotx(np.pi)
+
+    # -- 2. A manually drawn dot at required pos.
+    return R,p
+
 def getChessboardPose(img,
         camera_intrinsics,
         distortion_coeffs,
@@ -73,10 +90,17 @@ def refineImageCorners(gray_image, corners):
                 objpoints, imgpoints, camera_intrinsics, distortion_coeffs)
         R, _ = cv2.Rodrigues(R_vec)
 
+        # Generate a unique chessboard frame, by:
+        #   1. Let chessboard z axis pointing to the camera
+        #   2. I manually draw a black dot near the center of board.
+        #       I will rotate the frame so that this dot's pos is (d_row=0.5, d_col=-0.5)
+        R, p = helperMakeUniqueChessboardFrame(R, p)
 
         return flag_find_chessboard, R, p, imgpoints
 
-
+
+# ------------------------------------ Drawing Functions ------------------------------------
+
 def drawChessboardToImage(img_display, imgpoints, CHECKER_ROWS, CHECKER_COLS):
     flag_find_chessboard = True
     img_display = cv2.drawChessboardCorners(

src_python/lib_cloud_registration.py

@@ -35,7 +35,7 @@ def combineTwoClouds(cloud1, cloud2, radius_downsample=0.002):
     return result
 
 
-def registerClouds(src, target, radius_base=0.002):
+def registerClouds(src, target, radius_regi=0.005, radius_merge=0.002):
     # -- Use colored ICP to register src onto dst, and return the combined cloud
     # This function is mainly copied from here.
     # http://www.open3d.org/docs/tutorial/Advanced/colored_pointcloud_registration.html
@@ -45,16 +45,16 @@ def registerClouds(src, target, radius_base=0.002):
     COLORED_ICP_DRAW = False    
 
     # -- Params
-    ICP_distance_threshold = radius_base*4
-    voxel_radiuses = [radius_base*8, radius_base*2, radius_base]
-    max_iters = [50, 30, 14]
+    ICP_distance_threshold = radius_regi*8
+    voxel_radiuses = [radius_regi*8, radius_regi*2, radius_regi]
+    max_iters = [50, 25, 10]
 
     # -- Vars
     current_transformation = np.identity(4)
 
     # -- Point to plane ICP
     if ICP:
-        radius = radius_base
+        radius = radius_regi
         src_down = voxel_down_sample(src, radius)
         target_down = voxel_down_sample(target, radius)
         estimate_normals(src, KDTreeSearchParamHybrid(
@@ -128,7 +128,7 @@ def registerClouds(src, target, radius_base=0.002):
     print("-- Colored ICP completes.\n\n")
     print "src:",src_tmp
     print "target:",target
-    result_cloud = combineTwoClouds(src_tmp, target, radius_base)
+    result_cloud = combineTwoClouds(src_tmp, target, radius_merge)
     print "result_cloud: ", result_cloud
     return result_cloud, result_trans.transformation
 

src_python/lib_geo_trans.py

@@ -9,61 +9,111 @@
 from tf.transformations import*
 
 # ---------------- Basic trans
+
+
 def form_T(R, p):
     T = np.identity(4)
     T[0:3, 0:3] = R
-    T[0:3, 3:4] = np.array(p).reshape((3,1))
+    T[0:3, 3:4] = np.array(p).reshape((3, 1))
     return T
 
+
 def get_Rp_from_T(T):
     R = T[0:3, 0:3]
     p = T[0:3, 3:4]
     return (R, p)
 
+
 def invRp(R, p):
-    T=form_T(R,p)
-    T=np.linalg.inv(T)
-    R_inv,p_inv=get_Rp_from_T(T)
+    T = form_T(R, p)
+    T = np.linalg.inv(T)
+    R_inv, p_inv = get_Rp_from_T(T)
     return R_inv, p_inv
 
+# ---------------- Euler angles
+
+
+def rot3x3_to_4x4(R):
+    T = np.identity(4)
+    T[0:3, 0:3] = R
+    return T
+
+def rot(axis, angle, matrix_len=3):
+    R_vec = np.array(axis).astype(float)*angle
+    R, _ = cv2.Rodrigues(R_vec)
+    if matrix_len == 4:
+        R = rot3x3_to_4x4(R)
+    return R
+
+def rotx(angle, matrix_len=3):
+    return rot([1,0,0], angle, matrix_len)
+
+def roty(angle, matrix_len=3):
+    return rot([0,1,0], angle, matrix_len)
+
+def rotz(angle, matrix_len=3):
+    return rot([0,0,1], angle, matrix_len)
+
+def euler2matrix(x, y, z, order='rxyz'):
+    return rotx(x).dot(roty(y)).dot(rotz(z))
+
 
 # ----------------Point's pos transformation between world/camera/image
 
 # Distort a point. Input x,y are the point's pos on the camera normalized plane (z=0)
+
+
 def distortPoint(x, y, distortion_coeffs):
     r2 = x*x + y*y
     r4 = r2*r2
     r6 = r4*r2
     d = distortion_coeffs
-    k1,k2,k3,p1,p2=d[0],d[1],d[2],d[3],d[4]
-    x_distort = x * (1 + k1 * r2 + k2 * r4 + k3 * r6)+ 2*p1*x*y + p2*(r2 + 2*x*x)
-    y_distort = y * (1 + k1 * r2 + k2 * r4 + k3 * r6)+ p1*(r2 + 2*y*y) + 2*p2*x*y
-    pt_cam_distort=np.array([[x_distort, y_distort, 1]]).transpose()
+    k1, k2, k3, p1, p2 = d[0], d[1], d[2], d[3], d[4]
+    x_distort = x * (1 + k1 * r2 + k2 * r4 + k3 * r6) + \
+        2*p1*x*y + p2*(r2 + 2*x*x)
+    y_distort = y * (1 + k1 * r2 + k2 * r4 + k3 * r6) + \
+        p1*(r2 + 2*y*y) + 2*p2*x*y
+    pt_cam_distort = np.array([[x_distort, y_distort, 1]]).transpose()
     return pt_cam_distort
 
 # Represent a point from using world coordinate to using camera coordinate
-def world2cam(p, T_cam_to_world): 
-    if type(p)==list:
-        p=np.array(p).reshape((len(p),1))
-    if p.shape[0]==3:
-        p=np.vstack((p, 1))
+
+
+def world2cam(p, T_cam_to_world):
+    if type(p) == list:
+        p = np.array(p).reshape((len(p), 1))
+    if p.shape[0] == 3:
+        p = np.vstack((p, 1))
     p_cam = T_cam_to_world.dot(p)
-    return p_cam[0:3,0:1]
+    return p_cam[0:3, 0:1]
 
 # Project a point represented in camera coordinate onto the image plane
-def cam2pixel(p, camera_intrinsics, distortion_coeffs = None):
-
+
+
+def cam2pixel(p, camera_intrinsics, distortion_coeffs=None):
+
     # Transform to camera normalized plane (z=1)
-    p = p/p[2,0] # z=1
+    p = p/p[2, 0]  # z=1
 
     # Distort point
     if distortion_coeffs is not None:
-        p = distortPoint(p[0,0], p[1,0], distortion_coeffs)
+        p = distortPoint(p[0, 0], p[1, 0], distortion_coeffs)
 
     # Project to image plane
-    pt_pixel_distort=camera_intrinsics.dot(p)
-    return pt_pixel_distort[0:2,0]
+    pt_pixel_distort = camera_intrinsics.dot(p)
+    return pt_pixel_distort[0:2, 0]
 
 # A combination of the above two
+
+
 def world2pixel(p, T_cam_to_world, camera_intrinsics, distortion_coeffs):
-    return cam2pixel(world2cam(p, T_cam_to_world), camera_intrinsics, distortion_coeffs)
+    return cam2pixel(world2cam(p, T_cam_to_world), camera_intrinsics, distortion_coeffs)
+
+
+# ---------------------- Test -----------------------
+if __name__ == "__main__":
+
+    R = euler2matrix(np.pi/2, 0, 0)
+    # R = rotz(np.pi/2)
+    # R = rotx(np.pi/2)
+    print R

src_python/lib_geo_trans_ros.py

@@ -5,35 +5,21 @@
 from geometry_msgs.msg import Pose, Point, Quaternion
 import cv2
 
-def form_T(R, p):
-    T = np.identity(4)
-    T[0:3, 0:3] = R
-    try:
-        T[0:3, 3:4] = p[0:3, 0:1]
-    except:
-        T[0, 3] = p[0]
-        T[1, 3] = p[1]
-        T[2, 3] = p[2]
-    return T
-
+from lib_geo_trans import *
 
-def get_Rp_from_T(T):
-    R = T[0:3, 0:3]
-    p = T[0:3, 3:4]
-    return (R, p)
 
+if 0: # Functions below has already been defined in lib_geo_trans
+    def rotx(angle, matrix_len=4):
+        xaxis=(1, 0, 0)
+        return rotation_matrix(angle, xaxis)
 
-def rotx(angle):
-    xaxis=(1, 0, 0)
-    return rotation_matrix(angle, xaxis)
-
-def roty(angle):
-    yaxis=(0, 1, 0)
-    return rotation_matrix(angle, yaxis)
-
-def rotz(angle):
-    zaxis=(0, 0, 1)
-    return rotation_matrix(angle, zaxis)
+    def roty(angle, matrix_len=4):
+        yaxis=(0, 1, 0)
+        return rotation_matrix(angle, yaxis)
+
+    def rotz(angle, matrix_len=4):
+        zaxis=(0, 0, 1)
+        return rotation_matrix(angle, zaxis)
 
 # a bit wrap for geometry_msgs.msg.Pose
 def toRosPose(pos, quaternion):