Esempio n. 1
0
/*
ATAN-model calibrated cameras (fx/width, fy/height, cx/width, cy/height, d),which is not supported here
only OpenCV (pinhole) model is supported right now
*/
vo_system::vo_system(){


    ///vo_system launch the three threads, tracking, semidense mapping and dense mapping (3D superpixels)

    cv::FileStorage  fs2( (ros::package::getPath("dpptam")+"/src/data.yml").c_str(), cv::FileStorage::READ);

    std::string camera_path;
    fs2["camera_path"] >> camera_path;
    cont_frames = 0;

    int calculate_superpixels = (int)fs2["calculate_superpixels"];



    image_transport::ImageTransport it(nh);
    sub1 = it.subscribe(camera_path,1, & vo_system::imgcb,this);


    odom_pub = nh.advertise<nav_msgs::Odometry>("odom1", 50);



    /// advertising 3D map and camera poses in rviz
    pub_cloud = nh.advertise<sensor_msgs::PointCloud2> ("dpptam/map", 1);
    pub_poses = nh.advertise<sensor_msgs::PointCloud2> ("points_poses", 1);
    vis_pub = nh.advertise<visualization_msgs::Marker>( "dpptam/visualization_marker", 0 );
    /// advertising 3D map and camera poses in rviz


    /// pubishing current frame and the reprojection of the 3D map
    pub_image = it.advertise("dpptam/camera/image",1);
    /// pubishing current frame and the reprojection of the 3D map


    semidense_tracker.cont_frames = &cont_frames;
    semidense_tracker.stamps = &stamps;
    semidense_tracker.image_frame = &image_frame_aux;
    semidense_tracker.stamps_ros = &stamps_ros ;

    ///Launch semidense tracker thread*/
    boost::thread thread_semidense_tracker(&ThreadSemiDenseTracker,&images,&semidense_mapper,&semidense_tracker,&dense_mapper,&Map,&odom_pub,&pub_poses,&vis_pub,&pub_image);

    ///Launch semidense mapper thread
    boost::thread thread_semidense_mapper(&ThreadSemiDenseMapper,&images,&images_previous_keyframe,&semidense_mapper,&semidense_tracker,&dense_mapper,&Map,&pub_cloud);


    if (calculate_superpixels > 0.5)
    {
        ///launch dense mapper thread
         boost::thread thread_dense_mapper(&ThreadDenseMapper,&dense_mapper,&pub_cloud);
    }
    
    cout << "***    DPPTAM is working     *** " <<  endl << endl;
    cout << "***    Launch the example sequences or use your own sequence / live camera and update the file 'data.yml' with the corresponding camera_path and calibration parameters    ***"  << endl;

}
Esempio n. 2
0
vo_system::vo_system(){


    ///vo_system launch the three threads, tracking, semidense mapping and dense mapping (3D superpixels)

    cv::FileStorage  fs2("src/dpptam/src/data.yml", cv::FileStorage::READ);

    std::string camera_path;
    fs2["camera_path"] >> camera_path;
    cont_frames = 0;

    int calculate_superpixels = (int)fs2["calculate_superpixels"];



    image_transport::ImageTransport it(nh);
    sub1 = it.subscribe(camera_path,1, & vo_system::imgcb,this);


    odom_pub = nh.advertise<nav_msgs::Odometry>("odom1", 50);



    /// advertising 3D map and camera poses in rviz
    pub_cloud = nh.advertise<sensor_msgs::PointCloud2> ("points2", 1);
    pub_poses = nh.advertise<sensor_msgs::PointCloud2> ("points_poses", 1);
    vis_pub = nh.advertise<visualization_msgs::Marker>( "visualization_marker", 0 );
    /// advertising 3D map and camera poses in rviz


    pub_image = it.advertise("camera/image",1);

    semidense_tracker.cont_frames = &cont_frames;
    semidense_tracker.stamps = &stamps;
    semidense_tracker.image_frame = &image_frame_aux;
    semidense_tracker.stamps_ros = &stamps_ros ;

    ///Launch semidense tracker thread*/
    boost::thread thread_semidense_tracker(&ThreadSemiDenseTracker,&images,&semidense_mapper,&semidense_tracker,&dense_mapper,&Map,&odom_pub,&pub_poses,&vis_pub,&pub_image);

    ///Launch semidense mapper thread
    boost::thread thread_semidense_mapper(&ThreadSemiDenseMapper,&images,&images_previous_keyframe,&semidense_mapper,&semidense_tracker,&dense_mapper,&Map,&pub_cloud);

    if (calculate_superpixels > 0.5)
    {
        ///launch dense mapper thread
         boost::thread thread_dense_mapper(&ThreadDenseMapper,&dense_mapper,&pub_cloud);
    }
}