From f9b9fa011d77551a1705c86bdb8e4b96e3a99e4a Mon Sep 17 00:00:00 2001 From: Nicholas Gyde Date: Mon, 15 Apr 2019 15:14:58 -0700 Subject: [PATCH 1/4] Added a function to API to list actors in the scene. First pass - may have bugs. --- AirLib/include/api/RpcLibClientBase.hpp | 1 + AirLib/include/api/WorldSimApiBase.hpp | 1 + AirLib/src/api/RpcLibClientBase.cpp | 5 +++++ AirLib/src/api/RpcLibServerBase.cpp | 4 ++++ PythonClient/airsim/client.py | 3 +++ 5 files changed, 14 insertions(+) diff --git a/AirLib/include/api/RpcLibClientBase.hpp b/AirLib/include/api/RpcLibClientBase.hpp index 7609fb175f..60775bb117 100644 --- a/AirLib/include/api/RpcLibClientBase.hpp +++ b/AirLib/include/api/RpcLibClientBase.hpp @@ -48,6 +48,7 @@ class RpcLibClientBase { void simEnableWeather(bool enable); void simSetWeatherParameter(WorldSimApiBase::WeatherParameter param, float val); + vector simListSceneObjects(const string& name_regex = string(".*")) const; Pose simGetObjectPose(const std::string& object_name) const; bool simSetObjectPose(const std::string& object_name, const Pose& pose, bool teleport = true); diff --git a/AirLib/include/api/WorldSimApiBase.hpp b/AirLib/include/api/WorldSimApiBase.hpp index e7137984ea..4529ac3f48 100644 --- a/AirLib/include/api/WorldSimApiBase.hpp +++ b/AirLib/include/api/WorldSimApiBase.hpp @@ -42,6 +42,7 @@ class WorldSimApiBase { virtual void printLogMessage(const std::string& message, const std::string& message_param = "", unsigned char severity = 0) = 0; + virtual std::vector listSceneObjects(const std::string& name_regex) const = 0; virtual Pose getObjectPose(const std::string& object_name) const = 0; virtual bool setObjectPose(const std::string& object_name, const Pose& pose, bool teleport) = 0; diff --git a/AirLib/src/api/RpcLibClientBase.cpp b/AirLib/src/api/RpcLibClientBase.cpp index cd15f72cb9..3c34b103ea 100644 --- a/AirLib/src/api/RpcLibClientBase.cpp +++ b/AirLib/src/api/RpcLibClientBase.cpp @@ -268,6 +268,11 @@ void RpcLibClientBase::simSetTimeOfDay(bool is_enabled, const string& start_date celestial_clock_speed, update_interval_secs, move_sun); } +vector RpcLibClientBase::simListSceneObjects(const string& name_regex) const +{ + return pimpl_->client.call("simListSceneObjects", name_regex).as>(); +} + msr::airlib::Pose RpcLibClientBase::simGetObjectPose(const std::string& object_name) const { return pimpl_->client.call("simGetObjectPose", object_name).as().to(); diff --git a/AirLib/src/api/RpcLibServerBase.cpp b/AirLib/src/api/RpcLibServerBase.cpp index 067a2270b1..9592dba3d6 100644 --- a/AirLib/src/api/RpcLibServerBase.cpp +++ b/AirLib/src/api/RpcLibServerBase.cpp @@ -195,6 +195,10 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string& return RpcLibAdapatorsBase::CollisionInfo(collision_info); }); + pimpl_->server.bind("simListSceneObjects", [&](const std::string& name_regex) -> std::vector { + return getWorldSimApi()->listSceneObjects(name_regex); + }); + pimpl_->server.bind("simGetObjectPose", [&](const std::string& object_name) -> RpcLibAdapatorsBase::Pose { const auto& pose = getWorldSimApi()->getObjectPose(object_name); return RpcLibAdapatorsBase::Pose(pose); diff --git a/PythonClient/airsim/client.py b/PythonClient/airsim/client.py index 8ebd82b47a..2a92803f4d 100644 --- a/PythonClient/airsim/client.py +++ b/PythonClient/airsim/client.py @@ -117,6 +117,9 @@ def simGetObjectPose(self, object_name): def simSetObjectPose(self, object_name, pose, teleport = True): return self.client.call('simSetObjectPose', object_name, pose, teleport) + def simListSceneObjects(self, name_regex): + return self.client.call('simListSceneObjects', name_regex) + def simSetSegmentationObjectID(self, mesh_name, object_id, is_name_regex = False): return self.client.call('simSetSegmentationObjectID', mesh_name, object_id, is_name_regex) def simGetSegmentationObjectID(self, mesh_name): From 83e3c87ded1b59c1a0f68bcddc14ba1b5ff724c5 Mon Sep 17 00:00:00 2001 From: Nicholas Gyde Date: Fri, 26 Apr 2019 09:40:20 -0700 Subject: [PATCH 2/4] new feature, bug fixes, and cleanup: --- PythonClient/airsim/client.py | 2 +- .../capture_ir_segmentation.py | 110 +++++++++++++++--- .../create_ir_segmentation_map.py | 31 +++-- .../Plugins/AirSim/Source/AirBlueprintLib.cpp | 17 +++ .../Plugins/AirSim/Source/AirBlueprintLib.h | 2 + Unreal/Plugins/AirSim/Source/PIPCamera.cpp | 24 ++-- Unreal/Plugins/AirSim/Source/WorldSimApi.cpp | 10 ++ Unreal/Plugins/AirSim/Source/WorldSimApi.h | 1 + 8 files changed, 150 insertions(+), 47 deletions(-) diff --git a/PythonClient/airsim/client.py b/PythonClient/airsim/client.py index 2a92803f4d..d7e302dc2f 100644 --- a/PythonClient/airsim/client.py +++ b/PythonClient/airsim/client.py @@ -117,7 +117,7 @@ def simGetObjectPose(self, object_name): def simSetObjectPose(self, object_name, pose, teleport = True): return self.client.call('simSetObjectPose', object_name, pose, teleport) - def simListSceneObjects(self, name_regex): + def simListSceneObjects(self, name_regex = '.*'): return self.client.call('simListSceneObjects', name_regex) def simSetSegmentationObjectID(self, mesh_name, object_id, is_name_regex = False): diff --git a/PythonClient/computer_vision/capture_ir_segmentation.py b/PythonClient/computer_vision/capture_ir_segmentation.py index b84ddce5f6..be8b3dafc2 100644 --- a/PythonClient/computer_vision/capture_ir_segmentation.py +++ b/PythonClient/computer_vision/capture_ir_segmentation.py @@ -7,6 +7,74 @@ import glob from airsim import * +def rotation_matrix_from_angles(pry): + pitch = pry[0] + roll = pry[1] + yaw = pry[2] + sy = numpy.sin(yaw) + cy = numpy.cos(yaw) + sp = numpy.sin(pitch) + cp = numpy.cos(pitch) + sr = numpy.sin(roll) + cr = numpy.cos(roll) + + #The rotation matrices in x-y-z = North-East-Down coordinates + Rx = numpy.array([ + [1, 0, 0], + [0, cr, -sr], + [0, sr, cr] + ]) + + Ry = numpy.array([ + [cp, 0, -sp], + [0, 1, 0], + [sp, 0, cp] + ]) + + Rz = numpy.array([ + [cy, sy, 0], + [-sy, cy, 0], + [0, 0, 1] + ]) + + #Roll is applied first, then pitch, then yaw. + return numpy.matmul(Rz, Ry, Rx) + +def project_3d_point_to_screen(subjectXYZ, camXYZ, camQuaternion, camProjMatrix4x4, imageWidthHeight): + subjectXYZ.append(1) + #Turn the position into a column vector. + camPosition = numpy.transpose([camXYZ]) + + #Convert the quaternion rotation to yaw, pitch, roll angles. + pitchRollYaw = utils.to_eularian_angles(camQuaternion) + + #Create a rotation matrix from camera pitch, roll, and yaw angles. + camRotation = rotation_matrix_from_angles(pitchRollYaw) + + #Create the view matrix. (Tthe matrix that transforms points from world space to the camera's coordinate frame.) + inverseViewMatrix = numpy.concatenate([camRotation, camPosition], axis = 1) + inverseViewMatrix = numpy.concatenate([inverseViewMatrix, [[0, 0, 0, 1]]], axis = 0) + + print("Before viewMatrix: " + str(subjectXYZ)) + camViewMatrix4x4 = numpy.linalg.inv(inverseViewMatrix) + print("After viewMatrix: " + str(numpy.matmul(camViewMatrix4x4, subjectXYZ))) + + #Transform the 3D point by the view matrix and the projection matrix to get its screen space coordinates. + viewProj4x4 = numpy.matmul(camProjMatrix4x4, camViewMatrix4x4) + + #The result is normalized screen coordinates in the (-1, 1) x (-1, 1) rectangle with a normalization factor W. + XYZW = numpy.matmul(viewProj4x4, numpy.transpose([subjectXYZ])) + print("XYZW: " + str(XYZW)) + XYZW = XYZW / XYZW[3] + + #Move origin to the upper-left corner of the screen and multiply by size to get pixel values. + normX = (1 + XYZW[0]) / 2 + normY = (1 - XYZW[1]) / 2 + return [ + imageWidthHeight[0] * normX, + imageWidthHeight[1] * normY + ] + def get_image(x, y, z, pitch, roll, yaw, client): """ title:: @@ -77,7 +145,7 @@ def main(client, roll=0, yaw=0, z=-122, - writeIR=False, + writeIR=True, writeScene=False, irFolder='', sceneFolder=''): @@ -116,15 +184,17 @@ def main(client, i = 0 for o in objectList: startTime = time.time() - currentTime = time.time() - startTime + elapsedTime = 0 pose = client.simGetObjectPose(o); + o1 = objectList[1] + secondObjectPose = client.simGetObjectPose(o1) #Capture images for a certain amount of time in seconds (half hour now) - while currentTime < 1800: + while elapsedTime < 1800: #Capture image - pose.position x_val access may change w/ AirSim #version (pose.position.x_val new, pose.position[b'x_val'] old) vector, angle, ir, scene = get_image(pose.position.x_val, - pose.position.y_val, + pose.position.y_val - 20, z, pitch, roll, @@ -142,10 +212,17 @@ def main(client, scene) i += 1 - currentTime = time.time() - startTime + elapsedTime = time.time() - startTime pose = client.simGetObjectPose(o); - - + camInfo = client.simGetCameraInfo("0") + object_xy_in_pic = project_3d_point_to_screen( + [secondObjectPose.position.x_val, secondObjectPose.position.y_val, secondObjectPose.position.z_val], + [camInfo.pose.position.x_val, camInfo.pose.position.y_val, camInfo.pose.position.z_val], + camInfo.pose.orientation, + camInfo.proj_mat.matrix, + ir.shape[:2][::-1] + ) + print("Object projected to pixel\n{!s}.".format(object_xy_in_pic)) if __name__ == '__main__': @@ -153,29 +230,34 @@ def main(client, client = MultirotorClient() client.confirmConnection() - #Tags for poachers in each of the three groups in Africa enviornment. - objectList = ['Poacher1A', 'Poacher1B', 'Poacher1C'] - + #Look for objects with names that match a regular expression. + poacherList = client.simListSceneObjects('.*?Poacher.*?') + elephantList = client.simListSceneObjects('.*?Elephant.*?') + crocList = client.simListSceneObjects('.*?Croc.*?') + hippoList = client.simListSceneObjects('.*?Hippo.*?') + + objectList = hippoList + #Sample calls to main, varying camera angle and altitude. #straight down, 400ft main(client, objectList, - folder=r'auto\winter\400ft\down') + irFolder=r'C:\\Users\\v-nigyde\\Documents\\SCREENSHOTS\\')#auto\winter\400ft\down') #straight down, 200ft main(client, objectList, z=-61, - folder=r'auto\winter\200ft\down') + irFolder=r'auto\winter\200ft\down') #45 degrees, 200ft -- note that often object won't be scene since position #is set exactly to object's main(client, objectList, z=-61, pitch=numpy.radians(315), - folder=r'auto\winter\200ft\45') + irFolder=r'auto\winter\200ft\45') #45 degrees, 400ft -- note that often object won't be scene since position #is set exactly to object's main(client, objectList, pitch=numpy.radians(315), - folder=r'auto\winter\400ft\45') \ No newline at end of file + irFolder=r'auto\winter\400ft\45') \ No newline at end of file diff --git a/PythonClient/computer_vision/create_ir_segmentation_map.py b/PythonClient/computer_vision/create_ir_segmentation_map.py index 08e61e0595..065d6b74da 100644 --- a/PythonClient/computer_vision/create_ir_segmentation_map.py +++ b/PythonClient/computer_vision/create_ir_segmentation_map.py @@ -129,10 +129,8 @@ def set_segmentation_ids(segIdDict, tempEmissivityNew, client): #First set everything to 0. success = client.simSetSegmentationObjectID("[\w]*", 0, True); - if success != True: - msg = 'There was a problem setting all segmentation object IDs to 0. ' - msg += 'Please try again.' - print(msg) + if not success: + print('There was a problem setting all segmentation object IDs to 0. ') sys.exit(1) #Next set all objects of interest provided to corresponding object IDs @@ -143,13 +141,9 @@ def set_segmentation_ids(segIdDict, tempEmissivityNew, client): success = client.simSetSegmentationObjectID("[\w]*"+key+"[\w]*", objectID, True); - if success != True: - msg = 'There was a problem setting "' + key - msg += '" segmentation object ID to ' + str(objectID) + '. ' - msg += 'Please try again.' - print(msg) - sys.exit(1) - + if not success: + print('There was a problem setting {0} segmentation object ID to {1!s}, or no {0} was found.'.format(key, objectID)) + time.sleep(0.1) @@ -158,7 +152,7 @@ def set_segmentation_ids(segIdDict, tempEmissivityNew, client): #Connect to AirSim, UAV mode. client = MultirotorClient() client.confirmConnection() - + segIdDict = {'Base_Terrain':'soil', 'elephant':'elephant', 'zebra':'zebra', @@ -173,8 +167,7 @@ def set_segmentation_ids(segIdDict, tempEmissivityNew, client): #Choose temperature values for winter or summer. #""" #winter - tempEmissivity = numpy.array([['nothing',0,0], - ['elephant',290,0.96], + tempEmissivity = numpy.array([['elephant',290,0.96], ['zebra',298,0.98], ['rhinoceros',291,0.96], ['hippopotamus',290,0.96], @@ -189,8 +182,7 @@ def set_segmentation_ids(segIdDict, tempEmissivityNew, client): #""" """ #summer - tempEmissivity = numpy.array([['nothing',0,0], - ['elephant',298,0.96], + tempEmissivity = numpy.array([['elephant',298,0.96], ['zebra',307,0.98], ['rhinoceros',299,0.96], ['hippopotamus',298,0.96], @@ -205,7 +197,12 @@ def set_segmentation_ids(segIdDict, tempEmissivityNew, client): """ #Read camera response. - response = numpy.load('camera_response.npy') + response = None + camResponseFile = 'camera_response.npy' + try: + numpy.load(camResponseFile) + except: + print("{} not found. Using default response.".format(camResponseFile)) #Calculate radiance. tempEmissivityNew = get_new_temp_emiss_from_radiance(tempEmissivity, diff --git a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp index 1c8ff605ed..082e32dd2c 100644 --- a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp +++ b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp @@ -336,6 +336,23 @@ int UAirBlueprintLib::GetMeshStencilID(const std::string& mesh_name) return -1; } +std::vector UAirBlueprintLib::ListMatchingActors(const UObject *context, const std::string& name_regex) +{ + std::vector results; + auto world = context->GetWorld(); + std::regex compiledRegex(name_regex, std::regex::optimize); + for (TActorIterator actorIterator(world); actorIterator; ++actorIterator) + { + AActor *actor = *actorIterator; + auto name = std::string(TCHAR_TO_UTF8(*actor->GetName())); + bool match = std::regex_match(name, compiledRegex); + if (match) + results.push_back(name); + } + return results; +} + + bool UAirBlueprintLib::HasObstacle(const AActor* actor, const FVector& start, const FVector& end, const AActor* ignore_actor, ECollisionChannel collision_channel) { FCollisionQueryParams trace_params; diff --git a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h index eef0c32644..7de3d4dbeb 100644 --- a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h +++ b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h @@ -74,6 +74,8 @@ class UAirBlueprintLib : public UBlueprintFunctionLibrary UGameplayStatics::GetAllActorsOfClass(context, T::StaticClass(), foundActors); } + static std::vector ListMatchingActors(const UObject *context, const std::string& name_regex); + static bool HasObstacle(const AActor* actor, const FVector& start, const FVector& end, const AActor* ignore_actor = nullptr, ECollisionChannel collision_channel = ECC_Visibility); static bool GetObstacle(const AActor* actor, const FVector& start, const FVector& end, diff --git a/Unreal/Plugins/AirSim/Source/PIPCamera.cpp b/Unreal/Plugins/AirSim/Source/PIPCamera.cpp index ff4a9a66b4..900b61d1da 100644 --- a/Unreal/Plugins/AirSim/Source/PIPCamera.cpp +++ b/Unreal/Plugins/AirSim/Source/PIPCamera.cpp @@ -92,18 +92,12 @@ msr::airlib::ProjectionMatrix APIPCamera::getProjectionMatrix(const APIPCamera:: if (capture) { FMatrix proj_mat; - float x_axis_multiplier; - float y_axis_multiplier; FIntPoint render_target_size(capture->TextureTarget->GetSurfaceWidth(), capture->TextureTarget->GetSurfaceHeight()); + float x_axis_multiplier = 1.0f; + float y_axis_multiplier = render_target_size.X / (float)render_target_size.Y; - if (render_target_size.X > render_target_size.Y) - { - // if the viewport is wider than it is tall - x_axis_multiplier = 1.0f; - y_axis_multiplier = render_target_size.X / static_cast(render_target_size.Y); - } - else - { + if (render_target_size.X < render_target_size.Y) + { // if the viewport is taller than it is wide x_axis_multiplier = render_target_size.Y / static_cast(render_target_size.X); y_axis_multiplier = 1.0f; @@ -130,12 +124,12 @@ msr::airlib::ProjectionMatrix APIPCamera::getProjectionMatrix(const APIPCamera:: } else { - float fov = Utils::degreesToRadians(capture->FOVAngle); + float halfFov = Utils::degreesToRadians(capture->FOVAngle) / 2; if ((int32)ERHIZBuffer::IsInverted) { proj_mat = FReversedZPerspectiveMatrix( - fov, - fov, + halfFov, + halfFov, x_axis_multiplier, y_axis_multiplier, GNearClippingPlane, @@ -145,8 +139,8 @@ msr::airlib::ProjectionMatrix APIPCamera::getProjectionMatrix(const APIPCamera:: else { proj_mat = FPerspectiveMatrix( - fov, - fov, + halfFov, + halfFov, x_axis_multiplier, y_axis_multiplier, GNearClippingPlane, diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp index 9a978566cf..9799613f22 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp @@ -65,6 +65,16 @@ void WorldSimApi::printLogMessage(const std::string& message, UAirBlueprintLib::LogMessageString(message, message_param, static_cast(severity)); } +std::vector WorldSimApi::listSceneObjects(const std::string& name_regex) const +{ + std::vector result; + UAirBlueprintLib::RunCommandOnGameThread([this, &name_regex, &result]() { + result = UAirBlueprintLib::ListMatchingActors(simmode_, name_regex); + }, true); + return result; +} + + WorldSimApi::Pose WorldSimApi::getObjectPose(const std::string& object_name) const { Pose result; diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.h b/Unreal/Plugins/AirSim/Source/WorldSimApi.h index f8f5f3c24c..b84ac7d0e9 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.h +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.h @@ -31,6 +31,7 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase { virtual void printLogMessage(const std::string& message, const std::string& message_param = "", unsigned char severity = 0) override; + virtual std::vector listSceneObjects(const std::string& name_regex) const override; virtual Pose getObjectPose(const std::string& object_name) const override; virtual bool setObjectPose(const std::string& object_name, const Pose& pose, bool teleport) override; From 0f69786c17f270cb8272a0643480ffcdec964824 Mon Sep 17 00:00:00 2001 From: Nicholas Gyde Date: Tue, 30 Apr 2019 14:06:26 -0700 Subject: [PATCH 3/4] simGetCamInfo now returns the correct projection matrix Removed debug path. --- .../capture_ir_segmentation.py | 60 +++++++++---------- Unreal/Plugins/AirSim/Source/PIPCamera.cpp | 44 +++++++++----- 2 files changed, 56 insertions(+), 48 deletions(-) diff --git a/PythonClient/computer_vision/capture_ir_segmentation.py b/PythonClient/computer_vision/capture_ir_segmentation.py index be8b3dafc2..a6aab5f548 100644 --- a/PythonClient/computer_vision/capture_ir_segmentation.py +++ b/PythonClient/computer_vision/capture_ir_segmentation.py @@ -18,7 +18,6 @@ def rotation_matrix_from_angles(pry): sr = numpy.sin(roll) cr = numpy.cos(roll) - #The rotation matrices in x-y-z = North-East-Down coordinates Rx = numpy.array([ [1, 0, 0], [0, cr, -sr], @@ -26,54 +25,51 @@ def rotation_matrix_from_angles(pry): ]) Ry = numpy.array([ - [cp, 0, -sp], + [cp, 0, sp], [0, 1, 0], - [sp, 0, cp] + [-sp, 0, cp] ]) Rz = numpy.array([ - [cy, sy, 0], - [-sy, cy, 0], + [cy, -sy, 0], + [sy, cy, 0], [0, 0, 1] ]) #Roll is applied first, then pitch, then yaw. - return numpy.matmul(Rz, Ry, Rx) + RyRx = numpy.matmul(Ry, Rx) + return numpy.matmul(Rz, RyRx) def project_3d_point_to_screen(subjectXYZ, camXYZ, camQuaternion, camProjMatrix4x4, imageWidthHeight): - subjectXYZ.append(1) - #Turn the position into a column vector. + #Turn the camera position into a column vector. camPosition = numpy.transpose([camXYZ]) - #Convert the quaternion rotation to yaw, pitch, roll angles. + #Convert the camera's quaternion rotation to yaw, pitch, roll angles. pitchRollYaw = utils.to_eularian_angles(camQuaternion) #Create a rotation matrix from camera pitch, roll, and yaw angles. camRotation = rotation_matrix_from_angles(pitchRollYaw) - #Create the view matrix. (Tthe matrix that transforms points from world space to the camera's coordinate frame.) - inverseViewMatrix = numpy.concatenate([camRotation, camPosition], axis = 1) - inverseViewMatrix = numpy.concatenate([inverseViewMatrix, [[0, 0, 0, 1]]], axis = 0) - - print("Before viewMatrix: " + str(subjectXYZ)) - camViewMatrix4x4 = numpy.linalg.inv(inverseViewMatrix) - print("After viewMatrix: " + str(numpy.matmul(camViewMatrix4x4, subjectXYZ))) - - #Transform the 3D point by the view matrix and the projection matrix to get its screen space coordinates. - viewProj4x4 = numpy.matmul(camProjMatrix4x4, camViewMatrix4x4) - - #The result is normalized screen coordinates in the (-1, 1) x (-1, 1) rectangle with a normalization factor W. - XYZW = numpy.matmul(viewProj4x4, numpy.transpose([subjectXYZ])) + #Change coordinates to get subjectXYZ in the camera's local coordinate system. + XYZW = numpy.transpose([subjectXYZ]) + XYZW = numpy.add(XYZW, -camPosition) print("XYZW: " + str(XYZW)) + XYZW = numpy.matmul(numpy.transpose(camRotation), XYZW) + print("XYZW derot: " + str(XYZW)) + + #Recreate the perspective projection of the camera. + XYZW = numpy.concatenate([XYZW, [[1]]]) + XYZW = numpy.matmul(camProjMatrix4x4, XYZW) XYZW = XYZW / XYZW[3] - #Move origin to the upper-left corner of the screen and multiply by size to get pixel values. - normX = (1 + XYZW[0]) / 2 - normY = (1 - XYZW[1]) / 2 - return [ + #Move origin to the upper-left corner of the screen and multiply by size to get pixel values. Note that screen is in y,-z plane. + normX = (1 - XYZW[0]) / 2 + normY = (1 + XYZW[1]) / 2 + + return numpy.array([ imageWidthHeight[0] * normX, imageWidthHeight[1] * normY - ] + ]).reshape(2,) def get_image(x, y, z, pitch, roll, yaw, client): """ @@ -186,15 +182,13 @@ def main(client, startTime = time.time() elapsedTime = 0 pose = client.simGetObjectPose(o); - o1 = objectList[1] - secondObjectPose = client.simGetObjectPose(o1) #Capture images for a certain amount of time in seconds (half hour now) while elapsedTime < 1800: #Capture image - pose.position x_val access may change w/ AirSim #version (pose.position.x_val new, pose.position[b'x_val'] old) vector, angle, ir, scene = get_image(pose.position.x_val, - pose.position.y_val - 20, + pose.position.y_val, z, pitch, roll, @@ -216,7 +210,7 @@ def main(client, pose = client.simGetObjectPose(o); camInfo = client.simGetCameraInfo("0") object_xy_in_pic = project_3d_point_to_screen( - [secondObjectPose.position.x_val, secondObjectPose.position.y_val, secondObjectPose.position.z_val], + [pose.position.x_val, pose.position.y_val, pose.position.z_val], [camInfo.pose.position.x_val, camInfo.pose.position.y_val, camInfo.pose.position.z_val], camInfo.pose.orientation, camInfo.proj_mat.matrix, @@ -236,13 +230,13 @@ def main(client, crocList = client.simListSceneObjects('.*?Croc.*?') hippoList = client.simListSceneObjects('.*?Hippo.*?') - objectList = hippoList + objectList = elephantList #Sample calls to main, varying camera angle and altitude. #straight down, 400ft main(client, objectList, - irFolder=r'C:\\Users\\v-nigyde\\Documents\\SCREENSHOTS\\')#auto\winter\400ft\down') + irFolder=r'auto\winter\400ft\down') #straight down, 200ft main(client, objectList, diff --git a/Unreal/Plugins/AirSim/Source/PIPCamera.cpp b/Unreal/Plugins/AirSim/Source/PIPCamera.cpp index 900b61d1da..bba6340421 100644 --- a/Unreal/Plugins/AirSim/Source/PIPCamera.cpp +++ b/Unreal/Plugins/AirSim/Source/PIPCamera.cpp @@ -90,7 +90,7 @@ msr::airlib::ProjectionMatrix APIPCamera::getProjectionMatrix(const APIPCamera:: const_cast(this)->setCameraTypeEnabled(image_type, true); const USceneCaptureComponent2D* capture = const_cast(this)->getCaptureComponent(image_type, false); if (capture) { - FMatrix proj_mat; + FMatrix proj_mat_transpose; FIntPoint render_target_size(capture->TextureTarget->GetSurfaceWidth(), capture->TextureTarget->GetSurfaceHeight()); float x_axis_multiplier = 1.0f; @@ -115,7 +115,7 @@ msr::airlib::ProjectionMatrix APIPCamera::getProjectionMatrix(const APIPCamera:: const float ZScale = 1.0f / (FarPlane - NearPlane); const float ZOffset = -NearPlane; - proj_mat = FReversedZOrthoMatrix( + proj_mat_transpose = FReversedZOrthoMatrix( OrthoWidth, OrthoHeight, ZScale, @@ -127,7 +127,7 @@ msr::airlib::ProjectionMatrix APIPCamera::getProjectionMatrix(const APIPCamera:: float halfFov = Utils::degreesToRadians(capture->FOVAngle) / 2; if ((int32)ERHIZBuffer::IsInverted) { - proj_mat = FReversedZPerspectiveMatrix( + proj_mat_transpose = FReversedZPerspectiveMatrix( halfFov, halfFov, x_axis_multiplier, @@ -138,7 +138,8 @@ msr::airlib::ProjectionMatrix APIPCamera::getProjectionMatrix(const APIPCamera:: } else { - proj_mat = FPerspectiveMatrix( + //The FPerspectiveMatrix() constructor actually returns the transpose of the perspective matrix. + proj_mat_transpose = FPerspectiveMatrix( halfFov, halfFov, x_axis_multiplier, @@ -148,17 +149,30 @@ msr::airlib::ProjectionMatrix APIPCamera::getProjectionMatrix(const APIPCamera:: ); } } - msr::airlib::ProjectionMatrix mat; - for (auto i = 0; i < 4; ++i) - for (auto j = 0; j < 4; ++j) - mat.matrix[i][j] = proj_mat.M[i][j]; - return mat; - } - else { - msr::airlib::ProjectionMatrix mat; - mat.setTo(Utils::nan()); - return mat; - } + + //Takes a vector from NORTH-EAST-DOWN coordinates (AirSim) to EAST-UP-SOUTH coordinates (Unreal). Leaves W coordinate unchanged. + FMatrix coordinateChangeTranspose = FMatrix( + FPlane(0, 0, -1, 0), + FPlane(1, 0, 0, 0), + FPlane(0, -1, 0, 0), + FPlane(0, 0, 0, 1) + ); + + FMatrix projMatTransposeInAirSim = coordinateChangeTranspose * proj_mat_transpose; + + //Copy the result to an airlib::ProjectionMatrix while taking transpose. + msr::airlib::ProjectionMatrix mat; + for (auto row = 0; row < 4; ++row) + for (auto col = 0; col < 4; ++col) + mat.matrix[col][row] = projMatTransposeInAirSim.M[row][col]; + + return mat; + } + else { + msr::airlib::ProjectionMatrix mat; + mat.setTo(Utils::nan()); + return mat; + } } void APIPCamera::Tick(float DeltaTime) From e304419d13b2f27af4dd8dbbcf71d42bd2df7184 Mon Sep 17 00:00:00 2001 From: Nicholas Gyde Date: Tue, 14 May 2019 10:49:50 -0700 Subject: [PATCH 4/4] tabs->spaces --- AirLib/include/api/RpcLibClientBase.hpp | 2 +- AirLib/include/api/WorldSimApiBase.hpp | 2 +- AirLib/src/api/RpcLibClientBase.cpp | 2 +- AirLib/src/api/RpcLibServerBase.cpp | 6 +- .../Plugins/AirSim/Source/AirBlueprintLib.cpp | 24 +-- .../Plugins/AirSim/Source/AirBlueprintLib.h | 2 +- Unreal/Plugins/AirSim/Source/PIPCamera.cpp | 160 +++++++++--------- Unreal/Plugins/AirSim/Source/WorldSimApi.cpp | 10 +- Unreal/Plugins/AirSim/Source/WorldSimApi.h | 2 +- 9 files changed, 105 insertions(+), 105 deletions(-) diff --git a/AirLib/include/api/RpcLibClientBase.hpp b/AirLib/include/api/RpcLibClientBase.hpp index 60775bb117..11e7e9cd81 100644 --- a/AirLib/include/api/RpcLibClientBase.hpp +++ b/AirLib/include/api/RpcLibClientBase.hpp @@ -48,7 +48,7 @@ class RpcLibClientBase { void simEnableWeather(bool enable); void simSetWeatherParameter(WorldSimApiBase::WeatherParameter param, float val); - vector simListSceneObjects(const string& name_regex = string(".*")) const; + vector simListSceneObjects(const string& name_regex = string(".*")) const; Pose simGetObjectPose(const std::string& object_name) const; bool simSetObjectPose(const std::string& object_name, const Pose& pose, bool teleport = true); diff --git a/AirLib/include/api/WorldSimApiBase.hpp b/AirLib/include/api/WorldSimApiBase.hpp index 4529ac3f48..949c5efd4a 100644 --- a/AirLib/include/api/WorldSimApiBase.hpp +++ b/AirLib/include/api/WorldSimApiBase.hpp @@ -42,7 +42,7 @@ class WorldSimApiBase { virtual void printLogMessage(const std::string& message, const std::string& message_param = "", unsigned char severity = 0) = 0; - virtual std::vector listSceneObjects(const std::string& name_regex) const = 0; + virtual std::vector listSceneObjects(const std::string& name_regex) const = 0; virtual Pose getObjectPose(const std::string& object_name) const = 0; virtual bool setObjectPose(const std::string& object_name, const Pose& pose, bool teleport) = 0; diff --git a/AirLib/src/api/RpcLibClientBase.cpp b/AirLib/src/api/RpcLibClientBase.cpp index 3c34b103ea..0c5cace3ac 100644 --- a/AirLib/src/api/RpcLibClientBase.cpp +++ b/AirLib/src/api/RpcLibClientBase.cpp @@ -270,7 +270,7 @@ void RpcLibClientBase::simSetTimeOfDay(bool is_enabled, const string& start_date vector RpcLibClientBase::simListSceneObjects(const string& name_regex) const { - return pimpl_->client.call("simListSceneObjects", name_regex).as>(); + return pimpl_->client.call("simListSceneObjects", name_regex).as>(); } msr::airlib::Pose RpcLibClientBase::simGetObjectPose(const std::string& object_name) const diff --git a/AirLib/src/api/RpcLibServerBase.cpp b/AirLib/src/api/RpcLibServerBase.cpp index 9592dba3d6..44f841bdee 100644 --- a/AirLib/src/api/RpcLibServerBase.cpp +++ b/AirLib/src/api/RpcLibServerBase.cpp @@ -195,9 +195,9 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string& return RpcLibAdapatorsBase::CollisionInfo(collision_info); }); - pimpl_->server.bind("simListSceneObjects", [&](const std::string& name_regex) -> std::vector { - return getWorldSimApi()->listSceneObjects(name_regex); - }); + pimpl_->server.bind("simListSceneObjects", [&](const std::string& name_regex) -> std::vector { + return getWorldSimApi()->listSceneObjects(name_regex); + }); pimpl_->server.bind("simGetObjectPose", [&](const std::string& object_name) -> RpcLibAdapatorsBase::Pose { const auto& pose = getWorldSimApi()->getObjectPose(object_name); diff --git a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp index 082e32dd2c..e165792210 100644 --- a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp +++ b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp @@ -338,18 +338,18 @@ int UAirBlueprintLib::GetMeshStencilID(const std::string& mesh_name) std::vector UAirBlueprintLib::ListMatchingActors(const UObject *context, const std::string& name_regex) { - std::vector results; - auto world = context->GetWorld(); - std::regex compiledRegex(name_regex, std::regex::optimize); - for (TActorIterator actorIterator(world); actorIterator; ++actorIterator) - { - AActor *actor = *actorIterator; - auto name = std::string(TCHAR_TO_UTF8(*actor->GetName())); - bool match = std::regex_match(name, compiledRegex); - if (match) - results.push_back(name); - } - return results; + std::vector results; + auto world = context->GetWorld(); + std::regex compiledRegex(name_regex, std::regex::optimize); + for (TActorIterator actorIterator(world); actorIterator; ++actorIterator) + { + AActor *actor = *actorIterator; + auto name = std::string(TCHAR_TO_UTF8(*actor->GetName())); + bool match = std::regex_match(name, compiledRegex); + if (match) + results.push_back(name); + } + return results; } diff --git a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h index 7de3d4dbeb..21fcf8b1e9 100644 --- a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h +++ b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h @@ -74,7 +74,7 @@ class UAirBlueprintLib : public UBlueprintFunctionLibrary UGameplayStatics::GetAllActorsOfClass(context, T::StaticClass(), foundActors); } - static std::vector ListMatchingActors(const UObject *context, const std::string& name_regex); + static std::vector ListMatchingActors(const UObject *context, const std::string& name_regex); static bool HasObstacle(const AActor* actor, const FVector& start, const FVector& end, const AActor* ignore_actor = nullptr, ECollisionChannel collision_channel = ECC_Visibility); diff --git a/Unreal/Plugins/AirSim/Source/PIPCamera.cpp b/Unreal/Plugins/AirSim/Source/PIPCamera.cpp index bba6340421..7374b407f2 100644 --- a/Unreal/Plugins/AirSim/Source/PIPCamera.cpp +++ b/Unreal/Plugins/AirSim/Source/PIPCamera.cpp @@ -92,87 +92,87 @@ msr::airlib::ProjectionMatrix APIPCamera::getProjectionMatrix(const APIPCamera:: if (capture) { FMatrix proj_mat_transpose; - FIntPoint render_target_size(capture->TextureTarget->GetSurfaceWidth(), capture->TextureTarget->GetSurfaceHeight()); - float x_axis_multiplier = 1.0f; - float y_axis_multiplier = render_target_size.X / (float)render_target_size.Y; - - if (render_target_size.X < render_target_size.Y) - { - // if the viewport is taller than it is wide - x_axis_multiplier = render_target_size.Y / static_cast(render_target_size.X); - y_axis_multiplier = 1.0f; - } - - if (capture->ProjectionType == ECameraProjectionMode::Orthographic) - { - check((int32)ERHIZBuffer::IsInverted); - const float OrthoWidth = capture->OrthoWidth / 2.0f; - const float OrthoHeight = capture->OrthoWidth / 2.0f * x_axis_multiplier / y_axis_multiplier; - - const float NearPlane = 0; - const float FarPlane = WORLD_MAX / 8.0f; - - const float ZScale = 1.0f / (FarPlane - NearPlane); - const float ZOffset = -NearPlane; - - proj_mat_transpose = FReversedZOrthoMatrix( - OrthoWidth, - OrthoHeight, - ZScale, - ZOffset - ); - } - else - { + FIntPoint render_target_size(capture->TextureTarget->GetSurfaceWidth(), capture->TextureTarget->GetSurfaceHeight()); + float x_axis_multiplier = 1.0f; + float y_axis_multiplier = render_target_size.X / (float)render_target_size.Y; + + if (render_target_size.X < render_target_size.Y) + { + // if the viewport is taller than it is wide + x_axis_multiplier = render_target_size.Y / static_cast(render_target_size.X); + y_axis_multiplier = 1.0f; + } + + if (capture->ProjectionType == ECameraProjectionMode::Orthographic) + { + check((int32)ERHIZBuffer::IsInverted); + const float OrthoWidth = capture->OrthoWidth / 2.0f; + const float OrthoHeight = capture->OrthoWidth / 2.0f * x_axis_multiplier / y_axis_multiplier; + + const float NearPlane = 0; + const float FarPlane = WORLD_MAX / 8.0f; + + const float ZScale = 1.0f / (FarPlane - NearPlane); + const float ZOffset = -NearPlane; + + proj_mat_transpose = FReversedZOrthoMatrix( + OrthoWidth, + OrthoHeight, + ZScale, + ZOffset + ); + } + else + { float halfFov = Utils::degreesToRadians(capture->FOVAngle) / 2; - if ((int32)ERHIZBuffer::IsInverted) - { - proj_mat_transpose = FReversedZPerspectiveMatrix( - halfFov, - halfFov, - x_axis_multiplier, - y_axis_multiplier, - GNearClippingPlane, - GNearClippingPlane - ); - } - else - { - //The FPerspectiveMatrix() constructor actually returns the transpose of the perspective matrix. - proj_mat_transpose = FPerspectiveMatrix( - halfFov, - halfFov, - x_axis_multiplier, - y_axis_multiplier, - GNearClippingPlane, - GNearClippingPlane - ); - } - } - - //Takes a vector from NORTH-EAST-DOWN coordinates (AirSim) to EAST-UP-SOUTH coordinates (Unreal). Leaves W coordinate unchanged. - FMatrix coordinateChangeTranspose = FMatrix( - FPlane(0, 0, -1, 0), - FPlane(1, 0, 0, 0), - FPlane(0, -1, 0, 0), - FPlane(0, 0, 0, 1) - ); - - FMatrix projMatTransposeInAirSim = coordinateChangeTranspose * proj_mat_transpose; - - //Copy the result to an airlib::ProjectionMatrix while taking transpose. - msr::airlib::ProjectionMatrix mat; - for (auto row = 0; row < 4; ++row) - for (auto col = 0; col < 4; ++col) - mat.matrix[col][row] = projMatTransposeInAirSim.M[row][col]; - - return mat; - } - else { - msr::airlib::ProjectionMatrix mat; - mat.setTo(Utils::nan()); - return mat; - } + if ((int32)ERHIZBuffer::IsInverted) + { + proj_mat_transpose = FReversedZPerspectiveMatrix( + halfFov, + halfFov, + x_axis_multiplier, + y_axis_multiplier, + GNearClippingPlane, + GNearClippingPlane + ); + } + else + { + //The FPerspectiveMatrix() constructor actually returns the transpose of the perspective matrix. + proj_mat_transpose = FPerspectiveMatrix( + halfFov, + halfFov, + x_axis_multiplier, + y_axis_multiplier, + GNearClippingPlane, + GNearClippingPlane + ); + } + } + + //Takes a vector from NORTH-EAST-DOWN coordinates (AirSim) to EAST-UP-SOUTH coordinates (Unreal). Leaves W coordinate unchanged. + FMatrix coordinateChangeTranspose = FMatrix( + FPlane(0, 0, -1, 0), + FPlane(1, 0, 0, 0), + FPlane(0, -1, 0, 0), + FPlane(0, 0, 0, 1) + ); + + FMatrix projMatTransposeInAirSim = coordinateChangeTranspose * proj_mat_transpose; + + //Copy the result to an airlib::ProjectionMatrix while taking transpose. + msr::airlib::ProjectionMatrix mat; + for (auto row = 0; row < 4; ++row) + for (auto col = 0; col < 4; ++col) + mat.matrix[col][row] = projMatTransposeInAirSim.M[row][col]; + + return mat; + } + else { + msr::airlib::ProjectionMatrix mat; + mat.setTo(Utils::nan()); + return mat; + } } void APIPCamera::Tick(float DeltaTime) diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp index 9799613f22..1627aaca44 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp @@ -67,11 +67,11 @@ void WorldSimApi::printLogMessage(const std::string& message, std::vector WorldSimApi::listSceneObjects(const std::string& name_regex) const { - std::vector result; - UAirBlueprintLib::RunCommandOnGameThread([this, &name_regex, &result]() { - result = UAirBlueprintLib::ListMatchingActors(simmode_, name_regex); - }, true); - return result; + std::vector result; + UAirBlueprintLib::RunCommandOnGameThread([this, &name_regex, &result]() { + result = UAirBlueprintLib::ListMatchingActors(simmode_, name_regex); + }, true); + return result; } diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.h b/Unreal/Plugins/AirSim/Source/WorldSimApi.h index b84ac7d0e9..66a87c5ae5 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.h +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.h @@ -31,7 +31,7 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase { virtual void printLogMessage(const std::string& message, const std::string& message_param = "", unsigned char severity = 0) override; - virtual std::vector listSceneObjects(const std::string& name_regex) const override; + virtual std::vector listSceneObjects(const std::string& name_regex) const override; virtual Pose getObjectPose(const std::string& object_name) const override; virtual bool setObjectPose(const std::string& object_name, const Pose& pose, bool teleport) override;