diff --git a/config/params_sim.yaml b/config/params_sim.yaml
index a772f3fa12364137374dbe713c72a1b0cd0c94d7..977883e1b995ceb18fd711ca98ced59b18c812b3 100644
--- a/config/params_sim.yaml
+++ b/config/params_sim.yaml
@@ -7,4 +7,9 @@ undistort_points: false
publish_tf: true
publish_markers: true
filter_outliers: false
-outliers_max_sq_error: 4.0
\ No newline at end of file
+outliers_max_sq_error: 4.0
+use_manual_params: true
+fx: 70
+fy: 70
+cx: 320.5
+cy: 240.5
\ No newline at end of file
diff --git a/include/visual_gps_alg.h b/include/visual_gps_alg.h
index 3962285c4898b4e73995eb860713e52ab2affe33..291aaa16c78d3c5e256f3fd61b5fa1687887665a 100644
--- a/include/visual_gps_alg.h
+++ b/include/visual_gps_alg.h
@@ -66,6 +66,7 @@ class VisualGpsAlgorithm
std::vector<cv::Point3f> map_points;
std::vector<cv::Point3f> img_points;
+ void set_manual_params(double fx, double fy, double cx, double cy);
void update_points(std::vector<iri_blob_detector::blob> &blobs, std::vector<cv::Point3f> &points_3d,std::vector<cv::Point2f> &pixels_img, std::vector<cv::Point3f> &points_img_3d, std::vector<std::string> &ids);
bool OptimalRigidTransformation(std::vector<cv::Point3f> &pw, std::vector<cv::Point3f> &pc, std::vector<std::string> &ids, geometry_msgs::Pose &pose);
public:
diff --git a/launch/sim.launch b/launch/sim.launch
index 89dbfe2a3f3c2ad48656ddc433ab1f9e5e8be1b7..6799cb4d9ca9ea73c0b9d198882b5d68096cdb8a 100644
--- a/launch/sim.launch
+++ b/launch/sim.launch
@@ -64,6 +64,7 @@
<arg name="param_file" value="params_sim.yaml"/>
<arg name="param_path" value="$(find iri_visual_gps)/config"/>
<arg name="camera_name" value="usb_cam"/>
+ <arg name="camera_info_topic" value="/undistort_blob_blue/camera_info"/>
<arg name="blobs1_topic" value="blob_detector_red/blobs_undist"/>
<arg name="blobs2_topic" value="blob_detector_green/blobs_undist"/>
<arg name="blobs3_topic" value="blob_detector_blue/blobs_undist"/>
diff --git a/src/visual_gps_alg.cpp b/src/visual_gps_alg.cpp
index 7e6e764158abe5aeaad04ee0d6e30fffe268a6f0..22a460a33530ef6fb277e98fd637e7b955a375f9 100644
--- a/src/visual_gps_alg.cpp
+++ b/src/visual_gps_alg.cpp
@@ -23,25 +23,7 @@ void VisualGpsAlgorithm::config_update(Config& config, uint32_t level)
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);
+ this->set_manual_params(this->config_.fx, this->config_.fy, this->config_.cx, this->config_.cy);
}
else
{
@@ -51,6 +33,29 @@ void VisualGpsAlgorithm::config_update(Config& config, uint32_t level)
this->unlock();
}
+void VisualGpsAlgorithm::set_manual_params(double fx, double fy, double cx, double cy)
+{
+ if(fx != -1)
+ this->K.at<double>(0,0)=fx;
+ else
+ this->K.at<double>(0,0)=this->K0.at<double>(0,0);
+
+ if(fy != -1)
+ this->K.at<double>(1,1)=fy;
+ else
+ this->K.at<double>(1,1)=this->K0.at<double>(1,1);
+
+ if(cx != -1)
+ this->K.at<double>(0,2)=cx;
+ else
+ this->K.at<double>(0,2)=this->K0.at<double>(0,2);
+
+ if(cy != -1)
+ this->K.at<double>(1,2)=cy;
+ else
+ this->K.at<double>(1,2)=this->K0.at<double>(1,2);
+}
+
// VisualGpsAlgorithm Public API
void VisualGpsAlgorithm::update_points(std::vector<iri_blob_detector::blob> &blobs, std::vector<cv::Point3f> &points_3d,std::vector<cv::Point2f> &pixels_img, std::vector<cv::Point3f> &points_img_3d, std::vector<std::string> &ids)
{
diff --git a/src/visual_gps_alg_node.cpp b/src/visual_gps_alg_node.cpp
index 19182eca70513178278296782de5da79b47f4a1b..b9ea453f97d8fdcac36e5e60db62efa1051b901d 100644
--- a/src/visual_gps_alg_node.cpp
+++ b/src/visual_gps_alg_node.cpp
@@ -140,12 +140,15 @@ void VisualGpsAlgNode::camera_info_callback(const sensor_msgs::CameraInfo::Const
this->alg_.lock();
this->camera_info_mutex_enter();
cv::Mat K=(cv::Mat_<double>(3,3) << msg->K[0] , msg->K[1] , msg->K[2] , msg->K[3] , msg->K[4] , msg->K[5] , msg->K[6] , msg->K[7] , msg->K[8]);
- cv::Mat D=(cv::Mat_<double>(4,1) << msg->D[0] , msg->D[1] , msg->D[4] , 0.0);
+ cv::Mat D;
+ if(msg->D.size()>=5)
+ D=(cv::Mat_<double>(4,1) << msg->D[0] , msg->D[1] , msg->D[4] , 0.0);
cv::Mat R=(cv::Mat_<double>(3,3) << msg->R[0] , msg->R[1] , msg->R[2] , msg->R[3] , msg->R[4] , msg->R[5] , msg->R[6] , msg->R[7] , msg->R[8]);
cv::Mat P=(cv::Mat_<double>(3,4) << msg->P[0] , msg->P[1] , msg->P[2] , msg->P[3] , msg->P[4] , msg->P[5] , msg->P[6] , msg->P[7] , msg->P[8] , msg->P[9] , msg->P[10] , msg->P[11]);
this->alg_.set_camera_params(K,D,R,P,msg->width,msg->height);
this->camera_info_subscriber_.shutdown();
cam_info_received = true;
+ ROS_INFO("VisualGpsAlgNode::camera_info_callback: camera_info received");
this->camera_info_mutex_exit();
this->alg_.unlock();
}