Get instance segmentation

LibCarla/source/carla/client/Actor.cpp

@@ -8,6 +8,7 @@
 
 #include "carla/Logging.h"
 #include "carla/client/detail/Simulator.h"
+#include <stdio.h>
 
 namespace carla {
 namespace client {
@@ -36,6 +37,10 @@ namespace client {
     return GetEpisode().Lock()->GetActorAcceleration(*this);
   }
 
+  rpc::Keypoints Actor::GetKeypoints() const {
+    return GetEpisode().Lock()->GetActorSnapshot(*this).keypoints;
+  }
+
   void Actor::SetLocation(const geom::Location &location) {
     GetEpisode().Lock()->SetActorLocation(*this, location);
   }

LibCarla/source/carla/client/Actor.h

@@ -10,6 +10,7 @@
 #include "carla/Memory.h"
 #include "carla/client/detail/ActorState.h"
 #include "carla/profiler/LifetimeProfiled.h"
+#include "carla/rpc/Keypoints.h"
 
 namespace carla {
 namespace client {
@@ -58,6 +59,8 @@ namespace client {
     /// acceleration calculated after the actor's velocity.
     geom::Vector3D GetAcceleration() const;
 
+    rpc::Keypoints GetKeypoints() const;
+
     /// Teleport the actor to @a location.
     void SetLocation(const geom::Location &location);
 

LibCarla/source/carla/client/ActorSnapshot.h

@@ -21,6 +21,7 @@ namespace client {
     geom::Vector3D angular_velocity;
     geom::Vector3D acceleration;
     sensor::data::ActorDynamicState::TypeDependentState state;
+    sensor::data::detail::PackedKeypoints keypoints;
   };
 
 } // namespace client

LibCarla/source/carla/client/Walker.h

@@ -34,6 +34,7 @@ namespace client {
     /// received in the last tick.
     Control GetWalkerControl() const;
 
+
   private:
 
     Control _control;

LibCarla/source/carla/client/detail/EpisodeState.cpp

@@ -28,7 +28,8 @@ namespace detail {
               actor.velocity,
               actor.angular_velocity,
               actor.acceleration,
-              actor.state});
+              actor.state,
+              actor.keypoints});
       DEBUG_ASSERT(result.second);
     }
   }

LibCarla/source/carla/rpc/Keypoints.h

@@ -0,0 +1,30 @@
+// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma
+// de Barcelona (UAB).
+//
+// This work is licensed under the terms of the MIT license.
+// For a copy, see <https://opensource.org/licenses/MIT>.
+
+#pragma once
+
+#include "carla/MsgPack.h"
+#include "carla/geom/Vector3D.h"
+#include <utility>
+#include <vector>
+
+namespace carla {
+namespace rpc {
+
+  class Keypoints {
+  // The keypoints are just a vector of pairs <string, Vector3D>
+  public:
+
+    Keypoints() = default;
+
+    std::vector<std::pair<std::string, geom::Vector3D>> keypoints;
+
+    MSGPACK_DEFINE_ARRAY(keypoints)
+
+  };
+
+} // namespace rpc
+} // namespace carla

LibCarla/source/carla/sensor/data/ActorDynamicState.h

@@ -12,8 +12,10 @@
 #include "carla/rpc/TrafficLightState.h"
 #include "carla/rpc/VehicleControl.h"
 #include "carla/rpc/WalkerControl.h"
+#include "carla/rpc/Keypoints.h"
 
 #include <cstdint>
