Index: pr2_calibration_launch/view_results/head_arm_config.yaml
===================================================================
--- pr2_calibration_launch/view_results/head_arm_config.yaml	(revision 48511)
+++ pr2_calibration_launch/view_results/head_arm_config.yaml	(working copy)
@@ -65,3 +65,17 @@
   plot_ops:
     color: r
     marker: s
+
+- name: "Kinect: Left Arm"
+  3d: left_arm_chain
+  cam: kinect_rect
+  plot_ops:
+    color: r
+    marker: o
+
+- name: "Kinect: Right Arm"
+  3d: right_arm_chain
+  cam: kinect_rect
+  plot_ops:
+    color: r
+    marker: s
Index: pr2_calibration_launch/view_results/head_laser_config.yaml
===================================================================
--- pr2_calibration_launch/view_results/head_laser_config.yaml	(revision 48511)
+++ pr2_calibration_launch/view_results/head_laser_config.yaml	(working copy)
@@ -33,3 +33,10 @@
   plot_ops:
     color: m
     marker: o
+
+- name: "Tilt Laser: Kinect"
+  3d: tilt_laser
+  cam: kinect_rect
+  plot_ops:
+    color: m
+    marker: o
Index: pr2_calibration_launch/capture_data/intersection.launch
===================================================================
--- pr2_calibration_launch/capture_data/intersection.launch	(revision 48511)
+++ pr2_calibration_launch/capture_data/intersection.launch	(working copy)
@@ -14,6 +14,7 @@
     <remap from="forearm_right_rect" to="r_forearm_cam/settled_interval" />
     <remap from="forearm_left_rect" to="l_forearm_cam/settled_interval" />
     <remap from="prosilica_rect" to="prosilica/settled_interval" />
+    <remap from="kinect_rect" to="camera/rgb/settled_interval" />	
 
     <remap from="interval" to="request_interval" />
   </node>
@@ -22,4 +23,4 @@
 
 
 
-</launch>
\ No newline at end of file
+</launch>
Index: pr2_calibration_launch/capture_data/hardware_config/cam_config.yaml
===================================================================
--- pr2_calibration_launch/capture_data/hardware_config/cam_config.yaml	(revision 48511)
+++ pr2_calibration_launch/capture_data/hardware_config/cam_config.yaml	(working copy)
@@ -279,3 +279,42 @@
         subpixel_zero_zone: 1
       led_detector:
         active: False
+
+kinect_rect:
+  cb_detector_config:  /camera/rgb/cb_detector_config
+  led_detector_config: /camera/rgb/led_detector
+  settler_config:      /camera/rgb/monocam_settler_config
+
+  configs:
+    cb_7x6:
+      settler:
+        tolerance: 2.00
+        ignore_failures: True
+        max_step: 1.0
+        cache_size: 100
+      cb_detector:
+        active: True
+        num_x: 7
+        num_y: 6
+        width_scaling: 1
+        height_scaling: 1
+        subpixel_window: 4
+        subpixel_zero_zone: 1
+      led_detector:
+        active: False
+    small_cb_4x5:
+      settler:
+        tolerance: 2.00
+        ignore_failures: True
+        max_step: 1.0
+        cache_size: 100
+      cb_detector:
+        active: True
+        num_x: 4
+        num_y: 5
+        width_scaling: 1
+        height_scaling: 1
+        subpixel_window: 4
+        subpixel_zero_zone: 1
+      led_detector:
+        active: False
\ No newline at end of file
Index: pr2_calibration_launch/capture_data/samples_pr2_beta/far/far_0000.yaml
===================================================================
--- pr2_calibration_launch/capture_data/samples_pr2_beta/far/far_0000.yaml	(revision 48511)
+++ pr2_calibration_launch/capture_data/samples_pr2_beta/far/far_0000.yaml	(working copy)
@@ -4,6 +4,7 @@
 - {cam_id: wide_right_rect, config: cb_7x6}
 - {cam_id: narrow_right_rect, config: cb_7x6}
 - {cam_id: prosilica_rect, config: cb_7x6}
+- {cam_id: kinect_rect, config: cb_7x6}
 joint_commands:
 #- controller: head_traj_controller
 #  segments:
