rgbdslam with single rgb and depth images [closed]

edit

Hi all,

I try to use rgbdslam for my dataset consisting out of depth and rgb images saved as png.

To make my live easy I tried to write a bag file from these images and then use this bagfile for the rgbdslam. But nothing gets displayed in the rgbdslam-gui when I playback my self-written bag (directly recorded bagfile from the kinect is working).

Which is kind of weird because I can display my bag file with $rosrun image_view image_vie image:=/camera/rgb/image_color so the sensor_msgs delivered by the bag file seems to be okay.

Could be a wrong encoding be the problem?

I publish the following topics:

/camera/depth/camera_info  
/camera/depth/image  
/camera/rgb/camera_info  
/camera/rgb/image_color

----------------------------------------------------------------------------------------

here is my bagwriter.cpp

int main(int argc, char **argv){

        ros::init(argc, argv, "listener");
        ros::NodeHandle nh;
        camera_info_manager::CameraInfoManager *cam_info_man_1;
        camera_info_manager::CameraInfoManager *cam_info_man_2;
        sensor_msgs::CameraInfo depth_info;
        sensor_msgs::CameraInfo rgb_info;
        sensor_msgs::ImagePtr imgDepth;
        sensor_msgs::ImagePtr imgRGB;
        cv::Mat matDepth;
        cv::Mat matRGB;
        std::string depth_frame_id = "/openni_depth_optical_frame";
        std::string rgb_frame_id = "/openni_rgb_optical_frame";
        rosbag::Bag bag;
        bag.open("test.bag", rosbag::bagmode::Write);
        int seq;

        // fill in camera_info from calibration file
        cam_info_man_1 = new camera_info_manager::CameraInfoManager(nh, rgb_frame_id, "file:///path_to_calibration.yaml" );
        rgb_info = cam_info_man_1->getCameraInfo();
        rgb_info.header.frame_id = rgb_frame_id;

        cam_info_man_2 = new camera_info_manager::CameraInfoManager(nh, depth_frame_id, "file:///path_to_calibration.yaml");
        depth_info = cam_info_man_2->getCameraInfo();
        depth_info.header.frame_id = depth_frame_id;

        //load images and save to the bag file
        for(int i=0 ; i <= atoi(argv[1]) ; i++){

            std::stringstream ss;
            ss << i;

            matDepth = cv::imread("/path_to_image", CV_LOAD_IMAGE_UNCHANGED);
            matRGB = cv::imread("/path_to_image", CV_LOAD_IMAGE_UNCHANGED);
            imgDepth = SubscriberTools::matToImage(matDepth);
            imgRGB = SubscriberTools::matToImage(matRGB);

            for(int j=0; j <= 30; j++ ){
                depth_info.header.stamp = rgb_info.header.stamp = ros::Time::now();
                depth_info.header.seq = rgb_info.header.seq = seq;
                bag.write("camera/depth/camera_info", ros::Time::now(), depth_info );
                bag.write("camera/depth/image", ros::Time::now(), imgDepth );
                bag.write("camera/rgb/camera_info", ros::Time::now(), rgb_info );
                bag.write("camera/rgb/image_color", ros::Time::now(), imgRGB );
                seq++;
                //rate.sleep();
            }

            ROS_INFO("saved %i images to bag",i);
        }  
        bag.close();
}

here is my mat2image function

sensor_msgs::ImagePtr SubscriberTools::matToImage(cv::Mat mat)
{
    sensor_msgs::ImagePtr output(new sensor_msgs::Image());

    // copy header
    output->header.stamp = ros::Time::now();
    output->width = mat.cols;
    output->height = mat.rows;
    output->step = mat.cols * mat.elemSize();
    output->is_bigendian = false;
    output->encoding = idToEncoding(mat.type());

    // copy actual data
    output->data.assign(mat.data, mat.data + size_t(mat.rows * output->step));
    return output;
}