Merge pull request #3039 from saihv/PR/distortion

Programmable camera distortion

AirLib/include/api/RpcLibClientBase.hpp

@@ -103,14 +103,15 @@ class RpcLibClientBase {
     CollisionInfo simGetCollisionInfo(const std::string& vehicle_name = "") const;
 
     CameraInfo simGetCameraInfo(const std::string& camera_name, const std::string& vehicle_name = "") const;
+    void simSetDistortionParam(const std::string& camera_name, const std::string& param_name, float value, const std::string& vehicle_name = "");
+    std::vector<float> simGetDistortionParams(const std::string& camera_name, const std::string& vehicle_name = "");
     void simSetCameraPose(const std::string& camera_name, const Pose& pose, const std::string& vehicle_name = "");
     void simSetCameraFov(const std::string& camera_name, float fov_degrees, const std::string& vehicle_name = "");
     // This is a backwards-compatibility wrapper over simSetCameraPose, and can be removed in future major releases
     void simSetCameraOrientation(const std::string& camera_name, const Quaternionr& orientation, const std::string& vehicle_name = "");
     bool simCreateVoxelGrid(const Vector3r& position, const int& x_size, const int& y_size, const int& z_size, const float& res, const std::string& output_file);
     msr::airlib::Kinematics::State simGetGroundTruthKinematics(const std::string& vehicle_name = "") const;
     msr::airlib::Environment::State simGetGroundTruthEnvironment(const std::string& vehicle_name = "") const;
-
 	std::vector<std::string> simSwapTextures(const std::string& tags, int tex_id = 0, int component_id = 0, int material_id = 0);
 
     // Recording APIs

AirLib/include/api/VehicleSimApiBase.hpp

@@ -56,6 +56,8 @@ class VehicleSimApiBase : public msr::airlib::UpdatableObject {
     virtual CameraInfo getCameraInfo(const std::string& camera_name) const = 0;
     virtual void setCameraPose(const std::string& camera_name, const Pose& pose) = 0;
     virtual void setCameraFoV(const std::string& camera_name, float fov_degrees) = 0;
+    virtual void setDistortionParam(const std::string& camera_name, const std::string& param_name, float value) = 0;
+    virtual std::vector<float> getDistortionParams(const std::string& camera_name) = 0;
 
     virtual CollisionInfo getCollisionInfo() const = 0;
     virtual int getRemoteControlID() const = 0; //which RC to use, 0 is first one, -1 means disable RC (use keyborad)

AirLib/include/api/WorldSimApiBase.hpp

@@ -63,7 +63,6 @@ class WorldSimApiBase {
     virtual bool setObjectPose(const std::string& object_name, const Pose& pose, bool teleport) = 0;
     virtual bool runConsoleCommand(const std::string& command) = 0;
     virtual bool setObjectScale(const std::string& object_name, const Vector3r& scale) = 0;
-
 	virtual std::unique_ptr<std::vector<std::string>> swapTextures(const std::string& tag, int tex_id = 0, int component_id = 0, int material_id = 0) = 0;
     virtual vector<MeshPositionVertexBuffersResponse> getMeshPositionVertexBuffers() const = 0;
 

AirLib/src/api/RpcLibClientBase.cpp

@@ -398,6 +398,16 @@ void RpcLibClientBase::simSetCameraFov(const std::string& camera_name, float fov
     pimpl_->client.call("simSetCameraFov", camera_name, fov_degrees, vehicle_name);
 }
 
+void RpcLibClientBase::simSetDistortionParam(const std::string& camera_name, const std::string& param_name, float value, const std::string& vehicle_name)
+{
+    pimpl_->client.call("simSetDistortionParam", camera_name, param_name, value, vehicle_name);
+}
+
+std::vector<float> RpcLibClientBase::simGetDistortionParams(const std::string& camera_name, const std::string& vehicle_name)
+{
+    return pimpl_->client.call("simGetDistortionParams", camera_name, vehicle_name).as<std::vector<float>>();
+}
+
 msr::airlib::Kinematics::State RpcLibClientBase::simGetGroundTruthKinematics(const std::string& vehicle_name) const
 {
     return pimpl_->client.call("simGetGroundTruthKinematics", vehicle_name).as<RpcLibAdapatorsBase::KinematicsState>().to();

AirLib/src/api/RpcLibServerBase.cpp

@@ -238,6 +238,14 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string&
         return RpcLibAdapatorsBase::CameraInfo(camera_info);
     });
 
+    pimpl_->server.bind("simSetDistortionParam", [&](const std::string& camera_name, const std::string& param_name, float value, const std::string& vehicle_name) -> void {
+        getVehicleSimApi(vehicle_name)->setDistortionParam(camera_name, param_name, value);
+    });
+
+    pimpl_->server.bind("simGetDistortionParams", [&](const std::string& camera_name, const std::string& vehicle_name) -> std::vector<float> {
+        return getVehicleSimApi(vehicle_name)->getDistortionParams(camera_name);
+    });
+
     pimpl_->server.bind("simSetCameraPose", [&](const std::string& camera_name, const RpcLibAdapatorsBase::Pose& pose, 
         const std::string& vehicle_name) -> void {
         getVehicleSimApi(vehicle_name)->setCameraPose(camera_name, pose.to());

PythonClient/airsim/client.py

@@ -482,6 +482,46 @@ def simGetCameraInfo(self, camera_name, vehicle_name = ''):
         # TODO: below str() conversion is only needed for legacy reason and should be removed in future
         return CameraInfo.from_msgpack(self.client.call('simGetCameraInfo', str(camera_name), vehicle_name))
 
+    def simGetDistortionParams(self, camera_name, vehicle_name = ''):
+        """
+        Get camera distortion parameters
+
+        Args:
+            camera_name (str): Name of the camera, for backwards compatibility, ID numbers such as 0,1,etc. can also be used
+            vehicle_name (str, optional): Vehicle which the camera is associated with
+
+        Returns:
+            List (float): List of distortion parameter values corresponding to K1, K2, K3, P1, P2 respectively.
+        """
+
+        return self.client.call('simGetDistortionParams', str(camera_name), vehicle_name)
+
+    def simSetDistortionParams(self, camera_name, distortion_params, vehicle_name = ''):
+        """
+        Set camera distortion parameters
+
+        Args:
+            camera_name (str): Name of the camera, for backwards compatibility, ID numbers such as 0,1,etc. can also be used
+            distortion_params (dict): Dictionary of distortion param names and corresponding values
+                                        {"K1": 0.0, "K2": 0.0, "K3": 0.0, "P1": 0.0, "P2": 0.0}
+            vehicle_name (str, optional): Vehicle which the camera is associated with
+        """
+
+        for param_name, value in distortion_params.items():
+            self.client.call('simSetDistortionParam', str(camera_name), param_name, value, vehicle_name)
+
+    def simSetDistortionParam(self, camera_name, param_name, value, vehicle_name = ''):
+        """
+        Set single camera distortion parameter
+
+        Args:
+            camera_name (str): Name of the camera, for backwards compatibility, ID numbers such as 0,1,etc. can also be used
+            param_name (str): Name of distortion parameter
+            value (float): Value of distortion parameter
+            vehicle_name (str, optional): Vehicle which the camera is associated with
+        """
+        self.client.call('simSetDistortionParam', str(camera_name), param_name, value, vehicle_name)
+
     def simSetCameraPose(self, camera_name, pose, vehicle_name = ''):
         """
         - Control the pose of a selected camera

Unity/AirLibWrapper/AirsimWrapper/Source/PInvokeWrapper.cpp

@@ -13,6 +13,8 @@ AirSimCarState(*GetCarState)(const char* vehicleName);
 AirSimCameraInfo(*GetCameraInfo)(const char* cameraName, const char* vehicleName);
 bool(*SetCameraPose)(const char* cameraName, AirSimPose pose, const char* vehicleName);
 bool(*SetCameraFoV)(const char* cameraName, const float fov_degrees, const char* vehicleName);
+bool(*SetCameraDistortionParam)(const char* cameraName, const char* paramName, const float value, const char* vehicleName);
+bool(*GetCameraDistortionParams)(const char* cameraName, const char* vehicleName);
 bool(*SetSegmentationObjectId)(const char* meshName, int objectId, bool isNameRegex);
 int(*GetSegmentationObjectId)(const char* meshName);
 bool(*PrintLogMessage) (const char* message, const char* messageParam, const char* vehicleName, int severity);
@@ -35,6 +37,8 @@ void InitVehicleManager(
 	AirSimCameraInfo(*getCameraInfo)(const char* cameraName, const char* vehicleName),
 	bool(*setCameraPose)(const char* cameraName, AirSimPose pose, const char* vehicleName),
 	bool(*setCameraFoV)(const char* cameraName, const float fov_degrees, const char* vehicleName),
+	bool(*setDistortionParam)(const char* cameraName, const char* paramName, const float value, const char* vehicleName),
+	bool(*getDistortionParams)(const char* cameraName, const char* vehicleName),
 	bool(*setSegmentationObjectId)(const char* meshName, int objectId, bool isNameRegex),
 	int(*getSegmentationObjectId)(const char* meshName),
 	bool(*printLogMessage) (const char* message, const char* messageParam, const char* vehicleName, int severity),
@@ -57,6 +61,8 @@ void InitVehicleManager(
 	GetCameraInfo = getCameraInfo;
 	SetCameraPose = setCameraPose;
 	SetCameraFoV = setCameraFoV;
+	SetCameraDistortionParam = setDistortionParam;
+	GetCameraDistortionParams = getDistortionParams;
 	SetSegmentationObjectId = setSegmentationObjectId;
 	GetSegmentationObjectId = getSegmentationObjectId;
 	PrintLogMessage = printLogMessage;

Unity/AirLibWrapper/AirsimWrapper/Source/PInvokeWrapper.h

@@ -27,6 +27,8 @@ extern AirSimCarState(*GetCarState)(const char* vehicleName);
 extern AirSimCameraInfo(*GetCameraInfo)(const char* cameraName, const char* vehicleName);
 extern bool(*SetCameraPose)(const char* cameraName, AirSimPose pose, const char* vehicleName);
 extern bool(*SetCameraFoV)(const char* cameraName, const float fov_degrees, const char* vehicleName);
+extern bool(*SetCameraDistortionParam)(const char* cameraName, const char* paramName, const float value, const char* vehicleName);
+extern bool(*GetCameraDistortionParams)(const char* cameraName, const char* vehicleName);
 extern bool(*SetSegmentationObjectId)(const char* meshName, int objectId, bool isNameRegex);
 extern int(*GetSegmentationObjectId)(const char* meshName);
 extern bool(*PrintLogMessage) (const char* message, const char* messageParam, const char* vehicleName, int severity);
@@ -51,6 +53,8 @@ extern "C" EXPORT void InitVehicleManager(
 	AirSimCameraInfo(*getCameraInfo)(const char* cameraName, const char* vehicleName),
 	bool(*setCameraPose)(const char* cameraName, AirSimPose pose, const char* vehicleName),
 	bool(*setCameraFoV)(const char* cameraName, const float fov_degrees, const char* vehicleName),
+	bool(*setDistortionParam)(const char* cameraName, const char* paramName, const float value, const char* vehicleName),
+	bool(*getDistortionParams)(const char* cameraName, const char* vehicleName),
 	bool(*setSegmentationObjectId)(const char* meshName, int objectId, bool isNameRegex),
 	int(*getSegmentationObjectId)(const char* meshName),
 	bool(*printLogMessage) (const char* message, const char* messageParam, const char* vehicleName, int severity),

Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.cpp

@@ -220,6 +220,19 @@ void PawnSimApi::setCameraFoV(const std::string& camera_name, float fov_degrees)
 	SetCameraFoV(camera_name.c_str(), fov_degrees, params_.vehicle_name.c_str());
 }
 
+void PawnSimApi::setDistortionParam(const std::string& camera_name, const std::string& param_name, float value)
+{
+	// not implemented
+}
+
+std::vector<float> PawnSimApi::getDistortionParams(const std::string& camera_name)
+{
+	// not implemented
+	std::vector<float> params(5, 0.0);
+	return params;
+}
+
+
 //parameters in NED frame
 PawnSimApi::Pose PawnSimApi::getPose() const
 {

Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.h

@@ -74,6 +74,8 @@ class PawnSimApi : public msr::airlib::VehicleSimApiBase
 	virtual msr::airlib::CameraInfo getCameraInfo(const std::string& camera_name) const override;
 	virtual void setCameraPose(const std::string& camera_name, const Pose& pose) override;
 	virtual void setCameraFoV(const std::string& camera_name, float fov_degrees) override;
+	virtual void setDistortionParam(const std::string& camera_name, const std::string& param_name, float value) override;
+	virtual std::vector<float> getDistortionParams(const std::string& camera_name) override;
 	virtual CollisionInfo getCollisionInfo() const override;
 	virtual int getRemoteControlID() const override;
 	virtual msr::airlib::RCData getRCData() const override;

Unity/UnityDemo/Assets/AirSimAssets/Scripts/Vehicles/VehicleCompanion.cs

@@ -194,6 +194,11 @@ private static bool SetCameraFoV(string cameraName, float fov_degrees, string ve
             return vehicle.VehicleInterface.SetCameraFoV(cameraName, fov_degrees);
         }
 
+        private static bool SetDistortionParam(string cameraName, string paramName, float value, string vehicleName) {
+            var vehicle = Vehicles.Find(element => element.vehicleName == vehicleName);
+            return vehicle.VehicleInterface.SetDistortionParam(cameraName, fov_degrees);
+        }
+
         private static bool PrintLogMessage(string message, string messageParams, string vehicleName, int severity) {
             var vehicle = Vehicles.Find(element => element.vehicleName == vehicleName);
             return vehicle.VehicleInterface.PrintLogMessage(message, messageParams, vehicleName, severity);

Unreal/Plugins/AirSim/Source/PIPCamera.cpp

@@ -4,7 +4,6 @@
 #include "Camera/CameraComponent.h"
 #include "Engine/TextureRenderTarget2D.h"
 #include "Engine/World.h"
-#include "Materials/MaterialInstanceDynamic.h"
 #include "ImageUtils.h"
 
 #include <string>
@@ -23,6 +22,16 @@ APIPCamera::APIPCamera()
         UAirBlueprintLib::LogMessageString("Cannot create noise material for the PIPCamera", 
             "", LogDebugLevel::Failure);
 
+    static ConstructorHelpers::FObjectFinder<UMaterial> dist_mat_finder(TEXT("Material'/AirSim/HUDAssets/CameraDistortion.CameraDistortion'"));
+    if (dist_mat_finder.Succeeded())
+    {
+        distortion_material_static_ = dist_mat_finder.Object;
+        distortion_param_collection_ = Cast<UMaterialParameterCollection>(StaticLoadObject(UMaterialParameterCollection::StaticClass(), NULL, TEXT("'/AirSim/HUDAssets/DistortionParams.DistortionParams'")));
+    }
+    else
+        UAirBlueprintLib::LogMessageString("Cannot create distortion material for the PIPCamera",
+            "", LogDebugLevel::Failure);
+
     PrimaryActorTick.bCanEverTick = true;
 
     image_type_to_pixel_format_map_.Add(0, EPixelFormat::PF_B8G8R8A8);
@@ -33,6 +42,7 @@ APIPCamera::APIPCamera()
     image_type_to_pixel_format_map_.Add(5, EPixelFormat::PF_B8G8R8A8);
     image_type_to_pixel_format_map_.Add(6, EPixelFormat::PF_B8G8R8A8);
     image_type_to_pixel_format_map_.Add(7, EPixelFormat::PF_B8G8R8A8);
+
 }
 
 void APIPCamera::PostInitializeComponents()
@@ -66,6 +76,7 @@ void APIPCamera::BeginPlay()
     Super::BeginPlay();
 
     noise_materials_.AddZeroed(imageTypeCount() + 1);
+    distortion_materials_.AddZeroed(imageTypeCount() + 1);
 
     //by default all image types are disabled
     camera_type_enabled_.assign(imageTypeCount(), false);
@@ -82,6 +93,9 @@ void APIPCamera::BeginPlay()
     gimbal_stabilization_ = 0;
     gimbald_rotator_ = this->GetActorRotation();
     this->SetActorTickEnabled(false);
+
+    if (distortion_param_collection_)
+        distortion_param_instance_ = this->GetWorld()->GetParameterCollectionInstance(distortion_param_collection_);
 }
 
 msr::airlib::ProjectionMatrix APIPCamera::getProjectionMatrix(const APIPCamera::ImageType image_type) const
@@ -205,6 +219,18 @@ void APIPCamera::EndPlay(const EEndPlayReason::Type EndPlayReason)
     noise_material_static_ = nullptr;
     noise_materials_.Empty();
 
+    if (distortion_materials_.Num()) {
+        for (unsigned int image_type = 0; image_type < imageTypeCount(); ++image_type) {
+            if (distortion_materials_[image_type + 1])
+                captures_[image_type]->PostProcessSettings.RemoveBlendable(distortion_materials_[image_type + 1]);
+        }
+        if (distortion_materials_[0])
+            camera_->PostProcessSettings.RemoveBlendable(distortion_materials_[0]);
+    }
+
+    distortion_material_static_ = nullptr;
+    distortion_materials_.Empty();
+
     for (unsigned int image_type = 0; image_type < imageTypeCount(); ++image_type) {
         //use final color for all calculations
         captures_[image_type] = nullptr;
@@ -309,12 +335,12 @@ void APIPCamera::setupCameraFromSettings(const APIPCamera::CameraSetting& camera
                         false);
                     break;
             }
-
+            setDistortionMaterial(image_type, captures_[image_type], captures_[image_type]->PostProcessSettings);
             setNoiseMaterial(image_type, captures_[image_type], captures_[image_type]->PostProcessSettings, noise_setting);
         }
         else { //camera component
             updateCameraSetting(camera_, capture_setting, ned_transform);
-
+            setDistortionMaterial(image_type, camera_, camera_->PostProcessSettings);
             setNoiseMaterial(image_type, camera_, camera_->PostProcessSettings, noise_setting);
         }
     }
@@ -420,30 +446,36 @@ void APIPCamera::updateCameraPostProcessingSetting(FPostProcessSettings& obj, co
     }
 }
 
+void APIPCamera::setDistortionMaterial(int image_type, UObject* outer, FPostProcessSettings& obj)
+{
+    UMaterialInstanceDynamic* distortion_material = UMaterialInstanceDynamic::Create(distortion_material_static_, outer);
+    distortion_materials_[image_type + 1] = distortion_material;
+    obj.AddBlendable(distortion_material, 1.0f);
+}
+
 void APIPCamera::setNoiseMaterial(int image_type, UObject* outer, FPostProcessSettings& obj, const NoiseSetting& settings)
 {
     if (!settings.Enabled)
         return;
 
-    UMaterialInstanceDynamic* noise_material_ = UMaterialInstanceDynamic::Create(noise_material_static_, outer);
-    noise_materials_[image_type + 1] = noise_material_;
-
-
-    noise_material_->SetScalarParameterValue("HorzWaveStrength", settings.HorzWaveStrength);
-    noise_material_->SetScalarParameterValue("RandSpeed", settings.RandSpeed);
-    noise_material_->SetScalarParameterValue("RandSize", settings.RandSize);
-    noise_material_->SetScalarParameterValue("RandDensity", settings.RandDensity);
-    noise_material_->SetScalarParameterValue("RandContrib", settings.RandContrib);
-    noise_material_->SetScalarParameterValue("HorzWaveContrib", settings.HorzWaveContrib);
-    noise_material_->SetScalarParameterValue("HorzWaveVertSize", settings.HorzWaveVertSize);
-    noise_material_->SetScalarParameterValue("HorzWaveScreenSize", settings.HorzWaveScreenSize);
-    noise_material_->SetScalarParameterValue("HorzNoiseLinesContrib", settings.HorzNoiseLinesContrib);
-    noise_material_->SetScalarParameterValue("HorzNoiseLinesDensityY", settings.HorzNoiseLinesDensityY);
-    noise_material_->SetScalarParameterValue("HorzNoiseLinesDensityXY", settings.HorzNoiseLinesDensityXY);
-    noise_material_->SetScalarParameterValue("HorzDistortionStrength", settings.HorzDistortionStrength);
-    noise_material_->SetScalarParameterValue("HorzDistortionContrib", settings.HorzDistortionContrib);
-
-    obj.AddBlendable(noise_material_, 1.0f);
+    UMaterialInstanceDynamic* noise_material = UMaterialInstanceDynamic::Create(noise_material_static_, outer);
+    noise_materials_[image_type + 1] = noise_material;
+
+    noise_material->SetScalarParameterValue("HorzWaveStrength", settings.HorzWaveStrength);
+    noise_material->SetScalarParameterValue("RandSpeed", settings.RandSpeed);
+    noise_material->SetScalarParameterValue("RandSize", settings.RandSize);
+    noise_material->SetScalarParameterValue("RandDensity", settings.RandDensity);
+    noise_material->SetScalarParameterValue("RandContrib", settings.RandContrib);
+    noise_material->SetScalarParameterValue("HorzWaveContrib", settings.HorzWaveContrib);
+    noise_material->SetScalarParameterValue("HorzWaveVertSize", settings.HorzWaveVertSize);
+    noise_material->SetScalarParameterValue("HorzWaveScreenSize", settings.HorzWaveScreenSize);
+    noise_material->SetScalarParameterValue("HorzNoiseLinesContrib", settings.HorzNoiseLinesContrib);
+    noise_material->SetScalarParameterValue("HorzNoiseLinesDensityY", settings.HorzNoiseLinesDensityY);
+    noise_material->SetScalarParameterValue("HorzNoiseLinesDensityXY", settings.HorzNoiseLinesDensityXY);
+    noise_material->SetScalarParameterValue("HorzDistortionStrength", settings.HorzDistortionStrength);
+    noise_material->SetScalarParameterValue("HorzDistortionContrib", settings.HorzDistortionContrib);
+
+    obj.AddBlendable(noise_material, 1.0f);
 }
 
 void APIPCamera::enableCaptureComponent(const APIPCamera::ImageType type, bool is_enabled)