remove hardcoded camera name from injected virtual joints

Cristina Garcia Cifuentes · Cristina Garcia Cifuentes · commit 47075ef7dc71 · 2017-02-24T17:20:06.000+01:00
diff --git a/source/dbrt/kinematics_from_urdf.cpp b/source/dbrt/kinematics_from_urdf.cpp
@@ -132,13 +132,13 @@ void KinematicsFromURDF::inject_offset_joints_and_links(
 {
     rename_camera_frame(camera_frame, urdf);
 
-    std::string camera_frame_ = "camera";
-    std::vector<std::string> dofs = {camera_frame_ + "_X",
-                                     camera_frame_ + "_Y",
-                                     camera_frame_ + "_Z",
-                                     camera_frame_ + "_PITCH",
-                                     camera_frame_ + "_YAW",
-                                     camera_frame_ + "_ROLL"};
+//    std::string camera_frame_ = "camera";
+    std::vector<std::string> dofs = {camera_frame + "_X",
+                                     camera_frame + "_Y",
+                                     camera_frame + "_Z",
+                                     camera_frame + "_PITCH",
+                                     camera_frame + "_YAW",
+                                     camera_frame + "_ROLL"};
 
     std::string current_parent_link = camera_frame + "_DEFAULT";
 
@@ -349,12 +349,12 @@ Eigen::VectorXd KinematicsFromURDF::sensor_msg_to_eigen(
 
     if (use_camera_offset_)
     {
-        joint_state.name.push_back("camera_X_JOINT");
-        joint_state.name.push_back("camera_Y_JOINT");
-        joint_state.name.push_back("camera_Z_JOINT");
-        joint_state.name.push_back("camera_ROLL_JOINT");
-        joint_state.name.push_back("camera_PITCH_JOINT");
-        joint_state.name.push_back("camera_YAW_JOINT");
+        joint_state.name.push_back(cam_frame_name_ + "_X_JOINT");
+        joint_state.name.push_back(cam_frame_name_ + "_Y_JOINT");
+        joint_state.name.push_back(cam_frame_name_ + "_Z_JOINT");
+        joint_state.name.push_back(cam_frame_name_ + "_ROLL_JOINT");
+        joint_state.name.push_back(cam_frame_name_ + "_PITCH_JOINT");
+        joint_state.name.push_back(cam_frame_name_ + "_YAW_JOINT");
 
         joint_state.position.push_back(0);
         joint_state.position.push_back(0);
diff --git a/source/dbrt/kinematics_from_urdf.h b/source/dbrt/kinematics_from_urdf.h
@@ -74,6 +74,8 @@ class KinematicsFromURDF
     // get the joint index in state array
     int name_to_index(const std::string& name);
 
+    const std::string& camera_frame_id() const { return cam_frame_name_; }
+
 private:
     void rename_camera_frame(const std::string& camera_frame,
                              urdf::Model& urdf);
diff --git a/source/dbrt/tracker/rotary_tracker_factory.cpp b/source/dbrt/tracker/rotary_tracker_factory.cpp
@@ -31,6 +31,9 @@ namespace dbrt
  *     parameter prefix, e.g. fusion_tracker
  * \param kinematics
  *     URDF robot kinematics
+ * \param camera_frame_id
+ *     This is the camera frame id or name used when the 
+ *     estimate_camera_offset is on.
  */
 std::shared_ptr<dbrt::RotaryTracker> create_rotary_tracker(
     std::string prefix,
@@ -72,15 +75,22 @@ std::shared_ptr<dbrt::RotaryTracker> create_rotary_tracker(
 
     if (estimate_camera_offset)
     {
-        transition_joint_sigmas_map.insert(
-            camera_transition_joint_sigmas_map.begin(),
-            camera_transition_joint_sigmas_map.end());
+        insert_map_with_prefixed_keys(
+            camera_transition_joint_sigmas_map,
+            kinematics->camera_frame_id() + "_",
+            transition_joint_sigmas_map);
 
-        joint_bias_sigmas_map.insert(camera_joint_bias_sigmas_map.begin(),
-                                     camera_joint_bias_sigmas_map.end());
 
-        joint_bias_factors_map.insert(camera_joint_bias_factors_map.begin(),
-                                      camera_joint_bias_factors_map.end());
+        insert_map_with_prefixed_keys(
+            camera_joint_bias_sigmas_map,
+            kinematics->camera_frame_id() + "_",
+            joint_bias_sigmas_map);
+
+
+        insert_map_with_prefixed_keys(
+            camera_joint_bias_factors_map,
+            kinematics->camera_frame_id() + "_",
+            joint_bias_factors_map);
     }
 
     // linear state transition parameters
@@ -103,9 +113,10 @@ std::shared_ptr<dbrt::RotaryTracker> create_rotary_tracker(
 
     if (estimate_camera_offset)
     {
-        observation_joint_sigmas_map.insert(
-            camera_observation_joint_sigmas_map.begin(),
-            camera_observation_joint_sigmas_map.end());
+        insert_map_with_prefixed_keys(
+            camera_observation_joint_sigmas_map,
+            kinematics->camera_frame_id() + "_",
+            observation_joint_sigmas_map);
     }
 
     sensor_parameters.joint_sigmas =
diff --git a/source/dbrt/tracker/visual_tracker_factory.cpp b/source/dbrt/tracker/visual_tracker_factory.cpp
@@ -83,9 +83,9 @@ std::shared_ptr<dbrt::VisualTracker> create_visual_tracker(
         read_maps_from_map_list(prefix + "joint_transition/joint_sigmas", nh);
     if (estimate_camera_offset)
     {
-        transition_joint_sigmas_map.insert(
-            camera_transition_joint_sigmas_map.begin(),
-            camera_transition_joint_sigmas_map.end());
+       insert_map_with_prefixed_keys(camera_transition_joint_sigmas_map, 
+                                     kinematics->camera_frame_id() + "_", 
+                                     transition_joint_sigmas_map);
     }
 
     // linear state transition parameters
@@ -172,8 +172,8 @@ std::shared_ptr<dbrt::VisualTracker> create_visual_tracker(
     if (estimate_camera_offset)
     {
         sampling_blocks_definition = merge_sampling_block_definitions(
-            camera_offset_sampling_blocks_definition,
-            sampling_blocks_definition);
+	   sampling_blocks_definition,
+           camera_offset_sampling_blocks_definition, kinematics->camera_frame_id() + '_');
     }
 
     tracker_parameters.sampling_blocks =
diff --git a/source/dbrt/util/parameter_tools.h b/source/dbrt/util/parameter_tools.h
@@ -48,29 +48,38 @@ inline std::vector<std::vector<int>> definition_to_sampling_block(
 
 inline SamplingBlocksDefinition merge_sampling_block_definitions(
     const SamplingBlocksDefinition& definition_A,
-    const SamplingBlocksDefinition& definition_B)
+    const SamplingBlocksDefinition& definition_B,
+    const std::string& prefixB)
 {
     SamplingBlocksDefinition merged = definition_A;
 
     for (auto block_definition_B : definition_B)
     {
-        auto block_B = block_definition_B.begin();
+      std::map<std::string, std::vector<std::string>> prefixed_block_definition_B;
+      for (auto block : block_definition_B)
+      {
+	for (auto value : block.second)
+	{
+	  prefixed_block_definition_B[block.first].push_back(prefixB + value);
+	}
+      }
 
+      auto block_B = prefixed_block_definition_B.begin();
         bool added = false;
         for (auto& block_definition_A : merged)
         {
             auto block_A = block_definition_A.begin();
             if (block_A->first == block_B->first)
             {
-                block_A->second.insert(std::end(block_A->second),
-                                       std::begin(block_B->second),
-                                       std::end(block_B->second));
-                added = true;
+	       block_A->second.insert(std::end(block_A->second),
+                                      std::begin(block_B->second),
+                                      std::end(block_B->second));
+               added = true;
             }
         }
         if (!added)
         {
-            merged.push_back(block_definition_B);
+            merged.push_back(prefixed_block_definition_B);
         }
     }
 
@@ -101,16 +110,26 @@ inline std::vector<double> extract_ordered_values(
     std::vector<double> ordered_values(parameter_map.size());
     for (auto entry : parameter_map)
     {
-        if (kinematics->name_to_index(entry.first) > ordered_values.size())
+        std::string joint_name = entry.first;
+        if (kinematics->name_to_index(joint_name) > ordered_values.size())
         {
             ROS_ERROR(
                 "Joint index exceeds number of read joints. "
                 "Did you forget to merge joint lists?");
             exit(-1);
         }
-        ordered_values[kinematics->name_to_index(entry.first)] = entry.second;
+        ordered_values[kinematics->name_to_index(joint_name)] = entry.second;
     }
 
     return ordered_values;
 }
+
+inline void insert_map_with_prefixed_keys(
+    const std::map<std::string, double>& new_values, 
+    const std::string& prefix, 
+    std::map<std::string, double>& destination){
+  for (auto new_value: new_values){
+    destination[prefix + new_value.first] = new_value.second;
+  }
+}
 }

Original file line number	Diff line number	Diff line change
`@@ -83,9 +83,9 @@ std::shared_ptr<dbrt::VisualTracker> create_visual_tracker(`
`83`	`83`	`read_maps_from_map_list(prefix + "joint_transition/joint_sigmas", nh);`
`84`	`84`	`if (estimate_camera_offset)`
`85`	`85`	`{`
`86`		`- transition_joint_sigmas_map.insert(`
`87`		`- camera_transition_joint_sigmas_map.begin(),`
`88`		`- camera_transition_joint_sigmas_map.end());`
	`86`	`+ insert_map_with_prefixed_keys(camera_transition_joint_sigmas_map,`
	`87`	`+ kinematics->camera_frame_id() + "_",`
	`88`	`+ transition_joint_sigmas_map);`
`89`	`89`	`}`
`90`	`90`
`91`	`91`	`// linear state transition parameters`
`@@ -172,8 +172,8 @@ std::shared_ptr<dbrt::VisualTracker> create_visual_tracker(`
`172`	`172`	`if (estimate_camera_offset)`
`173`	`173`	`{`
`174`	`174`	`sampling_blocks_definition = merge_sampling_block_definitions(`
`175`		`- camera_offset_sampling_blocks_definition,`
`176`		`- sampling_blocks_definition);`
	`175`	`+ sampling_blocks_definition,`
	`176`	`+ camera_offset_sampling_blocks_definition, kinematics->camera_frame_id() + '_');`
`177`	`177`	`}`
`178`	`178`
`179`	`179`	`tracker_parameters.sampling_blocks =`
Original file line number	Diff line number	Diff line change
`@@ -48,29 +48,38 @@ inline std::vector<std::vector<int>> definition_to_sampling_block(`
`48`	`48`
`49`	`49`	`inline SamplingBlocksDefinition merge_sampling_block_definitions(`
`50`	`50`	`const SamplingBlocksDefinition& definition_A,`
`51`		`- const SamplingBlocksDefinition& definition_B)`
	`51`	`+ const SamplingBlocksDefinition& definition_B,`
	`52`	`+ const std::string& prefixB)`
`52`	`53`	`{`
`53`	`54`	`SamplingBlocksDefinition merged = definition_A;`
`54`	`55`
`55`	`56`	`for (auto block_definition_B : definition_B)`
`56`	`57`	`{`
`57`		`- auto block_B = block_definition_B.begin();`
	`58`	`+ std::map<std::string, std::vector<std::string>> prefixed_block_definition_B;`
	`59`	`+ for (auto block : block_definition_B)`
	`60`	`+ {`
	`61`	`+ for (auto value : block.second)`
	`62`	`+ {`
	`63`	`+ prefixed_block_definition_B[block.first].push_back(prefixB + value);`
	`64`	`+ }`
	`65`	`+ }`
`58`	`66`
	`67`	`+ auto block_B = prefixed_block_definition_B.begin();`
`59`	`68`	`bool added = false;`
`60`	`69`	`for (auto& block_definition_A : merged)`
`61`	`70`	`{`
`62`	`71`	`auto block_A = block_definition_A.begin();`
`63`	`72`	`if (block_A->first == block_B->first)`
`64`	`73`	`{`
`65`		`- block_A->second.insert(std::end(block_A->second),`
`66`		`- std::begin(block_B->second),`
`67`		`- std::end(block_B->second));`
`68`		`- added = true;`
	`74`	`+ block_A->second.insert(std::end(block_A->second),`
	`75`	`+ std::begin(block_B->second),`
	`76`	`+ std::end(block_B->second));`
	`77`	`+ added = true;`
`69`	`78`	`}`
`70`	`79`	`}`
`71`	`80`	`if (!added)`
`72`	`81`	`{`
`73`		`- merged.push_back(block_definition_B);`
	`82`	`+ merged.push_back(prefixed_block_definition_B);`
`74`	`83`	`}`
`75`	`84`	`}`
`76`	`85`
`@@ -101,16 +110,26 @@ inline std::vector<double> extract_ordered_values(`
`101`	`110`	`std::vector<double> ordered_values(parameter_map.size());`
`102`	`111`	`for (auto entry : parameter_map)`
`103`	`112`	`{`
`104`		`- if (kinematics->name_to_index(entry.first) > ordered_values.size())`
	`113`	`+ std::string joint_name = entry.first;`
	`114`	`+ if (kinematics->name_to_index(joint_name) > ordered_values.size())`
`105`	`115`	`{`
`106`	`116`	`ROS_ERROR(`
`107`	`117`	`"Joint index exceeds number of read joints. "`
`108`	`118`	`"Did you forget to merge joint lists?");`
`109`	`119`	`exit(-1);`
`110`	`120`	`}`
`111`		`- ordered_values[kinematics->name_to_index(entry.first)] = entry.second;`
	`121`	`+ ordered_values[kinematics->name_to_index(joint_name)] = entry.second;`
`112`	`122`	`}`
`113`	`123`
`114`	`124`	`return ordered_values;`
`115`	`125`	`}`
	`126`	`+`
	`127`	`+inline void insert_map_with_prefixed_keys(`
	`128`	`+ const std::map<std::string, double>& new_values,`
	`129`	`+ const std::string& prefix,`
	`130`	`+ std::map<std::string, double>& destination){`
	`131`	`+ for (auto new_value: new_values){`
	`132`	`+ destination[prefix + new_value.first] = new_value.second;`
	`133`	`+ }`
	`134`	`+}`
`116`	`135`	`}`