Add camera calibration notes

config/config.yaml

@@ -1,3 +1,3 @@
 
 
-topic_name_kinect_cloud: "/camera/depth_registered/points"
+topic_name_rgbd_cloud: "/camera/depth_registered/points"

launch/run_main_3d_scanner.launch

@@ -2,27 +2,14 @@
 
     <!-- This launches the 3 main nodes for "Baxter 3D Object Scanner"-->
 
-   <!--================================== Debug Settings ======================================== -->
-
-    <arg name="run_node1"  default="true" doc="DEBUG: whether run node 1"/>
-    <arg name="run_node2"  default="true" doc="DEBUG: whether run node 2"/>
-    <arg name="run_node3"  default="true" doc="DEBUG: whether run node 3"/>
-
-    <!-- DEBUG_MODE: If true, simulate Kinect point cloud by reading from file -->
-    <param name="DEBUG_MODE_FOR_BAXTER" value="true" type="bool" />
-
-    <!-- DEBUG_MODE: If true, publish fake Baxter poses. No need to connect to real Baxter or Gazebo Baxter -->
-    <param name="DEBUG_MODE_FOR_KINECT" value="true" type="bool" />
 
    <!--================================== Parameters ======================================== -->
 
-
-
     <!-- Number of poses the Baxter will move to for taking picture -->
     <param name="num_goalposes" type="int" value="2" /> 
 
-    <!-- The topic for receiving PointCloud2 from Kinect -->
-    <param name="topic_name_kinect_cloud" value="/camera/depth_registered/points" /> 
+    <!-- The topic for receiving PointCloud2 from RgbdCam -->
+    <param name="topic_name_rgbd_cloud" value="/camera/depth_registered/points" /> 
 
     <!-- File names for readign point cloud and debug -->
     <param name="debug_file_folder" value="$(find scan3d_by_baxter)/data_debug/" /> 
@@ -43,6 +30,20 @@
     <param name="topic_n2_to_rviz" value="my/cloud_rotated" /> 
     <param name="topic_n2_to_n3" value="my/cloud_segmented" /> 
 
+   <!--================================== Debug Settings ======================================== -->
+
+    <arg name="run_node1"  default="true" doc="DEBUG: whether run node 1"/>
+    <arg name="run_node2"  default="true" doc="DEBUG: whether run node 2"/>
+    <arg name="run_node3"  default="false" doc="DEBUG: whether run node 3"/>
+
+    <!-- DEBUG_MODE: If true, simulate Kinect point cloud by reading from file -->
+    <param name="DEBUG_MODE_FOR_BAXTER" value="true" type="bool" />
+
+    <!-- DEBUG_MODE: If true, no need to connect to real Baxter or Gazebo. Publish fake poses.  -->
+    <param name="DEBUG_MODE_FOR_RGBDCAM" value="true" type="bool" />
+    <param name="topic_name_rgbd_cloud" value="/if_DEBUG_MODE_is_false_Please_comment_this_line_out" /> 
+
+
    <!--================================== Nodes ============================================== -->
 
    <!-- node 1: main node for controlling work flow -->

launch/run_rgbd_camera.launch

@@ -0,0 +1,18 @@
+<launch>
+
+<!-- This is for launching Asus rgbd-camera.
+This is also a replacement of the following operations:
+
+$ roslaunch openni2_launch openni2.launch
+
+$ rosrun rqt_reconfigure rqt_reconfigure  
+Select camera/driver, set "depth_registration" to true
+(Optional: Set "color_depth_synchronization" to true.)
+
+ -->
+
+<include file="$(find openni2_launch)/launch/openni2.launch">
+    <arg name="depth_registration" default="true" />
+</include>
+
+</launch>

launch/run_rviz.launch

@@ -1,5 +1,4 @@
 <launch>
-  <!-- <node type="rviz" name="rviz" pkg="rviz" args="-d $(find scan3d_by_baxter)/config/rviz_pc_only.rviz" /> -->
+  <node type="rviz" name="rviz" pkg="rviz" args="-d $(find scan3d_by_baxter)/config/rviz_pc_only.rviz" />
   <!-- <node type="rviz" name="rviz" pkg="rviz" args="-d $(find scan3d_by_baxter)/config/rviz_with_baxter.rviz" /> -->
