Merge pull request #1959 from madratman/AfricaTrackingFeature

Africa tracking feature

AirLib/include/api/RpcLibClientBase.hpp

@@ -48,6 +48,7 @@ class RpcLibClientBase {
     void simEnableWeather(bool enable);
     void simSetWeatherParameter(WorldSimApiBase::WeatherParameter param, float val);
 
+    vector<string> 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);
 

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<std::string> 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;
 

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<string> RpcLibClientBase::simListSceneObjects(const string& name_regex) const
+{
+    return pimpl_->client.call("simListSceneObjects", name_regex).as<vector<string>>();
+}
+
 msr::airlib::Pose RpcLibClientBase::simGetObjectPose(const std::string& object_name) const
 {
     return pimpl_->client.call("simGetObjectPose", object_name).as<RpcLibAdapatorsBase::Pose>().to();

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<string> {
+        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);

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):

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,40 +206,52 @@ 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__':
 
     #Connect to AirSim, UAV mode.
     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') 
+         irFolder=r'auto\winter\400ft\45') 

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, 

Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp

@@ -336,6 +336,23 @@ int UAirBlueprintLib::GetMeshStencilID(const std::string& mesh_name)
     return -1;
 }
 
+std::vector<std::string> UAirBlueprintLib::ListMatchingActors(const UObject *context, const std::string& name_regex)
+{
+    std::vector<std::string> results;
+    auto world = context->GetWorld();
+    std::regex compiledRegex(name_regex, std::regex::optimize);
+    for (TActorIterator<AActor> 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;

Unreal/Plugins/AirSim/Source/AirBlueprintLib.h

@@ -74,6 +74,8 @@ class UAirBlueprintLib : public UBlueprintFunctionLibrary
         UGameplayStatics::GetAllActorsOfClass(context, T::StaticClass(), foundActors);
     }
 
+    static std::vector<std::string> 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,