Index: pr2_calibration_launch/capture_data/samples_pr2_beta/left/l_arm_0000.yaml
===================================================================
--- pr2_calibration_launch/capture_data/samples_pr2_beta/left/l_arm_0000.yaml	(revision 48511)
+++ pr2_calibration_launch/capture_data/samples_pr2_beta/left/l_arm_0000.yaml	(working copy)
@@ -4,6 +4,7 @@
 - {cam_id: wide_right_rect, config: small_cb_4x5}
 - {cam_id: narrow_right_rect, config: small_cb_4x5}
 - {cam_id: prosilica_rect, config: small_cb_4x5}
+- {cam_id: kinect_rect, config: small_cb_4x5}
 joint_commands:
 - controller: head_traj_controller
   segments:
Index: pr2_calibration_launch/capture_data/samples_pr2_beta/left/l_arm_0001.yaml
===================================================================
--- pr2_calibration_launch/capture_data/samples_pr2_beta/left/l_arm_0001.yaml	(revision 48511)
+++ pr2_calibration_launch/capture_data/samples_pr2_beta/left/l_arm_0001.yaml	(working copy)
@@ -4,6 +4,7 @@
 - {cam_id: wide_right_rect, config: small_cb_4x5}
 - {cam_id: narrow_right_rect, config: small_cb_4x5}
 - {cam_id: prosilica_rect, config: small_cb_4x5}
+- {cam_id: kinect_rect, config: small_cb_4x5}
 joint_commands:
 - controller: head_traj_controller
   segments:
Index: pr2_calibration_launch/capture_data/samples_pr2_beta/right/r_arm_0000.yaml
===================================================================
--- pr2_calibration_launch/capture_data/samples_pr2_beta/right/r_arm_0000.yaml	(revision 48511)
+++ pr2_calibration_launch/capture_data/samples_pr2_beta/right/r_arm_0000.yaml	(working copy)
@@ -4,6 +4,7 @@
 - {cam_id: wide_right_rect, config: small_cb_4x5}
 - {cam_id: narrow_right_rect, config: small_cb_4x5}
 - {cam_id: prosilica_rect, config: small_cb_4x5}
+- {cam_id: kinect_rect, config: small_cb_4x5}
 joint_commands:
 - controller: head_traj_controller
   segments:
Index: pr2_calibration_launch/capture_data/samples_pr2_beta/right/r_arm_0001.yaml
===================================================================
--- pr2_calibration_launch/capture_data/samples_pr2_beta/right/r_arm_0001.yaml	(revision 48511)
+++ pr2_calibration_launch/capture_data/samples_pr2_beta/right/r_arm_0001.yaml	(working copy)
@@ -4,6 +4,7 @@
 - {cam_id: wide_right_rect, config: small_cb_4x5}
 - {cam_id: narrow_right_rect, config: small_cb_4x5}
 - {cam_id: prosilica_rect, config: small_cb_4x5}
+- {cam_id: kinect_rect, config: small_cb_4x5}
 joint_commands:
 - controller: head_traj_controller
   segments:
Index: pr2_calibration_launch/capture_data/all_viewers.launch
===================================================================
--- pr2_calibration_launch/capture_data/all_viewers.launch	(revision 48511)
+++ pr2_calibration_launch/capture_data/all_viewers.launch	(working copy)
@@ -15,6 +15,9 @@
   <include file="$(find pr2_calibration_launch)/capture_data/monocam_viewer.launch"
            ns="prosilica" />
 
+  <include file="$(find pr2_calibration_launch)/capture_data/monocam_viewer.launch"
+           ns="camera/rgb" />
+
   <include file="$(find pr2_calibration_launch)/capture_data/double_stereo_viewer.launch" />
 
   <include file="$(find pr2_calibration_launch)/capture_data/monocam_viewer.launch"
Index: pr2_calibration_launch/capture_data/all_pipelines.launch
===================================================================
--- pr2_calibration_launch/capture_data/all_pipelines.launch	(revision 48511)
+++ pr2_calibration_launch/capture_data/all_pipelines.launch	(working copy)
@@ -25,6 +25,13 @@
         name="forearm_r_throttle"
         args="messages r_forearm_cam/image_rect 2" />
 
+  <include file="$(find pr2_calibration_launch)/capture_data/monocam_pipeline.launch"
+           ns="camera/rgb" />
+  <node type="throttle"
+        pkg="topic_tools"
+        name="kinect_throttle"
+        args="messages camera/rgb/image_mono 2 camera/rgb/image_rect_throttle" />
+
   <include file="$(find pr2_calibration_launch)/capture_data/intersection.launch" />
   <include file="$(find pr2_calibration_launch)/capture_data/laser_pipelines.launch" />
 
