Function render_currentpose

uav_simulator/local_sensing/src/euroc.cpp:214–257

}

void render_currentpose() {
  solve_pnp();

  double this_time = ros::Time::now().toSec();

  Matrix4d cam_pose = cam2world.inverse();

  depthrender.render_pose(cam_pose, depth_hostptr);

  depth_mat = cv::Mat::zeros(height, width, CV_32FC1);
  double min = 0.5;
  double max = 1.0f;
  for (int i = 0; i < height; i++)
    for (int j = 0; j < width; j++) {
      float depth = (float)depth_hostptr[i * width + j] / 1000.0f;
      depth = depth < 500.0f ? depth : 0;
      max = depth > max ? depth : max;
      depth_mat.at<float>(i, j) = depth;
    }
  ROS_INFO("render cost %lf ms.", (ros::Time::now().toSec() - this_time) * 1000.0f);
  printf("max_depth %lf.\n", max);

  cv_bridge::CvImage out_msg;
  out_msg.header.stamp = receive_stamp;
  out_msg.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
  out_msg.image = depth_mat.clone();
  pub_depth.publish(out_msg.toImageMsg());

  cv::Mat adjMap;
  depth_mat.convertTo(adjMap, CV_8UC1, 255 / (max - min), -min);
  cv::Mat falseColorsMap;
  cv::applyColorMap(adjMap, falseColorsMap, cv::COLORMAP_RAINBOW);
  cv::Mat bgr_image;
  cv::cvtColor(undistorted_image, bgr_image, cv::COLOR_GRAY2BGR);
  cv::addWeighted(bgr_image, 0.2, falseColorsMap, 0.8, 0.0, falseColorsMap);
  cv_bridge::CvImage cv_image_colored;
  cv_image_colored.header.frame_id = "depthmap";
  cv_image_colored.encoding = sensor_msgs::image_encodings::BGR8;
  cv_image_colored.image = falseColorsMap;
  pub_color.publish(cv_image_colored.toImageMsg());

  cv::imshow("bluefox_image", bgr_image);
  cv::imshow("depth_image", adjMap);
}

int main(int argc, char** argv) {
  ros::init(argc, argv, "cloud_banchmark");