Commit 77859955 authored by James Ward's avatar James Ward
Browse files

Remove topics from loaded parameters

Also deduce number of rings automatically from pointcloud.
parent bb246396
......@@ -5,8 +5,6 @@ feature_extraction:
y_max: 1.0
z_min: -0.7
z_max: 2.0
image_topic: /gmsl/A2/image_color
lidar_topic: /velodyne/front/points
fisheye_model: true
lidar_ring_count: 16
chessboard:
......
......@@ -9,10 +9,8 @@ namespace cam_lidar_calibration
{
struct initial_parameters_t
{
std::string camera_topic;
std::string lidar_topic;
bool fisheye_model;
int lidar_ring_count;
int lidar_ring_count = 0;
cv::Size chessboard_pattern_size;
int square_length; // in millimetres
cv::Size board_dimensions; // in millimetres
......
......@@ -10,6 +10,8 @@
<rosparam file="$(find cam_lidar_calibration)/cfg/params.yaml" />
<node pkg="cam_lidar_calibration" type="feature_extraction" name="feature_extraction" output="screen">
<remap from="~image" to="/gmsl/A2/image_color" />
<remap from="~pointcloud" to="/velodyne/front/points" />
</node>
<node type="rviz" name="rviz" pkg="rviz" args="-d $(find cam_lidar_calibration)/rviz/cam_lidar_calibration.rviz" />
......
......@@ -62,8 +62,8 @@ void FeatureExtractor::onInit()
server->setCallback(f);
// Synchronizer to get synchronized camera-lidar scan pairs
image_sub_ = std::make_shared<image_sub_type>(public_nh, i_params.camera_topic, queue_rate_);
pc_sub_ = std::make_shared<pc_sub_type>(public_nh, i_params.lidar_topic, queue_rate_);
image_sub_ = std::make_shared<image_sub_type>(private_nh, "image", queue_rate_);
pc_sub_ = std::make_shared<pc_sub_type>(private_nh, "pointcloud", queue_rate_);
image_pc_sync_ = std::make_shared<message_filters::Synchronizer<ImageLidarSyncPolicy>>(
ImageLidarSyncPolicy(queue_rate_), *image_sub_, *pc_sub_);
image_pc_sync_->registerCallback(boost::bind(&FeatureExtractor::extractRegionOfInterest, this, _1, _2));
......@@ -307,6 +307,13 @@ FeatureExtractor::extractBoard(const PointCloud::Ptr& cloud)
void FeatureExtractor::extractRegionOfInterest(const sensor_msgs::Image::ConstPtr& image,
const PointCloud::ConstPtr& pointcloud)
{
// Check if we have deduced the lidar ring count
if (i_params.lidar_ring_count == 0)
{
pcl::PointXYZIR min, max;
pcl::getMinMax3D(*pointcloud, min, max);
i_params.lidar_ring_count = max.ring + 1;
}
PointCloud::Ptr cloud_bounded(new PointCloud);
cam_lidar_calibration::OptimisationSample sample;
passthrough(pointcloud, cloud_bounded);
......
......@@ -5,10 +5,7 @@ namespace cam_lidar_calibration
void loadParams(const ros::NodeHandle& n, initial_parameters_t& i_params)
{
int cb_w, cb_h, w, h, e_x, e_y, i_l, i_b;
n.getParam("image_topic", i_params.camera_topic);
n.getParam("lidar_topic", i_params.lidar_topic);
n.getParam("fisheye_model", i_params.fisheye_model);
n.getParam("lidar_ring_count", i_params.lidar_ring_count);
n.getParam("chessboard/pattern_size/width", cb_w);
n.getParam("chessboard/pattern_size/height", cb_h);
i_params.chessboard_pattern_size = cv::Size(cb_w, cb_h);
......
......@@ -701,8 +701,8 @@ Optimiser::Optimiser()
cloud = pcl::PointCloud<pcl::PointXYZIR>::Ptr(new pcl::PointCloud<pcl::PointXYZIR>);
cv_ptr = cv_bridge::CvImagePtr(new cv_bridge::CvImage);
message_filters::Subscriber<sensor_msgs::Image> image_sub(n, i_params.camera_topic, 5);
message_filters::Subscriber<sensor_msgs::PointCloud2> pcl_sub(n, i_params.lidar_topic, 5);
message_filters::Subscriber<sensor_msgs::Image> image_sub(n, "image", 5);
message_filters::Subscriber<sensor_msgs::PointCloud2> pcl_sub(n, "pointcloud", 5);
sync = std::make_shared<message_filters::Synchronizer<ImageLidarSyncPolicy>>(ImageLidarSyncPolicy(5), image_sub,
pcl_sub);
sync->registerCallback(boost::bind(&Optimiser::sensorInfoCB, this, _1, _2));
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment