Can't Display PointCloud2 in Rviz

edit

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 ...