all stereo gen functionality working

AirLib/include/common/common_utils/RandomGenerator.hpp

@@ -11,6 +11,8 @@ namespace common_utils {
 template<typename TReturn, typename TDistribution, unsigned int Seed=42>
 class RandomGenerator {
 public:
+    //for uniform distribution supply min and max (inclusive)
+    //for gaussian distribution supply mean and sigma
     template<typename... DistArgs>
     RandomGenerator(DistArgs... dist_args)
         : dist_(dist_args...), rand_(Seed)

AirLib/src/rpc/RpcLibClient.cpp

@@ -261,7 +261,7 @@ void RpcLibClient::confirmConnection()
         std::cout << "X" << std::flush;
         clock->sleep_for(pause_time); 
     }
-    std::cout << "Connected!" << std::endl;
+    std::cout << std::endl << "Connected!" << std::endl;
 
     std::cout << "Waiting for drone to report a valid GPS location..." << std::flush;
     auto gps = getGpsLocation();

Examples/StereoImageGenerator.hpp

@@ -7,6 +7,7 @@
 #include <iomanip>
 #include "common/Common.hpp"
 #include "common/common_utils/FileSystem.hpp"
+#include "common/ClockFactory.hpp"
 #include "rpc/RpcLibClient.hpp"
 #include "controllers/DroneControllerBase.hpp"
 STRICT_MODE_OFF
@@ -22,14 +23,19 @@ class StereoImageGenerator {
         : storage_dir_(storage_dir)
     {
         FileSystem::ensureFolder(storage_dir);
-        std::string files_list_name = FileSystem::combine(storage_dir, "files_list.txt");
 
         client.confirmConnection();
     }
 
     void generate(int num_samples)
     {
+        msr::airlib::ClockBase* clock = msr::airlib::ClockFactory::get();
+        PoseGenerator pose_generator(& client);
+        std::ofstream file_list(FileSystem::combine(storage_dir_, "files_list.txt"));
+
         for (int i = 0; i < num_samples; ++i) {
+            auto start_nanos = clock->nowNanos();
+
             std::vector<ImageRequest> request = { 
                 ImageRequest(0, ImageType_::Scene), 
                 ImageRequest(1, ImageType_::Scene),
@@ -41,10 +47,13 @@ class StereoImageGenerator {
                 continue;
             }
 
+            std::string left_file_name = Utils::stringf("left_%06d.png", i);
+            std::string right_file_name = Utils::stringf("right_%06d.png", i);
+            std::string disparity_file_name  = Utils::stringf("disparity_%06d.pfm", i);
             saveImageToFile(response.at(0).image_data, 
-                FileSystem::combine(storage_dir_, Utils::stringf("right_%06d.png", i)));
+                FileSystem::combine(storage_dir_, left_file_name));
             saveImageToFile(response.at(1).image_data, 
-                FileSystem::combine(storage_dir_, Utils::stringf("left_%06d.png", i)));
+                FileSystem::combine(storage_dir_, right_file_name));
 
             std::vector<float> depth_data;
             const auto& depth_raw = response.at(2).image_data;
@@ -53,15 +62,21 @@ class StereoImageGenerator {
                 depth_data.push_back(pixel_float);
             }
 
-            writeFilePFM(depth_data, response.at(2).width, response.at(2).height,
-                FileSystem::combine(storage_dir_, Utils::stringf("depth_%06d.pfm", i)));
+            //writeFilePFM(depth_data, response.at(2).width, response.at(2).height,
+            //    FileSystem::combine(storage_dir_, Utils::stringf("depth_%06d.pfm", i)));
 
             convertToPlanDepth(depth_data, response.at(2).width, response.at(2).height);
 
             float f = response.at(2).width / 2.0f - 1;
             convertToDisparity(depth_data, response.at(2).width, response.at(2).height, f, 25 / 100.0f);
             writeFilePFM(depth_data, response.at(2).width, response.at(2).height,
-                FileSystem::combine(storage_dir_, Utils::stringf("disparity_%06d.pfm", i)));
+                FileSystem::combine(storage_dir_, disparity_file_name));
+
+            file_list << left_file_name << "," << right_file_name << "," << disparity_file_name << std::endl;
+
+            std::cout << "Image #" << i << " done in " << (clock->nowNanos() - start_nanos) / 1.0E6f << "ms" << std::endl;
+
+            pose_generator.next();
         }
     }
 