+#include <stdio.h>
 
 namespace carla {
 namespace sensor {
@@ -102,6 +104,56 @@ namespace detail {
     uint32_t pole_index;
   };
 #pragma pack(pop)
+
+#pragma pack(push, 1)
+  struct PackedKeypoint {
+    // Helper structure that stores the name of a bone
+    // (up to 24 chars) and its position (a float 3)
+    PackedKeypoint() = default;
+
+    char name[24];
+    float location[3];
+  };
+#pragma pack(pop)
+
+#pragma pack(push, 1)
+struct PackedKeypoints {
+  // Helper class that stores up to 72 keypoints (yeah it's quite heavy,
+  // This is 2kB right there...)
+
+  PackedKeypoints() = default;
+  PackedKeypoint keypoints[72];
+  int count=0;
+
+
+  void addKeypoint(std::string name, float x, float y, float z){
+      if (count >= 72){
+          return;
+      }
+      strncpy(keypoints[count].name, name.c_str(), sizeof(keypoints[count]));
+      keypoints[count].location[0] = x;
+      keypoints[count].location[1] = y;
+      keypoints[count].location[2] = z;
+      count ++;
+  }
+
+  operator rpc::Keypoints() const {
+      rpc::Keypoints kps;
+      for (int i=0; i<count; i++)
+      {
+          geom::Vector3D v(keypoints[i].location[0], keypoints[i].location[1],
+                     keypoints[i].location[2]);
+          std::string name = std::string(keypoints[i].name);
+          std::pair<std::string, geom::Vector3D> pair1(name, v);
+          kps.keypoints.push_back(pair1);
+      }
+    return kps;
+  };
+
+};
+#pragma pack(pop)
+
+
 } // namespace detail
 
 #pragma pack(push, 1)
@@ -124,16 +176,19 @@ namespace detail {
       detail::VehicleData vehicle_data;
       detail::PackedWalkerControl walker_control;
     } state;
+
+    detail::PackedKeypoints keypoints;
+
   };
 
 #pragma pack(pop)
 
-static_assert(
-    sizeof(ActorDynamicState) == 93u,
-    "Invalid ActorDynamicState size! "
-    "If you modified this class please update the size here, else you may "
-    "comment this assert, but your platform may have compatibility issues "
-    "connecting to other platforms.");
+// static_assert(
+//     sizeof(ActorDynamicState) == 204u,
+//     "Invalid ActorDynamicState size! "
+//     "If you modified this class please update the size here, else you may "
+//     "comment this assert, but your platform may have compatibility issues "
+//     "connecting to other platforms.");
 
 } // namespace data
 } // namespace sensor

PythonAPI/carla/source/libcarla/Actor.cpp

@@ -10,6 +10,7 @@
 #include <carla/client/Walker.h>
 #include <carla/client/WalkerAIController.h>
 #include <carla/rpc/TrafficLightState.h>
+#include <carla/rpc/Keypoints.h>
 
 #include <boost/python/suite/indexing/vector_indexing_suite.hpp>
 
@@ -24,6 +25,11 @@ namespace client {
     return out;
   }
 
+  // std::ostream &operator<<(std::ostream &out, const carla::rpc::Keypoints &keypoints) {
+  //   out << "Keypoints(keypoints(" << keypoints.keypoints.size() << "))";
+  //   return out;
+  // }
+
 } // namespace client
 } // namespace carla
 
@@ -49,6 +55,18 @@ static void ApplyControl(carla::client::Walker &self, const ControlT &control) {
   self.ApplyControl(control);
 }
 
