Yo's profile - activity

[Note: At karthik's suggestion, I have also posted this to the PCL-user's list.]

I've written the following method to compute the orthogonal distance regression plane for a given PointCloud, using the method detailed here.

However, visualizing it in rviz has proven to be a bit tricky, probably mostly due to my inexperience with quaternions. What I've got below seems to mostly work, but sometimes the plane (represented by a set of PoseStamped axes oriented along the plane's normal vector -- call this vector n) displayed by rviz seems in some cases to be a bit more off than I would expect, and so I'm still unsure if I'm doing everything properly.

Would someone please help check me on this?

The two major places I'm uncertain about are in my implementation of the fitting algorithm from the above link (eg. am I properly using the Eigen::JacobiSVD and related matrices to calculate the correct things?), and in my calculation of the orientation quaternion.

Here's the idea behind the calculation of the quaternion the way I have it. As from Wikipedia, the quaternion q is defined in relation to rotations by:

q = cos(a/2) + u * sin(a/2)

Where a is the angle to rotate by, and u is the unit vector about which to rotate.

So I assumed the 'starting' vector would be (0,0,1), which should be rotated by q until it is pointing in the same direction as the plane normal. Thus, the rotation axis u is the cross product of (0,0,1) and n, and a is the angle between them, computed as a = arcsine((0,0,1) DOT n).

Thank you!

void Converter::publishLSPlane(pcl::PointCloud<pcl::PointXYZ> points, std_msgs::Header header) {

    if (points.size() >= 3) {

        Eigen::MatrixXd m(points.size(), 3);

        // Compute centroid
        double centroid_x = 0;
        double centroid_y = 0;
        double centroid_z = 0;
        BOOST_FOREACH(const pcl::PointXYZ p, points) {
            centroid_x += p.x;
            centroid_y += p.y;
            centroid_z += p.z;
        }
        centroid_x /= points.size();
        centroid_y /= points.size();
        centroid_z /= points.size();

        // Define m
        int i=0;
        BOOST_FOREACH(const pcl::PointXYZ p, points) {
            m(i,0) = p.x - centroid_x;
            m(i,1) = p.y - centroid_y;
            m(i,2) = p.z - centroid_z;
            i++;
        }

        // Compute SVD
        Eigen::JacobiSVD<Eigen::MatrixXd> svd(m, Eigen::ComputeThinV);

        const int last = svd.cols() - 1;
        double normal_x = svd.matrixV()(0,last);
        double normal_y = svd.matrixV()(1,last);
        double normal_z = svd.matrixV()(2,last);

        ROS_INFO("Centroid: [%f, %f, %f]", centroid_x, centroid_y, centroid_z);
        ROS_INFO("Normal:   [%f, %f, %f]\n", normal_x, normal_y, normal_z);

        // Publish the normal for display in rviz
        geometry_msgs::PoseStamped normal;
        normal.header = header;
        normal.pose.position.x = centroid_x;
        normal.pose.position.y = centroid_y;
        normal.pose.position.z = centroid_z;

        // Yes I realize some of the math here could probably be simplified using 
        // trig identities, but for now I just want it to work. ;)
        normal.pose.orientation.w = std::cos(0.5 * std::asin(normal_z));
        normal.pose.orientation.x = -normal_y * std::sin(0.5 * std::asin(normal_z));
        normal.pose.orientation ...

I'd like to record some bag files of Kinect data and then run RGBDSLAM on them later. (The computer I'm doing the collection with isn't up to doing the RGBDSLAM itself.)

The problem is that I want to keep the bag files as small as possible, so I want to collect the minimal amount of data necessary.

In particular, that means I would like to record to the bag file the smallest number of topics possible, and preferably only image topics and the like -- PointCloud2 messages are quite large in comparison.

Can someone tell me exactly which topics are necessary for running RGBDSLAM off a bag file, and walk me through how to get RGBDSLAM to use only them? (Also, I'm a little confused about whether I should be using the dynamic_reconfigure reconfigure_gui to turn on OpenNI's depth registration or not...)

I thought I needed only RGB and depth image and possibly camera info topics, but when I tried running RGBDSLAM off of only those, it kept saying it was waiting for various other things. I think maybe I just need to change the parameters in the launch file to tell it that those are the only things I want it to work with, but I can't figure out how to do that properly.

Any help would be greatly appreciated! :)

Edit:

As a specific example of one of the things I've tried, I ran the following:

$ rosbag record /camera/depth_registered/image_rect_raw /camera/rgb/image_rect_color /camera/rgb/camera_info

I then ran the RGBDSLAM GUI and pressed the space key. The status bar displayed:

Processing. Waiting for motion information.

I then started playback of the bag file. Nothing changed.

Edit 2:

I followed Felix's directions, and everything worked swell... except for one problem. The depth images were not registered with the RGB images, so the textures in the resulting map were all incorrect.

So I tried again, this time using openni_launch openni.launch, using the dynamic_reconfigure reconfigure_gui to turn on depth registration (/camera/driver --> depth registration checkbox), and then using the following call to rosbag to record the bag file:

$ rosbag record -o /home/full/path/and/filename_prefix /tf /camera/rgb/image_color /camera/rgb/camera_info /camera/depth_registered/image /camera/depth/camera_info

I then ran the following launch file on this bag file (pressing the spacebar to start the processing once the GUI came up):

<launch>
  <node pkg="rgbdslam" type="rgbdslam" name="rgbdslam" cwd="node" required="false" output="log" > 
    <!-- see rgbslam_sample_config.launch for all available parameters and their default values -->
      <param name="config/topic_image_mono"              value="/camera/rgb/image_color"/>

<param name="config/topic_image_depth"             value="/camera/depth_registered/image"/>
<param name="config/topic_points"                  value=""/> <!--if empty, poincloud will be reconstructed from image and depth -->
<param name="config/camera_info_topic"             value="/camera/rgb/camera_info"/>
<param name="config/bagfile_name"                  value="/home/full/path/and/filename.bag"/>
<param name="config/start_paused"                  value="true"/>

      <param name="config/wide_topic"                    value=""/>;
      <param name="config/wide_cloud_topic"              value=""/>;
      <param name="config/drop_async_frames"             value="true"/> <!-- Check association of ...