@@ -132,10 +147,66 @@ class StereoImageGenerator {
         }
     }
 
+    class PoseGenerator {
+    public:
+        PoseGenerator(msr::airlib::RpcLibClient* client)
+            : client_(client), 
+              rand_xy_(-1.0f, 1.0f), rand_z_(-0.2f, 0.2f), rand_pitch_yaw_(-2 * M_PIf / 360, 2 * M_PIf / 360),
+              min_position_(-1000, -1000, -10), max_position_(1000, 1000, 0), min_pitch_(-0.25f * M_PIf), max_pitch_(0.25f * M_PIf)
+        {
+        }
+
+        void next()
+        {
+            const auto& collision_info = client_->getCollisionInfo();
+            auto position = client_->getPosition();
+            auto orientation = client_->getOrientation();
+
+            if (collision_info.has_collided) {
+                position = collision_info.position + collision_info.normal*2 + collision_info.normal * collision_info.penetration_depth * 2;
+            }
+            else {
+                position.x() += rand_xy_.next();
+                position.y() += rand_xy_.next();
+                position.z() += rand_z_.next();
+
+                position.x() = Utils::clip(position.x(), min_position_.x(), max_position_.x());
+                position.y() = Utils::clip(position.y(), min_position_.y(), max_position_.y());
+                position.z() = Utils::clip(position.z(), min_position_.z(), max_position_.z());
+
+                float pitch, roll, yaw;
+                VectorMath::toEulerianAngle(orientation, pitch, roll, yaw);
+                pitch += rand_pitch_yaw_.next();
+                yaw += rand_pitch_yaw_.next();
+
+                pitch = Utils::clip(pitch, min_pitch_, max_pitch_);
+
+                orientation = VectorMath::toQuaternion(pitch, roll, yaw);
+            }
+
+            client_->simSetPosition(position);
+            client_->simSetOrientation(orientation);
+        }
+    private:
+        typedef common_utils::RandomGeneratorF RandomGeneratorF;
+        typedef msr::airlib::Vector3r Vector3r;
+        typedef msr::airlib::Quaternionr Quaternionr;
+        typedef common_utils::Utils Utils;
+
+        msr::airlib::RpcLibClient* client_;
+        RandomGeneratorF rand_xy_, rand_z_, rand_pitch_yaw_;
+
+        Vector3r min_position_, max_position_;
+        float min_pitch_, max_pitch_;
+    };
+
 private:
     typedef common_utils::FileSystem FileSystem;
     typedef common_utils::Utils Utils;
+    typedef msr::airlib::VectorMath VectorMath;
+    typedef common_utils::RandomGeneratorF RandomGeneratorF;
     typedef msr::airlib::Vector3r Vector3r;
+    typedef msr::airlib::Quaternionr Quaternionr;
     typedef msr::airlib::DroneControllerBase::ImageRequest ImageRequest;
     typedef msr::airlib::VehicleCameraBase::ImageResponse ImageResponse;
     typedef msr::airlib::VehicleCameraBase::ImageType_ ImageType_;

Examples/main.cpp

@@ -54,7 +54,7 @@ int runStandAlonePhysics(int argc, const char *argv[])
 void runSteroImageGenerator()
 {
     StereoImageGenerator gen("c:\\temp\\stig");
-    gen.generate(1);
+    gen.generate(100);
 }
 
 int main(int argc, const char *argv[])

Unreal/Plugins/AirSim/Source/ManualPoseController.cpp

@@ -47,17 +47,26 @@ void UManualPoseController::updateActorPose()
     if (actor_ != nullptr) {
         FVector location = actor_->GetActorLocation();
         FRotator rotation = actor_->GetActorRotation();
-        actor_->SetActorLocationAndRotation(location + delta_location_, rotation + delta_rotation_);
+        actor_->SetActorLocationAndRotation(location + delta_position_, rotation + delta_rotation_);
         resetDelta();
     }
     else {
         UAirBlueprintLib::LogMessageString("UManualPoseController::updateActorPose should not be called when actor is not set", "", LogDebugLevel::Failure);
     }
 }
 
