diff --git a/AirSim.sln b/AirSim.sln index 87ea9bdd9f..5593a915b7 100644 --- a/AirSim.sln +++ b/AirSim.sln @@ -39,6 +39,10 @@ Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "UnrealPluginFiles", "Unreal EndProject Project("{888888A0-9F3D-457C-B088-3A5042F75D52}") = "PythonClient", "PythonClient\PythonClient.pyproj", "{E2049E20-B6DD-474E-8BCA-1C8DC54725AA}" EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "sgmstereo", "SGM\src\sgmstereo\sgmstereo_vc15.vcxproj", "{A01E543F-EF34-46BB-8F3F-29AB84E7A5D4}" +EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "stereoPipeline", "SGM\src\stereoPipeline\stereoPipeline_vc15.vcxproj", "{E512EB59-4EAB-49D1-9174-0CAF1B40CED0}" +EndProject Global GlobalSection(SolutionConfigurationPlatforms) = preSolution Debug|Any CPU = Debug|Any CPU @@ -151,6 +155,26 @@ Global {E2049E20-B6DD-474E-8BCA-1C8DC54725AA}.Release|Any CPU.ActiveCfg = Release|Any CPU {E2049E20-B6DD-474E-8BCA-1C8DC54725AA}.Release|x64.ActiveCfg = Release|Any CPU {E2049E20-B6DD-474E-8BCA-1C8DC54725AA}.Release|x86.ActiveCfg = Release|Any CPU + {A01E543F-EF34-46BB-8F3F-29AB84E7A5D4}.Debug|Any CPU.ActiveCfg = Debug|Win32 + {A01E543F-EF34-46BB-8F3F-29AB84E7A5D4}.Debug|x64.ActiveCfg = Debug|x64 + {A01E543F-EF34-46BB-8F3F-29AB84E7A5D4}.Debug|x64.Build.0 = Debug|x64 + {A01E543F-EF34-46BB-8F3F-29AB84E7A5D4}.Debug|x86.ActiveCfg = Debug|Win32 + {A01E543F-EF34-46BB-8F3F-29AB84E7A5D4}.Debug|x86.Build.0 = Debug|Win32 + {A01E543F-EF34-46BB-8F3F-29AB84E7A5D4}.Release|Any CPU.ActiveCfg = Release|Win32 + {A01E543F-EF34-46BB-8F3F-29AB84E7A5D4}.Release|x64.ActiveCfg = Release|x64 + {A01E543F-EF34-46BB-8F3F-29AB84E7A5D4}.Release|x64.Build.0 = Release|x64 + {A01E543F-EF34-46BB-8F3F-29AB84E7A5D4}.Release|x86.ActiveCfg = Release|Win32 + {A01E543F-EF34-46BB-8F3F-29AB84E7A5D4}.Release|x86.Build.0 = Release|Win32 + {E512EB59-4EAB-49D1-9174-0CAF1B40CED0}.Debug|Any CPU.ActiveCfg = Debug|Win32 + {E512EB59-4EAB-49D1-9174-0CAF1B40CED0}.Debug|x64.ActiveCfg = Debug|x64 + {E512EB59-4EAB-49D1-9174-0CAF1B40CED0}.Debug|x64.Build.0 = Debug|x64 + {E512EB59-4EAB-49D1-9174-0CAF1B40CED0}.Debug|x86.ActiveCfg = Debug|Win32 + {E512EB59-4EAB-49D1-9174-0CAF1B40CED0}.Debug|x86.Build.0 = Debug|Win32 + {E512EB59-4EAB-49D1-9174-0CAF1B40CED0}.Release|Any CPU.ActiveCfg = Release|Win32 + {E512EB59-4EAB-49D1-9174-0CAF1B40CED0}.Release|x64.ActiveCfg = Release|x64 + {E512EB59-4EAB-49D1-9174-0CAF1B40CED0}.Release|x64.Build.0 = Release|x64 + {E512EB59-4EAB-49D1-9174-0CAF1B40CED0}.Release|x86.ActiveCfg = Release|Win32 + {E512EB59-4EAB-49D1-9174-0CAF1B40CED0}.Release|x86.Build.0 = Release|Win32 EndGlobalSection GlobalSection(SolutionProperties) = preSolution HideSolutionNode = FALSE diff --git a/Examples/DataCollectorSGM.h b/Examples/DataCollectorSGM.h new file mode 100644 index 0000000000..afabb46414 --- /dev/null +++ b/Examples/DataCollectorSGM.h @@ -0,0 +1,353 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#pragma once + +#include +#include +#include "common/Common.hpp" +#include "common/common_utils/ProsumerQueue.hpp" +#include "common/common_utils/FileSystem.hpp" +#include "common/ClockFactory.hpp" +#include "vehicles/multirotor/api/MultirotorRpcLibClient.hpp" +#include "vehicles/multirotor/api/MultirotorApiBase.hpp" +#include "RandomPointPoseGeneratorNoRoll.h" +#include "../SGM/src/sgmstereo/sgmstereo.h" +#include "../SGM/src/stereoPipeline/StateStereo.h" +#include "writePNG.h" +STRICT_MODE_OFF +#ifndef RPCLIB_MSGPACK +#define RPCLIB_MSGPACK clmdep_msgpack +#endif // !RPCLIB_MSGPACK +#include "rpc/rpc_error.h" +STRICT_MODE_ON + + +class DataCollectorSGM { +public: + DataCollectorSGM(std::string storage_dir) + : storage_dir_(storage_dir) + { + FileSystem::ensureFolder(storage_dir); + FileSystem::ensureFolder(FileSystem::combine(storage_dir,"left")); + FileSystem::ensureFolder(FileSystem::combine(storage_dir,"right")); + FileSystem::ensureFolder(FileSystem::combine(storage_dir,"depth_gt")); + FileSystem::ensureFolder(FileSystem::combine(storage_dir,"disparity_gt")); + FileSystem::ensureFolder(FileSystem::combine(storage_dir,"disparity_gt_viz")); + FileSystem::ensureFolder(FileSystem::combine(storage_dir,"depth_sgm")); + FileSystem::ensureFolder(FileSystem::combine(storage_dir,"disparity_sgm")); + FileSystem::ensureFolder(FileSystem::combine(storage_dir,"disparity_sgm_viz")); + FileSystem::ensureFolder(FileSystem::combine(storage_dir,"confidence_sgm")); + } + + int generate(int num_samples) + { + msr::airlib::MultirotorRpcLibClient client; + client.confirmConnection(); + client.reset(); + + std::vector request = { + ImageRequest("front_left", ImageType::Scene, false, false), + ImageRequest("front_right", ImageType::Scene, false, false), + ImageRequest("front_left", ImageType::DepthPlanner, true), + ImageRequest("front_left", ImageType::DisparityNormalized, true) + }; + const std::vector& response = client.simGetImages(request); + w = response.at(0).width; + h = response.at(0).height; + + msr::airlib::ClockBase* clock = msr::airlib::ClockFactory::get(); + RandomPointPoseGeneratorNoRoll pose_generator(static_cast(clock->nowNanos())); + std::fstream file_list(FileSystem::combine(storage_dir_, "files_list.txt"), + std::ios::out | std::ios::in | std::ios_base::app); + + int sample = getImageCount(file_list); + + p_state = new CStateStereo(); + p_state->Initialize(params,h,w); + //Print SGM parameters + params.Print(); + + try { + while(sample < num_samples) { + + pose_generator.next(); + client.simSetVehiclePose(Pose(pose_generator.position, pose_generator.orientation), true); + + const auto& collision_info = client.simGetCollisionInfo(); + if (collision_info.has_collided) { + std::cout << "Collision at " << VectorMath::toString(collision_info.position) << std::endl; + continue; + } + ++sample; + //Get into position + clock->sleep_for(0.5); + + auto start_nanos = clock->nowNanos(); + + const std::vector& response = client.simGetImages(request); + if (response.size() != 4) { + std::cout << "Images were not received!" << std::endl; + start_nanos = clock->nowNanos(); + continue; + } + + ImagesResult result; + result.file_list = &file_list; + result.response = response; + result.sample = sample; + result.render_time = clock->elapsedSince(start_nanos);; + result.storage_dir_ = storage_dir_; + result.position = pose_generator.position; + result.orientation = pose_generator.orientation; + + processImages(&result); + + } + } catch (rpc::timeout &t) { + // will display a message like + // rpc::timeout: Timeout of 50ms while calling RPC function 'sleep' + + std::cout << t.what() << std::endl; + } + + return 0; + } + + +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::Pose Pose; + typedef msr::airlib::ImageCaptureBase::ImageRequest ImageRequest; + typedef msr::airlib::ImageCaptureBase::ImageResponse ImageResponse; + typedef msr::airlib::ImageCaptureBase::ImageType ImageType; + + std::string storage_dir_; + bool spawn_ue4 = false; + SGMOptions params; + CStateStereo *p_state; + //Image resolution + int w; + int h; + float dtime = 0; + + //baseline * focal_length = depth * disparity + float fov = Utils::degreesToRadians(90.0f); + float f = w / (2 * tan(fov/2)); + float B = 0.25; + +private: + struct ImagesResult { + std::vector response; + msr::airlib::TTimeDelta render_time; + std::string storage_dir_; + std::fstream* file_list; + int sample; + Vector3r position; + Quaternionr orientation; + }; + + static int getImageCount(std::fstream& file_list) + { + int sample = 0; + std::string line; + while (std::getline(file_list, line)) + ++sample; + if (file_list.eof()) + file_list.clear(); //otherwise we can't do any further I/O + else if (file_list.bad()) { + throw std::runtime_error("Error occurred while reading files_list.txt"); + } + + return sample; + } + + void processImages(ImagesResult* result) + { + + msr::airlib::ClockBase* clock = msr::airlib::ClockFactory::get(); + + + auto process_time = clock->nowNanos(); + + std::string left_file_name = Utils::stringf("left/%06d.png", result->sample); + std::string right_file_name = Utils::stringf("right/%06d.png", result->sample); + std::string depth_gt_file_name = Utils::stringf("depth_gt/%06d.pfm", result->sample); + std::string disparity_gt_file_name = Utils::stringf("disparity_gt/%06d.pfm", result->sample); + std::string disparity_gt_viz_file_name = Utils::stringf("disparity_gt_viz/%06d.png", result->sample); + std::string depth_sgm_file_name = Utils::stringf("depth_sgm/%06d.pfm", result->sample); + std::string disparity_sgm_file_name = Utils::stringf("disparity_sgm/%06d.pfm", result->sample); + std::string disparity_sgm_viz_file_name = Utils::stringf("disparity_sgm_viz/%06d.png", result->sample); + std::string confidence_sgm_file_name = Utils::stringf("confidence_sgm/%06d.png", result->sample); + + //Remove alpha + std::vector left_img(h*w*3); + std::vector right_img(h*w*3); + int counter = 0; + for (int idx = 0; idx < (h*w*4); idx++) { + if ((idx+1) % 4 == 0) { + counter++; + continue; + } + left_img[idx-counter] = result->response.at(0).image_data_uint8 [idx]; + right_img[idx-counter] = result->response.at(1).image_data_uint8 [idx]; + } + + std::vector gt_depth_data = result->response.at(2).image_data_float; + std::vector gt_disparity_data = result->response.at(3).image_data_float; + + std::vector sgm_depth_data(h*w); + std::vector sgm_disparity_data(h*w); + std::vector sgm_confidence_data(h*w); + + p_state->ProcessFrameAirSim(result->sample,dtime,left_img, right_img); + + for (int idx = 0; idx < (h*w); idx++) + { + float d = p_state->dispMap[idx]; + if (d < FLT_MAX) + { + sgm_depth_data[idx] = -(B*f/d); + sgm_disparity_data[idx] = -d; + } + sgm_confidence_data[idx] = p_state->confMap[idx]; + + } + + FILE *img_l = fopen(FileSystem::combine(result->storage_dir_, left_file_name).c_str(), "wb"); + svpng(img_l,w,h,reinterpret_cast(left_img.data()),0); + fclose(img_l); + + FILE *img_r = fopen(FileSystem::combine(result->storage_dir_, right_file_name).c_str(), "wb"); + svpng(img_r,w,h,reinterpret_cast(right_img.data()),0); + fclose(img_r); + + //below is not needed because we get disparity directly + //convertToPlanDepth(depth_data, w, h); + //float f = result.response.at(2).width / 2.0f - 1; + //convertToDisparity(depth_data, f, 25 / 100.0f); + + Utils::writePfmFile(gt_depth_data.data(), w, h, FileSystem::combine(result->storage_dir_, depth_gt_file_name)); + denormalizeDisparity(gt_disparity_data, w); + Utils::writePfmFile(gt_disparity_data.data(), w, h, FileSystem::combine(result->storage_dir_, disparity_gt_file_name)); + + Utils::writePfmFile(sgm_depth_data.data(), w, h, FileSystem::combine(result->storage_dir_, depth_sgm_file_name)); + Utils::writePfmFile(sgm_disparity_data.data(), w, h, FileSystem::combine(result->storage_dir_, disparity_sgm_file_name)); + + FILE *sgm_c = fopen(FileSystem::combine(result->storage_dir_, confidence_sgm_file_name).c_str(), "wb"); + svpng(sgm_c,w,h,reinterpret_cast(sgm_confidence_data.data()),0,1); + fclose(sgm_c); + + //Disparity for visulatization + std::vector sgm_disparity_viz(h*w*3); + getColorVisualization(sgm_disparity_data, sgm_disparity_viz, h, w, 0.05f*w); + FILE *disparity_sgm = fopen(FileSystem::combine(result->storage_dir_, disparity_sgm_viz_file_name).c_str(), "wb"); + svpng(disparity_sgm,w,h,reinterpret_cast(sgm_disparity_viz.data()), 0); + fclose(disparity_sgm); + + std::vector gt_disparity_viz(h*w*3); + getColorVisualization(gt_disparity_data, gt_disparity_viz, h, w, 0.05f*w); + FILE *disparity_gt = fopen(FileSystem::combine(result->storage_dir_, disparity_gt_viz_file_name).c_str(), "wb"); + svpng(disparity_gt,w,h,reinterpret_cast(gt_disparity_viz.data()), 0); + fclose(disparity_gt); + + (* result->file_list) << left_file_name << "," << right_file_name << "," << depth_gt_file_name << "," << disparity_gt_file_name << "," << depth_sgm_file_name << "," << disparity_sgm_file_name << "," << confidence_sgm_file_name << std::endl; + + std::cout << "Image #" << result->sample + << " pos:" << VectorMath::toString(result->position) + << " ori:" << VectorMath::toString(result->orientation) + << " render time " << result->render_time * 1E3f << "ms" + << " process time " << clock->elapsedSince(process_time) * 1E3f << " ms" + << std::endl; + + } + + void getcolor(float d, float max_d, float&r, float&g, float&b) { + + float x = 6.0f; + float inc = x / max_d; + + if (d < max_d) + x = d * inc; + else + x = max_d * inc; + + r = 0.0f; g = 0.0f; b = 0.0f; + if ((0 <= x && x <= 1) || (5 <= x && x < 6)) r = 1.0f; + else if (4 <= x && x < 5) r = x - 4; + else if (1 <= x && x < 2) r = 1.0f - (x - 1); + + if (1 <= x && x < 3) g = 1.0f; + else if (0 <= x && x < 1) g = x - 0; + else if (3 <= x && x < 4) g = 1.0f - (x - 3); + + if (3 <= x && x < 5) b = 1.0f; + else if (2 <= x && x < 3) b = x - 2; + else if (5 <= x && x < 6) b = 1.0f - (x - 5); + } + + template + void getColorVisualization(T& img, std::vector& img_viz, int h, int w, float max_val = 255) { + for (int idx = 0; idx < (h*w); idx++) + { + float r,g,b; + getcolor(img[idx],max_val,r,g,b); + img_viz[idx*3] = (uint8_t) (r*255); + img_viz[idx*3+1] = (uint8_t) (g*255); + img_viz[idx*3+2] = (uint8_t) (b*255); + } + } + + template + void getGrayVisualization(T& img, std::vector& img_viz, int h, int w, int invert = 0, float scale = 1) { + img_viz.resize(h*w); + for (int idx = 0; idx < (h*w); idx++) + { + img_viz[idx] = (uint8_t) (invert ? scale/img[idx] : img[idx]*scale); + } + } + + static void saveImageToFile(const std::vector& image_data, const std::string& file_name) + { + std::ofstream file(file_name , std::ios::binary); + file.write((char*) image_data.data(), image_data.size()); + file.close(); + } + + static void convertToPlanDepth(std::vector& image_data, int width, int height, float f_px = 320) + { + float center_i = width / 2.0f - 1; + float center_j = height / 2.0f - 1; + + for (int i = 0; i < width; ++i) { + for (int j = 0; j < height; ++j) { + float dist = std::sqrt((i - center_i)*(i - center_i) + (j - center_j)*(j - center_j)); + float denom = (dist / f_px); + denom *= denom; + denom = std::sqrt(1 + denom); + image_data[j * width + i] /= denom; + } + } + } + + static void convertToDisparity(std::vector& image_data, float f_px = 320, float baseline_meters = 1) + { + for (int i = 0; i < image_data.size(); ++i) { + image_data[i] = f_px * baseline_meters * (1.0f / image_data[i]); + } + } + + static void denormalizeDisparity(std::vector& image_data, int width) + { + for (int i = 0; i < image_data.size(); ++i) { + image_data[i] = image_data[i] * width; + } + } + +}; + diff --git a/Examples/DataCollectorSGM_settings.json b/Examples/DataCollectorSGM_settings.json new file mode 100644 index 0000000000..7395f41ce3 --- /dev/null +++ b/Examples/DataCollectorSGM_settings.json @@ -0,0 +1,45 @@ +{ + "SeeDocsAt": "https://github.com/Microsoft/AirSim/blob/master/docs/settings.md", + "SettingsVersion": 1.2, + "SimMode": "ComputerVision", + "CameraDefaults": { + "CaptureSettings": [ + { + "ImageType": 0, + "Width": 960, + "Height": 540, + "FOV_Degrees": 90 + }, + { + "ImageType": 1, + "Width": 960, + "Height": 540, + "FOV_Degrees": 90 + }, + { + "ImageType": 4, + "Width": 960, + "Height": 540, + "FOV_Degrees": 90 + } + ], + "Gimbal": { + "Stabilization": 1, + "Pitch": 0, "Roll": 1, "Yaw": 0 + } + }, + "SubWindows": [ + {"WindowID": 0, "CameraID": 1, "ImageType": 0, "Visible": true}, + {"WindowID": 1, "CameraID": 2, "ImageType": 0, "Visible": true}, + {"WindowID": 2, "CameraID": 2, "ImageType": 3, "Visible": true} + ], + "Recording": { + "RecordOnMove": false, + "RecordInterval": 0.05, + "Cameras": [ + { "CameraID": 0, "ImageType": 0, "PixelsAsFloat": false, "Compress": true }, + { "CameraID": 1, "ImageType": 0, "PixelsAsFloat": false, "Compress": true }, + { "CameraID": 2, "ImageType": 0, "PixelsAsFloat": false, "Compress": true } + ] + } +} \ No newline at end of file diff --git a/Examples/Examples.vcxproj b/Examples/Examples.vcxproj index e90f3a7e0f..18d20e7004 100644 --- a/Examples/Examples.vcxproj +++ b/Examples/Examples.vcxproj @@ -28,6 +28,12 @@ {8510c7a4-bf63-41d2-94f6-d8731d137a5a} + + {a01e543f-ef34-46bb-8f3f-29ab84e7a5d4} + + + {e512eb59-4eab-49d1-9174-0caf1b40ced0} + {C679466F-9D35-4AFC-B9AE-F9FB5448FB99} @@ -185,15 +191,18 @@ + + + diff --git a/Examples/Examples.vcxproj.filters b/Examples/Examples.vcxproj.filters index 8382c5e75b..3d3b794aad 100644 --- a/Examples/Examples.vcxproj.filters +++ b/Examples/Examples.vcxproj.filters @@ -47,5 +47,14 @@ Header Files + + Header Files + + + Header Files + + + Header Files + \ No newline at end of file diff --git a/Examples/main.cpp b/Examples/main.cpp index 75154e7deb..5fc4a859df 100644 --- a/Examples/main.cpp +++ b/Examples/main.cpp @@ -1,11 +1,21 @@ #include "StandAloneSensors.hpp" #include "StandAlonePhysics.hpp" #include "StereoImageGenerator.hpp" +#include "DataCollectorSGM.h" #include "GaussianMarkovTest.hpp" #include "DepthNav/DepthNavCost.hpp" +#include "DepthNav/DepthNavThreshold.hpp" #include "DepthNav/DepthNavOptAStar.hpp" #include #include +#include "../SGM/src/sgmstereo/sgmstereo.h" +#include "../SGM/src/stereoPipeline/StateStereo.h" +#include +#ifndef _USE_MATH_DEFINES +#define _USE_MATH_DEFINES +#endif +#include +#include "writePNG.h" int runStandAloneSensors(int argc, const char *argv[]) { @@ -56,6 +66,19 @@ int runStandAlonePhysics(int argc, const char *argv[]) return 0; } +void runDataCollectorSGM(int num_samples, std::string storage_path) +{ + DataCollectorSGM gen(storage_path); + gen.generate(num_samples); +} + +void runDataCollectorSGM(int argc, const char *argv[]) +{ + runDataCollectorSGM(argc < 2 ? 5000 : std::stoi(argv[1]), argc < 3 ? + common_utils::FileSystem::combine( + common_utils::FileSystem::getAppDataFolder(), "data_sgm") + : std::string(argv[2])); +} void runSteroImageGenerator(int num_samples, std::string storage_path) { StereoImageGenerator gen(storage_path); @@ -78,12 +101,18 @@ void runGaussianMarkovTest() test.run(); } -void runDepthNavTest() +void runDepthNavGT() { + Pose startPose = Pose(Vector3r(0, 0, -1), Quaternionr(1, 0, 0, 0)); //start pose + Pose goalPose = Pose(Vector3r(50, 105, -1), Quaternionr(1, 0, 0, 0)); //final pose + //Pose goalPose = client.simGetObjectPose("OrangeBall"); + RpcLibClientBase client; client.confirmConnection(); + client.reset(); - Pose goalPose = client.simGetObjectPose("OrangeBall"); + client.simSetVehiclePose(startPose, true); + std::cout << "Press Enter to start" << std::endl; std::cin.get(); //Allow some time to reach startPose //DepthNavThreshold depthNav; //DepthNavCost depthNav; @@ -91,9 +120,43 @@ void runDepthNavTest() depthNav.gotoGoal(goalPose, client); } +void runDepthNavSGM() +{ + Pose startPose = Pose(Vector3r(0, 0, -1), Quaternionr(1, 0, 0, 0)); //start pose + Pose goalPose = Pose(Vector3r(50, 105, -1), Quaternionr(1, 0, 0, 0)); //final pose + + RpcLibClientBase client; + client.confirmConnection(); + client.reset(); + + client.simSetVehiclePose(startPose, true); + std::cout << "Press Enter to start" << std::endl; std::cin.get(); //Allow some time to reach startPose + + //DepthNavThreshold depthNav; + DepthNavCost depthNav; + //DepthNavOptAStar depthNav; + SGMOptions params; + CStateStereo * p_state; + + if (params.maxImageDimensionWidth != (int) depthNav.params_.depth_width) + printf("WARNING: Width Mismatch between SGM and DepthNav. Overwriting parameters.\n"); + params.maxImageDimensionWidth = depthNav.params_.depth_width; + + params.Print(); + p_state = new CStateStereo(); + p_state->Initialize(params, depthNav.params_.depth_height, depthNav.params_.depth_width); + + depthNav.gotoGoalSGM(goalPose, client, p_state); + + //Cleanup + delete p_state; +} + int main(int argc, const char *argv[]) { - runDepthNavTest(); - - return 0; + //runDepthNavGT(); + runDepthNavSGM(); + //runDataCollectorSGM(argc, argv); + + return 0; } \ No newline at end of file