-  <node type="rviz" name="rviz" pkg="rviz" args="-d $(find scan3d_by_baxter)/config/rviz_Asus.rviz" />
 </launch>

launch/sub_cloud_press_key_to_save.launch

@@ -0,0 +1,16 @@
+<launch>
+
+    <!-- not implemented yet -->
+
+    <!-- <param name="topic_name_rgbd_cloud" value="/camera/depth_registered/points" /> 
+
+    <param name="file_folder" value="$(find scan3d_by_baxter)/data_debug/" /> 
+    <param name="file_name_to_save" value="sample_cloud_" /> 
+   
+    <node name="sub_cloud_press_key_to_save"
+        type="sub_cloud_press_key_to_save"
+        pkg="scan3d_by_baxter" output = "screen">
+    </node> -->
+
+</launch>
+

launch/test_open3d_read_pub_sub_display.launch

@@ -3,7 +3,7 @@
     <!-- roslaunch scan3d_by_baxter test_open3d_read_pub_sub_display.launch -->
 
     <!-- The topic for pub/sub PointCloud2 -->
-    <param name="topic_name_kinect_cloud" value="/camera/depth_registered/points" /> 
+    <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/" /> 

launch/test_pcl_read_pub_sub_display.launch

@@ -3,7 +3,7 @@
     <!-- roslaunch scan3d_by_baxter test_pcl_read_pub_sub_display.launch -->
 
     <!-- The topic for pub/sub PointCloud2 -->
-    <param name="topic_name_kinect_cloud" value="/camera/depth_registered/points" /> 
+    <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/" /> 

launch/test_publish_a_cloud.launch

@@ -3,7 +3,7 @@
     <!-- roslaunch scan3d_by_baxter test_pcl_read_pub_sub_display.launch -->
 
     <!-- The topic for pub/sub PointCloud2 -->
-    <param name="topic_name_kinect_cloud" value="/camera/depth_registered/points" /> 
+    <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/" /> 

launch/test_subscribe_a_cloud_and_display.launch

@@ -3,7 +3,7 @@
     <!-- roslaunch scan3d_by_baxter test_pcl_read_pub_sub_display.launch -->
 
     <!-- The topic for pub/sub PointCloud2 -->
-    <param name="topic_name_kinect_cloud" value="/camera/depth_registered/points" /> 
+    <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/" /> 

launch/useful_commands.bash

@@ -1,9 +1,9 @@
 # This is just for storing usefull command-line commands
 
-
 ## ===============================
-## Start Asus rgb-d camera
-$ roslaunch openni2_launch openni2.launch
+# connect to Baxter
+$ nmcli connection up Rethink
+$ rosrun baxter_tools enable_robot.py -e
 
 ## ===============================
 ## my services to print Baxter pose

src_main/camera_calibration/notes_Asus.md → src_main/calib_camera_pose/notes_AsusXtion.md

@@ -1,13 +1,20 @@
+Xtion PRO LIVE
+2013/11/05
 
+# Camera specifications
+https://www.asus.com/us/3D-Sensor/Xtion_PRO_LIVE/specifications/
+58° H, 45° V, 70° D (Horizontal, Vertical, Diagonal)
+Between 0.8m and 3.5m
 
+# How to use
 http://wiki.ros.org/openni2_launch
 
 Run driver:
 $ roslaunch openni2_launch openni2.launch
 
 Config registration settings:  
 $ rosrun rqt_reconfigure rqt_reconfigure  
-select camera/driver, set "depth_registration" to true, set "color_depth_synchronization" to true.  
+Select camera/driver, set "depth_registration" to true, set "color_depth_synchronization" to true.  
 
 Show:  
 $ rviz  

src_main/calib_camera_pose/notes_Kinect.md

