Integration with PTAM

[rnunziata]

I've intergrated skimap with ptam. I like this better because you do not have to do a mod since ptam published pose msg. I have one issue: it works find when I feed it tiago_bar.bag but if I feed it a bag from other slam dataset rgbd_dataset_freiburg3_long_office_household_validation.bag even when adjusted for camera parameters the point cloud and map are way off. see pictures below. Any idea why this would be. Is there different about the camera set up . I strip out all but the image and depth messages in both bag files.

My integrations launch file is as follows. note; ptam requires that you set the plane using its gui which will pop up once set it will forward pose.

<launch>

    <param name="use_sim_time" value="true" /> 

    <arg name="bag_file"         default="/home/rjn/data/bags/skimap_ros/new_tiago_bar.bag" />   
    <arg name="bag_image_topic"  default="/xtion/rgb/image_raw" />
    <arg name="bag_depth_topic"  default="/xtion/depth/image_raw" />    
    <arg name="bag_camera_frame" default="/xtion_rgb_optical_frame" />   
    <arg name="pose_base_frame"  default="/world" /> 
    <arg name="pose_topic"       default="/vslam/pose_world" />      <!-- "/vslam/pose" -->

     <!-- *** Make sue you update SkimapPtamFixParams to correct camera instrinces -->   
    <rosparam command="load" file="$(find skimap_ros)/SkimapPtamFixParams.yaml"/> 

    <node pkg="rosbag" type="play" name="player" output="screen" 
          args=" --clock --delay 15 -r 0.25 $(arg bag_file) /camera/rgb/image_color:=$(arg bag_image_topic)"/> 

    <!-- De-mosaics and undistorts the raw camera image stream. create mono gray image -->
    <node pkg="image_proc" type="image_proc" name="proc" ns="camera"> 
        <remap from="/camera/image_raw" to="$(arg bag_image_topic)"/>
    </node> 

    <!--   must be mono gray image -->
    <node name="ptam" pkg="ptam" type="ptam" clear_params="true" output="screen">
         <remap from="image"         to="/camera/image_mono" /> 
         <param name="parent_frame"  value="$(arg pose_base_frame)"/> 
         <rosparam command="load" file="$(find skimap_ros)/SkimapPtamFixParams.yaml"/>         
    </node>

    <node name="pose_tf" pkg="pose_tf" type="pose_tf" clear_params="true" output="screen">
         <param name="pose_topic"     value="$(arg pose_topic)"/>    
         <param name="world_frame"    value="$(arg pose_base_frame)"/>
         <param name="target_frame"   value="$(arg bag_camera_frame)"/>                  
    </node>

    <node name="skimap_live" output="screen" pkg="skimap_ros" type="skimap_live">
        <param name="camera_rgb_topic"   value="$(arg bag_image_topic)"/>
        <param name="camera_depth_topic" value="$(arg bag_depth_topic)"/>
        <param name="base_frame_name"    value="$(arg pose_base_frame)"/>
        <param name="camera_frame_name"  value="$(arg bag_camera_frame)"/>   
        <rosparam command="load" file="$(find skimap_ros)/SkimapPtamFixParams.yaml"/>     

    </node>

    <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find skimap_ros)/rviz/skimap_live_orb.rviz" />

</launch>

pose_tf.cpp : code for node

pose_tf node is as follows:

#include <ros/ros.h>
#include <ros/console.h>
#include <tf/transform_broadcaster.h>
#include "geometry_msgs/PoseWithCovarianceStamped.h"

std::string pose_topic   = "                      ";
std::string world_frame  = "                      ";
std::string target_frame = "                      ";

void poseCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg)
{
    tf::TransformBroadcaster br;
    tf::Transform transform;

    geometry_msgs::Pose pose = msg->pose.pose;
    geometry_msgs::Point position = pose.position;
    geometry_msgs::Quaternion orientation = pose.orientation;

    transform.setOrigin( tf::Vector3(position.x, position.y, position.z) );
    transform.setRotation( tf::Quaternion( orientation.x, orientation.y, orientation.z, orientation.w) );  
    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), world_frame, target_frame));
}

int main(int argc, char** argv){
    ros::init(argc, argv, "pose_tf_broadcaster");
    ROS_INFO("Broadcasted transformation pose_tf_broadcaster");
    ros::NodeHandle nh;
    nh.param<std::string>("pose_topic",   pose_topic,  "/xxxxx/pose"); 
    std::cout <<   pose_topic << std::endl;
    std::cout <<   pose_topic << std::endl;   
    nh.param<std::string>("world_frame",  world_frame,  "/xxxx/world");   
    nh.param<std::string>("target_frame", target_frame, "/xxxx/openni_rgb_optical_frame");          
    ros::Subscriber sub = nh.subscribe(pose_topic, 1000, poseCallback);

    ros::spin();

    return 0;
}

I combined the Parameter files as follows:

# ----------------------------------------------------------------------
#          PTAM: beware of duplicate params in SKIPMAP below
# ----------------------------------------------------------------------

gui: True

WiggleScale: 0.1
BundleMEstimator: Tukey
TrackerMEstimator: Tukey
MinTukeySigma: 0.4
CandidateMinSTScore: 70  
Calibrator_BlurSigma: 1.0
Calibrator_MeanGate: 10
Calibrator_MinCornersForGrabbedImage: 20
Calibrator_Optimize: 0
Calibrator_Show: 1
Calibrator_NoDistortion: 0
CameraCalibrator_MaxStepDistFraction: 0.3
CameraCalibrator_CornerPatchSize: 20
GLWindowMenu_Enable: True
GLWindowMenu_mgvnMenuItemWidth: 90
GLWindowMenu_mgvnMenuTextOffset: 20
InitLevel: 1
MaxKFDistWiggleMult: 1
MaxPatchesPerFrame: 300
MaxKF: 15
TrackingQualityFoundPixels: 50
UseKFPixelDist: True
NoLevelZeroMapPoints: True
FASTMethod: OAST16
MaxStereoInitLoops: 4
AutoInitPixel: 20

# from ATANCamera.cc
#  mvFocal[0] = mvImageSize[0] * mgvvCameraParams[0];
#  mvFocal[1] = mvImageSize[1] * mgvvCameraParams[1];
#  mvCenter[0] = mvImageSize[0] * mgvvCameraParams[2] - 0.5;
#  mvCenter[1] = mvImageSize[1] * mgvvCameraParams[3] - 0.5;
#                           fx      fy      cx      cy
#         Freiburg 3 RGB    535.4   539.2   320.1   247.6

ImageSizeX: 640
ImageSizeY: 480
ARBuffer_width: 1200
ARBuffer_height: 900

Cam_fx: 0.836562  # 535.4/640 
Cam_fy: 1.1233333   # 539.2/480 
Cam_cx: 0.500156    # 320.1/640
Cam_cy: 0.515833    # 247.6/480 
Cam_s:  0.25         # radial distortion

# ----------------------------------------------------------------------
#          SKIP MAP
# ----------------------------------------------------------------------

# RGB-D Parameters -->
rows: 480  
cols: 640  
fx: 535.4 
fy: 539.2 
cx: 320.1  
cy: 247.6  

 # Generic Parameters -->
hz: 10 

# Cloud parameters  -->
#point_cloud_downscale: 2  

# Mapping parameters -->       
mapping: true
map_resolution: 0.01
camera_distance_max: 8
#camera_distance_min: 0.45
max_integration_step: 1  
max_deintegration_step: 5 

[rnunziata]

If this does not belong here please let me know.....sorry for any inconvenience.

[rnunziata]

I will close this and re-post as separate issue related to bag files. PTAM integration is not appropriate here.