Ask Your Question
0

Can't Display PointCloud2 in Rviz

asked 2015-02-14 18:43:43 -0500

wng gravatar image

updated 2015-02-14 20:52:09 -0500

I'm trying to display a monochrome image/inverse depth map as a PointCloud2 in Rviz, but I'm getting garbage (basically random points) and can't figure out what I'm doing wrong. I've verified that the image/K matrix is good and I'm setting the inverse depth to 1, so it's something to do with how I'm filling in the PointCloud2 message. Any help would be appreciated. Thanks!

void bufferVertices(const cv::Mat& img,
                    const Eigen::MatrixXfR& idepth,
                    const Eigen::Matrix3f& Kinv,
                    uint32_t point_step,
                    std::vector<uint8_t>* buffer) {
  Eigen::Vector3f uv1;
  Eigen::Vector3f xyz;
  uint32_t offset = 0;
  for (int ii = 0; ii < img.rows; ii++) {
    for (int jj = 0; jj < img.cols; jj++) {
      uv1(0) = jj;
      uv1(1) = ii;
      uv1(2) = 1.0;

      xyz = Kinv * uv1/idepth(ii, jj);
      memcpy(&((*buffer)[offset + 0]), &(xyz(0)), sizeof(float));
      memcpy(&((*buffer)[offset + 4]), &(xyz(1)), sizeof(float));
      memcpy(&((*buffer)[offset + 8]), &(xyz(2)), sizeof(float));

      float g = static_cast<float>(img.at<uint8_t>(ii, jj));
      memcpy(&((*buffer)[offset + 12]), &g, sizeof(float));

      // uint8_t g = img.at<uint8_t>(ii, jj);
      // int32_t rgb = (g << 16) | (g << 8) | g;
      // memcpy(&((*buffer)[offset + 12]), &rgb, sizeof(int32_t));

      offset += point_step;
    }
  }

  return;
}

void publishPointCloud(const std::string& frame_id,
                       const cv::Mat& img,
                       const Eigen::MatrixXfR& idepth,
                       const Eigen::Matrix3f& Kinv,
                       ros::Publisher* pub) {
  sensor_msgs::PointCloud2Ptr msg(new sensor_msgs::PointCloud2());
  msg->header = std_msgs::Header();
  msg->header.stamp = ros::Time::now();
  msg->header.frame_id = frame_id;

  msg->height = img.rows;
  msg->width = img.cols;

  sensor_msgs::PointCloud2Modifier mod(*msg);
  mod.setPointCloud2Fields(4, "x", 1, sensor_msgs::PointField::FLOAT32,
                           "y", 1, sensor_msgs::PointField::FLOAT32,
                           "z", 1, sensor_msgs::PointField::FLOAT32,
                           "intensities", 1, sensor_msgs::PointField::FLOAT32);

  msg->is_bigendian = false;
  msg->point_step = 4*sizeof(float);

  msg->row_step = msg->point_step * msg->width;
  msg->is_dense = true;

  msg->data.resize(msg->row_step * msg->height);
  bufferVertices(img, idepth, Kinv, msg->point_step, &msg->data);

  pub->publish(msg);

  return;
}

EDIT: I've tried writing the PointCloud2 message to a PCD file using pcl_ros and can visualize it just fine in pcl_viewer. In addition, the cloud appeared in Rviz for maybe a single frame and then disappeared. Maybe this is a tf issue? I thought I was publishing the requisite transforms (world -> camera_world ->keyframe). This is my main loop:

  /* Main loop. */
  tf::Transform camworld_to_world_tf;
  tf::Quaternion camworld_to_world_q(0.5, -0.5, 0.5, -0.5);
  camworld_to_world_tf.setOrigin(tf::Vector3(0, 0, 0));
  camworld_to_world_tf.setRotation(camworld_to_world_q);

  std::string frame_id;
  ros::Rate loop_rate(30);
  while (ros::ok()) {
    /* Do optimization step. */

    /* Publish tf from camera_world to world. */
    tf_camworld_to_world_pub.sendTransform(tf::StampedTransform(
      camworld_to_world_tf, ros::Time::now(), "world", "camera_world"));

    /* Publish tf from camera to camera_world. */
    frame_id = "keyframe" + std::to_string(kf.id);
    Eigen::Vector3f eig_trans = cam_to_world.translation();
    Eigen::Quaternionf eig_q = cam_to_world.quaternion();

    tf::Transform cam_to_camworld_tf;
    cam_to_camworld_tf.setOrigin(tf::Vector3(eig_trans[0],
                                             eig_trans[1],
                                             eig_trans[2]));
    tf::Quaternion q(eig_q.x(), eig_q.y(), eig_q.z(), eig_q.w());  // This is scaled!
    // cam_to_camworld_tf.setRotation(q);
    // cam_to_camworld_tf.setOrigin(tf::Vector3(0, 0, 0));
    cam_to_camworld_tf.setRotation(tf::Quaternion(0, 0, 0, 1));
    tf_cam_to_camworld_pub.sendTransform(tf::StampedTransform(
                         cam_to_camworld_tf, ros::Time::now ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2015-02-15 15:44:41 -0500

wng gravatar image

Just tried this on another machine and it worked fine. Seems like the issue was that my machine was using an older version of OpenGL (2.1 GLSL 1.2). On a machine with OpenGL 4.4 GLSL 4.4, it works fine.

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

2 followers

Stats

Asked: 2015-02-14 18:43:43 -0500

Seen: 840 times

Last updated: Feb 15 '15