+static auto GetKeypoints(const carla::rpc::Keypoints &self) {
+  namespace py = boost::python;
+  auto keypoints = self.keypoints;
+  py::list result;
+  for (auto kp : keypoints) {
+    auto name = kp.first;
+    auto location = kp.second;
+    result.append(py::make_tuple(name, location));
+  }
+  return result;
+}
+
 void export_actor() {
   using namespace boost::python;
   namespace cc = carla::client;
@@ -59,6 +77,11 @@ void export_actor() {
       .def(self_ns::str(self_ns::self))
   ;
 
+  class_<cr::Keypoints>("Keypoints")
+    .def(init<>())
+    .add_property("keypoints", &GetKeypoints)
+  ;
+
   class_<cc::Actor, boost::noncopyable, boost::shared_ptr<cc::Actor>>("Actor", no_init)
   // work-around, force return copy to resolve Actor instead of ActorState.
       .add_property("id", CALL_RETURNING_COPY(cc::Actor, GetId))
@@ -78,6 +101,7 @@ void export_actor() {
       .def("get_transform", &cc::Actor::GetTransform)
       .def("get_velocity", &cc::Actor::GetVelocity)
       .def("get_angular_velocity", &cc::Actor::GetAngularVelocity)
+      .def("get_keypoints", &cc::Actor::GetKeypoints)
       .def("get_acceleration", &cc::Actor::GetAcceleration)
       .def("set_location", &cc::Actor::SetLocation, (arg("location")))
       .def("set_transform", &cc::Actor::SetTransform, (arg("transform")))

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/Tagger.cpp

@@ -14,12 +14,15 @@
 #include "EngineUtils.h"
 #include "PhysicsEngine/PhysicsAsset.h"
 
+
 template <typename T>
 static auto CastEnum(T label)
 {
   return static_cast<typename std::underlying_type<T>::type>(label);
 }
 
+#define CARLA_TAGGER_EXTRA_LOG 1
+
 static ECityObjectLabel GetLabelByFolderName(const FString &String) {
   if      (String == "Buildings")       return ECityObjectLabel::Buildings;
   else if (String == "Fences")          return ECityObjectLabel::Fences;
@@ -45,14 +48,39 @@ static ECityObjectLabel GetLabelByPath(const T *Object)
   return (StringArray.Num() > 4 ? GetLabelByFolderName(StringArray[4]) : ECityObjectLabel::None);
 }
 
+unsigned int SimpleHash(unsigned int x, unsigned int cap) {
+    x = ((x >> 16) ^ x) * 0x45d9f3b;
+    x = ((x >> 16) ^ x) * 0x45d9f3b;
+    x = (x >> 16) ^ x;
+    return x % cap;
+}
+
 static void SetStencilValue(
-    UPrimitiveComponent &Component,
-    const ECityObjectLabel &Label,
-    const bool bSetRenderCustomDepth) {
-  Component.SetCustomDepthStencilValue(CastEnum(Label));
-  Component.SetRenderCustomDepth(
-      bSetRenderCustomDepth &&
-      (Label != ECityObjectLabel::None));
+      UPrimitiveComponent &Component,
+      const ECityObjectLabel &Label,
+      const bool bSetRenderCustomDepth,
+      const uint32 id
+    ) {
+    uint8 StencilValue = CastEnum(Label);
+    if (Label == ECityObjectLabel::Pedestrians){
+      StencilValue += 16 + SimpleHash(id, 112);
+      #ifdef CARLA_TAGGER_EXTRA_LOG
+        UE_LOG(LogCarla, Log, TEXT("Pedestrian"));
+      #endif // CARLA_TAGGER_EXTRA_LOG
+    }
+    if (Label == ECityObjectLabel::Vehicles){
+      StencilValue += 128 + SimpleHash(id, 112);
+      #ifdef CARLA_TAGGER_EXTRA_LOG
+        UE_LOG(LogCarla, Log, TEXT("Vehicle"));
+      #endif // CARLA_TAGGER_EXTRA_LOG
+    }
+    #ifdef CARLA_TAGGER_EXTRA_LOG
+      UE_LOG(LogCarla, Log, TEXT("setuped"));
+    #endif // CARLA_TAGGER_EXTRA_LOG
+    Component.SetCustomDepthStencilValue(StencilValue);
+    Component.SetRenderCustomDepth(
+        bSetRenderCustomDepth &&
+        (Label != ECityObjectLabel::None));
 }
 
 // =============================================================================
@@ -70,21 +98,21 @@ void ATagger::TagActor(const AActor &Actor, bool bTagForSemanticSegmentation)
   Actor.GetComponents<UStaticMeshComponent>(StaticMeshComponents);
   for (UStaticMeshComponent *Component : StaticMeshComponents) {
     const auto Label = GetLabelByPath(Component->GetStaticMesh());
-    SetStencilValue(*Component, Label, bTagForSemanticSegmentation);
+    SetStencilValue(*Component, Label, bTagForSemanticSegmentation, Actor.GetUniqueID());
 #ifdef CARLA_TAGGER_EXTRA_LOG
     UE_LOG(LogCarla, Log, TEXT("  + StaticMeshComponent: %s"), *Component->GetName());
     UE_LOG(LogCarla, Log, TEXT("    - Label: \"%s\""), *GetTagAsString(Label));
 #endif // CARLA_TAGGER_EXTRA_LOG
   }
 
-  // Iterate skeletal meshes.
-  TArray<USkeletalMeshComponent *> SkeletalMeshComponents;
-  Actor.GetComponents<USkeletalMeshComponent>(SkeletalMeshComponents);
-  for (USkeletalMeshComponent *Component : SkeletalMeshComponents) {
+  // Iterate over any fucking mesh we can fucking find
+  TArray<USkinnedMeshComponent *> SkinnedMeshComponents;
+  Actor.GetComponents<USkinnedMeshComponent>(SkinnedMeshComponents);
+  for (USkinnedMeshComponent *Component : SkinnedMeshComponents) {
     const auto Label = GetLabelByPath(Component->GetPhysicsAsset());
-    SetStencilValue(*Component, Label, bTagForSemanticSegmentation);
+    SetStencilValue(*Component, Label, bTagForSemanticSegmentation, Actor.GetUniqueID());
 #ifdef CARLA_TAGGER_EXTRA_LOG
-    UE_LOG(LogCarla, Log, TEXT("  + SkeletalMeshComponent: %s"), *Component->GetName());
+    UE_LOG(LogCarla, Log, TEXT("  + SkinnedMeshComponent: %s"), *Component->GetName());
     UE_LOG(LogCarla, Log, TEXT("    - Label: \"%s\""), *GetTagAsString(Label));
 #endif // CARLA_TAGGER_EXTRA_LOG
   }

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/WorldObserver.cpp

@@ -9,6 +9,9 @@
 
 #include "Carla/Traffic/TrafficLightBase.h"
 #include "Carla/Walker/WalkerController.h"
+#include "Components/PoseableMeshComponent.h"
+#include "Components/PrimitiveComponent.h"
+#include "GameFramework/CharacterMovementComponent.h"
 
 #include "CoreGlobals.h"
 
@@ -57,6 +60,7 @@ static auto FWorldObserver_GetActorState(const FActorView &View, const FActorReg
     if (Controller != nullptr)
     {
       state.walker_control = carla::rpc::WalkerControl{Controller->GetWalkerControl()};
+
     }
   }
   else if (AType::TrafficLight == View.GetActorType())
@@ -78,6 +82,31 @@ static auto FWorldObserver_GetActorState(const FActorView &View, const FActorReg
   return state;
 }
 
+static auto FWorldObserver_GetActorKeyPoints(const FActorView &View, const FActorRegistry &Registry)
+{
+  carla::sensor::data::detail::PackedKeypoints keypoints;
+  auto Pawn = Cast<APawn>(View.GetActor());
+  if (Pawn != nullptr)
+  {
+    TArray<USkeletalMeshComponent *> SkeletalMeshes;
+    Pawn->GetComponents<USkeletalMeshComponent>(SkeletalMeshes, false);
+    USkeletalMeshComponent *SkeletalMesh = SkeletalMeshes.IsValidIndex(0) ? SkeletalMeshes[0] : nullptr;
+
+    if (SkeletalMesh)
+    {
+        int32 NumBones = SkeletalMesh->GetNumBones();
+        for (int32 i = 0; i < NumBones; ++i)
+        {
+          FName const BoneName = SkeletalMesh->GetBoneName(i);
+          std::string key = std::string(TCHAR_TO_UTF8(*BoneName.ToString()));
+          FVector location = SkeletalMesh->GetBoneLocation(BoneName, EBoneSpaces::Type::WorldSpace);
+          keypoints.addKeypoint(key, location.X, location.Y, location.Z);
+      	}
+    }
+  }
+  return keypoints;
+}
+
 static carla::geom::Vector3D FWorldObserver_GetAngularVelocity(const AActor &Actor)
 {
   const auto RootComponent = Cast<UPrimitiveComponent>(Actor.GetRootComponent());
@@ -138,7 +167,8 @@ static carla::Buffer FWorldObserver_Serialize(
       carla::geom::Vector3D{Velocity.X, Velocity.Y, Velocity.Z},
       FWorldObserver_GetAngularVelocity(*View.GetActor()),
       FWorldObserver_GetAcceleration(View, Velocity, DeltaSeconds),
-      FWorldObserver_GetActorState(View, Registry)
+      FWorldObserver_GetActorState(View, Registry),
+      FWorldObserver_GetActorKeyPoints(View, Registry)
     };
     write_data(info);
   }