Method publish

opt_calibration/src/opt_calibration.cpp:291–339

}

void OPTCalibration::publish()
{
  for (size_t i = 0; i < node_vec_.size(); ++i)
  {
    TreeNode::Ptr sensor_node = node_vec_[i];
    geometry_msgs::TransformStamped transform_msg;
    if (sensor_node->sensor()->toTF(transform_msg))
      tf_pub_.sendTransform(transform_msg);
  }

  for (size_t i = 0; i < checkerboard_vec_.size(); ++i)
  {
    cb::Checkerboard::Ptr checkerboard = checkerboard_vec_[i];
    visualization_msgs::Marker marker;
    checkerboard->toMarker(marker);
    marker.ns = "optimized checkerboard";
    marker.id = i;
    marker_pub_.publish(marker);
  }

  if (floor_estimated_)
  {
    cb::Vector3 floor_origin = -floor_.normal() * floor_.offset();
    cb::Vector3 floor_x = (floor_.projection(floor_origin + cb::Vector3::UnitX()) - floor_origin).normalized();

    cb::Transform floor_pose;
    floor_pose.linear().col(2) = floor_.normal();
    floor_pose.linear().col(0) = floor_x;
    floor_pose.linear().col(1) = floor_pose.linear().col(2).cross(floor_pose.linear().col(0));
    floor_pose.translation() = floor_origin;

    cb::PlanarObject plane("/floor");
    plane.transform(floor_pose);
    plane.setParent(world_);

    visualization_msgs::Marker marker;
    plane.toMarker(marker);
    marker.ns = "floor";
    marker.id = 0;
    marker.scale.x = 10;
    marker.scale.y = 10;
    marker_pub_.publish(marker);

    geometry_msgs::TransformStamped transform_msg;
    if (plane.toTF(transform_msg))
      tf_pub_.sendTransform(transform_msg);
  }

}

bool OPTCalibration::estimateFloor()
{