void Stereoproc::onInit()
{
ros::NodeHandle &nh = getNodeHandle();
ros::NodeHandle &private_nh = getPrivateNodeHandle();
it_.reset(new image_transport::ImageTransport(nh));
// Synchronize inputs. Topic subscriptions happen on demand in the connection
// callback. Optionally do approximate synchronization.
int queue_size;
private_nh.param("queue_size", queue_size, 5);
bool approx;
private_nh.param("approximate_sync", approx, false);
if (approx)
{
approximate_sync_.reset( new ApproximateSync(ApproximatePolicy(queue_size),
sub_l_raw_image_, sub_l_info_,
sub_r_raw_image_, sub_r_info_) );
approximate_sync_->registerCallback(boost::bind(&Stereoproc::imageCb,
this, _1, _2, _3, _4));
}
else
{
exact_sync_.reset( new ExactSync(ExactPolicy(queue_size),
sub_l_raw_image_, sub_l_info_,
sub_r_raw_image_, sub_r_info_) );
exact_sync_->registerCallback(boost::bind(&Stereoproc::imageCb,
this, _1, _2, _3, _4));
}
// Set up dynamic reconfiguration
ReconfigureServer::CallbackType f = boost::bind(&Stereoproc::configCb,
this, _1, _2);
reconfigure_server_.reset(new ReconfigureServer(config_mutex_, private_nh));
reconfigure_server_->setCallback(f);
// Monitor whether anyone is subscribed to the output
ros::SubscriberStatusCallback connect_cb = boost::bind(&Stereoproc::connectCb, this);
// Make sure we don't enter connectCb() between advertising and assigning to pub_disparity_
boost::lock_guard<boost::mutex> lock(connect_mutex_);
int publisher_queue_size;
private_nh.param("publisher_queue_size", publisher_queue_size, 1);
pub_mono_left_ = nh.advertise<sensor_msgs::Image>("left/image_mono", publisher_queue_size, connect_cb, connect_cb);
pub_mono_right_ = nh.advertise<sensor_msgs::Image>("right/image_mono", publisher_queue_size, connect_cb, connect_cb);
pub_color_left_ = nh.advertise<sensor_msgs::Image>("left/image_color", publisher_queue_size, connect_cb, connect_cb);
pub_color_right_ = nh.advertise<sensor_msgs::Image>("right/image_color", publisher_queue_size, connect_cb, connect_cb);
pub_mono_rect_left_ = nh.advertise<sensor_msgs::Image>("left/rect_mono", publisher_queue_size, connect_cb, connect_cb);
pub_color_rect_left_ = nh.advertise<sensor_msgs::Image>("left/rect_color", publisher_queue_size, connect_cb, connect_cb);
pub_mono_rect_right_ = nh.advertise<sensor_msgs::Image>("right/rect_mono", publisher_queue_size, connect_cb, connect_cb);
pub_color_rect_right_ = nh.advertise<sensor_msgs::Image>("right/rect_color", publisher_queue_size, connect_cb, connect_cb);
pub_disparity_ = nh.advertise<stereo_msgs::DisparityImage>("disparity", publisher_queue_size, connect_cb, connect_cb);
pub_disparity_vis_ = nh.advertise<sensor_msgs::Image>("disparity_vis", publisher_queue_size, connect_cb, connect_cb);
pub_pointcloud_ = nh.advertise<sensor_msgs::PointCloud2>("pointcloud", publisher_queue_size, connect_cb, connect_cb);
cv::cuda::printShortCudaDeviceInfo(cv::cuda::getDevice());
}
sptam::sptam_node::sptam_node(ros::NodeHandle& nh, ros::NodeHandle& nhp)
:
sptam_(nullptr),
motionModel_(new MotionModel(cv::Point3d(0,0,0), cv::Vec4d(1,0,0,0))),
it_( nhp ),
odom_to_map_(tf::Transform::getIdentity()),
transform_thread_(nullptr)
{
// Get node parameters
nhp.param<std::string>("odom_frame", odom_frame_, "/odom");
nhp.param<std::string>("base_link_frame", base_frame_, "/base_link");
nhp.param<std::string>("camera_frame", camera_frame_, "/camera");
nhp.param<std::string>("map_frame", map_frame_, "/map");
nhp.param<bool>("use_odometry", use_odometry_, false);
nhp.param<double>("transform_publish_freq", transform_publish_freq_, 30.0);
nhp.param<double>("tf_delay", tf_delay_, 1.0/transform_publish_freq_);
bool use_approx_sync;
nhp.param<bool>("approximate_sync", use_approx_sync, false);
// load feature detector
{
std::string detectorName;
nhp.param<std::string>("FeatureDetector/Name", detectorName, "GFTT");
std::cout << "detector: " << detectorName << std::endl;
featureDetector_ = cv::FeatureDetector::create( detectorName );
if ( not featureDetector_ )
ROS_ERROR_STREAM("could not load feature detector with name " << detectorName);
setParameters( nhp, featureDetector_, "FeatureDetector" );
}
// load descriptor extractor
{
std::string extractorName;
nhp.param<std::string>("DescriptorExtractor/Name", extractorName, "BRIEF");
std::cout << "extractor: " << extractorName << std::endl;
descriptorExtractor_ = cv::DescriptorExtractor::create( extractorName );
if ( not descriptorExtractor_ )
ROS_ERROR_STREAM("could not load descriptor extractor with name " << extractorName);
setParameters( nhp, descriptorExtractor_, "DescriptorExtractor" );
}
// load descriptor matcher
{
std::string matcherName;
nhp.param<std::string>("DescriptorMatcher/Name", matcherName, "BruteForce-Hamming");
std::cout << "matcher: " << matcherName << std::endl;
mapper_params_.descriptorMatcher = cv::DescriptorMatcher::create( matcherName );
if ( not mapper_params_.descriptorMatcher )
ROS_ERROR_STREAM("could not load descriptor matcher with name " << matcherName);
setParameters( nhp, mapper_params_.descriptorMatcher, "DescriptorMatcher" );
}
// Mapper Parameters
// nhp.param is not overloaded for unsigned int
int matchingCellSizeParam, framesBetweenKeyFramesParam, matchingNeighborhoodParam;
nhp.param<int>("MatchingCellSize", matchingCellSizeParam, 30);
mapper_params_.matchingCellSize = matchingCellSizeParam;
nhp.param<double>("MatchingDistance", mapper_params_.matchingDistanceThreshold, 25.0);
nhp.param<int>("MatchingNeighborhood", matchingNeighborhoodParam, 1);
mapper_params_.matchingNeighborhoodThreshold = matchingNeighborhoodParam;
nhp.param<double>("EpipolarDistance", mapper_params_.epipolarDistanceThreshold, 0.0);
nhp.param<double>("KeyFrameDistance", mapper_params_.keyFrameDistanceThreshold, 0.0);
nhp.param<int>("FramesBetweenKeyFrames", framesBetweenKeyFramesParam, 0);
mapper_params_.framesBetweenKeyFrames = framesBetweenKeyFramesParam;
// Camera Calibration Parameters
nhp.param<double>("FrustumNearPlaneDist", cameraParametersLeft_.frustumNearPlaneDist, 0.1);
nhp.param<double>("FrustumFarPlaneDist", cameraParametersLeft_.frustumFarPlaneDist, 1000.0);
cameraParametersRight_.frustumNearPlaneDist = cameraParametersLeft_.frustumNearPlaneDist;
cameraParametersRight_.frustumFarPlaneDist = cameraParametersLeft_.frustumFarPlaneDist;
// Create RowMatcher instance
rowMatcher_ = new RowMatcher( mapper_params_.matchingDistanceThreshold, mapper_params_.descriptorMatcher, mapper_params_.epipolarDistanceThreshold );
// Subscribe to images messages
sub_l_image_.subscribe(nhp, "/stereo/left/image_rect", 1);
sub_l_info_ .subscribe(nhp, "/stereo/left/camera_info", 1);
sub_r_image_.subscribe(nhp, "/stereo/right/image_rect", 1);
sub_r_info_ .subscribe(nhp, "/stereo/right/camera_info", 1);
if ( use_approx_sync )
{
approximate_sync_.reset( new ApproximateSync( ApproximatePolicy(10),
sub_l_image_, sub_l_info_,
sub_r_image_, sub_r_info_ ) );
approximate_sync_->registerCallback( boost::bind( &sptam::sptam_node::onImages,
this, _1, _2, _3, _4 ) );
}
else
{
exact_sync_.reset( new ExactSync( ExactPolicy(1),
sub_l_image_, sub_l_info_,
sub_r_image_, sub_r_info_ ) );
exact_sync_->registerCallback( boost::bind( &sptam::sptam_node::onImages,
this, _1, _2, _3, _4 ) );
}
mapPub_ = nhp.advertise<sensor_msgs::PointCloud2>("point_cloud", 100);
posePub_ = nhp.advertise<geometry_msgs::PoseStamped>("robot/pose", 100);
leftKpPub_ = it_.advertise("/stereo/left/keypoints", 1);
rightKpPub_ = it_.advertise("/stereo/right/keypoints", 1);
// start map->odom transform publisher thread
if ( use_odometry_ )
// this loop periodically publishes map->odom transform from another thread
transform_thread_ = new std::thread( boost::bind(&sptam::sptam_node::publishTransformLoop, this) );
else
// this loop periodically publishes map->base_link transform from another thread
transform_thread_ = new std::thread( boost::bind(&sptam::sptam_node::publishTransformLoop2, this) );
ROS_INFO_STREAM("sptam node initialized");
}