@@ -0,0 +1,35 @@
+
+(I just found there is a Asus camera in the lab, and it is very easy to use! So this notes is not useful anymore.)  
+
+# Camera specifications   
+RGB：1920 x 1080 @ 30 / 15 FPS  
+Depth：512 x 424 @ 30 FPS、16bit （mm）  
+IR cam：512 x 484，30 Hz   
+FOV: 70° x 60°   
+Depth range: 0.5–4.5 meters   
+RGB cam: 1080p 30 Hz (week light: 15 Hz)  
+
+# Tools for using the Kinect One (Kinect v2) in ROS  
+https://github.com/code-iai/iai_kinect2  
+
+====== USE ======   
+` How to get point clouds   
+$ roslaunch kinect2_bridge kinect2_bridge.launch   
+
+` How to view  
+kinect_viewer or rostopic hz   
+
+
+====== DEBUG ======   
+
+`kinect2_bridge is not working / crashing, what is wrong?   
+
+bug1: kinect2_bridge   
+bug2: libfreenect2   
+
+` Check libfreenect2  
+use this tool: libfreenect2/build/bin/Protonect   
+
+Before running kinect2_bridge please make sure Protonect is working and showing color, depth and ir images    
+
+

src_main/calib_camera_pose/notes_calib_rgb_camera.md

@@ -0,0 +1,33 @@
+
+
+# Open camera
+
+Run the laptop's usb cam:
+{ Method1: 
+    $ rosrun usb_cam usb_cam_node _video_device:=/dev/video0 _pixel_format:=yuyv _camera_name:=tracker_camera
+}
+{ Method2: Use uvc_camera.
+    $ rosrun uvc_camera uvc_camera_node _device:=/dev/video0 # remember to remap
+}
+
+See camera info:
+$ rostopic echo -n 1 /usb_cam/camera_info
+
+Before calibration, all params are zero.
+
+# Calibrate
+http://wiki.ros.org/camera_calibration/Tutorials/MonocularCalibration
+
+$ rosrun camera_calibration cameracalibrator.py --size 9x7 --square 0.0158 image:=/usb_cam/image_raw camera:=/usb_cam
+
+Data (calibration data and images used for calibration) will be written to /tmp/calibrationdata.tar.gz.
+
+# Load calibration result
+
+Copy calibration result to some folder.
+
+Run the camera driver with a url to the calib result:
+$ rosrun camera_calibration cameracalibrator.py --size 9x7 --square 0.0158 image:=/usb_cam/image_raw camera:=/usb_cam camera_info_url:=/home/feiyu/tmp/my_laptop_cam_calib_result
+
+Run image proc:
+$ ROS_NAMESPACE=usb_cam rosrun image_proc image_proc

src_main/n1_work_flow_control.py

@@ -58,7 +58,7 @@ def getCloudSize(open3d_cloud):
 if __name__ == "__main__":
     rospy.init_node('node1')
     DEBUG_MODE_FOR_BAXTER = rospy.get_param("DEBUG_MODE_FOR_BAXTER")
-    DEBUG_MODE_FOR_KINECT = rospy.get_param("DEBUG_MODE_FOR_KINECT")
+    DEBUG_MODE_FOR_RGBDCAM = rospy.get_param("DEBUG_MODE_FOR_RGBDCAM")
 
     file_folder = rospy.get_param("file_folder")
     file_name_pose = rospy.get_param("file_name_pose")
@@ -107,12 +107,12 @@ def publishPose(pose):
 
     # Settings for debug kinect:
     #   Simulate fake point cloud publisher by reading point_cloud from file.
-    if DEBUG_MODE_FOR_KINECT:
+    if DEBUG_MODE_FOR_RGBDCAM:
         debug_file_folder = rospy.get_param("debug_file_folder")
         debug_file_name = rospy.get_param("debug_file_name") # without suffix "i.pcd"
-        topic_name_kinect_cloud = rospy.get_param("topic_name_kinect_cloud")
+        topic_name_rgbd_cloud = rospy.get_param("topic_name_rgbd_cloud")
 
