Skimap with synthetic data

[CaptainTrunky]

First of all, thanks for the awesome tool. I'm trying to build a map of a synthetic scene using SkiMap. To do so, I convert my own data into bag file and replay it. Resulting point clouds look decent to me:

However, whole map gets filled with noisy obstacles quite fast:

The black thing in front of the agent should be a few voxels thick, but it's drifting a lot.

Definitely, there is something fishy about agent's poses, which I have to convert from my own environment to /tf. I use simplified version of a transformation tree that includes just map, odom, base_link and optical_frame (see figures above). I fix agent's pose at the very first frame and during map reconstruction I store differences between initial state and a current one:

        cur_position = position - init_pose

        cur_orientation = quaternion_multiply(init_orient, quaternion_conjugate(orientation))

        cur_pose = {cur_position, cur_orientation}

        baselink_tf = pose_to_tf(cur_pose, 'odom', 'base_link', timestamp)

Could you provide any insights about what could I try?

[m4nh]

I think it is wrong to compute Current POse in that way. You need to multiply 4x4 matrices in order to obtain the correct position, because in your difference (position - init_pose) you are not taking into account the Roto-Traslation component.

[CaptainTrunky]

Thank you. It looks like I've nailed it with your help.

[fangzhangyun]

@CaptainTrunky I find you have solved this skimap , I have tried to achieve it , but i failed , Can you help me ? i don't know how to change parameters in the launch file .

[CaptainTrunky]

@CaptainTrunky I find you have solved this skimap , I have tried to achieve it , but i failed , Can you help me ? i don't know how to change parameters in the launch file .

Could you provide more details? What's exactly wrong when you're running skimap?

[fangzhangyun]

this is my ros_rgbd.cc . ` cv::Mat camera_pose; camera_pose= mpSLAM->TrackRGBD(cv_ptrRGB->image,cv_ptrD->image,cv_ptrRGB->header.stamp.toSec());

cv::Mat TWC = camera_pose.inv();
cv::Mat RWC= TWC.rowRange(0,3).colRange(0,3);
cv::Mat tWC= TWC.rowRange(0,3).col(3);

tf::Matrix3x3 M(RWC.at<float>(0,0),RWC.at<float>(0,1),RWC.at<float>(0,2),
                RWC.at<float>(1,0),RWC.at<float>(1,1),RWC.at<float>(1,2),
                RWC.at<float>(2,0),RWC.at<float>(2,1),RWC.at<float>(2,2));
tf::Vector3 V(tWC.at<float>(0), tWC.at<float>(1), tWC.at<float>(2));
tf::Quaternion q;
M.getRotation(q);
q.normalize();

tf::Transform transform = tf::Transform(M, V);

static tf::TransformBroadcaster br;
br.sendTransform(tf::StampedTransform(transform,ros::Time(0),"world","camera"));`

this is my skimap_live.launch `

<arg name="camera" default="camera" />

<node name="skimap_live" output="screen" pkg="skimap_ros" type="skimap_live">

    <!-- Generic Parameters -->
    <param name="hz" value="10" />

    <!-- Topics Parameters -->
    <param name="camera_rgb_topic" value="/camera/rgb/image_color" />
    <param name="camera_depth_topic" value="/camera/depth/image" />

    <!-- Cloud parameters  -->
    <param name="point_cloud_downscale" value="1" />

    <!-- RGB-D Parameters -->
    <param name="fx" value="535.4" />
    <param name="fy" value="539.2" />
    <param name="cx" value="320.1" />
    <param name="cy" value="247.6" />
    <param name="rows" value="480" />
    <param name="cols" value="640" />
    <param name="camera_distance_max" value="3" />
    <param name="camera_distance_min" value="0.45" />

    <!-- Mapping parameters -->
    <param name="base_frame_name" value="world" />
    <param name="camera_frame_name" value="camera" />
    <param name="map_resolution" value="0.05" />
    <param name="min_voxel_weight" value="200" />
    <param name="ground_level" value="0.05" />
    <param name="agent_height" value="1.5" />
    <param name="enable_chisel" value="false" />
    <param name="chisel_step" value="30" />
    <param name="height_color" value="true" />

</node>

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

` my bag file is the TUM dataset, and this picture is the result:

so i think my parameters is bad.