Add dynamic reconfigure manual camera parameter set capability

cfg/VisualGps.cfg

@@ -44,29 +44,38 @@ gen.add("odom_gps_frame"        ,  str_t,     0,                               "
 gen.add("odom_frame"            ,  str_t,     0,                               "Odometry reference frame",       "odom")
 gen.add("base_frame"            ,  str_t,     0,                               "Base frame id",                  "base_link")
 gen.add("camera_frame"          ,  str_t,     0,                               "Camera frame id",                "top_camera_optical")
+
 gen.add("undistort_points"      ,  bool_t,    0,                               "Enable point undistorsion",      False)
 gen.add("publish_tf"            ,  bool_t,    0,                               "Enable tf broadcasting",         True)
 gen.add("publish_markers"       ,  bool_t,    0,                               "Enable marker publishing",       True)
+
 gen.add("filter_outliers"       ,  bool_t,    0,                               "Enable outlier filtering",       True)
 gen.add("outliers_max_sq_error" ,  double_t,  0,                               "Max sq error value to filter",   4,         0.0,100.0)
+
+gen.add("use_manual_params"     ,  bool_t,    0,                               "Enable manual camera params set", False)
+gen.add("fx"                    ,  double_t,  0,                               "Focal length x",                -1.0,        -1.0,2000.0)
+gen.add("fy"                    ,  double_t,  0,                               "Focal length y",                -1.0,        -1.0,2000.0)
+gen.add("cx"                    ,  double_t,  0,                               "Focal length y",                -1.0,        -1.0,2000.0)
+gen.add("cy"                    ,  double_t,  0,                               "Focal length y",                -1.0,        -1.0,2000.0)
+
 gen.add("color1_id"             ,  str_t,     0,                               "Color 1 identifier",             "color1")
 gen.add("color1_x"              ,  double_t,  0,                               "Color 1 X position",             0.5,      -100.0,100.0)
 gen.add("color1_y"              ,  double_t,  0,                               "Color 1 Y position",             0.5,      -100.0,100.0)
 gen.add("color1_z"              ,  double_t,  0,                               "Color 1 Z position",             0.5,      -100.0,100.0)
+
 gen.add("color2_id"             ,  str_t,     0,                               "Color 2 identifier",             "color2")
 gen.add("color2_x"              ,  double_t,  0,                               "Color 2 X position",             0.5,      -100.0,100.0)
 gen.add("color2_y"              ,  double_t,  0,                               "Color 2 Y position",             0.5,      -100.0,100.0)
 gen.add("color2_z"              ,  double_t,  0,                               "Color 2 Z position",             0.5,      -100.0,100.0)
+
 gen.add("color3_id"             ,  str_t,     0,                               "Color 3 identifier",             "color3")
 gen.add("color3_x"              ,  double_t,  0,                               "Color 3 X position",             0.5,      -100.0,100.0)
 gen.add("color3_y"              ,  double_t,  0,                               "Color 3 Y position",             0.5,      -100.0,100.0)
 gen.add("color3_z"              ,  double_t,  0,                               "Color 3 Z position",             0.5,      -100.0,100.0)
+
 gen.add("color4_id"             ,  str_t,     0,                               "Color 4 identifier",             "color4")
 gen.add("color4_x"              ,  double_t,  0,                               "Color 4 X position",             0.5,      -100.0,100.0)
 gen.add("color4_y"              ,  double_t,  0,                               "Color 4 Y position",             0.5,      -100.0,100.0)
 gen.add("color4_z"              ,  double_t,  0,                               "Color 4 Z position",             0.5,      -100.0,100.0)
 
-
-
-
 exit(gen.generate(PACKAGE, "VisualGpsAlgorithm", "VisualGps"))

config/params_lab.yaml

 world_frame:    map
 odom_gps_frame: gps_odom
 base_frame:     base_link
-camera_frame:   camera_rgb_optical_frame
+camera_frame:   top_camera_optical
\ No newline at end of file
+
+undistort_points: false
+publish_tf: true
+publish_markers: true
+filter_outliers: false
+outliers_max_sq_error: 4.0
\ No newline at end of file

config/params_sample.yaml

+world_frame:    map
+odom_gps_frame: gps_odom
+base_frame:     base_link
+camera_frame:   camera_optical_frame
\ No newline at end of file

config/params_sim.yaml

 world_frame:    map
 odom_gps_frame: gps_odom
 base_frame:     base_link
-camera_frame:   top_camera_optical
+camera_frame:   top_camera_optical
\ No newline at end of file
+
+undistort_points: false
+publish_tf: true
+publish_markers: true
+filter_outliers: false
+outliers_max_sq_error: 4.0
\ No newline at end of file

include/visual_gps_alg.h

@@ -54,7 +54,7 @@ class VisualGpsAlgorithm
     pthread_mutex_t access_;    
 
     // private attributes and methods
-    cv::Mat K,D,R,P;
+    cv::Mat K,K0,D,R,P;
     geometry_msgs::Pose pose; 
     
     Eigen::Vector3d t_base_2_camera;

launch/node.launch

@@ -3,8 +3,8 @@
 <launch>
 
   <arg name="node_name"      default="iri_visual_gps"/>
-  <arg name="positions_file" default="position_lab.yaml"/>
-  <arg name="param_file"     default="sample_params.yaml"/>
+  <arg name="positions_file" default="position_sample.yaml"/>
+  <arg name="param_file"     default="params_sample.yaml"/>
   <arg name="param_path"     default="$(find iri_visual_gps)/config"/>
   <arg name="camera_name"    default="usb_cam"/>
   <arg name="blobs1_topic"   default="blobs1"/>

src/visual_gps_alg.cpp

@@ -5,6 +5,8 @@
 VisualGpsAlgorithm::VisualGpsAlgorithm(void)
 {
   pthread_mutex_init(&this->access_,NULL);
+  this->K= cv::Mat::zeros(3,3,CV_64F);
+  this->K0= cv::Mat::zeros(3,3,CV_64F);
 }
 
 VisualGpsAlgorithm::~VisualGpsAlgorithm(void)
@@ -19,6 +21,33 @@ void VisualGpsAlgorithm::config_update(Config& config, uint32_t level)
   // save the current configuration
   this->config_=config;
   
+  if(this->config_.use_manual_params)
+  {
+    if(this->config_.fx != -1)
+      this->K.at<double>(0,0)=this->config_.fx;
+    else
+      this->K.at<double>(0,0)=this->K0.at<double>(0,0);
+    
+    if(this->config_.fy != -1)
+      this->K.at<double>(1,1)=this->config_.fy;
+    else
+      this->K.at<double>(1,1)=this->K0.at<double>(1,1);
+    
+    if(this->config_.cx != -1)
+      this->K.at<double>(0,2)=this->config_.cx;
+    else
+      this->K.at<double>(0,2)=this->K0.at<double>(0,2);
+    
+    if(this->config_.cy != -1)
+      this->K.at<double>(1,2)=this->config_.cy;
+    else
+      this->K.at<double>(1,2)=this->K0.at<double>(1,2);
+  }
+  else
+  {
+    this->K = this->K0.clone();
+  }
+  
   this->unlock();
 }
 
@@ -79,6 +108,7 @@ void VisualGpsAlgorithm::update_points(std::vector<iri_blob_detector::blob> &blo
 void VisualGpsAlgorithm::set_camera_params(cv::Mat &K,cv::Mat &D,cv::Mat &R,cv::Mat &P,unsigned int width,unsigned int height)
 {
   this->K=K;
+  this->K0=this->K.clone();
   this->D=D;
   this->R=R;
   this->P=P;