diff --git a/AirLib/include/api/RpcLibClientBase.hpp b/AirLib/include/api/RpcLibClientBase.hpp index 7609fb175f..11e7e9cd81 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..949c5efd4a 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..0c5cace3ac 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..44f841bdee 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..d7e302dc2f 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): diff --git a/PythonClient/computer_vision/capture_ir_segmentation.py b/PythonClient/computer_vision/capture_ir_segmentation.py index b84ddce5f6..a6aab5f548 100644 --- a/PythonClient/computer_vision/capture_ir_segmentation.py +++ b/PythonClient/computer_vision/capture_ir_segmentation.py @@ -7,6 +7,70 @@ 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) + + 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. + RyRx = numpy.matmul(Ry, Rx) + return numpy.matmul(Rz, RyRx) + +def project_3d_point_to_screen(subjectXYZ, camXYZ, camQuaternion, camProjMatrix4x4, imageWidthHeight): + #Turn the camera position into a column vector. + camPosition = numpy.transpose([camXYZ]) + + #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) + + #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. 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): """ title:: @@ -77,7 +141,7 @@ def main(client, roll=0, yaw=0, z=-122, - writeIR=False, + writeIR=True, writeScene=False, irFolder='', sceneFolder=''): @@ -116,11 +180,11 @@ def main(client, i = 0 for o in objectList: startTime = time.time() - currentTime = time.time() - startTime + elapsedTime = 0 pose = client.simGetObjectPose(o); #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, @@ -142,10 +206,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( + [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, + ir.shape[:2][::-1] + ) + print("Object projected to pixel\n{!s}.".format(object_xy_in_pic)) if __name__ == '__main__': @@ -153,29 +224,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 = elephantList + #Sample calls to main, varying camera angle and altitude. #straight down, 400ft main(client, objectList, - folder=r'auto\winter\400ft\down') + irFolder=r'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..e165792210 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..21fcf8b1e9 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..7374b407f2 100644 --- a/Unreal/Plugins/AirSim/Source/PIPCamera.cpp +++ b/Unreal/Plugins/AirSim/Source/PIPCamera.cpp @@ -90,74 +90,82 @@ 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; - - float x_axis_multiplier; - float y_axis_multiplier; - FIntPoint render_target_size(capture->TextureTarget->GetSurfaceWidth(), capture->TextureTarget->GetSurfaceHeight()); - - 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 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 = FReversedZOrthoMatrix( - OrthoWidth, - OrthoHeight, - ZScale, - ZOffset - ); - } - else - { - float fov = Utils::degreesToRadians(capture->FOVAngle); - if ((int32)ERHIZBuffer::IsInverted) - { - proj_mat = FReversedZPerspectiveMatrix( - fov, - fov, - x_axis_multiplier, - y_axis_multiplier, - GNearClippingPlane, - GNearClippingPlane - ); - } - else - { - proj_mat = FPerspectiveMatrix( - fov, - fov, - x_axis_multiplier, - y_axis_multiplier, - GNearClippingPlane, - GNearClippingPlane - ); - } - } + 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 + { + 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 i = 0; i < 4; ++i) - for (auto j = 0; j < 4; ++j) - mat.matrix[i][j] = proj_mat.M[i][j]; + 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 { diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp index 9a978566cf..1627aaca44 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..66a87c5ae5 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;