Function image_pose_callback

uav_simulator/local_sensing/src/euroc.cpp:154–212

}

void image_pose_callback(const sensor_msgs::ImageConstPtr& image_input,
                         const geometry_msgs::TransformStampedConstPtr& pose_input) {
  // time diff
  double time_diff = fabs(image_input->header.stamp.toSec() - pose_input->header.stamp.toSec()) * 1000.0;
  printf("time diff is %lf ms.\n", time_diff);

  // pose
  Matrix4d Pose_receive = Matrix4d::Identity();

  // using vicon
  // Eigen::Vector3d request_position;
  // Eigen::Quaterniond request_pose;
  // request_position.x() = pose_input->transform.translation.x;
  // request_position.y() = pose_input->transform.translation.y;
  // request_position.z() = pose_input->transform.translation.z;
  // request_pose.x() = pose_input->transform.rotation.x;
  // request_pose.y() = pose_input->transform.rotation.y;
  // request_pose.z() = pose_input->transform.rotation.z;
  // request_pose.w() = pose_input->transform.rotation.w;
  // Pose_receive.block<3,3>(0,0) = request_pose.toRotationMatrix();
  // Pose_receive(0,3) = request_position(0);
  // Pose_receive(1,3) = request_position(1);
  // Pose_receive(2,3) = request_position(2);

  // using ground truth
  double image_time = image_input->header.stamp.toSec();
  double min_time_diff = 999.9;
  int min_time_index = 0;
  for (int i = 1; i < gt_pose_vect.size(); i++) {
    double time_diff = fabs(image_time - gt_pose_vect[i].time);
    if (time_diff < min_time_diff) {
      min_time_diff = time_diff;
      min_time_index = i;
    }
  }
  printf("min time diff index %d, with diff time %lf ms.\n", min_time_index, min_time_diff * 1000.0f);
  Pose_receive = gt_pose_vect[min_time_index].pose;

  // convert to body pose
  // Matrix4d body_pose = Pose_receive * vicon2body.inverse();
  Matrix4d body_pose = Pose_receive;

  // convert to cam pose
  cam2world = body_pose * cam02body * vicon2leica;

  receive_stamp = pose_input->header.stamp;

  // image
  cv_bridge::CvImageConstPtr cv_img_ptr =
      cv_bridge::toCvShare(image_input, sensor_msgs::image_encodings::MONO8);
  cv::Mat img_8uC1 = cv_img_ptr->image;
  undistorted_image.create(height, width, CV_8UC1);
  if (is_distorted) {
    cv::remap(img_8uC1, undistorted_image, undist_map1, undist_map2, CV_INTER_LINEAR);
  } else
    undistorted_image = img_8uC1;

  render_currentpose();