Extract util in tutorials

samples/tutorials/util/cv_painter.cc

@@ -0,0 +1,176 @@
+// Copyright 2018 Slightech Co., Ltd. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+#include "util/cv_painter.h"
+
+#include <glog/logging.h>
+
+#include <opencv2/imgproc/imgproc.hpp>
+
+#include <iomanip>
+#include <iostream>
+#include <memory>
+#include <utility>
+
+#define FONT_FACE cv::FONT_HERSHEY_PLAIN
+#define FONT_SCALE 1
+#define FONT_COLOR cv::Scalar(255, 255, 255)
+#define THICKNESS 1
+
+namespace {
+
+std::shared_ptr<std::ios> NewFormat(int width, int prec, char fillch = ' ') {
+  auto fmt = std::make_shared<std::ios>(nullptr);
+  fmt->setf(std::ios::fixed);
+  fmt->width(std::move(width));
+  fmt->precision(std::move(prec));
+  fmt->fill(std::move(fillch));
+  return fmt;
+}
+
+std::ostringstream &Clear(std::ostringstream &os) {
+  os.str("");
+  os.clear();
+  return os;
+}
+
+}  // namespace
+
+std::ostream &operator<<(
+    std::ostream &os, const std::shared_ptr<std::ios> &fmt) {
+  if (fmt)
+    os.copyfmt(*fmt);
+  return os;
+}
+
+CVPainter::CVPainter() {
+  VLOG(2) << __func__;
+}
+
+CVPainter::~CVPainter() {
+  VLOG(2) << __func__;
+}
+
+cv::Rect CVPainter::DrawSize(const cv::Mat &img, const gravity_t &gravity) {
+  std::ostringstream ss;
+  ss << img.cols << "x" << img.rows;
+  return DrawText(img, ss.str(), gravity, 5);
+}
+
+cv::Rect CVPainter::DrawImgData(
+    const cv::Mat &img, const mynteye::ImgData &data,
+    const gravity_t &gravity) {
+  int sign = 1;
+  if (gravity == BOTTOM_LEFT || gravity == BOTTOM_RIGHT)
+    sign = -1;
+
+  std::ostringstream ss;
+  ss << "frame_id: " << data.frame_id << ", stamp: " << data.timestamp
+     << ", expo: " << data.exposure_time;
+  cv::Rect rect_i = DrawText(img, ss.str(), gravity, 5);
+
+  Clear(ss) << "size: " << img.cols << "x" << img.rows;
+  cv::Rect rect_s =
+      DrawText(img, ss.str(), gravity, 5, 0, sign * (5 + rect_i.height));
+
+  // rect_i.width is the max one
+  if (sign > 0) {
+    return cv::Rect(
+        rect_i.tl(),
+        cv::Point(rect_i.x + rect_i.width, rect_s.y + rect_s.height));
+  } else {
+    return cv::Rect(rect_s.tl(), rect_i.br());
+  }
+}
+
+cv::Rect CVPainter::DrawImuData(
+    const cv::Mat &img, const mynteye::ImuData &data,
+    const gravity_t &gravity) {
+  static std::ostringstream ss;
+  static auto fmt_imu = NewFormat(8, 4);
+  static auto fmt_temp = NewFormat(6, 4);
+
+  int sign = 1;
+  if (gravity == BOTTOM_LEFT || gravity == BOTTOM_RIGHT)
+    sign = -1;
+
+  Clear(ss) << "frame_id: " << data.frame_id << ", stamp: " << data.timestamp
+            << ", temp: " << fmt_temp << data.temperature;
+  cv::Rect rect_i = DrawText(img, ss.str(), gravity, 5);
+
+  Clear(ss) << "accel(x,y,z): " << fmt_imu << data.accel[0] << "," << fmt_imu
+            << data.accel[1] << "," << fmt_imu << data.accel[2];
+  cv::Rect rect_a =
+      DrawText(img, ss.str(), gravity, 5, 0, sign * (5 + rect_i.height));
+
+  Clear(ss) << "gyro(x,y,z): " << fmt_imu << data.gyro[0] << "," << fmt_imu
+            << data.gyro[1] << "," << fmt_imu << data.gyro[2];
+  cv::Rect rect_g = DrawText(
+      img, ss.str(), gravity, 5, 0,
+      sign * (10 + rect_i.height + rect_a.height));
+
+  // rect_i.width is the max one
+  if (sign > 0) {
+    return cv::Rect(
+        rect_i.tl(),
+        cv::Point(rect_i.x + rect_i.width, rect_g.y + rect_g.height));
+  } else {
+    return cv::Rect(rect_g.tl(), rect_i.br());
+  }
+}
+
+cv::Rect CVPainter::DrawText(
+    const cv::Mat &img, const std::string &text, const gravity_t &gravity,
+    const int &margin, const int &offset_x, const int &offset_y) {
+  int w = img.cols, h = img.rows;
+
+  int baseline = 0;
+  cv::Size textSize =
+      cv::getTextSize(text, FONT_FACE, FONT_SCALE, THICKNESS, &baseline);
+
+  int x, y;
+  switch (gravity) {
+    case TOP_LEFT:
+      x = margin;
+      y = margin + textSize.height;
+      break;
+    case TOP_RIGHT:
+      x = w - margin - textSize.width;
+      y = margin + textSize.height;
+      break;
+    case BOTTOM_LEFT:
+      x = margin;
+      y = h - margin;
+      break;
+    case BOTTOM_RIGHT:
+      x = w - margin - textSize.width;
+      y = h - margin;
+      break;
+    default:  // TOP_LEFT
+      x = margin;
+      y = margin + textSize.height;
+      break;
+  }
+  x += offset_x;
+  y += offset_y;
+
+  cv::Point org(x, y);
+#ifdef USE_OPENCV2
+  cv::putText(
+      const_cast<cv::Mat &>(img), text, org, FONT_FACE, FONT_SCALE, FONT_COLOR,
+      THICKNESS);
+#else
+  cv::putText(img, text, org, FONT_FACE, FONT_SCALE, FONT_COLOR, THICKNESS);
+#endif
+  return cv::Rect(org, textSize);
+}

