Program Listing for File figure.cpp

↰ Return to documentation for file (src/tuw_geometry/figure.cpp)

#include "tuw_geometry/figure.hpp"

#include <tgmath.h>

#include <cfloat>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

using namespace tuw;

const cv::Scalar Figure::green(0, 255, 0);
const cv::Scalar Figure::green_bright(51, 255, 51);
const cv::Scalar Figure::green_dark(0, 102, 0);
const cv::Scalar Figure::red(0, 0, 255);
const cv::Scalar Figure::blue(255, 0, 0);
const cv::Scalar Figure::blue_bright(255, 51, 51);
const cv::Scalar Figure::blue_dark(139, 0, 0);
const cv::Scalar Figure::orange(0, 128, 255);
const cv::Scalar Figure::yellow(0, 255, 255);
const cv::Scalar Figure::cyan(255, 255, 0);
const cv::Scalar Figure::magenta(255, 0, 255);
const cv::Scalar Figure::gray_bright(224, 224, 224);
const cv::Scalar Figure::gray(128, 128, 128);
const cv::Scalar Figure::black(0, 0, 0);
const cv::Scalar Figure::white(255, 255, 255);

const cv::Scalar Figure::niceBlue(155, 100, 59);
const cv::Scalar Figure::niceMustard(28, 174, 184);
const cv::Scalar Figure::niceMagenta(147, 32, 220);
const cv::Scalar Figure::niceGreenBlue(181, 196, 36);
const cv::Scalar Figure::niceRed(82, 77, 204);
const cv::Scalar Figure::niceRedDark(52, 48, 158);
const cv::Scalar Figure::niceGreen(55, 142, 84);
const cv::Scalar Figure::niceGrey(132, 109, 106);
const cv::Scalar Figure::niceGreyLight(179, 165, 153);
const cv::Scalar Figure::niceGreyPurple(155, 135, 149);
const cv::Scalar Figure::niceGreenWashed(151, 166, 125);
const cv::Scalar Figure::niceGreyDark(85, 79, 74);
const cv::Scalar Figure::niceLime(23, 176, 154);
const cv::Scalar Figure::niceDirtyPink(78, 0, 120);

Figure::Figure(const std::string & title)
: title_(title), label_format_x_("x=%f"), label_format_y_("y=%f")
{
}

void Figure::setLabel(const std::string & label_format_x, const std::string & label_format_y)
{
  label_format_x_ = label_format_x, label_format_y_ = label_format_y;
}
const std::string & Figure::backgroundFileName() const {return background_filename_;}
const cv::Mat & Figure::view() const {return view_;}
cv::Mat & Figure::view() {return view_;}
const cv::Mat & Figure::background() const {return background_;}
cv::Mat & Figure::background() {return background_;}
const cv::Mat & Figure::background_image() const {return background_image_;}
cv::Mat & Figure::background_image() {return background_image_;}
void Figure::setView(const cv::Mat & view)
{
  if (view.empty()) {
    return;
  }
  view_.create(view.cols, view.rows, CV_8UC3);
  int depth = view.depth(), cn = view.channels();
  int type = CV_MAKETYPE(depth, cn);
  if (type == CV_8UC3) {
    view.copyTo(view_);
  } else if ((view.channels() == 1) && (view.depth() == CV_8U)) {
    cv::cvtColor(view, view_, cv::COLOR_GRAY2BGR);
  }
}

void Figure::init(
  int width_pixel, int height_pixel, cv::Matx33d Mw2m, double grid_scale_y, double grid_scale_x,
  const std::string & background_image)
{
  WorldScopedMaps::init(width_pixel, height_pixel, Mw2m);
  grid_scale_x_ = grid_scale_x, grid_scale_y_ = grid_scale_y;
  background_filename_ = background_image;
  if (!background_filename_.empty()) {
    background_image_ = cv::imread(background_filename_, cv::IMREAD_COLOR);
  } else {
    background_image_.create(height(), width(), CV_8UC3);
    background_image_.setTo(0xFF);
  }
  drawBackground();
  clear();
}

