diff --git a/CHANGELOG.md b/CHANGELOG.md index 9481ee0c2a6..afb7ea22757 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -29,6 +29,7 @@ * Exposed tire parameters for longitudinal and lateral stiffness in the PhysicsControl. * When setting a global plan at the LocalPlanner, it is now optional to stop the automatic fill of the waypoint buffer * Improved agent's vehicle detection to also take into account the actor bounding boxes + * Added Optical Flow camera * API extensions: - Added `set_wheel_steer_direction()` function to change the bone angle of each wheel of a vehicle - Added `get_wheel_steer_angle()` function to get the steer angle of a vehicle whee diff --git a/LibCarla/source/carla/sensor/SensorRegistry.h b/LibCarla/source/carla/sensor/SensorRegistry.h index fadee6f3de0..a0443ba7eff 100644 --- a/LibCarla/source/carla/sensor/SensorRegistry.h +++ b/LibCarla/source/carla/sensor/SensorRegistry.h @@ -19,6 +19,7 @@ #include "carla/sensor/s11n/EpisodeStateSerializer.h" #include "carla/sensor/s11n/GnssSerializer.h" #include "carla/sensor/s11n/ImageSerializer.h" +#include "carla/sensor/s11n/OpticalFlowImageSerializer.h" #include "carla/sensor/s11n/IMUSerializer.h" #include "carla/sensor/s11n/LidarSerializer.h" #include "carla/sensor/s11n/NoopSerializer.h" @@ -34,6 +35,7 @@ class AGnssSensor; class AInertialMeasurementUnit; class ALaneInvasionSensor; class AObstacleDetectionSensor; +class AOpticalFlowCamera; class ARadar; class ARayCastSemanticLidar; class ARayCastLidar; @@ -60,6 +62,7 @@ namespace sensor { std::pair, std::pair, std::pair, + std::pair, std::pair, std::pair, std::pair, @@ -84,6 +87,7 @@ namespace sensor { #include "Carla/Sensor/InertialMeasurementUnit.h" #include "Carla/Sensor/LaneInvasionSensor.h" #include "Carla/Sensor/ObstacleDetectionSensor.h" +#include "Carla/Sensor/OpticalFlowCamera.h" #include "Carla/Sensor/Radar.h" #include "Carla/Sensor/RayCastLidar.h" #include "Carla/Sensor/RayCastSemanticLidar.h" diff --git a/LibCarla/source/carla/sensor/data/Color.h b/LibCarla/source/carla/sensor/data/Color.h index 1b9eb591bce..f9133d29c1d 100644 --- a/LibCarla/source/carla/sensor/data/Color.h +++ b/LibCarla/source/carla/sensor/data/Color.h @@ -46,6 +46,30 @@ namespace data { static_assert(sizeof(Color) == sizeof(uint32_t), "Invalid color size!"); +#pragma pack(push, 1) + /// Optical flow pixel format. 2 channel float data. + struct OpticalFlowPixel { + OpticalFlowPixel() = default; + OpticalFlowPixel(const OpticalFlowPixel &) = default; + + OpticalFlowPixel(float x, float y) + : x(x), y(y) {} + + OpticalFlowPixel &operator=(const OpticalFlowPixel &) = default; + + bool operator==(const OpticalFlowPixel &rhs) const { + return (x == rhs.x) && (y == rhs.y); + } + + bool operator!=(const OpticalFlowPixel &rhs) const { + return !(*this == rhs); + } + + float x = 0; + float y = 0; + }; +#pragma pack(pop) + } // namespace data } // namespace sensor } // namespace carla diff --git a/LibCarla/source/carla/sensor/data/Image.h b/LibCarla/source/carla/sensor/data/Image.h index fd5a7b22890..6e8c7a63be6 100644 --- a/LibCarla/source/carla/sensor/data/Image.h +++ b/LibCarla/source/carla/sensor/data/Image.h @@ -13,9 +13,12 @@ namespace carla { namespace sensor { namespace data { - /// An image of 32-bit BGRA colors. + /// An image of 32-bit BGRA colors (8-bit channels) using Image = ImageTmpl; + /// An image of 64-bit BGRA colors (16-bit channels) + using OpticalFlowImage = ImageTmpl; + } // namespace data } // namespace sensor } // namespace carla diff --git a/LibCarla/source/carla/sensor/data/ImageTmpl.h b/LibCarla/source/carla/sensor/data/ImageTmpl.h index 9f68559fc4c..4d72c04ffd9 100644 --- a/LibCarla/source/carla/sensor/data/ImageTmpl.h +++ b/LibCarla/source/carla/sensor/data/ImageTmpl.h @@ -9,6 +9,7 @@ #include "carla/Debug.h" #include "carla/sensor/data/Array.h" #include "carla/sensor/s11n/ImageSerializer.h" +#include "carla/sensor/s11n/OpticalFlowImageSerializer.h" namespace carla { namespace sensor { @@ -21,8 +22,10 @@ namespace data { protected: using Serializer = s11n::ImageSerializer; + using SerializerOpticalFlow = s11n::OpticalFlowImageSerializer; friend Serializer; + friend SerializerOpticalFlow; explicit ImageTmpl(RawData &&data) : Super(Serializer::header_offset, std::move(data)) { diff --git a/LibCarla/source/carla/sensor/s11n/OpticalFlowImageSerializer.cpp b/LibCarla/source/carla/sensor/s11n/OpticalFlowImageSerializer.cpp new file mode 100644 index 00000000000..543f94dd209 --- /dev/null +++ b/LibCarla/source/carla/sensor/s11n/OpticalFlowImageSerializer.cpp @@ -0,0 +1,21 @@ +// +// Created by flo on 09.11.20. +// + +#include "OpticalFlowImageSerializer.h" +#include "carla/sensor/s11n/OpticalFlowImageSerializer.h" + +#include "carla/sensor/data/Image.h" + +namespace carla { + namespace sensor { + namespace s11n { + + SharedPtr OpticalFlowImageSerializer::Deserialize(RawData &&data) { + auto image = SharedPtr(new data::OpticalFlowImage{std::move(data)}); + return image; + } + + } // namespace s11n + } // namespace sensor +} // namespace carla diff --git a/LibCarla/source/carla/sensor/s11n/OpticalFlowImageSerializer.h b/LibCarla/source/carla/sensor/s11n/OpticalFlowImageSerializer.h new file mode 100644 index 00000000000..655360e48f2 --- /dev/null +++ b/LibCarla/source/carla/sensor/s11n/OpticalFlowImageSerializer.h @@ -0,0 +1,59 @@ +// +// Created by flo on 09.11.20. +// + +#pragma once + +#include "carla/Memory.h" +#include "carla/sensor/RawData.h" + +#include +#include + +namespace carla { + namespace sensor { + + class SensorData; + + namespace s11n { + + /// Serializes image buffers generated by camera sensors. + class OpticalFlowImageSerializer { + public: + +#pragma pack(push, 1) + struct ImageHeader { + uint32_t width; + uint32_t height; + float fov_angle; + }; +#pragma pack(pop) + + constexpr static auto header_offset = sizeof(ImageHeader); + + static const ImageHeader &DeserializeHeader(const RawData &data) { + return *reinterpret_cast(data.begin()); + } + + template + static Buffer Serialize(const Sensor &sensor, Buffer &&bitmap); + + static SharedPtr Deserialize(RawData &&data); + }; + + template + inline Buffer OpticalFlowImageSerializer::Serialize(const Sensor &sensor, Buffer &&bitmap) { + DEBUG_ASSERT(bitmap.size() > sizeof(ImageHeader)); + ImageHeader header = { + sensor.GetImageWidth(), + sensor.GetImageHeight(), + sensor.GetFOVAngle() + }; + std::memcpy(bitmap.data(), reinterpret_cast(&header), sizeof(header)); + return std::move(bitmap); + } + + } // namespace s11n + } // namespace sensor +} // namespace carla + diff --git a/PythonAPI/carla/setup.py b/PythonAPI/carla/setup.py index 2dd2889a5ef..b967f5da55b 100755 --- a/PythonAPI/carla/setup.py +++ b/PythonAPI/carla/setup.py @@ -39,6 +39,8 @@ def walk(folder, file_filter='*'): pwd = os.path.dirname(os.path.realpath(__file__)) pylib = "libboost_python%d%d.a" % (sys.version_info.major, sys.version_info.minor) + numpylib = 'libboost_numpy%d%d.a' % (sys.version_info.major, + sys.version_info.minor) if is_rss_variant_enabled(): print('Building AD RSS variant.') extra_link_args = [ os.path.join(pwd, 'dependencies/lib/libcarla_client_rss.a') ] @@ -84,6 +86,7 @@ def walk(folder, file_filter='*'): extra_link_args += ['-ltbb'] extra_link_args += [os.path.join(pwd, 'dependencies/lib', pylib)] + extra_link_args += [os.path.join(pwd, 'dependencies/lib', numpylib)] if 'TRAVIS' in os.environ and os.environ['TRAVIS'] == 'true': print('Travis CI build detected: disabling PNG support.') @@ -104,11 +107,14 @@ def walk(folder, file_filter='*'): pylib = 'libboost_python%d%d' % ( sys.version_info.major, sys.version_info.minor) + numpylib = 'libboost_numpy%d%d' % ( + sys.version_info.major, + sys.version_info.minor) extra_link_args = ['shlwapi.lib', 'Advapi32.lib', 'ole32.lib', 'shell32.lib'] required_libs = [ - pylib, 'libboost_filesystem', + pylib, numpylib, 'libboost_filesystem', 'rpc.lib', 'carla_client.lib', 'libpng.lib', 'zlib.lib', 'Recast.lib', 'Detour.lib', 'DetourCrowd.lib', diff --git a/PythonAPI/carla/source/libcarla/SensorData.cpp b/PythonAPI/carla/source/libcarla/SensorData.cpp index 58eaffd35de..e36835d663e 100644 --- a/PythonAPI/carla/source/libcarla/SensorData.cpp +++ b/PythonAPI/carla/source/libcarla/SensorData.cpp @@ -27,6 +27,10 @@ #include #include +#include +#include +#include +#include namespace carla { namespace sensor { @@ -40,6 +44,14 @@ namespace data { return out; } + std::ostream &operator<<(std::ostream &out, const OpticalFlowImage &image) { + out << "OpticalFlowImage(frame=" << std::to_string(image.GetFrame()) + << ", timestamp=" << std::to_string(image.GetTimestamp()) + << ", size=" << std::to_string(image.GetWidth()) << 'x' << std::to_string(image.GetHeight()) + << ')'; + return out; + } + std::ostream &operator<<(std::ostream &out, const LidarMeasurement &meas) { out << "LidarMeasurement(frame=" << std::to_string(meas.GetFrame()) << ", timestamp=" << std::to_string(meas.GetTimestamp()) @@ -198,6 +210,114 @@ static void ConvertImage(T &self, EColorConverter cc) { throw std::invalid_argument("invalid color converter!"); } } +#ifndef _WIN32 +// method to convert optical flow images to rgb +static boost::python::numpy::ndarray ColorCodedFlow ( + carla::sensor::data::OpticalFlowImage& image) { + namespace bp = boost::python; + namespace bn = boost::python::numpy; + namespace csd = carla::sensor::data; + constexpr float pi = 3.1415f; + constexpr float rad2ang = 360.f/(2.f*pi); + std::vector result; + result.resize(image.GetHeight()*image.GetWidth()* 4); + + // lambda for computing batches of pixels + auto command = [&] (size_t min_index, size_t max_index) { + for (size_t index = min_index; index < max_index; index++) { + const csd::OpticalFlowPixel& pixel = image[index]; + float vx = pixel.x; + float vy = pixel.y; + + float angle = 180.f + std::atan2(vy, vx)*rad2ang; + if (angle < 0) angle = 360.f + angle; + angle = std::fmod(angle, 360.f); + + float norm = std::sqrt(vx*vx + vy*vy); + const float shift = 0.999f; + const float a = 1.f/std::log(0.1f + shift); + float intensity = carla::geom::Math::Clamp(a*std::log(norm + shift), 0.f, 1.f); + + float& H = angle; + float S = 1.f; + float V = intensity; + float H_60 = H*(1.f/60.f); + + float C = V * S; + float X = C*(1.f - std::abs(std::fmod(H_60, 2.f) - 1.f)); + float m = V - C; + + float r = 0,g = 0,b = 0; + unsigned int angle_case = static_cast(H_60); + switch (angle_case) { + case 0: + r = C; + g = X; + b = 0; + break; + case 1: + r = X; + g = C; + b = 0; + break; + case 2: + r = 0; + g = C; + b = X; + break; + case 3: + r = 0; + g = X; + b = C; + break; + case 4: + r = X; + g = 0; + b = C; + break; + case 5: + r = C; + g = 0; + b = X; + break; + default: + r = 1; + g = 1; + b = 1; + break; + } + + uint8_t R = static_cast((r+m)*255.f); + uint8_t G = static_cast((g+m)*255.f); + uint8_t B = static_cast((b+m)*255.f); + result[4*index] = B; + result[4*index + 1] = G; + result[4*index + 2] = R; + result[4*index + 3] = 0; + } + }; + size_t num_threads = std::max(8u, std::thread::hardware_concurrency()); + size_t batch_size = image.size() / num_threads; + std::vector t(num_threads+1); + + for(size_t n = 0; n < num_threads; n++) { + t[n] = new std::thread(command, n * batch_size, (n+1) * batch_size); + } + t[num_threads] = new std::thread(command, num_threads * batch_size, image.size()); + + for(size_t n = 0; n <= num_threads; n++) { + if(t[n]->joinable()){ + t[n]->join(); + } + delete t[n]; + } + + bp::tuple shape = bp::make_tuple(result.size()); + bp::tuple stride = bp::make_tuple(sizeof(uint8_t)); + bn::dtype type = bn::dtype::get_builtin(); + return bn::from_data(&result[0], type, shape, stride, bp::object()).copy(); +} +#endif template static std::string SaveImageToDisk(T &self, std::string path, EColorConverter cc) { @@ -272,6 +392,25 @@ void export_sensor_data() { .def(self_ns::str(self_ns::self)) ; + class_, boost::noncopyable, boost::shared_ptr>("OpticalFlowImage", no_init) + .add_property("width", &csd::OpticalFlowImage::GetWidth) + .add_property("height", &csd::OpticalFlowImage::GetHeight) + .add_property("fov", &csd::OpticalFlowImage::GetFOVAngle) + .add_property("raw_data", &GetRawDataAsBuffer) + #ifndef _WIN32 + .def("get_color_coded_flow", &ColorCodedFlow) + #endif + .def("__len__", &csd::OpticalFlowImage::size) + .def("__iter__", iterator()) + .def("__getitem__", +[](const csd::OpticalFlowImage &self, size_t pos) -> csd::OpticalFlowPixel { + return self.at(pos); + }) + .def("__setitem__", +[](csd::OpticalFlowImage &self, size_t pos, csd::OpticalFlowPixel color) { + self.at(pos) = color; + }) + .def(self_ns::str(self_ns::self)) + ; + class_, boost::noncopyable, boost::shared_ptr>("LidarMeasurement", no_init) .add_property("horizontal_angle", &csd::LidarMeasurement::GetHorizontalAngle) .add_property("channels", &csd::LidarMeasurement::GetChannelCount) diff --git a/PythonAPI/carla/source/libcarla/libcarla.cpp b/PythonAPI/carla/source/libcarla/libcarla.cpp index 83b344b5940..3174af5fa36 100644 --- a/PythonAPI/carla/source/libcarla/libcarla.cpp +++ b/PythonAPI/carla/source/libcarla/libcarla.cpp @@ -209,6 +209,9 @@ static auto MakeCallback(boost::python::object callback) { } }; } +#ifndef _WIN32 +#include +#endif #include "Geom.cpp" #include "Actor.cpp" @@ -235,6 +238,9 @@ BOOST_PYTHON_MODULE(libcarla) { using namespace boost::python; #if PY_MAJOR_VERSION < 3 || PY_MINOR_VERSION < 7 PyEval_InitThreads(); +#endif +#ifndef _WIN32 + boost::python::numpy::initialize(); #endif scope().attr("__path__") = "libcarla"; export_geom(); diff --git a/PythonAPI/examples/manual_control.py b/PythonAPI/examples/manual_control.py index c0c867dbac3..2ac233493ae 100755 --- a/PythonAPI/examples/manual_control.py +++ b/PythonAPI/examples/manual_control.py @@ -154,7 +154,6 @@ def get_actor_display_name(actor, truncate=250): name = ' '.join(actor.type_id.replace('_', '.').title().split('.')[1:]) return (name[:truncate - 1] + u'\u2026') if len(name) > truncate else name - # ============================================================================== # -- World --------------------------------------------------------------------- # ============================================================================== @@ -395,7 +394,10 @@ def parse_events(self, client, world, clock): world.constant_velocity_enabled = True world.hud.notification("Enabled Constant Velocity Mode at 60 km/h") elif event.key > K_0 and event.key <= K_9: - world.camera_manager.set_sensor(event.key - 1 - K_0) + index_ctrl = 0 + if pygame.key.get_mods() & KMOD_CTRL: + index_ctrl = 9 + world.camera_manager.set_sensor(event.key - 1 - K_0 + index_ctrl) elif event.key == K_r and not (pygame.key.get_mods() & KMOD_CTRL): world.camera_manager.toggle_recording() elif event.key == K_r and (pygame.key.get_mods() & KMOD_CTRL): @@ -993,7 +995,9 @@ def __init__(self, parent_actor, hud, gamma_correction): {'lens_circle_multiplier': '3.0', 'lens_circle_falloff': '3.0', 'chromatic_aberration_intensity': '0.5', - 'chromatic_aberration_offset': '0'}]] + 'chromatic_aberration_offset': '0'}], + ['sensor.camera.optical_flow', cc.Raw, 'Optical Flow', {}], + ] world = self._parent.get_world() bp_library = world.get_blueprint_library() for item in self.sensors: @@ -1079,6 +1083,13 @@ def _parse_image(weak_self, image): # Blue is positive, red is negative dvs_img[dvs_events[:]['y'], dvs_events[:]['x'], dvs_events[:]['pol'] * 2] = 255 self.surface = pygame.surfarray.make_surface(dvs_img.swapaxes(0, 1)) + elif self.sensors[self.index][0].startswith('sensor.camera.optical_flow'): + array = image.get_color_coded_flow() + array = np.frombuffer(array, dtype=np.dtype("uint8")) + array = np.reshape(array, (image.height, image.width, 4)) + array = array[:, :, :3] + array = array[:, :, ::-1] + self.surface = pygame.surfarray.make_surface(array.swapaxes(0, 1)) else: image.convert(self.sensors[self.index][1]) array = np.frombuffer(image.raw_data, dtype=np.dtype("uint8")) diff --git a/PythonAPI/test/smoke/test_sensor_tick_time.py b/PythonAPI/test/smoke/test_sensor_tick_time.py index 72ea20efd17..f7a71f2b79e 100644 --- a/PythonAPI/test/smoke/test_sensor_tick_time.py +++ b/PythonAPI/test/smoke/test_sensor_tick_time.py @@ -32,6 +32,7 @@ def test_sensor_tick_time(self): sensor_exception = { "sensor.camera.depth", + "sensor.camera.optical_flow", "sensor.camera.rgb", "sensor.camera.semantic_segmentation", "sensor.camera.dvs", diff --git a/Unreal/CarlaUE4/Config/DefaultEngine.ini b/Unreal/CarlaUE4/Config/DefaultEngine.ini index 7cd2dcff374..c8d82afca98 100644 --- a/Unreal/CarlaUE4/Config/DefaultEngine.ini +++ b/Unreal/CarlaUE4/Config/DefaultEngine.ini @@ -19,6 +19,8 @@ GlobalDefaultServerGameMode=/Game/Carla/Blueprints/Game/CarlaGameMode.CarlaGameM [/Script/Engine.RendererSettings] r.DefaultFeature.MotionBlur=True +r.BasePassOutputsVelocity=True +r.BasePassForceOutputsVelocity=True r.AllowStaticLighting=False r.DiscardUnusedQuality=True r.DefaultFeature.Bloom=False diff --git a/Unreal/CarlaUE4/Plugins/Carla/Content/PostProcessingMaterials/VelocityMaterial.uasset b/Unreal/CarlaUE4/Plugins/Carla/Content/PostProcessingMaterials/VelocityMaterial.uasset new file mode 100644 index 00000000000..c92e139538b Binary files /dev/null and b/Unreal/CarlaUE4/Plugins/Carla/Content/PostProcessingMaterials/VelocityMaterial.uasset differ diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/OpticalFlowCamera.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/OpticalFlowCamera.cpp new file mode 100644 index 00000000000..884db12965f --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/OpticalFlowCamera.cpp @@ -0,0 +1,24 @@ +#include "Carla.h" +#include "Carla/Sensor/OpticalFlowCamera.h" + +#include "Carla/Sensor/PixelReader.h" + +FActorDefinition AOpticalFlowCamera::GetSensorDefinition() +{ + return UActorBlueprintFunctionLibrary::MakeCameraDefinition(TEXT("optical_flow")); +} + +AOpticalFlowCamera::AOpticalFlowCamera(const FObjectInitializer &ObjectInitializer) + : Super(ObjectInitializer) +{ + Enable16BitFormat(true); + AddPostProcessingMaterial( + TEXT("Material'/Carla/PostProcessingMaterials/PhysicLensDistortion.PhysicLensDistortion'")); + AddPostProcessingMaterial( + TEXT("Material'/Carla/PostProcessingMaterials/VelocityMaterial.VelocityMaterial'")); +} + +void AOpticalFlowCamera::PostPhysTick(UWorld *World, ELevelTick TickType, float DeltaSeconds) +{ + FPixelReader::SendPixelsInRenderThread(*this, true); +} diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/OpticalFlowCamera.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/OpticalFlowCamera.h new file mode 100644 index 00000000000..9647c8850ff --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/OpticalFlowCamera.h @@ -0,0 +1,24 @@ +#pragma once + +#include "Carla/Sensor/ShaderBasedSensor.h" + +#include "Carla/Actor/ActorDefinition.h" + +#include "OpticalFlowCamera.generated.h" + +/// Sensor that produces "optical flow" images. +UCLASS() +class CARLA_API AOpticalFlowCamera : public AShaderBasedSensor +{ + GENERATED_BODY() + +public: + + static FActorDefinition GetSensorDefinition(); + + AOpticalFlowCamera(const FObjectInitializer &ObjectInitializer); + +protected: + + void PostPhysTick(UWorld *World, ELevelTick TickType, float DeltaSeconds) override; +}; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/PixelReader.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/PixelReader.cpp index 9e0e8e06bb3..9698b466d69 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/PixelReader.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/PixelReader.cpp @@ -73,6 +73,45 @@ static void WritePixelsToBuffer_Vulkan( } } +// Temporal; this avoid allocating the array each time +TArray gFloatPixels; + +static void WriteFloatPixelsToBuffer_Vulkan( + const UTextureRenderTarget2D &RenderTarget, + carla::Buffer &Buffer, + uint32 Offset, + FRHICommandListImmediate &InRHICmdList) +{ + check(IsInRenderingThread()); + gFloatPixels.Empty(); + auto RenderResource = + static_cast(RenderTarget.Resource); + FTexture2DRHIRef Texture = RenderResource->GetRenderTargetTexture(); + if (!Texture) + { + return; + } + + FIntPoint Rect = RenderResource->GetSizeXY(); + + // NS: Extra copy here, don't know how to avoid it. + InRHICmdList.ReadSurfaceFloatData( + Texture, + FIntRect(0, 0, Rect.X, Rect.Y), + gFloatPixels, + CubeFace_PosX,0,0); + + TArray IntermediateBuffer; + IntermediateBuffer.Reserve(gFloatPixels.Num() * 2); + for (FFloat16Color& color : gFloatPixels) { + float x = (color.R.GetFloat() - 0.5f)*4.f; + float y = (color.G.GetFloat() - 0.5f)*4.f; + IntermediateBuffer.Add(x); + IntermediateBuffer.Add(y); + } + Buffer.copy_from(Offset, IntermediateBuffer); +} + // ============================================================================= // -- FPixelReader ------------------------------------------------------------- // ============================================================================= @@ -134,7 +173,8 @@ void FPixelReader::WritePixelsToBuffer( UTextureRenderTarget2D &RenderTarget, carla::Buffer &Buffer, uint32 Offset, - FRHICommandListImmediate &InRHICmdList + FRHICommandListImmediate &InRHICmdList, + bool use16BitFormat ) { TRACE_CPUPROFILER_EVENT_SCOPE_STR(__FUNCTION__); @@ -142,7 +182,14 @@ void FPixelReader::WritePixelsToBuffer( if (IsVulkanPlatform(GMaxRHIShaderPlatform) || IsD3DPlatform(GMaxRHIShaderPlatform, false)) { - WritePixelsToBuffer_Vulkan(RenderTarget, Buffer, Offset, InRHICmdList); + if (use16BitFormat) + { + WriteFloatPixelsToBuffer_Vulkan(RenderTarget, Buffer, Offset, InRHICmdList); + } + else + { + WritePixelsToBuffer_Vulkan(RenderTarget, Buffer, Offset, InRHICmdList); + } return; } @@ -155,7 +202,7 @@ void FPixelReader::WritePixelsToBuffer( FRHITexture2D *Texture = RenderTargetResource->GetRenderTargetTexture(); checkf(Texture != nullptr, TEXT("FPixelReader: UTextureRenderTarget2D missing render target texture")); - const uint32 BytesPerPixel = 4u; // PF_R8G8B8A8 + const uint32 BytesPerPixel = use16BitFormat ? 8u : 4u; // PF_R8G8B8A8 or PF_FloatRGBA const uint32 Width = Texture->GetSizeX(); const uint32 Height = Texture->GetSizeY(); const uint32 ExpectedStride = Width * BytesPerPixel; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/PixelReader.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/PixelReader.h index 106b54b5990..31ee36fd056 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/PixelReader.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/PixelReader.h @@ -62,7 +62,7 @@ class FPixelReader /// /// @pre To be called from game-thread. template - static void SendPixelsInRenderThread(TSensor &Sensor); + static void SendPixelsInRenderThread(TSensor &Sensor, bool use16BitFormat = false); private: @@ -73,7 +73,8 @@ class FPixelReader UTextureRenderTarget2D &RenderTarget, carla::Buffer &Buffer, uint32 Offset, - FRHICommandListImmediate &InRHICmdList); + FRHICommandListImmediate &InRHICmdList, + bool use16BitFormat = false); }; @@ -82,7 +83,7 @@ class FPixelReader // ============================================================================= template -void FPixelReader::SendPixelsInRenderThread(TSensor &Sensor) +void FPixelReader::SendPixelsInRenderThread(TSensor &Sensor, bool use16BitFormat) { TRACE_CPUPROFILER_EVENT_SCOPE(FPixelReader::SendPixelsInRenderThread); check(Sensor.CaptureRenderTarget != nullptr); @@ -100,7 +101,7 @@ void FPixelReader::SendPixelsInRenderThread(TSensor &Sensor) // game-thread. ENQUEUE_RENDER_COMMAND(FWritePixels_SendPixelsInRenderThread) ( - [&Sensor, Stream=Sensor.GetDataStream(Sensor)](auto &InRHICmdList) mutable + [&Sensor, Stream=Sensor.GetDataStream(Sensor), use16BitFormat](auto &InRHICmdList) mutable { TRACE_CPUPROFILER_EVENT_SCOPE_STR("FWritePixels_SendPixelsInRenderThread"); @@ -112,7 +113,8 @@ void FPixelReader::SendPixelsInRenderThread(TSensor &Sensor) *Sensor.CaptureRenderTarget, Buffer, carla::sensor::SensorRegistry::get::type::header_offset, - InRHICmdList); + InRHICmdList, use16BitFormat); + if(Buffer.data()) { SCOPE_CYCLE_COUNTER(STAT_CarlaSensorStreamSend); @@ -122,6 +124,7 @@ void FPixelReader::SendPixelsInRenderThread(TSensor &Sensor) } } ); + // Blocks until the render thread has finished all it's tasks Sensor.WaitForRenderThreadToFinsih(); } diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/SceneCaptureSensor.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/SceneCaptureSensor.cpp index a87cecc77ba..1ef1bc33b4a 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/SceneCaptureSensor.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/SceneCaptureSensor.cpp @@ -467,7 +467,8 @@ void ASceneCaptureSensor::BeginPlay() // Determine the gamma of the player. const bool bInForceLinearGamma = !bEnablePostProcessingEffects; - CaptureRenderTarget->InitCustomFormat(ImageWidth, ImageHeight, PF_B8G8R8A8, bInForceLinearGamma); + CaptureRenderTarget->InitCustomFormat(ImageWidth, ImageHeight, bEnable16BitFormat ? PF_FloatRGBA : PF_B8G8R8A8, + bInForceLinearGamma); if (bEnablePostProcessingEffects) { diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/SceneCaptureSensor.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/SceneCaptureSensor.h index 5e3fc4d0970..372870d8571 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/SceneCaptureSensor.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/SceneCaptureSensor.h @@ -63,6 +63,18 @@ class CARLA_API ASceneCaptureSensor : public ASensor return bEnablePostProcessingEffects; } + UFUNCTION(BlueprintCallable) + void Enable16BitFormat(bool Enable = false) + { + bEnable16BitFormat = Enable; + } + + UFUNCTION(BlueprintCallable) + bool Is16BitFormatEnabled() const + { + return bEnable16BitFormat; + } + UFUNCTION(BlueprintCallable) void SetFOVAngle(float FOVAngle); @@ -320,6 +332,10 @@ class CARLA_API ASceneCaptureSensor : public ASensor UPROPERTY(EditAnywhere) bool bEnablePostProcessingEffects = true; + /// Whether to change render target format to PF_A16B16G16R16, offering 16bit / channel + UPROPERTY(EditAnywhere) + bool bEnable16BitFormat = false; + FRenderCommandFence RenderFence; };