samples/tutorials/util/cv_painter.h

@@ -0,0 +1,50 @@
+// Copyright 2018 Slightech Co., Ltd. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+#ifndef MYNTEYE_TUTORIALS_CV_PAINTER_H_  // NOLINT
+#define MYNTEYE_TUTORIALS_CV_PAINTER_H_
+#pragma once
+
+#include <opencv2/core/core.hpp>
+
+#include <string>
+
+#include "mynteye/types.h"
+
+class CVPainter {
+ public:
+  typedef enum Gravity {
+    TOP_LEFT,
+    TOP_RIGHT,
+    BOTTOM_LEFT,
+    BOTTOM_RIGHT
+  } gravity_t;
+
+  CVPainter();
+  ~CVPainter();
+
+  cv::Rect DrawSize(const cv::Mat &img, const gravity_t &gravity = TOP_LEFT);
+  cv::Rect DrawImgData(
+      const cv::Mat &img, const mynteye::ImgData &data,
+      const gravity_t &gravity = TOP_LEFT);
+  cv::Rect DrawImuData(
+      const cv::Mat &img, const mynteye::ImuData &data,
+      const gravity_t &gravity = TOP_RIGHT);
+
+  cv::Rect DrawText(
+      const cv::Mat &img, const std::string &text,
+      const gravity_t &gravity = TOP_LEFT, const int &margin = 5,
+      const int &offset_x = 0, const int &offset_y = 0);
+};
+
+#endif  // MYNTEYE_TUTORIALS_CV_PAINTER_H_ NOLINT

samples/tutorials/util/pc_viewer.cc