void Figure::init(
  int width_pixel, int height_pixel, double min_x, double max_x, double min_y, double max_y,
  double rotation, double grid_scale_y, double grid_scale_x, const std::string & background_image)
{
  WorldScopedMaps::init(width_pixel, height_pixel, min_x, max_x, min_y, max_y, rotation);
  grid_scale_x_ = grid_scale_x, grid_scale_y_ = grid_scale_y;
  background_filename_ = background_image;
  if (!background_filename_.empty()) {
    background_image_ = cv::imread(background_filename_, cv::IMREAD_COLOR);
  } else {
    background_image_.create(height(), width(), CV_8UC3);
    background_image_.setTo(0xFF);
  }
  drawBackground();
  clear();
}

void Figure::drawBackground()
{
  cv::resize(background_image_, background_, cv::Size(height(), width()), cv::INTER_AREA);
  if ((grid_scale_y_ > 0) && (grid_scale_x_ > 0)) {
    char txt[0xFF];
    Point2D p0, p1, pm0, pm1;
    double min_y = round(WorldScopedMaps::min_y() / grid_scale_y_) * grid_scale_y_;
    double max_y = round(WorldScopedMaps::max_y() / grid_scale_y_) * grid_scale_y_;
    for (p0.y() = min_y; p0.y() <= max_y; p0.y() += grid_scale_y_) {
      p0.x() = round(max_x() / grid_scale_x_) * grid_scale_x_;
      p1.y() = p0.y();
      p1.x() = round(min_x() / grid_scale_x_) * grid_scale_x_;
      if (fabs(p0.y()) > FLT_MIN) {
        WorldScopedMaps::line(background_, p0, p1, gray_bright, 1, 8);
      } else {
        WorldScopedMaps::line(background_, p0, p1, gray, 1, 8);
      }
    }
    p0.set(0, max_y - grid_scale_y_ / 2.0);
    sprintf(txt, label_format_y_.c_str(), max_y);
    cv::putText(background_, txt, w2m(p0).cv(), cv::FONT_HERSHEY_PLAIN, 0.6, white, 3, cv::LINE_AA);
    cv::putText(background_, txt, w2m(p0).cv(), cv::FONT_HERSHEY_PLAIN, 0.6, gray, 1, cv::LINE_AA);
    p1.set(0, min_y + grid_scale_y_ / 2.0);
    sprintf(txt, label_format_y_.c_str(), min_y);
    cv::putText(background_, txt, w2m(p1).cv(), cv::FONT_HERSHEY_PLAIN, 0.6, white, 3, cv::LINE_AA);
    cv::putText(background_, txt, w2m(p1).cv(), cv::FONT_HERSHEY_PLAIN, 0.6, gray, 1, cv::LINE_AA);

    double min_x = round(WorldScopedMaps::min_x() / grid_scale_x_) * grid_scale_x_;
    double max_x = round(WorldScopedMaps::max_x() / grid_scale_x_) * grid_scale_x_;
    for (p0.x() = min_x; p0.x() <= max_x; p0.x() += grid_scale_x_) {
      p0.y() = round(WorldScopedMaps::max_y() / grid_scale_y_) * grid_scale_y_;
      p1.x() = p0.x();
      p1.y() = round(WorldScopedMaps::min_y() / grid_scale_y_) * grid_scale_y_;
      pm0 = w2m(p0);
      pm1 = w2m(p1);
      if (fabs(p0.x()) > FLT_MIN) {
        WorldScopedMaps::line(background_, p0, p1, gray_bright, 1, 8);
      } else {
        WorldScopedMaps::line(background_, p0, p1, gray, 1, 8);
      }
    }
    p0.set(max_x - grid_scale_x_ / 2.0, 0);
    sprintf(txt, label_format_x_.c_str(), max_x);
    cv::putText(background_, txt, w2m(p0).cv(), cv::FONT_HERSHEY_PLAIN, 0.6, white, 3, cv::LINE_AA);
    cv::putText(background_, txt, w2m(p0).cv(), cv::FONT_HERSHEY_PLAIN, 0.6, gray, 1, cv::LINE_AA);
    p1.set(min_x + grid_scale_x_ / 2.0, 0);
    sprintf(txt, label_format_x_.c_str(), min_x);
    cv::putText(background_, txt, w2m(p1).cv(), cv::FONT_HERSHEY_PLAIN, 0.6, white, 3, cv::LINE_AA);
    cv::putText(background_, txt, w2m(p1).cv(), cv::FONT_HERSHEY_PLAIN, 0.6, gray, 1, cv::LINE_AA);
  }
}
void Figure::clear()
{
  view_.create(background_.cols, background_.rows, CV_8UC3);
  background_.copyTo(view_);
}