+void UManualPoseController::getActorDeltaPose(FVector& delta_position, FRotator& delta_rotation, bool reset_delta)
+{
+    delta_position = delta_position_;
+    delta_rotation = delta_rotation_;
+
+    if (reset_delta)
+        resetDelta();
+}
+
 void UManualPoseController::resetDelta()
 {
-    delta_location_ = FVector::ZeroVector;
+    delta_position_ = FVector::ZeroVector;
     delta_rotation_ = FRotator::ZeroRotator;
 }
 
@@ -102,33 +111,33 @@ void UManualPoseController::enableBindings(bool enable)
 void UManualPoseController::inputManualLeft(float val)
 {
     if (!FMath::IsNearlyEqual(val, 0.f)) {
-        delta_location_ += delta_rotation_.RotateVector(FVector(0,-val*10,0));
+        delta_position_ += delta_rotation_.RotateVector(FVector(0,-val*10,0));
     }
 }
 void UManualPoseController::inputManualRight(float val)
 {
     if (!FMath::IsNearlyEqual(val, 0.f))
-        delta_location_ += delta_rotation_.RotateVector(FVector(0, val * 10, 0));
+        delta_position_ += delta_rotation_.RotateVector(FVector(0, val * 10, 0));
 }
 void UManualPoseController::inputManualForward(float val)
 {
     if (!FMath::IsNearlyEqual(val, 0.f))
-        delta_location_ += delta_rotation_.RotateVector(FVector(val * 10, 0, 0));
+        delta_position_ += delta_rotation_.RotateVector(FVector(val * 10, 0, 0));
 }
 void UManualPoseController::inputManualBackward(float val)
 {
     if (!FMath::IsNearlyEqual(val, 0.f))
-        delta_location_ += delta_rotation_.RotateVector(FVector(-val * 10, 0, 0));
+        delta_position_ += delta_rotation_.RotateVector(FVector(-val * 10, 0, 0));
 }
 void UManualPoseController::inputManualMoveUp(float val)
 {
     if (!FMath::IsNearlyEqual(val, 0.f))
-        delta_location_ += FVector(0, 0, val * 10);
+        delta_position_ += FVector(0, 0, val * 10);
 }
 void UManualPoseController::inputManualDown(float val)
 {
     if (!FMath::IsNearlyEqual(val, 0.f))
-        delta_location_ += FVector(0, 0, -val * 10);
+        delta_position_ += FVector(0, 0, -val * 10);
 }
 void UManualPoseController::inputManualLeftYaw(float val)
 {

Unreal/Plugins/AirSim/Source/ManualPoseController.h

@@ -14,6 +14,7 @@ class AIRSIM_API UManualPoseController : public UObject {
     void updateActorPose();
     void restoreLastActor();
     void enableBindings(bool enable);
+    void getActorDeltaPose(FVector& delta_position, FRotator& delta_rotation, bool reset_delta);
 
 private:
     void inputManualLeft(float val);
@@ -41,7 +42,7 @@ class AIRSIM_API UManualPoseController : public UObject {
     FInputAxisKeyMapping right_yaw_mapping_, down_pitch_mapping_;
 
 
-    FVector delta_location_;
+    FVector delta_position_;
     FRotator delta_rotation_;
 
     AActor *actor_, *last_actor_;

Unreal/Plugins/AirSim/Source/MultiRotorConnector.cpp

@@ -7,7 +7,7 @@
 using namespace msr::airlib;
 
 void MultiRotorConnector::initialize(AFlyingPawn* vehicle_pawn, msr::airlib::MultiRotorParams* vehicle_params, 
-    bool enable_rpc, std::string api_server_address, const UManualPoseController* manual_pose_controller)
+    bool enable_rpc, std::string api_server_address, UManualPoseController* manual_pose_controller)
 {
     enable_rpc_ = enable_rpc;
     api_server_address_ = api_server_address;
@@ -153,8 +153,21 @@ void MultiRotorConnector::updateRenderedState()
 
     //update ground level
     environment_.getState().min_z_over_ground = vehicle_pawn_->getMinZOverGround();
-    if (manual_pose_controller_ != nullptr && manual_pose_controller_->getActor() == vehicle_pawn_)
-        vehicle_.setPose(vehicle_pawn_->getPose());
+    if (manual_pose_controller_ != nullptr && manual_pose_controller_->getActor() == vehicle_pawn_) {
+        FVector delta_position;
+        FRotator delta_rotation;
+
+        manual_pose_controller_->getActorDeltaPose(delta_position, delta_rotation, true);
+        Vector3r delta_position_ned = NedTransform::toNedMeters(delta_position, false);
+        Quaternionr delta_rotation_ned = NedTransform::toQuaternionr(delta_rotation.Quaternion(), true);
+
+        auto pose = vehicle_.getPose();
+        pose.position += delta_position_ned;
+        pose.orientation = pose.orientation * delta_rotation_ned;
+        pose.orientation.normalize();
+
+        vehicle_.setPose(pose);
+    }
 
     last_pose = vehicle_.getPose();
 

Unreal/Plugins/AirSim/Source/MultiRotorConnector.h

@@ -31,7 +31,7 @@ class MultiRotorConnector : public VehicleConnectorBase
     //VehicleConnectorBase interface
     //implements game interface to update pawn
     void initialize(AFlyingPawn* vehicle_pawn, msr::airlib::MultiRotorParams* vehicle_params, 
-        bool enable_rpc, std::string api_server_address, const UManualPoseController* manual_pose_controller);
+        bool enable_rpc, std::string api_server_address, UManualPoseController* manual_pose_controller);
     virtual void beginPlay() override;
     virtual void endPlay() override;
     virtual void updateRenderedState() override;
@@ -82,7 +82,7 @@ class MultiRotorConnector : public VehicleConnectorBase
     bool enable_rpc_;
     std::string api_server_address_;
     msr::airlib::DroneControllerBase* controller_;
-    const UManualPoseController* manual_pose_controller_;
+    UManualPoseController* manual_pose_controller_;
 
     SimJoyStick joystick_;
     SimJoyStick::State joystick_state_;

Unreal/Plugins/AirSim/Source/NedTransform.cpp

@@ -30,9 +30,9 @@ float NedTransform::toNedMeters(float length)
     return length / world_to_meters_;
 }
 
-NedTransform::Vector3r NedTransform::toNedMeters(const FVector& position)
+NedTransform::Vector3r NedTransform::toNedMeters(const FVector& position, bool use_offset)
 {
-    return NedTransform::toVector3r(position - offset_, 1 / world_to_meters_, true);
+    return NedTransform::toVector3r(position - (use_offset ? offset_ : FVector::ZeroVector), 1 / world_to_meters_, true);
 }
 
 FVector NedTransform::toNeuUU(const NedTransform::Vector3r& position)

Unreal/Plugins/AirSim/Source/NedTransform.h

@@ -14,7 +14,7 @@ class NedTransform
 public:
     static void initialize(const AActor* pivot);
     static bool isInitialized();
-    static Vector3r toNedMeters(const FVector& position);
+    static Vector3r toNedMeters(const FVector& position, bool use_offset = true);
     static FVector toNeuUU(const Vector3r& position);
     static FQuat toFQuat(const Quaternionr& q, bool convert_from_ned);
     static Quaternionr toQuaternionr(const FQuat& q, bool convert_to_ned);

Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp

@@ -23,12 +23,15 @@ void ASimModeWorldBase::BeginPlay()
     HP Z840 desktop high-end config seems to be able to go up to 500Hz.
     To increase freq with limited CPU power, switch Barometer to constant ref mode.
     */
-    if (usage_scenario != kUsageScenarioComputerVision)
-        world_.startAsyncUpdator(3000000LL);
-    else {
+
+    if (usage_scenario == kUsageScenarioComputerVision) {
+        world_.startAsyncUpdator(30000000LL);
+
         manual_pose_controller->initializeForPlay();
         manual_pose_controller->setActor(getFpvVehiclePawn());
     }
+    else
+        world_.startAsyncUpdator(3000000LL);
 }
 
 void ASimModeWorldBase::createWorld()
@@ -74,9 +77,6 @@ void ASimModeWorldBase::Tick(float DeltaSeconds)
 {
     world_.lock();
 
-    if (usage_scenario == kUsageScenarioComputerVision)
-        manual_pose_controller->updateActorPose();
-
     for (auto& vehicle : vehicles_)
         vehicle->updateRenderedState();