-        pub_sim_cloud = rospy.Publisher(topic_name_kinect_cloud,
+        pub_sim_cloud = rospy.Publisher(topic_name_rgbd_cloud,
             PointCloud2, queue_size=10)
         def sim_pub_point_cloud(ith_goalpose):
             filename = debug_file_name+str(ith_goalpose)+".pcd"
@@ -146,7 +146,7 @@ def sim_pub_point_cloud(ith_goalpose):
         publishPose(pose)
         savePoseToFile(pose, ith_goalpose)
 
-        if DEBUG_MODE_FOR_KINECT: # simulate publishing a point cloud
+        if DEBUG_MODE_FOR_RGBDCAM: # simulate publishing a point cloud
             rospy.sleep(0.01)
             sim_pub_point_cloud(ith_goalpose)
         rospy.loginfo("--------------------------------")

src_main/n2_filt_and_seg_object.cpp

@@ -122,12 +122,12 @@ int main(int argc, char **argv)
     ros::NodeHandle nh;
 
     // Settings: topic names
-    string topic_n1_to_n2, topic_n2_to_n3, topic_name_kinect_cloud, topic_n2_to_rviz;
+    string topic_n1_to_n2, topic_n2_to_n3, topic_name_rgbd_cloud, topic_n2_to_rviz;
     if (!nh.getParam("topic_n1_to_n2", topic_n1_to_n2))
         assert(0);
     if (!nh.getParam("topic_n2_to_n3", topic_n2_to_n3))
         assert(0);
-    if (!nh.getParam("topic_name_kinect_cloud", topic_name_kinect_cloud))
+    if (!nh.getParam("topic_name_rgbd_cloud", topic_name_rgbd_cloud))
         assert(0);
     if (!nh.getParam("topic_n2_to_rviz", topic_n2_to_rviz))
         assert(0);
@@ -144,7 +144,7 @@ int main(int argc, char **argv)
 
     // Subscriber and Publisher
     ros::Subscriber sub_from_node1 = nh.subscribe(topic_n1_to_n2, 1, callbackFromNode1); // 1 is queue size
-    ros::Subscriber sub_from_kinect = nh.subscribe(topic_name_kinect_cloud, 1, callbackFromKinect);
+    ros::Subscriber sub_from_kinect = nh.subscribe(topic_name_rgbd_cloud, 1, callbackFromKinect);
     ros::Publisher pub_to_node3 = nh.advertise<sensor_msgs::PointCloud2>(topic_n2_to_n3, 1);
     ros::Publisher pub_to_rviz = nh.advertise<sensor_msgs::PointCloud2>(topic_n2_to_rviz, 1);
 

src_python/lib_cloud_conversion_between_Open3D_and_ROS.py

@@ -123,7 +123,7 @@ def convertCloudFromRosToOpen3d(ros_cloud):
     print("")
 
     # -- Set publisher
-    topic_name="kinect2/qhd/points"
+    topic_name="camera/depth_registered/points"
     pub = rospy.Publisher(topic_name, PointCloud2, queue_size=1)
 
     # -- Set subscriber

src_python/lib_cloud_registration.py

@@ -93,3 +93,33 @@ def registerClouds(src, target, radius_base=0.01):
     # Combine the two
     result_cloud = combineTwoClouds(src_tmp, target, radius_base)
     return result_cloud, result_icp.transformation
+
+
+
+if __name__ == "__main__":
+    import numpy as np
+    import copy
+    from open3d import *
+    import time
+    import sys, os
+
+    # -- Load data
+    src = read_point_cloud("../data_debug/ColoredICP_1.pcd")
+    target = read_point_cloud("../data_debug/ColoredICP_2.pcd")
+
+    # -- Draw initial alignment
+    if 0:
+        current_transformation = np.identity(4)
+        draw_registration_result_original_color(
+            src, target, current_transformation)
+
+    # -- Register clouds
+    result_cloud, transformation = registerClouds(src, target)
+
+    # -- Print and plot
+    # print(src)
+    # print(target)
+    print(result_cloud)
+    print(transformation)
+    time.sleep(0.3)
+    draw_geometries([result_cloud])