void Figure::line(
  const Point2D & p0, const Point2D & p1, const cv::Scalar & color, int thickness, int lineType)
{
  WorldScopedMaps::line(view_, p0, p1, color, thickness, lineType);
}

void Figure::symbol(const Point2D & p, const cv::Scalar & color) {symbol(view_, p, color);}
void Figure::symbol(cv::Mat & view, const Point2D & p, const cv::Scalar & color)
{
  cv::Point pi = w2m(p).cv();
  if ((pi.x < 0) || (pi.x >= view.cols) || (pi.y < 0) || (pi.y >= view.rows)) {
    return;
  }
  cv::Vec3b & pixel = view.at<cv::Vec3b>(pi);
  pixel[0] = color[0], pixel[1] = color[1], pixel[2] = color[2];
}
const std::string Figure::title() const {return title_;}

void Figure::circle(
  const Point2D & p, int radius, const cv::Scalar & color, int thickness, int lineType)
{
  WorldScopedMaps::circle(view_, p, radius, color, thickness, lineType);
}

void Figure::symbol(
  const Pose2D & p, double radius, const cv::Scalar & color, int thickness, int lineType)
{
  symbol(view_, p, radius, color, thickness, lineType);
}

void Figure::symbol(
  cv::Mat & view, const Pose2D & p, double radius, const cv::Scalar & color, int thickness,
  int lineType)
{
  circle(
    view, p.position(), radius * (fabs(scale_x()) + fabs(scale_y())) / 2., color, thickness,
    lineType);
  line(view, p.position(), p.point_ahead(radius), color, thickness, lineType);
}
void Figure::putText(
  const std::string & text, const Point2D & p, int fontFace, double fontScale, cv::Scalar color,
  int thickness, int lineType, bool bottomLeftOrigin)
{
  putText(view_, text, p, fontFace, fontScale, color, thickness, lineType, bottomLeftOrigin);
}
void Figure::putText(
  cv::Mat & view, const std::string & text, const Point2D & p, int fontFace, double fontScale,
  cv::Scalar color, int thickness, int lineType, bool bottomLeftOrigin)
{
  cv::putText(
    view, text, w2m(p).cv(), fontFace, fontScale, color, thickness, lineType, bottomLeftOrigin);
}

void Figure::appendToView(
  const cv::Mat & _mat, const cv::Scalar & _colMin, const cv::Scalar & _colMax,
  u_int8_t _truncateLayerVal)
{
  if (view().empty() || _mat.empty() || !initialized()) {
    return;
  }

  CV_Assert(_mat.depth() == CV_8U);
  int nRows = _mat.rows;
  // int channels = _mat.channels();
  // int nCols    = _mat.cols * channels;

  uint8_t const * p_s;
  cv::Vec3b * p_d;
  for (int i = 0; i < nRows; ++i) {
    for (p_s = _mat.ptr<const uint8_t>(i), p_d = view().ptr<cv::Vec3b>(i);
      p_s != _mat.ptr<uint8_t>(i + 1); p_d++, p_s++)
    {
      if ((*p_d == cv::Vec3b(255, 255, 255)) && (*p_s < 255 - _truncateLayerVal)) {
        double scale = *p_s / (255. - (double)_truncateLayerVal);
        *p_d = cv::Vec3b(
          _colMin[0] + scale * (_colMax[0] - _colMin[0]),
          _colMin[1] + scale * (_colMax[1] - _colMin[1]),
          _colMin[2] + scale * (_colMax[2] - _colMin[2]));
      }
    }
  }
}