From 4a6ab54188f6cf3f004a31a0fc24b6c15cbe1fd3 Mon Sep 17 00:00:00 2001
From: fherrero <fherrero@iri.upc.edu>
Date: Tue, 30 Oct 2018 12:59:25 +0100
Subject: [PATCH] Add dynamic reconfigure manual camera parameter set
capability
---
cfg/VisualGps.cfg | 15 ++++++++--
config/params_lab.yaml | 8 ++++-
config/params_sample.yaml | 4 +++
config/params_sim.yaml | 8 ++++-
...ample_params.yaml => position_sample.yaml} | 0
include/visual_gps_alg.h | 2 +-
launch/node.launch | 4 +--
src/visual_gps_alg.cpp | 30 +++++++++++++++++++
8 files changed, 63 insertions(+), 8 deletions(-)
create mode 100644 config/params_sample.yaml
rename config/{sample_params.yaml => position_sample.yaml} (100%)
diff --git a/cfg/VisualGps.cfg b/cfg/VisualGps.cfg
index 77e9ac5..dc03372 100755
--- a/cfg/VisualGps.cfg
+++ b/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"))
diff --git a/config/params_lab.yaml b/config/params_lab.yaml
index 909d853..a772f3f 100644
--- a/config/params_lab.yaml
+++ b/config/params_lab.yaml
@@ -1,4 +1,10 @@
world_frame: map
odom_gps_frame: gps_odom
base_frame: base_link
-camera_frame: camera_rgb_optical_frame
\ No newline at end of file
+camera_frame: top_camera_optical
+
+undistort_points: false
+publish_tf: true
+publish_markers: true
+filter_outliers: false
+outliers_max_sq_error: 4.0
\ No newline at end of file
diff --git a/config/params_sample.yaml b/config/params_sample.yaml
new file mode 100644
index 0000000..18f4ce1
--- /dev/null
+++ b/config/params_sample.yaml
@@ -0,0 +1,4 @@
+world_frame: map
+odom_gps_frame: gps_odom
+base_frame: base_link
+camera_frame: camera_optical_frame
\ No newline at end of file
diff --git a/config/params_sim.yaml b/config/params_sim.yaml
index aada361..a772f3f 100644
--- a/config/params_sim.yaml
+++ b/config/params_sim.yaml
@@ -1,4 +1,10 @@
world_frame: map
odom_gps_frame: gps_odom
base_frame: base_link
-camera_frame: top_camera_optical
\ No newline at end of file
+camera_frame: top_camera_optical
+
+undistort_points: false
+publish_tf: true
+publish_markers: true
+filter_outliers: false
+outliers_max_sq_error: 4.0
\ No newline at end of file
diff --git a/config/sample_params.yaml b/config/position_sample.yaml
similarity index 100%
rename from config/sample_params.yaml
rename to config/position_sample.yaml
diff --git a/include/visual_gps_alg.h b/include/visual_gps_alg.h
index 4dc8fd1..3962285 100644
--- a/include/visual_gps_alg.h
+++ b/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;
diff --git a/launch/node.launch b/launch/node.launch
index a6dc003..266ed9e 100644
--- a/launch/node.launch
+++ b/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"/>
diff --git a/src/visual_gps_alg.cpp b/src/visual_gps_alg.cpp
index 735176e..7e6e764 100644
--- a/src/visual_gps_alg.cpp
+++ b/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;
--
GitLab