2017-03-21 14:48:12 -07:00
|
|
|
#include "StandAloneSensors.hpp"
|
2017-06-15 16:15:22 -07:00
|
|
|
#include "StandAlonePhysics.hpp"
|
2018-09-06 17:13:26 -07:00
|
|
|
#include "DataCollection/StereoImageGenerator.hpp"
|
2021-05-11 19:55:55 -03:00
|
|
|
#include "DataCollection/DataCollectorSGM.h"
|
2017-07-27 17:27:26 -07:00
|
|
|
#include "GaussianMarkovTest.hpp"
|
2018-08-02 18:43:40 -07:00
|
|
|
#include "DepthNav/DepthNavCost.hpp"
|
2018-08-28 16:02:03 -07:00
|
|
|
#include "DepthNav/DepthNavThreshold.hpp"
|
2017-03-21 14:48:12 -07:00
|
|
|
#include <iostream>
|
|
|
|
|
#include <string>
|
2018-08-28 16:02:03 -07:00
|
|
|
#ifndef _USE_MATH_DEFINES
|
|
|
|
|
#define _USE_MATH_DEFINES
|
|
|
|
|
#endif
|
2022-03-05 16:41:17 +09:00
|
|
|
#include <cmath>
|
2017-03-21 14:48:12 -07:00
|
|
|
|
2021-05-13 17:40:06 -07:00
|
|
|
int runStandAloneSensors(int argc, const char* argv[])
|
2017-03-21 14:48:12 -07:00
|
|
|
{
|
|
|
|
|
if (argc < 2) {
|
|
|
|
|
std::cout << "Usage: " << argv[0] << " <out_file_name> <period_ms> <total_duration_sec>" << std::endl;
|
|
|
|
|
return 1;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
float period = 30E-3f;
|
|
|
|
|
if (argc >= 3)
|
|
|
|
|
period = std::stof(argv[2]) * 1E-3f;
|
|
|
|
|
|
|
|
|
|
float total_duration = 3600;
|
|
|
|
|
if (argc >= 4)
|
|
|
|
|
total_duration = std::stof(argv[3]);
|
|
|
|
|
|
|
|
|
|
std::cout << "Period is " << period << "sec" << std::endl;
|
|
|
|
|
std::cout << "Total duration is " << total_duration << "sec" << std::endl;
|
|
|
|
|
|
|
|
|
|
using namespace msr::airlib;
|
|
|
|
|
|
2017-07-10 19:23:40 -07:00
|
|
|
//60 acres park:
|
2021-05-11 19:55:55 -03:00
|
|
|
//GeoPoint testLocation(47.7037051477, -122.1415384809, 9.93f);
|
2017-07-10 19:23:40 -07:00
|
|
|
|
|
|
|
|
//marymoore park
|
|
|
|
|
//GeoPoint testLocation(47.662804385, -122.1167039875, 9.93f);
|
2017-04-05 20:25:36 -07:00
|
|
|
|
2022-03-05 16:41:17 +09:00
|
|
|
const GeoPoint testLocation(47.7631699747, -122.0685655406, 9.93f); // woodinville
|
|
|
|
|
constexpr float yawOffset = 0; // static_cast<float>(91.27622 * M_PI / 180.0); // I was aligned with the road...
|
2017-04-03 13:48:40 -07:00
|
|
|
|
2017-03-21 14:48:12 -07:00
|
|
|
std::ofstream out_file(argv[1]);
|
2017-07-10 19:23:40 -07:00
|
|
|
StandALoneSensors::generateImuStaticData(out_file, period, total_duration);
|
|
|
|
|
StandALoneSensors::generateBarometerStaticData(out_file, period, total_duration, testLocation);
|
|
|
|
|
StandALoneSensors::generateBarometerDynamicData(out_file, period, total_duration, testLocation);
|
2017-04-05 20:25:36 -07:00
|
|
|
StandALoneSensors::generateMagnetometer2D(out_file, period, total_duration, testLocation, yawOffset, true);
|
2017-07-10 19:23:40 -07:00
|
|
|
StandALoneSensors::generateMagnetometerMap(out_file);
|
2017-06-15 16:15:22 -07:00
|
|
|
|
|
|
|
|
return 0;
|
|
|
|
|
}
|
|
|
|
|
|
2021-05-13 17:40:06 -07:00
|
|
|
int runStandAlonePhysics(int argc, const char* argv[])
|
2017-06-15 16:15:22 -07:00
|
|
|
{
|
|
|
|
|
using namespace msr::airlib;
|
|
|
|
|
|
2021-05-11 19:55:55 -03:00
|
|
|
StandAlonePhysics::testCollision();
|
2017-06-15 16:15:22 -07:00
|
|
|
|
|
|
|
|
return 0;
|
|
|
|
|
}
|
|
|
|
|
|
2022-03-05 16:41:17 +09:00
|
|
|
void runDataCollectorSGM(const int num_samples, const std::string storage_path)
|
2018-08-28 16:02:03 -07:00
|
|
|
{
|
|
|
|
|
DataCollectorSGM gen(storage_path);
|
|
|
|
|
gen.generate(num_samples);
|
|
|
|
|
}
|
|
|
|
|
|
2022-03-05 16:41:17 +09:00
|
|
|
void runDataCollectorSGM(const int argc, const char* argv[])
|
2018-08-28 16:02:03 -07:00
|
|
|
{
|
2021-05-13 17:40:06 -07:00
|
|
|
runDataCollectorSGM(argc < 2 ? 5000 : std::stoi(argv[1]), argc < 3 ? common_utils::FileSystem::combine(common_utils::FileSystem::getAppDataFolder(), "data_sgm") : std::string(argv[2]));
|
2021-05-11 19:55:55 -03:00
|
|
|
}
|
2022-03-05 16:41:17 +09:00
|
|
|
|
|
|
|
|
void runStereoImageGenerator(const int num_samples, const std::string storage_path)
|
2017-07-07 11:03:01 -07:00
|
|
|
{
|
2017-07-08 21:46:06 -07:00
|
|
|
StereoImageGenerator gen(storage_path);
|
2017-07-07 15:49:28 -07:00
|
|
|
gen.generate(num_samples);
|
2017-07-07 11:03:01 -07:00
|
|
|
}
|
|
|
|
|
|
2022-03-05 16:41:17 +09:00
|
|
|
void runStereoImageGenerator(const int argc, const char* argv[])
|
2017-06-15 16:15:22 -07:00
|
|
|
{
|
2022-03-05 16:41:17 +09:00
|
|
|
runStereoImageGenerator(argc < 2 ? 50000 : std::stoi(argv[1]), argc < 3 ? common_utils::FileSystem::combine(common_utils::FileSystem::getAppDataFolder(), "stereo_gen") : std::string(argv[2]));
|
2017-06-15 16:15:22 -07:00
|
|
|
}
|
|
|
|
|
|
2018-07-31 19:42:34 -07:00
|
|
|
void runGaussianMarkovTest()
|
2017-07-27 17:27:26 -07:00
|
|
|
{
|
2021-05-13 17:40:06 -07:00
|
|
|
using namespace msr::airlib;
|
2018-07-31 11:49:42 -07:00
|
|
|
|
2021-05-13 17:40:06 -07:00
|
|
|
GaussianMarkovTest test;
|
|
|
|
|
test.run();
|
2018-07-31 19:42:34 -07:00
|
|
|
}
|
|
|
|
|
|
2018-08-28 16:02:03 -07:00
|
|
|
void runDepthNavGT()
|
2018-07-31 19:42:34 -07:00
|
|
|
{
|
2022-03-27 13:16:58 +05:30
|
|
|
using namespace msr::airlib;
|
|
|
|
|
|
2018-09-06 17:13:26 -07:00
|
|
|
typedef ImageCaptureBase::ImageRequest ImageRequest;
|
|
|
|
|
typedef ImageCaptureBase::ImageType ImageType;
|
2021-05-11 19:55:55 -03:00
|
|
|
|
2022-03-22 08:02:14 -07:00
|
|
|
const std::vector<ImageRequest> request{
|
2020-12-18 14:47:06 -08:00
|
|
|
ImageRequest("front_left", ImageType::DepthPlanar, true) /*,
|
2018-09-06 17:13:26 -07:00
|
|
|
ImageRequest("front_left", ImageType::Scene),
|
|
|
|
|
ImageRequest("front_left", ImageType::DisparityNormalized, true) */
|
|
|
|
|
};
|
|
|
|
|
|
2018-08-28 16:02:03 -07:00
|
|
|
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");
|
|
|
|
|
|
2018-07-31 19:42:34 -07:00
|
|
|
RpcLibClientBase client;
|
|
|
|
|
client.confirmConnection();
|
2018-08-28 16:02:03 -07:00
|
|
|
client.reset();
|
2018-07-31 19:42:34 -07:00
|
|
|
|
2018-08-28 16:02:03 -07:00
|
|
|
client.simSetVehiclePose(startPose, true);
|
2021-05-13 17:40:06 -07:00
|
|
|
std::cout << "Press Enter to start" << std::endl;
|
|
|
|
|
std::cin.get(); //Allow some time to reach startPose
|
2018-07-31 19:42:34 -07:00
|
|
|
|
2018-08-01 14:17:47 -07:00
|
|
|
//DepthNavThreshold depthNav;
|
2018-08-28 16:05:09 -07:00
|
|
|
DepthNavCost depthNav;
|
|
|
|
|
//DepthNavOptAStar depthNav;
|
2018-09-06 17:13:26 -07:00
|
|
|
depthNav.initialize(client, request);
|
|
|
|
|
depthNav.gotoGoal(goalPose, client, request);
|
2018-07-31 19:42:34 -07:00
|
|
|
}
|
|
|
|
|
|
2018-08-28 16:02:03 -07:00
|
|
|
void runDepthNavSGM()
|
|
|
|
|
{
|
2022-03-27 13:16:58 +05:30
|
|
|
using namespace msr::airlib;
|
|
|
|
|
|
2018-09-06 17:13:26 -07:00
|
|
|
typedef ImageCaptureBase::ImageRequest ImageRequest;
|
|
|
|
|
typedef ImageCaptureBase::ImageType ImageType;
|
|
|
|
|
|
2022-03-22 08:02:14 -07:00
|
|
|
const std::vector<ImageRequest> request{
|
2021-05-11 19:55:55 -03:00
|
|
|
ImageRequest("front_left", ImageType::Scene, false, false),
|
2018-09-06 17:13:26 -07:00
|
|
|
ImageRequest("front_right", ImageType::Scene, false, false), /*
|
2021-05-11 19:55:55 -03:00
|
|
|
ImageRequest("front_left", ImageType::DepthPlanar, true),
|
2018-09-06 17:13:26 -07:00
|
|
|
ImageRequest("front_left", ImageType::DisparityNormalized, true) */
|
|
|
|
|
};
|
|
|
|
|
|
2018-08-28 16:02:03 -07:00
|
|
|
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);
|
2021-05-13 17:40:06 -07:00
|
|
|
std::cout << "Press Enter to start" << std::endl;
|
|
|
|
|
std::cin.get(); //Allow some time to reach startPose
|
2018-08-28 16:02:03 -07:00
|
|
|
|
|
|
|
|
//DepthNavThreshold depthNav;
|
|
|
|
|
DepthNavCost depthNav;
|
2018-09-06 17:13:26 -07:00
|
|
|
//DepthNavOptAStar depthNav;
|
|
|
|
|
depthNav.initialize(client, request);
|
2021-05-11 19:55:55 -03:00
|
|
|
|
2018-08-28 16:02:03 -07:00
|
|
|
SGMOptions params;
|
2022-03-19 20:57:13 +09:00
|
|
|
CStateStereo p_state;
|
2018-08-28 16:02:03 -07:00
|
|
|
|
2022-03-05 16:41:17 +09:00
|
|
|
if (params.maxImageDimensionWidth != static_cast<int>(depthNav.params_.depth_width))
|
2018-08-28 16:02:03 -07:00
|
|
|
printf("WARNING: Width Mismatch between SGM and DepthNav. Overwriting parameters.\n");
|
2022-03-05 16:41:17 +09:00
|
|
|
params.maxImageDimensionWidth = static_cast<int>(depthNav.params_.depth_width);
|
2018-08-28 16:02:03 -07:00
|
|
|
|
2021-05-13 17:40:06 -07:00
|
|
|
params.Print();
|
2022-03-19 09:31:34 +09:00
|
|
|
|
2022-03-19 20:57:13 +09:00
|
|
|
p_state.Initialize(params, static_cast<int>(depthNav.params_.depth_height), static_cast<int>(depthNav.params_.depth_width));
|
2018-08-28 16:02:03 -07:00
|
|
|
|
2022-03-19 20:57:13 +09:00
|
|
|
depthNav.gotoGoalSGM(goalPose, client, request, &p_state);
|
2018-08-28 16:02:03 -07:00
|
|
|
}
|
|
|
|
|
|
2022-03-05 16:41:17 +09:00
|
|
|
int main(const int argc, const char* argv[])
|
2018-07-31 19:42:34 -07:00
|
|
|
{
|
2018-09-06 17:13:26 -07:00
|
|
|
//runDepthNavGT();
|
2018-08-28 16:05:09 -07:00
|
|
|
//runDepthNavSGM();
|
2018-09-06 17:13:26 -07:00
|
|
|
runDataCollectorSGM(argc, argv);
|
2018-08-28 16:02:03 -07:00
|
|
|
|
|
|
|
|
return 0;
|
2021-05-11 19:55:55 -03:00
|
|
|
}
|