Index: pr2_calibration_launch/capture_data/capture_exec_pr2.launch
===================================================================
--- pr2_calibration_launch/capture_data/capture_exec_pr2.launch	(revision 48511)
+++ pr2_calibration_launch/capture_data/capture_exec_pr2.launch	(working copy)
@@ -7,6 +7,11 @@
         output="screen"
         machine="c1">
 
+    <remap from="kinect_rect/camera_info" to="camera/rgb/camera_info"/>
+    <remap from="kinect_rect/image_rect"  to="camera/rgb/image_mono"/>
+    <remap from="kinect_rect/image"       to="camera/rgb/image_mono"/>
+    <remap from="kinect_rect/features"    to="camera/rgb/features"/>
+
     <remap from="prosilica_rect/camera_info" to="prosilica/camera_info"/>
     <remap from="prosilica_rect/image_rect"  to="prosilica/image_rect"/>
     <remap from="prosilica_rect/image"       to="prosilica/image_raw"/>
Index: pr2_calibration_launch/estimate_params/config_pr2_beta/free_arms_only_1.yaml
===================================================================
--- pr2_calibration_launch/estimate_params/config_pr2_beta/free_arms_only_1.yaml	(revision 48511)
+++ pr2_calibration_launch/estimate_params/config_pr2_beta/free_arms_only_1.yaml	(working copy)
@@ -51,6 +51,9 @@
     high_def_frame_joint:              [ 0, 0, 0, 0, 0, 0 ]
     high_def_optical_frame_joint:      [ 0, 0, 0, 0, 0, 0 ]
 
+    openni_rgb_frame_joint:            [ 0, 0, 0, 0, 0, 0 ]
+    openni_rgb_optical_frame_joint:    [ 0, 0, 0, 0, 0, 0 ]
+
     l_forearm_roll_adj:                [ 0, 0, 0, 0, 0, 0 ]
     l_forearm_cam_frame_joint:         [ 1, 1, 1, 1, 1, 1 ]
     l_forearm_cam_optical_frame_joint: [ 0, 0, 0, 0, 0, 0 ]
Index: pr2_calibration_launch/estimate_params/config_pr2_beta/system.yaml
===================================================================
--- pr2_calibration_launch/estimate_params/config_pr2_beta/system.yaml	(revision 48511)
+++ pr2_calibration_launch/estimate_params/config_pr2_beta/system.yaml	(working copy)
@@ -78,6 +78,10 @@
     high_def_frame_joint: [0, -0.109, 0.035, 0, 0, 0]
     high_def_optical_frame_joint: [0, 0, 0, -1.2091995761561452, 1.2091995761561452, -1.2091995761561452]
 
+    # Kinect RGB Frames
+    openni_rgb_frame_joint: [0, -0.109, 0.035, 0, 0, 0]
+    openni_rgb_optical_frame_joint: [0, 0, 0, -1.2091995761561452, 1.2091995761561452, -1.2091995761561452]
+
   rectified_cams:
     wide_left_rect:
       baseline_shift: 0.0
@@ -109,6 +113,13 @@
       cx_shift: 0.0
       cy_shift: 0.0
       cov: {u: 0.25, v: 0.25}
+    kinect_rect:
+      baseline_shift: 0.0
+      f_shift: 0.0
+      cx_shift: 0.0
+      cy_shift: 0.0
+      cov: {u: 0.25, v: 0.25}
+   
     forearm_left_rect:
       baseline_shift: 0.0
       f_shift: 0.0
Index: pr2_calibration_launch/estimate_params/config_pr2_beta/free_head_laser_0.yaml
===================================================================
--- pr2_calibration_launch/estimate_params/config_pr2_beta/free_head_laser_0.yaml	(revision 48511)
+++ pr2_calibration_launch/estimate_params/config_pr2_beta/free_head_laser_0.yaml	(working copy)
@@ -51,6 +51,9 @@
     high_def_frame_joint:              [ 0, 0, 0, 0, 0, 0 ]
     high_def_optical_frame_joint:      [ 0, 0, 0, 0, 0, 0 ]
 
+    openni_rgb_frame_joint:            [ 0, 0, 0, 0, 0, 0 ]
+    openni_rgb_optical_frame_joint:    [ 0, 0, 0, 0, 0, 0 ]
+
     l_forearm_roll_adj:                [ 0, 0, 0, 0, 0, 0 ]
     l_forearm_cam_frame_joint:         [ 0, 0, 0, 0, 0, 0 ]
     l_forearm_cam_optical_frame_joint: [ 0, 0, 0, 0, 0, 0 ]
