diff --git a/cfg/VisualGps.cfg b/cfg/VisualGps.cfg
index 77e9ac5532ed737b17fa3b1c9e3675b409fcc203..dc03372b1c13fc8563524307c51822223c336259 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 909d853f95a57db302f18202b13c7cf0b62a9a17..a772f3fa12364137374dbe713c72a1b0cd0c94d7 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 0000000000000000000000000000000000000000..18f4ce1d48b307c6bc13bd3fb9981e3f55ea4c71
--- /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 aada361b7425e5a354a639a45c978000e2b847e6..a772f3fa12364137374dbe713c72a1b0cd0c94d7 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 4dc8fd140493019d0332e9470b34fabb8d011170..3962285c4898b4e73995eb860713e52ab2affe33 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 a6dc00358796dbd92439832e8391a360e255d63a..266ed9ecd814203fdd0279560c870e4ec2d22074 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 735176e520a517967e4c7f8cba57ea3d63e8d6f3..7e6e764158abe5aeaad04ee0d6e30fffe268a6f0 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;