Ask Your Question
0

Merging 2 pcl from kinects

asked 2014-04-08 03:56:02 -0500

Andrii Matviienko gravatar image

updated 2016-10-24 09:02:39 -0500

ngrennan gravatar image

Hi all,

I am trying to merge two pointclouds from kinects, convert this merged pointcloud data into 2D image and publish the image data as a topic. I am using ros fuerte.

Here is my launch file:

<launch>

        <!--broadcasting the transformation from world frame to kinect frames-->
        <node pkg="tf" type="static_transform_publisher" name="overhead_kinect1" args="-0.37 0.94 1.04 0 0.83 -1.57 /world_base_link /overhead_kinect1_link  40" />
        <node pkg="tf" type="static_transform_publisher" name="overhead_kinect2" args=" 0.37 0.98 1.02 0 0.80 -1.57 /world_base_link /overhead_kinect2_link  40" />

    <include file="$(find openni_launch)/launch/openni.launch">
        <arg name="camera" value="overhead_kinect1"/>
        <arg name="depth_registration" value="false" />
        <!--<arg name="device_id" value="A00365911792039A"/>-->
        <arg name="device_id" value="002@18"/>
    </include>

    <include file="$(find openni_launch)/launch/openni.launch">
        <arg name="camera" value="overhead_kinect2"/>
        <arg name="depth_registration" value="false" />
        <!--<arg name="device_id" value="000000000000000"/>-->
        <arg name="device_id" value="002@20"/>
    </include>
<!--
        <node pkg="rviz" type="rviz" name="rviz"
         args="-d $(find ar_marker_detection_multiple)/ar_marker_detection.vcg"/>
 -->
        <node pkg="ar_marker_detection_multiple" type="ar_kinect" name="ar_kinect" respawn="false" output="screen"/>

</launch>

and part of the code responsible for merging:

ros::Publisher pub;
tf::TransformListener *listener;
sensor_msgs::PointCloud2 output, output1, output2;
pcl::PointCloud<pcl::PointXYZ> output_pcl, output1_pcl, output2_pcl;

void cloud_cb1(const sensor_msgs::PointCloud2ConstPtr& input1) {
    listener->waitForTransform("/world_base_link", (*input1).header.frame_id, (*input1).header.stamp, ros::Duration(5.0));
    sensor_msgs::PointCloud &pcout) const 
    pcl_ros::transformPointCloud("/world_base_link", *input1, output1, *listener);
    pcl::fromROSMsg(output1, output1_pcl);
    output_pcl = output1_pcl;
    output_pcl += output2_pcl;
    pcl::toROSMsg(output_pcl, output);
    pub.publish(output);
}

void cloud_cb2(const sensor_msgs::PointCloud2ConstPtr& input2) {
    listener->waitForTransform("/world_base_link", (*input2).header.frame_id, (*input2).header.stamp, ros::Duration(5.0));
    sensor_msgs::PointCloud &pcout) const 
    pcl_ros::transformPointCloud("/world_base_link", *input2, output2, *listener);
    pcl::fromROSMsg(output2, output2_pcl);
    output_pcl = output2_pcl;
    output_pcl += output1_pcl;
    pcl::toROSMsg(output_pcl, output);
    pub.publish(output);
}

int main (int argc, char **argv)
{
    ros::init (argc, argv, "ar_kinect");
    ros::NodeHandle n;
    ARPublisher ar_kinect (n);

    listener = new tf::TransformListener();
    ros::Subscriber sub1 = n.subscribe("/overhead_kinect1/depth_registered/points", 1, cloud_cb1);
    ros::Subscriber sub2 = n.subscribe("/overhead_kinect2/depth_registered/points", 1, cloud_cb2);

    pub = n.advertise<sensor_msgs::PointCloud2>("/overhead_kinect_merged/depth_registered/points", 1);
    ros::spin ();

    return 0;
}

When I run one kinect alone I get data from /camera/deprth_registered/points, but when I merge - no response from /overhead_kinect_merged/depth_registered/points. Also the serial number of one kinect is zeros (which is rather weird), that's why I switched to bus parameters.

I am running out of ideas what could be the cause for it. I would be very thankful for any help or idea.

edit retag flag offensive close merge delete

Comments

Do the two Kinects actually work? I mean without the whole merging stuff. Do you get an image from both Kinects?

BennyRe gravatar imageBennyRe ( 2014-04-08 20:05:33 -0500 )edit

Yes, both of them work absolutely fine separately

Andrii Matviienko gravatar imageAndrii Matviienko ( 2014-04-08 21:05:56 -0500 )edit

Do you mean with separately, both together but without merging or each Kinect at a time?

BennyRe gravatar imageBennyRe ( 2014-04-08 21:38:06 -0500 )edit

Both situations. If I run roslaunch openni_launch openni.launch for the 1st and then for the 2nd kinect - I get the data. When I run the lunch file specified above I get data from /overhead_kinect1 ( and 2) /depth_registered/points, but not from /overhead_kinect_merged/depth_registered/points

Andrii Matviienko gravatar imageAndrii Matviienko ( 2014-04-08 22:00:23 -0500 )edit

Did you check that your callbacks get called properly?

BennyRe gravatar imageBennyRe ( 2014-04-08 22:09:54 -0500 )edit

I don't know why, but when I changed 'depth_registered' to 'depth' it started giving me a merged point cloud. So it's solved, but why it did not work for 'depth_registered' is still a question.

Andrii Matviienko gravatar imageAndrii Matviienko ( 2014-04-08 22:20:22 -0500 )edit

1 Answer

Sort by » oldest newest most voted
1

answered 2014-04-16 10:44:51 -0500

Andrii Matviienko gravatar image

Finally found the solution to my problem. The solution is that both kinects have to be connected to the different buses on a computer. As far as kinects are connected to 2 different buses their clouds are not competing between each other anymore, but recognized and merged as 2 separated.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

Stats

Asked: 2014-04-08 03:56:02 -0500

Seen: 1,586 times

Last updated: Apr 16 '14