Index: pr2_calibration_launch/estimate_params/config_pr2_beta/free_head_laser_1.yaml
===================================================================
--- pr2_calibration_launch/estimate_params/config_pr2_beta/free_head_laser_1.yaml	(revision 48511)
+++ pr2_calibration_launch/estimate_params/config_pr2_beta/free_head_laser_1.yaml	(working copy)
@@ -51,6 +51,9 @@
     high_def_frame_joint:              [ 0, 0, 0, 0, 0, 0 ]
     high_def_optical_frame_joint:      [ 0, 0, 0, 0, 0, 0 ]
 
+    openni_rgb_frame_joint:            [ 0, 0, 0, 0, 0, 0 ]
+    openni_rgb_optical_frame_joint:    [ 0, 0, 0, 0, 0, 0 ]
+
     l_forearm_roll_adj:                [ 0, 0, 0, 0, 0, 0 ]
     l_forearm_cam_frame_joint:         [ 0, 0, 0, 0, 0, 0 ]
     l_forearm_cam_optical_frame_joint: [ 0, 0, 0, 0, 0, 0 ]
Index: pr2_calibration_launch/estimate_params/config_pr2_beta/free_head_laser_2.yaml
===================================================================
--- pr2_calibration_launch/estimate_params/config_pr2_beta/free_head_laser_2.yaml	(revision 48511)
+++ pr2_calibration_launch/estimate_params/config_pr2_beta/free_head_laser_2.yaml	(working copy)
@@ -51,6 +51,9 @@
     high_def_frame_joint:              [ 0, 0, 0, 0, 0, 0 ]
     high_def_optical_frame_joint:      [ 0, 0, 0, 0, 0, 0 ]
 
+    openni_rgb_frame_joint:            [ 0, 0, 0, 0, 0, 0 ]
+    openni_rgb_optical_frame_joint:    [ 0, 0, 0, 0, 0, 0 ]
+
     l_forearm_roll_adj:                [ 0, 0, 0, 0, 0, 0 ]
     l_forearm_cam_frame_joint:         [ 0, 0, 0, 0, 0, 0 ]
     l_forearm_cam_optical_frame_joint: [ 0, 0, 0, 0, 0, 0 ]
Index: pr2_calibration_launch/estimate_params/config_pr2_beta/free_head_laser_3.yaml
===================================================================
--- pr2_calibration_launch/estimate_params/config_pr2_beta/free_head_laser_3.yaml	(revision 48511)
+++ pr2_calibration_launch/estimate_params/config_pr2_beta/free_head_laser_3.yaml	(working copy)
@@ -51,6 +51,9 @@
     high_def_frame_joint:              [ 0, 0, 0, 0, 0, 0 ]
     high_def_optical_frame_joint:      [ 0, 0, 0, 0, 0, 0 ]
 
+    openni_rgb_frame_joint:            [ 0, 0, 0, 0, 0, 0 ]
+    openni_rgb_optical_frame_joint:    [ 0, 0, 0, 0, 0, 0 ]
+
     l_forearm_roll_adj:                [ 0, 0, 0, 0, 0, 0 ]
     l_forearm_cam_frame_joint:         [ 0, 0, 0, 0, 0, 0 ]
     l_forearm_cam_optical_frame_joint: [ 0, 0, 0, 0, 0, 0 ]
Index: pr2_calibration_launch/estimate_params/config_pr2_beta/free_head_laser_4.yaml
===================================================================
--- pr2_calibration_launch/estimate_params/config_pr2_beta/free_head_laser_4.yaml	(revision 48511)
+++ pr2_calibration_launch/estimate_params/config_pr2_beta/free_head_laser_4.yaml	(working copy)
@@ -51,6 +51,9 @@
     high_def_frame_joint:              [ 1, 1, 1, 1, 1, 1 ]
     high_def_optical_frame_joint:      [ 0, 0, 0, 0, 0, 0 ]
 
+    openni_rgb_frame_joint:            [ 1, 1, 1, 1, 1, 1 ]
+    openni_rgb_optical_frame_joint:    [ 0, 0, 0, 0, 0, 0 ]
+
     l_forearm_roll_adj:                [ 0, 0, 0, 0, 0, 0 ]
     l_forearm_cam_frame_joint:         [ 0, 0, 0, 0, 0, 0 ]
     l_forearm_cam_optical_frame_joint: [ 0, 0, 0, 0, 0, 0 ]
