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)
{
MapHdl::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)
{
MapHdl::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(MapHdl::min_y() / grid_scale_y_) * grid_scale_y_;
double max_y = round(MapHdl::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(MapHdl::min_x() / grid_scale_x_) * grid_scale_x_;
double max_x = round(MapHdl::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(MapHdl::max_y() / grid_scale_y_) * grid_scale_y_;
p1.x() = p0.x();
p1.y() = round(MapHdl::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]));
}
}
}
}