コード例 #1
0
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());
}
コード例 #2
0
ファイル: sptam_node.cpp プロジェクト: Aerobota/sptam
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");
}