Add log file. Find bug that I ignored the transformation between dept…

…h_cam's color and depth frame so my camera pose is wrong. Fix it tomorrow.
felixchenfy · Feb 4, 2019 · f4208dc · f4208dc
1 parent 7f8b2bb
commit f4208dc
Show file tree

Hide file tree

Showing 18 changed files with 110 additions and 235 deletions.
diff --git a/README.md b/README.md
@@ -76,7 +76,7 @@ Subscribe: (a) Message from node 1. (b) Point cloud from depth camera.
 
 Publish: (b) Rotated cloud to rviz. (c) Segmented cloud to node 3.
 
-Meanwhile, it also saves the (a) orignal cloud and (c) segmented cloud to the [data/](data/) folder. 
+Meanwhile, it also saves the (a) orignal cloud and (c) segmented cloud to the [data/data/](data/data/) folder. 
 
 ## 2.4. Node3: Register clouds
 file: [src_main/n3_register_clouds_to_object.py](src_main/n3_register_clouds_to_object.py)

diff --git a/doc/log.md b/doc/log.md
@@ -0,0 +1,42 @@
+
+---------------------------------------
+02-02 ANALYSIS
+
+Besides object itself, the algorithm needs additional info for better registration.
+However, if this addition info are not the same in different views, such as the different floor pixels under different camera view, it will on the contrary ruin the registration by its useless extra cloud points.
+
+Thus, I'd better use other objects instead of colored paper to assist registration.
+
+TODO: Check if there is a displacement between depth_cam's color and depth frame, by placing the chessboard on the floor and see if after tranformation its in the center of the frame. Also watch the depth_cam's topics and tfs.
+(The bug arises from the fact that I'm using colored image and colored image frame during calibration, but the point cloud might actually be in the depth frame.)
+
+TODO: Draw colored English words larger. Or, maybe I should use other obstacles to replace drawing. 
+
+
+---------------------------------------
+02-02 PROBLEMS
+
+` Calibration error:
+After rotate point clouds to the Baxter frames, I put 11 drillers clouds into one view, and then saw that the error is about: 5 cm in x and y direction, and 30 degrees. Besides, the z value is not 0, but 5 cm. There must be something wrong with the calibration.
+
+After I manually tranlate the camera from by x,y,z　= +-0.1m (add one line to the fake node1), I found that z=0.1 will make the driller center at the coord frame.
+So there should be a z=0.1 displacement apply to the current camera frame. Test it tommorrow.
+
+` Node 2, pcl has errr for the 9th clouds, where the driller disappears.
+
+` The Enlish words drawn on the paper is too thin. Even if I use 0.001 downsampling rate, it still cannot been clearly distinguished. It requires like 0.0002 to be very clear.
+
+` With small downsampling voxel size, the points number is kept increasing.
+20k for one cloud, then there are 200k for 10 clouds, due to the small registration error. This is ridiculous.
+
+
+---------------------------------------
+02-02 TESTINT RESULT
+` DATASET:  driller with colored board
+ALGORITHM: GLOBAL REGISTER, GOOD. PARAM: voxel_size=0.01~0.02
+ALGORITHM: ICP, GOOD. PARAM: voxel_size(for ICP)=0.005, global_regi_ratio=2, colored_ICP_ratio={1,1/2,1/4} 
+ALGORITHM: ICP + Colored-ICP, GOOD
+
+` DATASET:driller without colored board
+All algorithms are bad, including GLOBAL REGI and ICP.
+
diff --git a/doc/other_notes.md b/doc/other_notes.md
diff --git a/doc/references/copied_joint_position_keyboard.py b/doc/references/copied_joint_position_keyboard.py
diff --git a/launch/main_3d_scanner.launch b/launch/main_3d_scanner.launch
@@ -12,11 +12,11 @@
     <param name="topic_name_rgbd_cloud" value="/camera/depth_registered/points" /> 
 
     <!-- File names for reading point cloud and debug -->
-    <param name="debug_file_folder" value="$(find scan3d_by_baxter)/data_debug/" /> 
+    <param name="debug_file_folder" value="$(find scan3d_by_baxter)/data/data_debug/" /> 
     <param name="debug_file_name" value="ColoredICP_"  />
 
     <!-- File names for saving results -->
-    <param name="file_folder" value="$(find scan3d_by_baxter)/data/" /> 
+    <param name="file_folder" value="$(find scan3d_by_baxter)/data/data/" /> 
     <param name="file_name_cloud_src" value="src_" /> 
     <param name="file_name_cloud_segmented" value="segmented_" /> 
     <param name="file_name_cloud_final" value="final.pcd" /> 
@@ -125,16 +125,15 @@
    <!--================================== rviz ============================================== -->
 
     <node type="rviz" name="rviz1" pkg="rviz"
-       args="-d $(find scan3d_by_baxter)/config/rviz_baxter_and_clouds.rviz" /> 
+       args="-d $(find scan3d_by_baxter)/config/rviz_baxter_and_clouds.rviz" respawn="true"/> 
 
     <node type="rviz" name="rviz2" pkg="rviz"
-       args="-d $(find scan3d_by_baxter)/config/rviz_segmented_cloud.rviz" /> 
+       args="-d $(find scan3d_by_baxter)/config/rviz_segmented_cloud.rviz" respawn="true"/> 
 
     <node type="rviz" name="rviz3" pkg="rviz"
-       args="-d $(find scan3d_by_baxter)/config/rviz_final_cloud.rviz" />
+       args="-d $(find scan3d_by_baxter)/config/rviz_final_cloud.rviz" respawn="true"/>
 
     <node type="rviz" name="rviz_depth_image" pkg="rviz"
-       args="-d $(find scan3d_by_baxter)/config/rviz_depth_image.rviz" 
-    /> 
+       args="-d $(find scan3d_by_baxter)/config/rviz_depth_image.rviz" respawn="true"/> 
 
 </launch>
diff --git a/launch/test_point_cloud_pub_sub_display/test_open3d_read_pub_sub_display.launch b/launch/test_point_cloud_pub_sub_display/test_open3d_read_pub_sub_display.launch
@@ -6,7 +6,7 @@
     <param name="topic_name_rgbd_cloud" value="/camera/depth_registered/points" /> 
 
     <!-- The filename to read the point cloud from -->
-    <param name="file_folder" value="$(find scan3d_by_baxter)/data_debug/" /> 
+    <param name="file_folder" value="$(find scan3d_by_baxter)/data/data_debug/" /> 
     <param name="file_name" value="cloud_cluster_0.pcd"/> 
 
     <!-- ==================================================================== -->

diff --git a/launch/test_point_cloud_pub_sub_display/test_pcl_read_pub_sub_display.launch b/launch/test_point_cloud_pub_sub_display/test_pcl_read_pub_sub_display.launch
@@ -6,7 +6,7 @@
     <param name="topic_name_rgbd_cloud" value="/camera/depth_registered/points" /> 
 
     <!-- The filename to read the point cloud from -->
-    <param name="file_folder" value="$(find scan3d_by_baxter)/data_debug/" /> 
+    <param name="file_folder" value="$(find scan3d_by_baxter)/data/data_debug/" /> 
     <param name="file_name" value="cloud_cluster_0.pcd"/> 
 
     <!-- ==================================================================== -->

diff --git a/launch/test_point_cloud_pub_sub_display/test_publish_a_cloud.launch b/launch/test_point_cloud_pub_sub_display/test_publish_a_cloud.launch
@@ -6,7 +6,7 @@
     <param name="topic_name_rgbd_cloud" value="/camera/depth_registered/points" /> 
 
     <!-- The filename to read the point cloud from -->
-    <param name="file_folder" value="$(find scan3d_by_baxter)/data_debug/" /> 
+    <param name="file_folder" value="$(find scan3d_by_baxter)/data/data_debug/" /> 
     <param name="file_name" value="cloud_cluster_0.pcd"/> 
 
     <!-- ==================================================================== -->

diff --git a/launch/test_point_cloud_pub_sub_display/test_subscribe_a_cloud_and_display.launch b/launch/test_point_cloud_pub_sub_display/test_subscribe_a_cloud_and_display.launch
@@ -6,7 +6,7 @@
     <param name="topic_name_rgbd_cloud" value="/camera/depth_registered/points" /> 
 
     <!-- The filename to read the point cloud from -->
-    <!-- <param name="file_folder" value="$(find scan3d_by_baxter)/data_debug/" /> 
+    <!-- <param name="file_folder" value="$(find scan3d_by_baxter)/data/data_debug/" /> 
     <param name="file_name" value="cloud_cluster_0.pcd"/>  -->
 
     <!-- ==================================================================== -->

diff --git a/src_main/calib_camera_pose/calib_camera_pose.py b/src_main/calib_camera_pose/calib_camera_pose.py
@@ -121,7 +121,7 @@ def pubImage(publisher, cv2_image):
             I = sub_image(image_topic)
 
             if 0: # whether save to file
-                filename_to_save_img = PYTHON_FILE_PATH+"/../../data_debug/img_for_calib_"+str(i)+".png"
+                filename_to_save_img = PYTHON_FILE_PATH+"/../../data/data_debug/img_for_calib_"+str(i)+".png"
                 print "filename_to_save_img: "+filename_to_save_img
                 cv2.imwrite(filename_to_save_img, I)
 

diff --git a/src_main/n1_fake_data_publisher.py b/src_main/n1_fake_data_publisher.py
@@ -17,6 +17,7 @@
 # -- My lib
 sys.path.append(PYTHON_FILE_PATH + "../src_python")
 from lib_cloud_conversion_between_Open3D_and_ROS import convertCloudFromOpen3dToRos
+from lib_geo_trans import transXYZ
 
 # -- Message types
 # from std_msgs.msg import Int32  # used for indexing the ith robot pose
@@ -26,7 +27,7 @@
 
 # -------------------------------------------------------------
 # ------------------ Variables 
-FILE_FOLDER = PYTHON_FILE_PATH+"../data/without_board/"
+FILE_FOLDER = PYTHON_FILE_PATH+"../data/data/driller_floor/"
 ith_goalpose = 0
 
 # -------------------------------------------------------------
@@ -67,6 +68,9 @@ def readNextPose(self):
 
     def pub_pose(self):
         T = self.readNextPose()
+        # ------ DEBUG ------
+        # T = T.dot(transXYZ(x=0, y=0, z=0.1))
+        # ------
         # Trans to 1x16 array
         pose_1x16 = []
         for i in range(4):

diff --git a/src_main/n3_register_clouds_to_object.py b/src_main/n3_register_clouds_to_object.py
@@ -15,7 +15,7 @@
 # Include my lib
 sys.path.append(PYTHON_FILE_PATH + "../src_python")
 from lib_cloud_conversion_between_Open3D_and_ROS import convertCloudFromOpen3dToRos, convertCloudFromRosToOpen3d
-from lib_cloud_registration import CloudRegister, resizeCloudXYZ
+from lib_cloud_registration import CloudRegister, resizeCloudXYZ, mergeClouds, createXYZAxis
 
 from lib_geo_trans import rotx, roty, rotz
 
@@ -50,10 +50,12 @@ def __init__(self):
         self.pub = rospy.Publisher(topic_n3_to_rviz, PointCloud2, queue_size=10)
         self.updateView = lambda: None
         self.destroy_window = lambda: None
+        self.cloud_XYZaxis = createXYZAxis(coord_axis_length=0.1, num_points_in_axis=50)
 
     def updateCloud(self, new_cloud):
-        cloud_to_pub = resizeCloudXYZ(new_cloud, 5.0) # Resize cloud, so rviz has better view
-        self.pub.publish(convertCloudFromOpen3dToRos(cloud_to_pub))
+        new_cloud = mergeClouds(new_cloud, self.cloud_XYZaxis)
+        new_cloud = resizeCloudXYZ(new_cloud, 5.0) # Resize cloud, so rviz has better view
+        self.pub.publish(convertCloudFromOpen3dToRos(new_cloud))
 
 def chooseViewer():
     if 0: 
@@ -109,7 +111,7 @@ def popCloud(self):
     cloud_register = CloudRegister(
         voxel_size_regi=0.005, global_regi_ratio=2.0, 
         voxel_size_output=0.001,
-        USE_ICP=False, USE_COLORED_ICP=False)
+        USE_GLOBAL_REGI=False, USE_ICP=False, USE_COLORED_ICP=True)
 
 
     while not rospy.is_shutdown():

diff --git a/src_python/convert_pointcloud_file_format.py b/src_python/convert_pointcloud_file_format.py
@@ -1,8 +1,8 @@
 '''
 Run this script to change point cloud file types. Examples:
 
-$ python src_py/convert_point_cloud_file_type.py data/ColoredICP_1.ply data/ColoredICP_1.pcd
-$ python src_py/convert_point_cloud_file_type.py data/ColoredICP_2.ply data/ColoredICP_2.pcd
+$ python src_py/convert_point_cloud_file_type.py data/data/ColoredICP_1.ply data/data/ColoredICP_1.pcd
+$ python src_py/convert_point_cloud_file_type.py data/data/ColoredICP_2.ply data/data/ColoredICP_2.pcd
 
 '''