diff --git a/3DReconDemo.cpp b/3DReconDemo.cpp index 98f73d85..6a804b94 100644 --- a/3DReconDemo.cpp +++ b/3DReconDemo.cpp @@ -8,25 +8,23 @@ #include "SaveFrame.h" #include "Types.h" #include "SegmentedMesh.h" +#include "concurrency.h" +#include "MockD435iCamera.h" using namespace ark; +using boost::filesystem::path; int main(int argc, char **argv) { if (argc > 5) { - std::cerr << "Usage: ./" << argv[0] << " [configuration-yaml-file] [vocabulary-file] [frame output directory]" << std::endl + std::cerr << "Usage: ./" << argv[0] << " [configuration-yaml-file] [vocabulary-file] [frame output directory] [dataset-dir (if running offline reconstruction)]" << std::endl << "Args given: " << argc << std::endl; return -1; } google::InitGoogleLogging(argv[0]); - okvis::Duration deltaT(0.0); - if (argc == 5) { - deltaT = okvis::Duration(atof(argv[4])); - } - // read configuration file std::string configFilename; if (argc > 1) configFilename = argv[1]; @@ -40,7 +38,11 @@ int main(int argc, char **argv) if (argc > 3) frameOutput = argv[3]; else frameOutput = "./frames/"; - OkvisSLAMSystem slam(vocabFilename, configFilename); + std::string savedDataPath; + if (argc > 4) savedDataPath = argv[4]; + else savedDataPath = ""; + + OkvisSLAMSystem slam(vocabFilename, configFilename); //readConfig(configFilename); @@ -62,8 +64,21 @@ int main(int argc, char **argv) fflush(stdout); - D435iCamera camera; - camera.start(); + CameraSetup * camera; + + //running in real-time mode + if (savedDataPath == "") { + printf("Running in Real-Time mode.\nMake sure you have a supported camera plugged-in.\n"); + D435iCamera * c = new D435iCamera(); + camera = c; + } else { + printf("Running in offline mode.\nReading from provided directory %s.\n", savedDataPath); + path dataPath{ savedDataPath }; + MockD435iCamera * c = new MockD435iCamera(dataPath, configFilename); + camera = c; + } + + camera->start(); printf("Camera-IMU initialization complete\n"); fflush(stdout); @@ -75,7 +90,7 @@ int main(int argc, char **argv) int frame_counter = 1; bool do_integration = true; - SegmentedMesh * mesh = new SegmentedMesh(configFilename, slam, &camera); + SegmentedMesh * mesh = new SegmentedMesh(configFilename, slam, *camera); MyGUI::MeshWindow mesh_win("Mesh Viewer", mesh_view_width, mesh_view_height); MyGUI::Mesh mesh_obj("mesh", mesh); @@ -130,11 +145,10 @@ int main(int argc, char **argv) //Get current camera frame MultiCameraFrame::Ptr frame(new MultiCameraFrame); - camera.update(frame); - + camera->update(frame); //Get or wait for IMU Data until current frame std::vector> imuData; - camera.getImuToTime(frame->timestamp_, imuData); + camera->getImuToTime(frame->timestamp_, imuData); //Add data to SLAM system slam.PushIMU(imuData); @@ -147,7 +161,7 @@ int main(int argc, char **argv) cv::Mat imBGR; - cv::cvtColor(imRGB, imBGR, CV_RGB2BGR); + cv::cvtColor(imRGB, imBGR, cv::COLOR_RGB2BGR); cv::imshow("image", imBGR); diff --git a/CMakeLists.txt b/CMakeLists.txt index a7910488..e67f2367 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,6 +1,6 @@ cmake_minimum_required( VERSION 3.5 ) cmake_policy( VERSION 3.5 ) -#[[ For OpenARK Ubuntu and Jetson Xavier NX, +#[[ For OpenARK Ubuntu and Jetson Xavier NX, cmake_minimum_required( VERSION 3.18 ) cmake_policy( VERSION 3.18 ) ]] @@ -8,7 +8,7 @@ cmake_policy( VERSION 3.18 ) project( OpenARK ) set( OpenARK_VERSION_MAJOR 0 ) set( OpenARK_VERSION_MINOR 9 ) -set( OpenARK_VERSION_PATCH 3 ) +set( OpenARK_VERSION_PATCH 3 ) set( INCLUDE_DIR "${PROJECT_SOURCE_DIR}/include" ) set( CMAKE_CXX_STACK_SIZE "10000000" ) @@ -20,7 +20,7 @@ endif() message(STATUS "CMAKE_HOST_SYSTEM_PROCESSOR: ${CMAKE_HOST_SYSTEM_PROCESSOR}") if(${CMAKE_HOST_SYSTEM_PROCESSOR} MATCHES "arm*") - # NVIDIA Jetson Xavier NX + # NVIDIA Jetson Xavier NX add_definitions(-D__ARM_NEON__) message(STATUS "Host System Processor is ${CMAKE_HOST_SYSTEM_PROCESSOR}, will attempt to use NEON.") endif() @@ -44,6 +44,7 @@ set( DEMO_NAME "OpenARK_hand_demo" ) set( AVATAR_DEMO_NAME "OpenARK_avatar_demo") set( SLAM_DEMO_NAME "OpenARK_SLAM_demo") set( 3DRECON_DATA_NAME "3dRecon_Data_Recording") +set( 3DRECON_DATA_OFFLINE_NAME "3dRecon_Offline_Data_Recording") set( DATA_RECORDING_NAME "OpenARK_data_recording") set( SLAM_RECORDING_NAME "OpenARK_slam_recording") set( SLAM_REPLAYING_NAME "OpenARK_slam_replaying") @@ -55,6 +56,7 @@ option( BUILD_HAND_DEMO "BUILD_HAND_DEMO" ON ) option( BUILD_AVATAR_DEMO "BUILD_AVATAR_DEMO" ON) option( BUILD_SLAM_DEMO "BUILD_SLAM_DEMO" ON) option( BUILD_3DRECON_DEMO "BUILD_3DRECON_DEMO" ON) +option( BUILD_3DRECON_OFFLINE_DEMO "BUILD_3DRECON_OFFLINE_DEMO" ON) option( BUILD_DATA_RECORDING "BUILD_DATA_RECORDING" OFF) option( BUILD_SLAM_RECORDING "BUILD_SLAM_RECORDING" ON) option( BUILD_SLAM_REPLAYING "BUILD_SLAM_REPLAYING" ON) @@ -66,12 +68,12 @@ option( USE_RSSDK "USE_RSSDK" OFF ) option( USE_PMDSDK "USE_PMDSDK" OFF ) include( CheckCXXCompilerFlag ) -CHECK_CXX_COMPILER_FLAG( "-std=c++11" COMPILER_SUPPORTS_CXX11 ) +CHECK_CXX_COMPILER_FLAG( "-std=c++14" COMPILER_SUPPORTS_CXX14 ) CHECK_CXX_COMPILER_FLAG( "-std=c++0x" COMPILER_SUPPORTS_CXX0X ) if( COMPILER_SUPPORTS_CXX11 ) message(STATUS "c++11 supported") - set( CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11" ) + set( CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14" ) elseif( COMPILER_SUPPORTS_CXX0X ) message(status "c++ something supported") set( CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x" ) @@ -153,14 +155,15 @@ IF( DBoW2_FOUND ) ENDIF( DBoW2_FOUND ) # require DLoopDetector -set(ENV{DLoopDetector_INCLUDE_DIRS} "/usr/local/include/DLoopDetector") find_package( DLoopDetector REQUIRED ) IF( DLoopDetector_FOUND ) - MESSAGE( STATUS "Found DLoopDetector: $ENV{DLoopDetector_INCLUDE_DIRS} ${DLoopDetector_LIBRARIES} ${DLoopDetector_INCLUDE_DIRS}" ) + MESSAGE( STATUS "Found DLoopDetector: ${DLoopDetector_LIBRARIES} ${DLoopDetector_INCLUDE_DIRS}" ) ENDIF( DLoopDetector_FOUND ) if( DEFINED ENV{ARK_DEPS_DIR} ) include_directories(${DLoopDetector_INCLUDE_DIRS}) +elseif(${CMAKE_HOST_SYSTEM_NAME} MATCHES "Linux") + include_directories("/usr/local/include/DLoopDetector") else() include_directories($ENV{DLoopDetector_INCLUDE_DIRS}) endif( DEFINED ENV{ARK_DEPS_DIR} ) @@ -189,11 +192,11 @@ ENDIF(Ceres_FOUND) include_directories(${CERES_INCLUDE_DIRS}) # require SuiteSparse -find_package( SuiteSparse QUIET ) -IF(SuiteSparse_FOUND) - MESSAGE(STATUS "Found SuiteSparse: ${SuiteSparse_INCLUDE_DIRS} ${SuiteSparse_LIBRARIES}") -ENDIF(SuiteSparse_FOUND) -include_directories(${SuiteSparse_INCLUDE_DIRS}) +#find_package( SuiteSparse QUIET ) +#IF(SuiteSparse_FOUND) +# MESSAGE(STATUS "Found SuiteSparse: ${SuiteSparse_INCLUDE_DIRS} ${SuiteSparse_LIBRARIES}") +#ENDIF(SuiteSparse_FOUND) +#include_directories(${SuiteSparse_INCLUDE_DIRS}) find_package(OpenMP) if (OPENMP_FOUND) @@ -370,7 +373,7 @@ else() ) list(APPEND DEPENDENCIES m ${GLFW_LIBRARIES} ${LIBUSB1_LIBRARIES}) include_directories(${GLFW_INCLUDE_DIR}) -endif() +endif() if( NOT k4a_FOUND ) set( _AZURE_KINECT_SDK_ "//" ) @@ -511,7 +514,6 @@ if( ${BUILD_3DRECON_DEMO} AND realsense2_FOUND ) endif ( MSVC ) endif( ${BUILD_3DRECON_DEMO} AND realsense2_FOUND ) - if( ${BUILD_DATA_RECORDING} ) add_executable( ${DATA_RECORDING_NAME} DataRecording.cpp ) target_include_directories( ${DATA_RECORDING_NAME} PRIVATE ${INCLUDE_DIR} ) diff --git a/MockD435iCamera.cpp b/MockD435iCamera.cpp index f443105a..6c54f5dc 100644 --- a/MockD435iCamera.cpp +++ b/MockD435iCamera.cpp @@ -10,8 +10,8 @@ /** RealSense SDK2 Cross-Platform Depth Camera Backend **/ namespace ark { -MockD435iCamera::MockD435iCamera(path dir) : dataDir(dir), imuTxtPath(dir / "imu.txt"), metaTxtPath(dir / "meta.txt"), intrinFilePath(dir / "intrin.bin"), timestampTxtPath(dir / "timestamp.txt"), depthDir(dir / "depth/"), - rgbDir(dir / "rgb/"), infraredDir(dir / "infrared/"), infrared2Dir(dir / "infrared2/"), firstFrameId(-1), startTime(0) +MockD435iCamera::MockD435iCamera(path dir, std::string& configFilename) : dataDir(dir), imuTxtPath(dir / "imu.txt"), metaTxtPath(dir / "meta.txt"), timestampTxtPath(dir / "timestamp.txt"), depthDir(dir / "depth/"), + rgbDir(dir / "rgb/"), infraredDir(dir / "infrared/"), infrared2Dir(dir / "infrared2/"), firstFrameId(-1), startTime(0), configFilename(configFilename) { width = 640; height = 480; @@ -28,13 +28,83 @@ void MockD435iCamera::start() imuStream = ifstream(imuTxtPath.string()); timestampStream = ifstream(timestampTxtPath.string()); { - auto intrinStream = ifstream(intrinFilePath.string()); - boost::archive::text_iarchive ia(intrinStream); - ia >> depthIntrinsics; - std::cout << "depthIntrin: fx: " << depthIntrinsics.fx << " fy: " << depthIntrinsics.fy << " ppx: " << depthIntrinsics.ppx << " ppy: " << depthIntrinsics.ppy << '\n'; - - auto metaStream = ifstream(metaTxtPath.string()); + // loading color intrinsics + cv::FileStorage file; + struct stat buffer; + if (stat(configFilename.c_str(), &buffer) == 0) { + file = cv::FileStorage(configFilename, cv::FileStorage::READ); + } + else { + file = cv::FileStorage(); + } + + if (file["additional_cameras"][1]["focal_length"][0].isReal()) { + file["additional_cameras"][1]["focal_length"][0] >> colorIntrinsics.fx; + file["additional_cameras"][1]["focal_length"][1] >> colorIntrinsics.fy; + } + else { + std::cout << "option [additional_cameras][1][focal_length][0] not found, setting to default 6.10873962e+02" << std::endl; + colorIntrinsics.fx = 6.10873962e+02; + colorIntrinsics.fy = 6.11282288e+02; + } + + if (file["additional_cameras"][1]["principal_point"][0].isReal()) { + file["additional_cameras"][1]["principal_point"][0] >> colorIntrinsics.ppx; + file["additional_cameras"][1]["principal_point"][1] >> colorIntrinsics.ppy; + } + else { + std::cout << "option [additional_cameras][1][principal_point][0] not found, setting to default 3.18763977e+02" << std::endl; + colorIntrinsics.ppx = 3.18763977e+02; + colorIntrinsics.ppy = 2.45752747e+02; + } + // color intrin read done + + // loading depth intrinsics + if (file["additional_cameras"][0]["focal_length"][0].isReal()) { + file["additional_cameras"][0]["focal_length"][0] >> depthIntrinsics.fx; + file["additional_cameras"][0]["focal_length"][1] >> depthIntrinsics.fy; + } + else { + std::cout << "option [additional_cameras][0][focal_length][0] not found, setting to default 6.10873962e+02" << std::endl; + depthIntrinsics.fx = 3.83738525e+02; + depthIntrinsics.fy = 3.83738525e+02; + } + + if (file["additional_cameras"][0]["principal_point"][0].isReal()) { + file["additional_cameras"][0]["principal_point"][0] >> depthIntrinsics.ppx; + file["additional_cameras"][0]["principal_point"][1] >> depthIntrinsics.ppy; + } + else { + std::cout << "option [additional_cameras][0][principal_point][0] not found, setting to default 3.18763977e+02" << std::endl; + depthIntrinsics.ppx = 3.19075897e+02; + depthIntrinsics.ppy = 2.38199295e+02; + } + + if (file["additional_cameras"][0]["image_dimension"][0].isReal()) { + file["additional_cameras"][0]["image_dimension"][0] >> depthIntrinsics.width; + file["additional_cameras"][0]["image_dimension"][1] >> depthIntrinsics.height; + } + else { + std::cout << "option [additional_cameras][0][image_dimension][0] not found, setting to default 640x480" << std::endl; + depthIntrinsics.width = width; + depthIntrinsics.height = height; + } + + if (file["additional_cameras"][0]["distortion_coefficients"][0].isReal()) { + // TODO: find a way to read coeffs. trivial method as above does not work + depthIntrinsics.model = RS2_DISTORTION_MODIFIED_BROWN_CONRADY; + memset(depthIntrinsics.coeffs, 0, sizeof(depthIntrinsics.coeffs)); + } + else { + std::cout << "option [additional_cameras][0][distortion_coefficients][0] not found, breakinggg" << std::endl; + memset(depthIntrinsics.coeffs, 0, sizeof(depthIntrinsics.coeffs)); + depthIntrinsics.model = RS2_DISTORTION_MODIFIED_BROWN_CONRADY; + } + + std::cout << "depthIntrin: fx: " << depthIntrinsics.fx << " fy: " << depthIntrinsics.fy << " ppx: " << depthIntrinsics.ppx << " ppy: " << depthIntrinsics.ppy << '\n'; + + auto metaStream = ifstream(metaTxtPath.string()); std::string ph; metaStream >> ph >> scale; std::cout << "scale: " << scale << "\n"; @@ -52,13 +122,8 @@ bool MockD435iCamera::getImuToTime(double timestamp, std::vector MockD435iCamera::getColorIntrinsics() { - //TODO: once this class can read the intr.yaml, update this - //currently, this function should never be called, this should crash anything that calls this - std::vector vect{ -1, -1, -1, -1 }; - return vect; + return std::vector{colorIntrinsics.fx, colorIntrinsics.fy, colorIntrinsics.ppx, colorIntrinsics.ppy}; } const std::string MockD435iCamera::getModelName() const @@ -130,7 +192,7 @@ void MockD435iCamera::project(const cv::Mat &depth_frame, cv::Mat &xyz_map) } void MockD435iCamera::update(MultiCameraFrame::Ptr frame) -{ +{ std::string line; if (!std::getline(timestampStream, line)) { @@ -156,31 +218,23 @@ void MockD435iCamera::update(MultiCameraFrame::Ptr frame) fileNamess << std::setw(5) << std::setfill('0') << std::to_string(frameId) << ".png"; std::string fileName = fileNamess.str(); - std::vector pathList{infraredDir, infrared2Dir, depthDir, infraredDir, depthDir}; + std::vector pathList{infraredDir, infrared2Dir, depthDir, rgbDir, depthDir}; frame->images_.resize(pathList.size()); - // for (auto i = 0; i < pathList.size(); i++) - // { - // frame.images_[i] = loadImg(pathList[i] / fileName); - // } frame->images_[0] = cv::imread((pathList[0] / fileName).string(), cv::IMREAD_GRAYSCALE | cv::IMREAD_ANYDEPTH); frame->images_[1] = cv::imread((pathList[1] / fileName).string(), cv::IMREAD_GRAYSCALE | cv::IMREAD_ANYDEPTH); - frame->images_[3] = cv::imread((pathList[3] / fileName).string(), cv::IMREAD_GRAYSCALE | cv::IMREAD_ANYDEPTH); + frame->images_[3] = cv::imread((pathList[3] / fileName).string(), cv::IMREAD_COLOR); frame->images_[4] = cv::imread((pathList[4] / fileName).string(), cv::IMREAD_GRAYSCALE | cv::IMREAD_ANYDEPTH); // project the point cloud at 2 // TODO: should we mock the block time as well? //boost::this_thread::sleep_for(boost::chrono::milliseconds(33)); // TODO: not sure if this necessary - // printf("frame %d\n", frameId); - // std::cout << "RGB Size: " << frame.images_[3].total() << " type: " << frame.images_[3].type() << "\n"; - // std::cout << "DEPTH Size: " << frame.images_[4].total() << " type: " << frame.images_[4].type() << "\n"; frame->images_[2] = cv::Mat(cv::Size(width,height), CV_32FC3); project(frame->images_[4], frame->images_[2]); frame->images_[2] = frame->images_[2]*scale; - // std::cout << "Depth cloud: " << frame.images_[2].total() << "\n"; } } // namespace ark diff --git a/SegmentedMesh.cpp b/SegmentedMesh.cpp index 87666b54..b62000e2 100644 --- a/SegmentedMesh.cpp +++ b/SegmentedMesh.cpp @@ -79,8 +79,8 @@ namespace ark { } } - SegmentedMesh::SegmentedMesh(std::string& recon_config, OkvisSLAMSystem& slam, CameraSetup* camera, bool blocking/*= true*/) { - this->Setup(slam, camera); + SegmentedMesh::SegmentedMesh(std::string& recon_config, OkvisSLAMSystem& slam, CameraSetup& camera, bool blocking/*= true*/) { + this->SetupCallbacks(slam, camera); Initialize(recon_config, blocking); } @@ -123,7 +123,7 @@ namespace ark { } - void SegmentedMesh::Setup(OkvisSLAMSystem& slam, CameraSetup* camera) { + void SegmentedMesh::SetupCallbacks(OkvisSLAMSystem& slam, CameraSetup& camera) { FrameAvailableHandler updateFrameCounter([&, this] (MultiCameraFrame::Ptr frame) { if (this->frame_counter_ > 1000000) { @@ -165,8 +165,8 @@ namespace ark { slam.AddSparseMapMergeHandler(spmHandler, "mesh merge"); - std::vector intrinsics = camera->getColorIntrinsics(); - auto size = camera->getImageSize(); + std::vector intrinsics = camera.getColorIntrinsics(); + auto size = camera.getImageSize(); auto intr = open3d::camera::PinholeCameraIntrinsic(size.width, size.height, intrinsics[0], intrinsics[1], intrinsics[2], intrinsics[3]); diff --git a/SlamRecording.cpp b/SlamRecording.cpp index 12399c80..de0f00cb 100644 --- a/SlamRecording.cpp +++ b/SlamRecording.cpp @@ -63,7 +63,6 @@ int main(int argc, char **argv) path infrared2_path = directory_path / "infrared2/"; path rgb_path = directory_path / "rgb/"; path timestamp_path = directory_path / "timestamp.txt"; - path intrin_path = directory_path / "intrin.bin"; path meta_path = directory_path / "meta.txt"; path imu_path = directory_path / "imu.txt"; std::vector pathList{directory_path, depth_path, infrared_path, infrared2_path, rgb_path}; @@ -95,10 +94,6 @@ int main(int argc, char **argv) std::ofstream imu_ofs(imu_path.string()); std::ofstream timestamp_ofs(timestamp_path.string()); { - std::ofstream intrin_ofs(intrin_path.string()); - boost::archive::text_oarchive oa(intrin_ofs); - oa << camera.getDepthIntrinsics(); - std::ofstream meta_ofs(meta_path.string()); meta_ofs << "depth " << camera.getDepthScale(); } @@ -118,12 +113,12 @@ int main(int argc, char **argv) const auto &infrared = frame->images_[0]; const auto &infrared2 = frame->images_[1]; const auto &depth = frame->images_[4]; - //const auto &rgb = frame->images_[3]; + const auto &rgb = frame->images_[3]; saveImg(frameId, infrared, infrared_path); saveImg(frameId, infrared2, infrared2_path); saveImg(frameId, depth, depth_path); - //saveImg(frameId, rgb, rgb_path); + saveImg(frameId, rgb, rgb_path); timestamp_ofs << frameId << " " << std::setprecision(15) << frame->timestamp_ << "\n"; if(!quit) diff --git a/SlamReplaying.cpp b/SlamReplaying.cpp index 49a6e756..b90e87b4 100644 --- a/SlamReplaying.cpp +++ b/SlamReplaying.cpp @@ -26,9 +26,9 @@ int main(int argc, char **argv) std::signal(SIGABRT, signal_handler); std::signal(SIGSEGV, signal_handler); std::signal(SIGTERM, signal_handler); - if (argc > 5) + if (argc > 4) { - std::cerr << "Usage: ./" << argv[0] << " [configuration-yaml-file] [vocabulary-file] [skip-first-seconds] [data_path]" << std::endl + std::cerr << "Usage: ./" << argv[0] << " [configuration-yaml-file] [vocabulary-file] [data_path]" << std::endl << "Args given: " << argc << std::endl; return -1; } @@ -48,20 +48,14 @@ int main(int argc, char **argv) else vocabFilename = util::resolveRootPath("config/brisk_vocab.bn"); - okvis::Duration deltaT(0.0); - if (argc > 3) - { - deltaT = okvis::Duration(atof(argv[3])); - } + std::string savedDataPath; + if (argc > 3) savedDataPath = argv[3]; + else savedDataPath = "data"; - path dataPath{"./data_path_25-10-2019 16-47-28"}; - if (argc > 4) - { - dataPath = path(argv[4]); - } + path dataPath{ savedDataPath }; OkvisSLAMSystem slam(vocabFilename, configFilename); - + //setup display if (!MyGUI::Manager::init()) { @@ -71,7 +65,7 @@ int main(int argc, char **argv) printf("Camera initialization started...\n"); fflush(stdout); - MockD435iCamera camera(dataPath); + MockD435iCamera camera(dataPath, configFilename); printf("Camera-IMU initialization complete\n"); fflush(stdout); @@ -204,7 +198,7 @@ int main(int argc, char **argv) } catch (const std::exception &e) { - std::cerr << e.what() << '\n'; + std::cerr << e.what() << '\n'; } catch (...) { diff --git a/documentation/Ubuntu-18-build-instructions.md b/documentation/Ubuntu-18-build-instructions.md index 8b175fff..938a9c54 100644 --- a/documentation/Ubuntu-18-build-instructions.md +++ b/documentation/Ubuntu-18-build-instructions.md @@ -176,7 +176,7 @@ sudo make install ``` ## Installing Okvis+ -1. `git clone https://github.com/joemenke/okvis && cd okvis` +1. `git clone https://github.com/adamchang2000/okvis && cd okvis` 2. Apply the Fixing Eigen changes. 3. To run okvis demo, turn on the demo option in the CMakeListst.txt. `option (BUILD_APPS "Builds a demo app (which requires boost)" ON)` @@ -255,7 +255,6 @@ Example `./OpenARK_SLAM_demo "../mycam_intr.yaml" "../config/brisk_vocab.bn" "0. timestamp.txt imu.txt rgb - intrin.bin meta.txt Run as `./OpenARK_slam_recording "/path/to/save/data" "/path/to/camera_yaml_file"` @@ -265,9 +264,9 @@ Example `sudo ./OpenARK_slam_recording "../data" "../mycam_intr.yaml"` ### OpenARK_slam_replaying Run as `./OpenARK_slam_replaying "/path/to/camera_yaml_file path/to/brisk_vocab.bn" "0.0" "/path/to/data/collected/by/openark_slam_recording"` -Example `./OpenARK_slam_replaying "../mycam_intr.yaml" "../config/brisk_vocab.bn" "0.0" "../myroom4"` +Example `./OpenARK_slam_replaying "../mycam_intr.yaml" "../config/brisk_vocab.bn" "0.0" "../data_path"` -#### Dataset : [Google Drive](https://drive.google.com/drive/folders/1PyV8_0nDT9vURHWTvq-8RX9CV6gA2Abj?usp=sharing) +#### Dataset : [Google Drive](https://drive.google.com/drive/folders/1unSETlaCYssGTFtvpBCjkuKbC5Aq3P09) ### 3dRecon_Data_Recording Run as `./3dRecon_Data_Recording "/path/to/camera_yaml_file"` @@ -276,6 +275,14 @@ Example `./3dRecon_Data_Recording "../config/d435i_intr.yaml"` ## Yaml File Explained Please read the documentation of the individual parameters in the yaml file carefully. You have various options to trade-off accuracy and computational expense as well as to enable online calibration. +**Add these lines to your intrinsics yaml file** + + ``` + numKeypointsResetThreshold: 10 + durationResetThreshold: 0.5 + emitterPower: 0. + ``` + ## Installation Errors Linux/Ubuntu is case-sensitive. diff --git a/documentation/Windows-build-instructions-quick.md b/documentation/Windows-build-instructions-quick.md index ad1d57f2..a86c2779 100644 --- a/documentation/Windows-build-instructions-quick.md +++ b/documentation/Windows-build-instructions-quick.md @@ -32,14 +32,50 @@ modifying system variables. `cmake --build . --config Release` - **To run the OpenARK hand demo:** - `cd Release` - `OpenARK_hand_demo.exe` - -**To run the OpenARK SLAM demo:** - - `cd Release` - `OpenARK_SLAM_demo.exe` +## Running the OpenARK demo applications +- These applications will show you how core functionalities of OpenARK work +- These applications will also help you to verify successful installation of OpenARK. +- The demos apps are in `build` directory. +- If you do not want to create these demo files, you can do turn off building them in `CMakeLists.txt`. +`option( BUILD_HAND_DEMO "BUILD_HAND_DEMO" OFF )` + +### OpenARK_hand_demo +As a final sanity check, try running the demo executable in the build directory: +`./OpenARK-Hand-Demo` +You should see the hand detection and depth image windows. If you only see one window, drag it and see if the other is behind it. The static library is named: `libopenark_0_9_3.a`. + +### OpenARK_slam_demo +Run as `./OpenARK_SLAM_demo "/path/to/camera_intr.yaml" "../config/brisk_vocab.bn" "0.0"` + +Example `./OpenARK_SLAM_demo "../mycam_intr.yaml" "../config/brisk_vocab.bn" "0.0"` + +### OpenARK_slam_recording +#### Data Collected + depth + infrared + infrared2 + timestamp.txt + imu.txt + rgb + meta.txt + +Run as `./OpenARK_slam_recording "/path/to/save/data" "/path/to/camera_yaml_file"` + +Example `sudo ./OpenARK_slam_recording "../data" "../mycam_intr.yaml"` + +### OpenARK_slam_replaying +Run as `./OpenARK_slam_replaying "/path/to/camera_yaml_file path/to/brisk_vocab.bn" "0.0" "/path/to/data/collected/by/openark_slam_recording"` + +Example `./OpenARK_slam_replaying "../mycam_intr.yaml" "../config/brisk_vocab.bn" "0.0" "../data_path"` + +#### Dataset : [Google Drive](https://drive.google.com/drive/folders/1unSETlaCYssGTFtvpBCjkuKbC5Aq3P09) +### 3dRecon_Data_Recording +Run as `./3dRecon_Data_Recording "/path/to/camera_yaml_file"` + +Example `./3dRecon_Data_Recording "../config/d435i_intr.yaml"` + +## Yaml File Explained +Please read the documentation of the individual parameters in the yaml file carefully. You have various options to trade-off accuracy and computational expense as well as to enable online calibration. **Add these lines to your intrinsics yaml file** diff --git a/documentation/Windows-build-instructions.docx b/documentation/Windows-build-instructions.docx deleted file mode 100644 index 3ef0cc02..00000000 Binary files a/documentation/Windows-build-instructions.docx and /dev/null differ diff --git a/documentation/Windows-build-instructions.pdf b/documentation/Windows-build-instructions.pdf deleted file mode 100644 index c86d60bf..00000000 Binary files a/documentation/Windows-build-instructions.pdf and /dev/null differ diff --git a/documentation/windows-build-inst.md b/documentation/windows-build-inst.md index 24963db7..2766b9b8 100644 --- a/documentation/windows-build-inst.md +++ b/documentation/windows-build-inst.md @@ -144,7 +144,7 @@ Install DLoopDetector: Install Okvis+: -1. Clone source from https://github.com/joemenke/okvis/. Cd to the directory. +1. Clone source from https://github.com/adamchang2000/okvis/. Cd to the directory. 2. Follow CMAKE steps above. @@ -203,14 +203,50 @@ Install Open3D: `cmake --build . --config Release` - **To run the OpenARK hand demo:** - `cd Release` - `OpenARK_hand_demo.exe` +## Running the OpenARK demo applications +- These applications will show you how core functionalities of OpenARK work +- These applications will also help you to verify successful installation of OpenARK. +- The demos apps are in `build` directory. +- If you do not want to create these demo files, you can do turn off building them in `CMakeLists.txt`. +`option( BUILD_HAND_DEMO "BUILD_HAND_DEMO" OFF )` + +### OpenARK_hand_demo +As a final sanity check, try running the demo executable in the build directory: +`./OpenARK-Hand-Demo` +You should see the hand detection and depth image windows. If you only see one window, drag it and see if the other is behind it. The static library is named: `libopenark_0_9_3.a`. + +### OpenARK_slam_demo +Run as `./OpenARK_SLAM_demo "/path/to/camera_intr.yaml" "../config/brisk_vocab.bn" "0.0"` + +Example `./OpenARK_SLAM_demo "../mycam_intr.yaml" "../config/brisk_vocab.bn" "0.0"` + +### OpenARK_slam_recording +#### Data Collected + depth + infrared + infrared2 + timestamp.txt + imu.txt + rgb + meta.txt + +Run as `./OpenARK_slam_recording "/path/to/save/data" "/path/to/camera_yaml_file"` + +Example `sudo ./OpenARK_slam_recording "../data" "../mycam_intr.yaml"` + +### OpenARK_slam_replaying +Run as `./OpenARK_slam_replaying "/path/to/camera_yaml_file path/to/brisk_vocab.bn" "0.0" "/path/to/data/collected/by/openark_slam_recording"` + +Example `./OpenARK_slam_replaying "../mycam_intr.yaml" "../config/brisk_vocab.bn" "0.0" "../data_path"` + +#### Dataset : [Google Drive](https://drive.google.com/drive/folders/1unSETlaCYssGTFtvpBCjkuKbC5Aq3P09) +### 3dRecon_Data_Recording +Run as `./3dRecon_Data_Recording "/path/to/camera_yaml_file"` -**To run the OpenARK SLAM demo:** +Example `./3dRecon_Data_Recording "../config/d435i_intr.yaml"` - `cd Release` - `OpenARK_SLAM_demo.exe` +## Yaml File Explained +Please read the documentation of the individual parameters in the yaml file carefully. You have various options to trade-off accuracy and computational expense as well as to enable online calibration. **Add these lines to your intrinsics yaml file** diff --git a/include/CameraSetup.h b/include/CameraSetup.h index f5cc40a1..d0cf5418 100644 --- a/include/CameraSetup.h +++ b/include/CameraSetup.h @@ -20,6 +20,8 @@ namespace ark{ virtual cv::Size getImageSize() const =0; + virtual bool getImuToTime(double timestamp, std::vector>& data_out) =0; + }; //CameraSetup } //ark diff --git a/include/MockD435iCamera.h b/include/MockD435iCamera.h index 26d3dbc0..bfed1f66 100644 --- a/include/MockD435iCamera.h +++ b/include/MockD435iCamera.h @@ -26,7 +26,7 @@ namespace ark { /** * config the input dir */ - explicit MockD435iCamera(path dir); + explicit MockD435iCamera(path dir, std::string& configFilename); /** * Destructor @@ -43,7 +43,7 @@ namespace ark { */ cv::Size getImageSize() const override; - /** + /** * Dummy method */ void start() override; @@ -68,7 +68,6 @@ namespace ark { path imuTxtPath; path timestampTxtPath; path metaTxtPath; - path intrinFilePath; path depthDir; path rgbDir; path infraredDir; @@ -76,9 +75,11 @@ namespace ark { ifstream imuStream; ifstream timestampStream; rs2_intrinsics depthIntrinsics; + rs2_intrinsics colorIntrinsics; int firstFrameId; int width, height; time_t startTime; double scale; + std::string configFilename; }; } \ No newline at end of file diff --git a/include/SegmentedMesh.h b/include/SegmentedMesh.h index 66d13a51..ed42c622 100644 --- a/include/SegmentedMesh.h +++ b/include/SegmentedMesh.h @@ -36,7 +36,12 @@ namespace ark { class SegmentedMesh { public: - SegmentedMesh(std::string& recon_config, OkvisSLAMSystem& slam, CameraSetup* camera, bool blocking = true); + //passing in the camera and slam objects to setup online 3d reconstruction + //sets up SLAM callback function so it receives keyframes and integrates them automatically + SegmentedMesh(std::string& recon_config, OkvisSLAMSystem& slam, CameraSetup& camera, bool blocking = true); + + //initializes SegmentedMesh without any callbacks + //need to explicity call Integrate in order to integrate frames SegmentedMesh(std::string& recon_config); SegmentedMesh(); @@ -98,8 +103,8 @@ namespace ark { //initialize void Initialize(std::string& recon_config, bool blocking); - //setup sets up all callbacks - void Setup(OkvisSLAMSystem& slam, CameraSetup* camera); + //setup sets up all callbacks, performs integration with SLAM keyframes + void SetupCallbacks(OkvisSLAMSystem& slam, CameraSetup& camera); Eigen::Vector3i LocateBlock(const Eigen::Vector3d &point) { return Eigen::Vector3i((int)std::floor(point(0) / block_length_), diff --git a/openark-deps-vc14-win64.exe b/openark-deps-vc14-win64.exe index 8b6cce3e..6eaf9865 100644 Binary files a/openark-deps-vc14-win64.exe and b/openark-deps-vc14-win64.exe differ