[WIP] Implement SkeletonsRenderer

include/Clouds.h

@@ -47,6 +47,11 @@ class Clouds {
     io::CalibrationParams params;
   };
 
+  struct UpdateCloudAction {
+    Key key;
+    CloudPtr cloud;
+  };
+
   struct ChangeCloudVisibilityAction {
     Key key;
     bool visible;
@@ -136,6 +141,7 @@ class Clouds {
 
   void onPointsUpdate(const UpdatePointsAction &action);
   void onVerticesUpdate(const UpdateVerticesAction &action);
+  void onCloudUpdate(const UpdateCloudAction &action);
   void onCalibrationParamsUpdate(const UpdateCalibrationParamsAction &action);
   void onCloudVisibilityChange(const ChangeCloudVisibilityAction &action);
   void onCloudRemove(const RemoveCloudAction &action);

include/Skeleton.h

@@ -8,6 +8,10 @@
 #include <map>
 #include <msgpack.hpp>
 
+#ifdef USE_NITE2
+#include <NiTE.h>
+#endif
+
 struct Joint {
   float x;
   float y;
@@ -16,6 +20,24 @@ struct Joint {
   MSGPACK_DEFINE(x, y, type);
 };
 
+struct Bone {
+public:
+  enum struct Type {
+    HEAD,
+    TORSO,
+    LEFT_UP_ARM,
+    RIGHT_UP_ARM,
+    LEFT_DOWN_ARM,
+    RIGHT_DOWN_ARM,
+    LEFT_UP_LEG,
+    RIGHT_UP_LEG,
+    LEFT_DOWN_LEG,
+    RIGHT_DOWN_LEG,
+  };
+
+  static const std::multimap<Type, std::pair<int, int>> joint_pairs;
+};
+
 using Skeleton = std::map<int, Joint>;
 using Skeletons = std::map<int, Skeleton>;
 using SkeletonsPtr = std::shared_ptr<Skeletons>;

include/io/CapturedLog.h

@@ -30,8 +30,8 @@ class CapturedLog {
 private:
   const std::string serial_;
   const int user_id_;
-  std::map<int64_t, CloudPtr> clouds_;
-  std::string key_;
+  const std::map<int64_t, CloudPtr> clouds_;
+  const std::string key_;
   std::vector<int64_t> stamps_;
   std::vector<int64_t>::iterator stamp_itr_;
 

include/renderer/skeleton/SkeletonsRenderer.h

@@ -0,0 +1,35 @@
+//
+// Created by Masayuki IZUMI on 11/3/16.
+//
+
+#ifndef CIPOINTCLOUDVIEWERAPP_SKELETONSRENDERER_H
+#define CIPOINTCLOUDVIEWERAPP_SKELETONSRENDERER_H
+
+#include "cinder/Color.h"
+
+#include "Clouds.h"
+#include "renderer/BatchRenderer.h"
+
+namespace renderer {
+namespace skeleton {
+
+class SkeletonsRenderer : public BatchRenderer {
+public:
+  SkeletonsRenderer(const std::shared_ptr<Clouds> &clouds);
+  void update() override;
+
+
+protected:
+  cinder::gl::VertBatchRef createBatch() override;
+
+
+private:
+  static const std::map<Bone::Type, cinder::ColorA8u> kBoneColors;
+
+  const std::shared_ptr<Clouds> clouds_;
+};
+
+}
+}
+
+#endif //CIPOINTCLOUDVIEWERAPP_SKELETONSRENDERER_H

src/CiPointCloudViewerApp.cpp

@@ -22,6 +22,7 @@
 #include "io/exporter/Exporters.h"
 
 #include "renderer/cloud/CloudsRenderer.h"
+#include "renderer/skeleton/SkeletonsRenderer.h"
 #include "renderer/grid/GridRenderer.h"
 
 #include "Clouds.h"
@@ -55,6 +56,7 @@ class CiPointCloudViewerApp : public App {
 
   view::AppGui gui_;
   renderer::cloud::CloudsRenderer clouds_renderer_;
+  renderer::skeleton::SkeletonsRenderer skeletons_renderer_;
   renderer::grid::GridRenderer grid_renderer_;
 
   CameraUi camera_ui_;
@@ -70,6 +72,7 @@ CiPointCloudViewerApp::CiPointCloudViewerApp()
   , exporters_(new io::exporter::Exporters(clouds_))
   , gui_(this, clouds_, view_params_, config_, cloud_data_sources_, captured_log_manager_, sensor_device_manager_, exporters_)
   , clouds_renderer_(this, clouds_)
+  , skeletons_renderer_(clouds_)
   , grid_renderer_(view_params_)
   , camera_ui_(&(view_params_->camera()))
 {}
@@ -95,6 +98,7 @@ void CiPointCloudViewerApp::update() {
   gui_.update();
   grid_renderer_.update();
   clouds_renderer_.update();
+  skeletons_renderer_.update();
 
   setFullScreen(view_params_->is_full_screen());
 }
@@ -104,8 +108,11 @@ void CiPointCloudViewerApp::draw() {
   gl::setMatrices(view_params_->camera());
   gl::pointSize(view_params_->point_size());
 
+  gl::lineWidth(1);
   grid_renderer_.render();
   clouds_renderer_.render();
+  gl::lineWidth(5);
+  skeletons_renderer_.render();
 }
 
 CINDER_APP( CiPointCloudViewerApp, RendererGl, [](App::Settings *settings) {

src/Clouds.cpp

@@ -19,6 +19,7 @@ void Clouds::initializeConnections() {
   namespace ph = std::placeholders;
   Signal<UpdatePointsAction>::connect(std::bind(&Clouds::onPointsUpdate, this, ph::_1));
   Signal<UpdateVerticesAction>::connect(std::bind(&Clouds::onVerticesUpdate, this, ph::_1));
+  Signal<UpdateCloudAction>::connect(std::bind(&Clouds::onCloudUpdate, this, ph::_1));
   Signal<UpdateCalibrationParamsAction>::connect(std::bind(&Clouds::onCalibrationParamsUpdate, this, ph::_1));
   Signal<ChangeCloudVisibilityAction>::connect(std::bind(&Clouds::onCloudVisibilityChange, this, ph::_1));
   Signal<RemoveCloudAction>::connect(std::bind(&Clouds::onCloudRemove, this, ph::_1));
@@ -51,6 +52,10 @@ void Clouds::onVerticesUpdate(const UpdateVerticesAction &action) {
   }
 }
 
+void Clouds::onCloudUpdate(const UpdateCloudAction &action) {
+  clouds_[action.key] = action.cloud;
+}
+
 void Clouds::onCalibrationParamsUpdate(const UpdateCalibrationParamsAction &action) {
   calib_params_map_[action.key] = action.params;
 }

src/Skeleton.cpp

@@ -0,0 +1,25 @@
+//
+// Created by Masayuki IZUMI on 11/3/16.
+//
+
+#include "Skeleton.h"
+
+const std::multimap<Bone::Type, std::pair<int, int>> Bone::joint_pairs = {
+  { Bone::Type::HEAD, { nite::JOINT_HEAD, nite::JOINT_NECK } },
+  { Bone::Type::TORSO, { nite::JOINT_NECK, nite::JOINT_LEFT_SHOULDER } },
+  { Bone::Type::TORSO, { nite::JOINT_NECK, nite::JOINT_RIGHT_SHOULDER } },
+  { Bone::Type::TORSO, { nite::JOINT_NECK, nite::JOINT_TORSO } },
+  { Bone::Type::TORSO, { nite::JOINT_TORSO, nite::JOINT_LEFT_SHOULDER } },
+  { Bone::Type::TORSO, { nite::JOINT_TORSO, nite::JOINT_RIGHT_SHOULDER } },
+  { Bone::Type::TORSO, { nite::JOINT_TORSO, nite::JOINT_LEFT_HIP } },
+  { Bone::Type::TORSO, { nite::JOINT_TORSO, nite::JOINT_RIGHT_HIP } },
+  { Bone::Type::TORSO, { nite::JOINT_LEFT_HIP, nite::JOINT_RIGHT_HIP } },
+  { Bone::Type::LEFT_UP_ARM, { nite::JOINT_LEFT_SHOULDER, nite::JOINT_LEFT_ELBOW } },
+  { Bone::Type::LEFT_DOWN_ARM, { nite::JOINT_LEFT_ELBOW, nite::JOINT_LEFT_HAND } },
+  { Bone::Type::RIGHT_UP_ARM, { nite::JOINT_RIGHT_SHOULDER, nite::JOINT_RIGHT_ELBOW } },
+  { Bone::Type::RIGHT_DOWN_ARM, { nite::JOINT_RIGHT_ELBOW, nite::JOINT_RIGHT_HAND } },
+  { Bone::Type::LEFT_UP_LEG, { nite::JOINT_LEFT_HIP, nite::JOINT_LEFT_KNEE } },
+  { Bone::Type::LEFT_DOWN_LEG, { nite::JOINT_LEFT_KNEE, nite::JOINT_LEFT_FOOT } },
+  { Bone::Type::RIGHT_UP_LEG, { nite::JOINT_RIGHT_HIP, nite::JOINT_RIGHT_KNEE } },
+  { Bone::Type::RIGHT_DOWN_LEG, { nite::JOINT_RIGHT_KNEE, nite::JOINT_RIGHT_FOOT } },
+};

src/io/CapturedLog.cpp

@@ -16,16 +16,14 @@ CapturedLog::CapturedLog(
   : serial_ (serial)
   , user_id_(user_id)
   , clouds_ (clouds)
+  , key_    (serial_ + " (" + std::to_string(user_id) + ")")
 {
   initialize();
   Signal<UpdateTimestampAction>::connect(std::bind(&CapturedLog::onTimestampUpdate, this, std::placeholders::_1));
   Signal<ResetTimestampAction>::connect(std::bind(&CapturedLog::onTimestampReset, this, std::placeholders::_1));
 }
 
 void CapturedLog::initialize() {
-  std::stringstream ss;
-  ss << serial_ << " (" << user_id_ << ")" << std::endl;
-  key_ = ss.str();
   for (auto pair : clouds_) {
     stamps_.emplace_back(pair.first);
   }
@@ -35,7 +33,7 @@ void CapturedLog::initialize() {
 void CapturedLog::onTimestampUpdate(const UpdateTimestampAction &action) {
   if (action.timestamp > *stamp_itr_) {
     if (stamp_itr_ != stamps_.end()) {
-      Signal<Clouds::UpdatePointsAction>::emit({key_, clouds_[*stamp_itr_]->point_cloud()});
+      Signal<Clouds::UpdateCloudAction>::emit({key_, clouds_.at(*stamp_itr_)});
       stamp_itr_++;
     }
   }

src/renderer/skeleton/SkeletonsRenderer.cpp

@@ -0,0 +1,55 @@
+//
+// Created by Masayuki IZUMI on 11/3/16.
+//
+
+#include "glm/glm.hpp"
+#include "renderer/skeleton/SkeletonsRenderer.h"
+
+namespace renderer {
+namespace skeleton {
+
+SkeletonsRenderer::SkeletonsRenderer(
+  const std::shared_ptr<Clouds> &clouds
+)
+  : BatchRenderer()
+  , clouds_(clouds)
+{
+  set_batch(createBatch());
+}
+
+void SkeletonsRenderer::update() {
+  batch()->clear();
+  for (auto p1 : clouds_->clouds()) {
+    if (!p1.second->hasSkeleton()) { continue; }
+    auto skeleton = p1.second->skeleton();
+    for (auto p2 : Bone::joint_pairs) {
+      if (skeleton.count(p2.second.first) != 0 && skeleton.count(p2.second.second) != 0) {
+        auto j1 = skeleton[p2.second.first];
+        auto j2 = skeleton[p2.second.second];
+        batch()->color(kBoneColors.at(p2.first));
+        batch()->vertex(glm::vec3(j1.x, j1.y, j1.z));
+        batch()->vertex(glm::vec3(j2.x, j2.y, j2.z));
+      }
+    }
+  }
+}
+
+cinder::gl::VertBatchRef SkeletonsRenderer::createBatch() {
+  return cinder::gl::VertBatch::create(GL_LINES);
+}
+
+const std::map<Bone::Type, cinder::ColorA8u> SkeletonsRenderer::kBoneColors = {
+  { Bone::Type::HEAD,            cinder::ColorA8u(0xcc, 0x66, 0x66, 0xcc) },
+  { Bone::Type::TORSO,           cinder::ColorA8u(0x33, 0x33, 0xcc, 0xcc) },
+  { Bone::Type::LEFT_UP_ARM,     cinder::ColorA8u(0xcc, 0xcc, 0x99, 0xcc) },
+  { Bone::Type::RIGHT_UP_ARM,    cinder::ColorA8u(0xff, 0x66, 0xcc, 0xcc) },
+  { Bone::Type::LEFT_DOWN_ARM,   cinder::ColorA8u(0x33, 0x99, 0xff, 0xcc) },
+  { Bone::Type::RIGHT_DOWN_ARM,  cinder::ColorA8u(0x66, 0xcc, 0x66, 0xcc) },
+  { Bone::Type::LEFT_UP_LEG,     cinder::ColorA8u(0xcc, 0xcc, 0x99, 0xcc) },
+  { Bone::Type::RIGHT_UP_LEG,    cinder::ColorA8u(0xff, 0x66, 0xcc, 0xcc) },
+  { Bone::Type::LEFT_DOWN_LEG,   cinder::ColorA8u(0x33, 0x99, 0xff, 0xcc) },
+  { Bone::Type::RIGHT_DOWN_LEG,  cinder::ColorA8u(0x66, 0xcc, 0x66, 0xcc) }
+};
+
+}
+}