Index: pr2_calibration_launch/estimate_params/config_pr2_beta/sensors.yaml
===================================================================
--- pr2_calibration_launch/estimate_params/config_pr2_beta/sensors.yaml	(revision 48511)
+++ pr2_calibration_launch/estimate_params/config_pr2_beta/sensors.yaml	(working copy)
@@ -41,6 +41,15 @@
         dh_link_num:  1  # Connects after head pan
         after_chain:  [head_chain_tip_adj, head_plate_frame_joint, high_def_frame_joint, high_def_optical_frame_joint]
 
+    - sensor_id:    kinect_rect
+      camera_id:    kinect_rect
+      chain:
+        before_chain: [head_pan_joint]
+        chain_id:     head_chain
+        dh_link_num:  1  # Connects after head pan
+        after_chain:  [head_chain_tip_adj, head_plate_frame_joint, openni_rgb_frame_joint, openni_rgb_optical_frame_joint]
+
+
     - camera_id: forearm_left_rect
       sensor_id: forearm_left_rect
       chain:
Index: pr2_calibration_launch/estimate_params/head_then_arms_params.launch
===================================================================
--- pr2_calibration_launch/estimate_params/head_then_arms_params.launch	(revision 48511)
+++ pr2_calibration_launch/estimate_params/head_then_arms_params.launch	(working copy)
@@ -74,10 +74,11 @@
           - wide_right_rect
           - tilt_laser
           - prosilica_rect
+          - kinect_rect
         </rosparam>
         <param name="output_filename" type="string" value="head_laser_calibrated" />
       </group>
-      
+
       <group ns="B - 01 - Locking Head Sensors. Estimating Arm Joint Offsets">
         <param name="free_params" textfile="$(find pr2_calibration_launch)/estimate_params/config_pr2_beta/free_arms_only_1.yaml" />
         <param name="use_cov" type="bool" value="True" />
Index: pr2_calibration_estimation/test/pr2_head_fk_unittest.py
===================================================================
--- pr2_calibration_estimation/test/pr2_head_fk_unittest.py	(revision 48511)
+++ pr2_calibration_estimation/test/pr2_head_fk_unittest.py	(working copy)
@@ -217,6 +217,30 @@
         cmds = self.loadCommands('head_commands')
         self.run_test(full_chain, 'torso_lift_link', 'high_def_optical_frame', cmds)
 
+    def test_kinect_def(self):
+        print ""
+        config_str = '''
+        before_chain: [head_pan_joint]
+        chain_id:     head_chain
+        after_chain:  [head_chain_tip_adj, head_plate_frame_joint, openni_rgb_frame_joint]
+        dh_link_num:  1 '''
+        full_chain = FullChainRobotParams(yaml.load(config_str))
+        full_chain.update_config(self.robot_params)
+        cmds = self.loadCommands('head_commands')
+        self.run_test(full_chain, 'torso_lift_link', 'openni_rgb_frame', cmds)
+
+    def test_kinect_optical(self):
+        print ""
+        config_str = '''
+        before_chain: [head_pan_joint]
+        chain_id:     head_chain
+        after_chain:  [head_chain_tip_adj, head_plate_frame_joint, openni_rgb_frame_joint, openni_rgb_optical_frame_joint]
+        dh_link_num:  1 '''
+        full_chain = FullChainRobotParams(yaml.load(config_str))
+        full_chain.update_config(self.robot_params)
+        cmds = self.loadCommands('head_commands')
+        self.run_test(full_chain, 'torso_lift_link', 'openni_rgb_optical_frame', cmds)
+
     def check_tilt_laser(self, cmd):
         actual_T = self.robot_params.tilting_lasers["tilt_laser"].compute_pose([cmd])
         expected_T = self.getExpected("torso_lift_link", "laser_tilt_link", [cmd])
Index: pr2_calibration_estimation/src/pr2_calibration_estimation/multi_step_cov_estimator.py
===================================================================
--- pr2_calibration_estimation/src/pr2_calibration_estimation/multi_step_cov_estimator.py	(revision 48511)
+++ pr2_calibration_estimation/src/pr2_calibration_estimation/multi_step_cov_estimator.py	(working copy)
@@ -43,6 +43,7 @@
 import yaml
 import os.path
 import numpy
+import rosrecord
 
 import stat
 import os