@@ -0,0 +1,102 @@
+// Copyright 2018 Slightech Co., Ltd. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+#include "util/pc_viewer.h"
+
+#include <glog/logging.h>
+
+// #include <pcl/common/common_headers.h>
+
+#include <cmath>
+
+std::shared_ptr<pcl::visualization::PCLVisualizer> CustomColorVis(
+    pcl::PointCloud<pcl::PointXYZ>::ConstPtr pc) {
+  // --------------------------------------------
+  // -----Open 3D viewer and add point cloud-----
+  // --------------------------------------------
+  std::shared_ptr<pcl::visualization::PCLVisualizer> viewer(
+      new pcl::visualization::PCLVisualizer("PointCloud Viewer"));
+  viewer->setBackgroundColor(0, 0, 0);
+  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(
+      pc, 255, 255, 255);
+  viewer->addPointCloud<pcl::PointXYZ>(pc, single_color, "point cloud");
+  viewer->setPointCloudRenderingProperties(
+      pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "point cloud");
+  // viewer->addCoordinateSystem(1.0);
+  viewer->addCoordinateSystem(1000.0);
+  viewer->initCameraParameters();
+  viewer->setCameraPosition(0, 0, -150, 0, 1, 0);
+  return (viewer);
+}
+
+PCViewer::PCViewer() : viewer_(nullptr) {
+  VLOG(2) << __func__;
+}
+
+PCViewer::~PCViewer() {
+  VLOG(2) << __func__;
+  if (viewer_) {
+    // viewer_->saveCameraParameters("pcl_camera_params.txt");
+    viewer_->close();
+    viewer_ == nullptr;
+  }
+}
+
+void PCViewer::Update(const cv::Mat &xyz) {
+  pcl::PointCloud<pcl::PointXYZ>::Ptr pc(new pcl::PointCloud<pcl::PointXYZ>);
+  ConvertMatToPointCloud(xyz, pc);
+  Update(pc);
+}
+
+void PCViewer::Update(pcl::PointCloud<pcl::PointXYZ>::ConstPtr pc) {
+  if (viewer_ == nullptr) {
+    viewer_ = CustomColorVis(pc);
+  }
+  viewer_->updatePointCloud(pc, "point cloud");
+  viewer_->spinOnce();
+}
+
+bool PCViewer::WasVisual() const {
+  return viewer_ != nullptr;
+}
+
+bool PCViewer::WasStopped() const {
+  return viewer_ != nullptr && viewer_->wasStopped();
+}
+
+void PCViewer::ConvertMatToPointCloud(
+    const cv::Mat &xyz, pcl::PointCloud<pcl::PointXYZ>::Ptr pc) {
+  // cv::Mat channels[3];
+  // cv::split(xyz, channels);
+  // double min, max;
+  // cv::minMaxLoc(channels[2], &min, &max);
+
+  for (int i = 0; i < xyz.rows; i++) {
+    for (int j = 0; j < xyz.cols; j++) {
+      auto &&p = xyz.at<cv::Point3f>(i, j);
+      if (std::isfinite(p.x) && std::isfinite(p.y) && std::isfinite(p.z)) {
+        // LOG(INFO) << "[" << i << "," << j << "] x: " << p.x << ", y: " << p.y
+        // << ", z: " << p.z;
+        pcl::PointXYZ point;
+        point.x = p.x;
+        point.y = p.y;
+        point.z = p.z;
+        // point.z = p.z - min;
+        pc->points.push_back(point);
+      }
+    }
+  }
+
+  pc->width = static_cast<int>(pc->points.size());
+  pc->height = 1;
+}

samples/tutorials/util/pc_viewer.h

@@ -0,0 +1,43 @@
+// Copyright 2018 Slightech Co., Ltd. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+#ifndef MYNTEYE_TUTORIALS_PC_VIEWER_H_  // NOLINT
+#define MYNTEYE_TUTORIALS_PC_VIEWER_H_
+#pragma once
+
+#include <opencv2/core/core.hpp>
+
+#include <pcl/visualization/pcl_visualizer.h>
+
+#include <memory>
+
+class PCViewer {
+ public:
+  PCViewer();
+  ~PCViewer();
+
+  void Update(const cv::Mat &xyz);
+
+  void Update(pcl::PointCloud<pcl::PointXYZ>::ConstPtr pc);
+
+  bool WasVisual() const;
+  bool WasStopped() const;
+
+ private:
+  void ConvertMatToPointCloud(
+      const cv::Mat &xyz, pcl::PointCloud<pcl::PointXYZ>::Ptr pc);
+
+  std::shared_ptr<pcl::visualization::PCLVisualizer> viewer_;
+};
+
+#endif  // MYNTEYE_TUTORIALS_PC_VIEWER_H_ NOLINT