diff --git a/3DReconDemo.cpp b/3DReconDemo.cpp new file mode 100644 index 00000000..96826c46 --- /dev/null +++ b/3DReconDemo.cpp @@ -0,0 +1,388 @@ +#include "D435iCamera.h" +#include "OkvisSLAMSystem.h" +#include +//#include +#include +#include "glfwManager.h" +#include "Util.h" +#include "SaveFrame.h" +#include "Types.h" +#include "Open3D/Integration/ScalableTSDFVolume.h" +#include "Open3D/Integration/MovingTSDFVolume.h" +#include "Open3D/Visualization/Utility/DrawGeometry.h" +#include "Open3D/IO/ClassIO/TriangleMeshIO.h" +#include "Open3D/IO/ClassIO/ImageIO.h" +#include + +using namespace ark; + +float voxel_size, block_size, max_depth; +bool save_frames; +int mesh_view_width, mesh_view_height; + +std::shared_ptr generateRGBDImageFromCV(cv::Mat color_mat, cv::Mat depth_mat) { + + int height = 480; + int width = 640; + + auto color_im = std::make_shared(); + color_im->Prepare(width, height, 3, sizeof(uint8_t)); + + uint8_t *pi = (uint8_t *)(color_im->data_.data()); + + for (int i = 0; i < height; i++) { + for (int k = 0; k < width; k++) { + + cv::Vec3b pixel = color_mat.at(i, k); + + *pi++ = pixel[0]; + *pi++ = pixel[1]; + *pi++ = pixel[2]; + } + } + + + auto depth_im = std::make_shared(); + depth_im->Prepare(width, height, 1, sizeof(uint16_t)); + + uint16_t * p = (uint16_t *)depth_im->data_.data(); + + for (int i = 0; i < height; i++) { + for (int k = 0; k < width; k++) { + *p++ = depth_mat.at(i, k); + } + } + + //change the second-to-last parameter here to set maximum distance + auto rgbd_image = open3d::geometry::RGBDImage::CreateFromColorAndDepth(*color_im, *depth_im, 1000.0, max_depth, false); + return rgbd_image; +} + +void readConfig(std::string& recon_config) { + + cv::FileStorage file(recon_config, cv::FileStorage::READ); + + std::cout << "Add entries to the intr.yaml to configure 3drecon parameters." << std::endl; + + if (file["Recon_VoxelSize"].isReal()) { + file["Recon_VoxelSize"] >> voxel_size; + } else { + std::cout << "option <3dRecon_VoxelSize> not found, setting to default 0.03" << std::endl; + voxel_size = 0.03; + } + + if (file["Recon_BlockSize"].isReal()) { + file["Recon_BlockSize"] >> block_size; + } else { + std::cout << "option <3dRecon_BlockSize> not found, setting to default 2.0" << std::endl; + block_size = 2.0; + } + + if (file["Recon_MaxDepth"].isReal()) { + file["Recon_MaxDepth"] >> max_depth; + } else { + std::cout << "option <3dRecon_MaxDepth> not found, setting to default 2.5" << std::endl; + max_depth = 2.5; + } + + if (file["Recon_SaveFrames"].isInt()) { + int save; + file["Recon_SaveFrames"] >> save; + save_frames = (bool)save; + } else { + std::cout << "option <3dRecon_SaveFrames> not found, setting to default true" << std::endl; + save_frames = true; + } + + if (file["Recon_MeshWinWidth"].isInt()) { + file["Recon_MeshWinWidth"] >> mesh_view_width; + } else { + std::cout << "option <3dRecon_MeshWinWidth> not found, setting to default 1000px" << std::endl; + mesh_view_width = 1000; + } + + if (file["Recon_MeshWinHeight"].isInt()) { + file["Recon_MeshWinHeight"] >> mesh_view_height; + } else { + std::cout << "option <3dRecon_MeshWinHeight> not found, setting to default 1000px" << std::endl; + mesh_view_height = 1000; + } + +} + +int main(int argc, char **argv) +{ + + if (argc > 5) { + std::cerr << "Usage: ./" << argv[0] << " [configuration-yaml-file] [vocabulary-file] [frame output directory]" << 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]; + else configFilename = util::resolveRootPath("config/d435i_intr.yaml"); + + std::string vocabFilename; + if (argc > 2) vocabFilename = argv[2]; + else vocabFilename = util::resolveRootPath("config/brisk_vocab.bn"); + + std::string frameOutput; + if (argc > 3) frameOutput = argv[3]; + else frameOutput = "./frames/"; + + OkvisSLAMSystem slam(vocabFilename, configFilename); + + readConfig(configFilename); + + cv::namedWindow("image", cv::WINDOW_AUTOSIZE); + + SaveFrame * saveFrame = new SaveFrame(frameOutput); + + //setup display + if (!MyGUI::Manager::init()) + { + fprintf(stdout, "Failed to initialize GLFW\n"); + return -1; + } + + printf("Camera initialization started...\n"); + fflush(stdout); + D435iCamera camera; + camera.start(); + + printf("Camera-IMU initialization complete\n"); + fflush(stdout); + + //run until display is closed + okvis::Time start(0.0); + int id = 0; + + + + int frame_counter = 1; + bool do_integration = true; + + KeyFrameAvailableHandler saveFrameHandler([&saveFrame, &frame_counter, &do_integration](MultiCameraFrame::Ptr frame) { + if (!do_integration || frame_counter % 3 != 0) { + return; + } + + cv::Mat imRGB; + cv::Mat imDepth; + + frame->getImage(imRGB, 3); + + frame->getImage(imDepth, 4); + + Eigen::Matrix4d transform(frame->T_WC(3)); + + saveFrame->frameWrite(imRGB, imDepth, transform, frame->frameId_); + }); + + if (save_frames) { + slam.AddKeyFrameAvailableHandler(saveFrameHandler, "saveframe"); + } + + open3d::integration::MovingTSDFVolume * tsdf_volume = new open3d::integration::MovingTSDFVolume(voxel_size, voxel_size * 5, open3d::integration::TSDFVolumeColorType::RGB8, block_size); + + std::vector intrinsics = camera.getColorIntrinsics(); + auto intr = open3d::camera::PinholeCameraIntrinsic(640, 480, intrinsics[0], intrinsics[1], intrinsics[2], intrinsics[3]); + + FrameAvailableHandler tsdfFrameHandler([&tsdf_volume, &frame_counter, &do_integration, intr](MultiCameraFrame::Ptr frame) { + if (!do_integration || frame_counter % 3 != 0) { + return; + } + + cout << "Integrating frame number: " << frame->frameId_ << endl; + + cv::Mat color_mat; + cv::Mat depth_mat; + + + frame->getImage(color_mat, 3); + frame->getImage(depth_mat, 4); + + auto rgbd_image = generateRGBDImageFromCV(color_mat, depth_mat); + + tsdf_volume->Integrate(*rgbd_image, intr, frame->T_WC(3).inverse()); + }); + + slam.AddFrameAvailableHandler(tsdfFrameHandler, "tsdfframe"); + + MyGUI::MeshWindow mesh_win("Mesh Viewer", mesh_view_width, mesh_view_height); + MyGUI::Mesh mesh_obj("mesh"); + + mesh_win.add_object(&mesh_obj); + + std::vector, + Eigen::Matrix4d>> vis_mesh; + + FrameAvailableHandler meshHandler([&tsdf_volume, &frame_counter, &do_integration, &vis_mesh, &mesh_obj](MultiCameraFrame::Ptr frame) { + if (!do_integration || frame_counter % 30 != 1) { + return; + } + + vis_mesh = tsdf_volume->GetTriangleMeshes(); + + printf("new mesh extracted, sending to mesh obj\n"); + + int number_meshes = mesh_obj.get_number_meshes(); + + //only update active mesh + if (number_meshes == vis_mesh.size()) { + auto active_mesh = vis_mesh[vis_mesh.size() - 1]; + mesh_obj.update_active_mesh(active_mesh.first->vertices_, active_mesh.first->vertex_colors_, active_mesh.first->triangles_, active_mesh.second); + + std::vector mesh_transforms; + + for (int i = 0; i < vis_mesh.size(); ++i) { + mesh_transforms.push_back(vis_mesh[i].second); + } + + mesh_obj.update_transforms(mesh_transforms); + } + else { + std::vector> mesh_vertices; + std::vector> mesh_colors; + std::vector> mesh_triangles; + std::vector mesh_transforms; + + for (int i = 0; i < vis_mesh.size(); ++i) { + auto mesh = vis_mesh[i]; + mesh_vertices.push_back(mesh.first->vertices_); + mesh_colors.push_back(mesh.first->vertex_colors_); + mesh_triangles.push_back(mesh.first->triangles_); + mesh_transforms.push_back(mesh.second); + } + + + mesh_obj.update_mesh_vector(mesh_vertices, mesh_colors, mesh_triangles, mesh_transforms); + } + + + }); + + slam.AddFrameAvailableHandler(meshHandler, "meshupdate"); + + FrameAvailableHandler viewHandler([&mesh_obj, &tsdf_volume, &mesh_win, &frame_counter, &do_integration](MultiCameraFrame::Ptr frame) { + Eigen::Affine3d transform(frame->T_WC(3)); + mesh_obj.set_transform(transform.inverse()); + if (mesh_win.clicked()) { + do_integration = !do_integration; + if (do_integration) { + std::cout << "----INTEGRATION ENABLED----" << endl; + } + else { + std::cout << "----INTEGRATION DISABLED----" << endl; + } + } + }); + + slam.AddFrameAvailableHandler(viewHandler, "viewhandler"); + + KeyFrameAvailableHandler updateKFHandler([&tsdf_volume](MultiCameraFrame::Ptr frame) { + MapKeyFrame::Ptr kf = frame->keyframe_; + tsdf_volume->SetLatestKeyFrame(kf->T_WC(3), kf->frameId_); + }); + + slam.AddKeyFrameAvailableHandler(updateKFHandler, "updatekfhandler"); + + LoopClosureDetectedHandler loopHandler([&tsdf_volume, &slam, &frame_counter](void) { + + printf("loop closure detected\n"); + + std::vector frameIdOut; + std::vector traj; + + slam.getMappedTrajectory(frameIdOut, traj); + + std::map keyframemap; + + for (int i = 0; i < frameIdOut.size(); i++) { + keyframemap.insert(std::pair(frameIdOut[i], traj[i])); + } + + tsdf_volume->UpdateKeyFrames(keyframemap); + }); + + slam.AddLoopClosureDetectedHandler(loopHandler, "loophandler"); + + cv::namedWindow("image"); + + while (MyGUI::Manager::running()) { + + //Update the display + MyGUI::Manager::update(); + + //Get current camera frame + MultiCameraFrame::Ptr frame(new MultiCameraFrame); + camera.update(*frame); + + //Get or wait for IMU Data until current frame + std::vector imuData; + camera.getImuToTime(frame->timestamp_, imuData); + + //Add data to SLAM system + slam.PushIMU(imuData); + slam.PushFrame(frame); + + + frame_counter++; + + cv::Mat imRGB; + frame->getImage(imRGB, 3); + + cv::Mat imBGR; + + cv::cvtColor(imRGB, imBGR, CV_RGB2BGR); + + cv::imshow("image", imBGR); + + int k = cv::waitKey(2); + + if (k == 'q' || k == 'Q' || k == 27) break; // 27 is ESC + + } + + cout << "updating transforms" << endl; + + std::vector frameIdOut; + std::vector traj; + + slam.getMappedTrajectory(frameIdOut, traj); + + std::map keyframemap; + + for (int i = 0; i < frameIdOut.size(); i++) { + keyframemap.insert(std::pair(frameIdOut[i], traj[i])); + } + + saveFrame->updateTransforms(keyframemap); + + cout << "getting mesh" << endl; + + //make sure to add these back later | + + std::shared_ptr write_mesh = tsdf_volume->ExtractTotalTriangleMesh(); + + //const std::vector> mesh_vec = { mesh }; + + //open3d::visualization::DrawGeometries(mesh_vec); + + open3d::io::WriteTriangleMeshToPLY("mesh.ply", *write_mesh, false, false, true, true, false, false); + + printf("\nTerminate...\n"); + // Clean up + slam.ShutDown(); + printf("\nExiting...\n"); + return 0; +} diff --git a/CMakeLists.txt b/CMakeLists.txt index 8fdcd966..5c825fb7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -19,6 +19,7 @@ set( LIB_NAME "OpenARK" ) 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( DATA_RECORDING_NAME "OpenARK_data_recording") set( SLAM_RECORDING_NAME "OpenARK_slam_recording") set( SLAM_REPLAYING_NAME "OpenARK_slam_replaying") @@ -28,11 +29,12 @@ set( UNITY_PLUGIN_NAME "UnityPlugin" ) option( BUILD_HAND_DEMO "BUILD_HAND_DEMO" OFF ) option( BUILD_AVATAR_DEMO "BUILD_AVATAR_DEMO" OFF) option( BUILD_SLAM_DEMO "BUILD_SLAM_DEMO" ON) +option( BUILD_3DRECON_DEMO "BUILD_3DRECON_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) option( BUILD_TESTS "BUILD_TESTS" OFF ) -option( BUILD_UNITY_PLUGIN "BUILD_UNITY_PLUGIN" OFF ) +option( BUILD_UNITY_PLUGIN "BUILD_UNITY_PLUGIN" ON ) option( USE_AZURE_KINECT_SDK "USE_AZURE_KINECT_SDK" OFF ) option( USE_RSSDK2 "USE_RSSDK2" ON ) option( USE_RSSDK "USE_RSSDK" OFF ) @@ -136,11 +138,11 @@ include_directories(${OKVIS_INCLUDE_DIRS}) message(STATUS "Found Okvis: ${OKVIS_INCLUDE_DIRS} ${OKVIS_LIBRARIES}") # require OpenGV (should come with Okvis) -#if( DEFINED ENV{ARK_DEPS_DIR} ) -# find_package( OpenGV REQUIRED NO_MODULE ) -#else() +if( DEFINED ENV{ARK_DEPS_DIR} ) + find_package( OpenGV REQUIRED NO_MODULE ) +else() find_package( OpenGV REQUIRED ) -#endif( DEFINED ENV{ARK_DEPS_DIR} ) +endif( DEFINED ENV{ARK_DEPS_DIR} ) IF(OPENGV_FOUND) MESSAGE(STATUS "Found OpenGV: ${OPENGV_INCLUDE_DIR} ${OPENGV_LIBRARIES}") ENDIF(OPENGV_FOUND) @@ -171,6 +173,12 @@ find_package( brisk REQUIRED ) include_directories(${BRISK_INCLUDE_DIRS}) message(STATUS "Found Brisk: ${BRISK_INCLUDE_DIRS} ${BRISK_LIBRARIES}") +# require Open3D +find_package( Open3D REQUIRED ) +include_directories(${Open3D_INCLUDE_DIRS}) +link_directories(${Open3D_LIBRARY_DIRS}) +message(STATUS "Found Open3D: ${Open3D_INCLUDE_DIRS} ${Open3D_LIBRARIES}") + set( _FOUND_CAMERA_LIB OFF ) set( k4a_FOUND OFF ) if( USE_AZURE_KINECT_SDK ) @@ -219,12 +227,14 @@ set( ${OpenCV_LIBRARIES} ${PCL_LIBRARIES} ${GLOG_LIBRARIES} #${CXSPARSE_LIBRARIES} - #${SuiteSparse_LIBRARIES} + ${SuiteSparse_LIBRARIES} ${OKVIS_LIBRARIES} ${CERES_LIBRARIES} ${BRISK_LIBRARIES} ${DBoW2_LIBRARIES} ${OPENGV_LIBRARIES} + ${Boost_LIBRARIES} + ${Open3D_LIBRARIES} ) add_definitions( @@ -251,12 +261,13 @@ set( HandDetector.cpp PlaneDetector.cpp MockCamera.cpp + MockD435iCamera.cpp HumanDetector.cpp HumanBody.cpp Avatar.cpp glfwManager.cpp OkvisSLAMSystem.cpp - MockD435iCamera.cpp + SaveFrame.cpp ) set( @@ -278,6 +289,7 @@ set( ${INCLUDE_DIR}/HandDetector.h ${INCLUDE_DIR}/PlaneDetector.h ${INCLUDE_DIR}/MockCamera.h + ${INCLUDE_DIR}/MockD435iCamera.h ${INCLUDE_DIR}/HumanDetector.h ${INCLUDE_DIR}/HumanBody.h ${INCLUDE_DIR}/Avatar.h @@ -292,7 +304,7 @@ set( ${INCLUDE_DIR}/Types.h ${INCLUDE_DIR}/CorrespondenceRansac.h ${INCLUDE_DIR}/UKF.h - ${INCLUDE_DIR}/MockD435iCamera.h + ${INCLUDE_DIR}/SaveFrame.h stdafx.h ) @@ -451,6 +463,19 @@ if( ${BUILD_SLAM_DEMO} AND realsense2_FOUND ) set_target_properties( d435i_intrinsics_writer PROPERTIES COMPILE_FLAGS ${TARGET_COMPILE_FLAGS} ) endif( ${BUILD_SLAM_DEMO} AND realsense2_FOUND ) +if( ${BUILD_3DRECON_DEMO} AND realsense2_FOUND ) + add_executable( ${3DRECON_DATA_NAME} 3DReconDemo.cpp ) + target_include_directories( ${3DRECON_DATA_NAME} PRIVATE ${INCLUDE_DIR} ) + set_target_properties( ${3DRECON_DATA_NAME} PROPERTIES OUTPUT_NAME ${3DRECON_DATA_NAME} ) + set_target_properties( ${3DRECON_DATA_NAME} PROPERTIES COMPILE_FLAGS ${TARGET_COMPILE_FLAGS} ) + if ( MSVC ) + target_link_libraries( ${3DRECON_DATA_NAME} ${DEPENDENCIES} ${LIB_NAME} ) + else() + target_link_libraries( ${3DRECON_DATA_NAME} ${DEPENDENCIES} ${LIB_NAME} GLU GL glut ) + 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/D435iCamera.cpp b/D435iCamera.cpp index f36846e5..0a47e52d 100644 --- a/D435iCamera.cpp +++ b/D435iCamera.cpp @@ -17,6 +17,14 @@ namespace ark { cameraParameter(parameter), last_ts_g(0), kill(false) { //Setup camera //TODO: Make read from config file + + + //rs400::advanced_mode advanced_device(camera.getDevice()); + //auto depth_table = advanced_device.get_depth_table(); + //depth_table.depthClampMax = 1300; // 1m30 if depth unit at 0.001 + //advanced_device.set_depth_table(depth_table); + + rs2::context ctx; device = ctx.query_devices().front(); width = cameraParameter.width; @@ -35,6 +43,12 @@ namespace ark { depth_sensor = new rs2::depth_sensor(device.first()); scale = depth_sensor->get_option(RS2_OPTION_DEPTH_UNITS); + + + + + //depth_sensor->set_option(RS2_OPTION_MAX_DISTANCE, 16.0f); + rs2::sensor color_sensor = device.query_sensors()[1]; color_sensor.set_option(RS2_OPTION_ENABLE_AUTO_EXPOSURE,false); @@ -65,6 +79,8 @@ namespace ark { auto depthStream = selection.get_stream(RS2_STREAM_DEPTH) .as(); depthIntrinsics = depthStream.get_intrinsics(); + colorIntrinsics = selection.get_stream(RS2_STREAM_COLOR) + .as().get_intrinsics(); motion_pipe = std::make_shared(); rs2::pipeline_profile selection_motion = motion_pipe->start(motion_config); @@ -99,10 +115,14 @@ namespace ark { sensor.set_option((rs2_option)global_time_option, false); } } - + align_to_color = new rs2::align(RS2_STREAM_COLOR); imuReaderThread_ = std::thread(&D435iCamera::imuReader, this); } + std::vector D435iCamera::getColorIntrinsics() { + return std::vector{colorIntrinsics.fx, colorIntrinsics.fy, colorIntrinsics.ppx, colorIntrinsics.ppy}; + } + void D435iCamera::imuReader(){ while(!kill){ auto frames = motion_pipe->wait_for_frames(); @@ -188,16 +208,29 @@ namespace ark { if (frame.images_[1].empty()) frame.images_[1] = cv::Mat(cv::Size(width,height), CV_8UC1); std::memcpy( frame.images_[1].data, infrared2.get_data(),width * height); + if (frame.images_[2].empty()) frame.images_[2] = cv::Mat(cv::Size(width,height), CV_32FC3); project(depth, frame.images_[2]); frame.images_[2] = frame.images_[2]*scale; //depth is in mm by default + auto aligned_frames = align_to_color->process(frames); + auto aligned_depth = aligned_frames.get_depth_frame(); + + if (frame.images_[4].empty()) frame.images_[4] = cv::Mat(cv::Size(width, height), CV_16UC1, (void*)aligned_depth.get_data(), cv::Mat::AUTO_STEP); + if (frame.images_[3].empty()) frame.images_[3] = cv::Mat(cv::Size(width,height), CV_8UC3); std::memcpy( frame.images_[3].data, color.get_data(),3 * width * height); - if (frame.images_[4].empty()) frame.images_[4] = cv::Mat(cv::Size(width,height), CV_16UC1); - // 16 bits = 2 bytes - std::memcpy(frame.images_[4].data, depth.get_data(),width * height * 2); + //FILTER OUT ALL POINTS CLOSER THAN min_dist AND FARTHER THAN max_dist + int min_dist = 200; + int max_dist = 6000; + for (int i = 0; i < width; i++) { + for (int k = 0; k < height; k++) { + if (frame.images_[4].at(k, i) < min_dist || frame.images_[4].at(k, i) > max_dist) { + frame.images_[4].at(k, i) = 0; + } + } + } } catch (std::runtime_error e) { // Try reconnecting diff --git a/DEPENDENCIES.md b/DEPENDENCIES.md index 3049630e..32b0bd34 100644 --- a/DEPENDENCIES.md +++ b/DEPENDENCIES.md @@ -20,4 +20,6 @@ ### nanoflann is BSD Licensed +### Open3D is MIT Licensed + ### Note: if you wish to use the SMPL model for the Avatar demo, please refer to the SMPL website for licence and restrictions: http://smpl.is.tue.mpg.de/license_model diff --git a/OkvisSLAMSystem.cpp b/OkvisSLAMSystem.cpp index 7070d732..fe9db0a1 100644 --- a/OkvisSLAMSystem.cpp +++ b/OkvisSLAMSystem.cpp @@ -335,5 +335,9 @@ namespace ark { } } + void OkvisSLAMSystem::getMappedTrajectory(std::vector& frameIdOut, std::vector& trajOut) { + sparseMap_.getMappedTrajectory(frameIdOut, trajOut); + } + } //ark \ No newline at end of file diff --git a/README.md b/README.md index b5906229..f5912843 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # OpenARK -OpenARK is an open-source wearable augmented reality (AR) system founded at UC Berkeley in 2016. The C++ based software offers innovative core functionalities to power a wide range of off-the-shelf AR components, including see-through glasses, depth cameras, and IMUs. The open-source platform includes higher-level modules to aid human-computer interaction, such as 3D gesture recognition, plane detection, avatar/pose tracking, and multi-user collaboration, and also contains fundamental tools such as AR-based camera calibration, depth-to-stereo, and SLAM, and it also. Currently, it supports both PMD Pico Flexx and Intel RealSense SR300 cameras. The project builds natively on both Windows and Linux. +OpenARK is an open-source wearable augmented reality (AR) system founded at UC Berkeley in 2016. The C++ based software offers innovative core functionalities to power a wide range of off-the-shelf AR components, including see-through glasses, depth cameras, and IMUs. The open-source platform includes higher-level modules to aid human-computer interaction, such as 3D gesture recognition, plane detection, avatar/pose tracking, and multi-user collaboration, and also contains fundamental tools such as AR-based camera calibration, depth-to-stereo, and SLAM, and 3D Reconstruction. Currently, it supports both PMD Pico Flexx and Intel RealSense SR300 cameras. The project builds natively on both Windows and Linux. At a Glance @@ -97,7 +97,7 @@ Code used to run the demo video is included in HandDemo.cpp. Additional sample c ## Using OpenARK SLAM -Currently OpenARK only supports the Intel Realsense D435i Camera for use with OpenARK SLAM. OpenARK SLAM requires two configuration files to run, a camera intrinsicis file, and a vocab file. Examples of both files can be found in the config folder. To run the OpenARK SLAM Demo run: +Currently OpenARK only supports the Intel Realsense D435i Camera for use with OpenARK SLAM. OpenARK SLAM requires two configuration files to run, a camera intrinsics file, and a vocab file. Examples of both files can be found in the config folder. To run the OpenARK SLAM Demo run: `OpenARK_SLAM_demo.exe ` @@ -109,6 +109,22 @@ This will generate a complete intrinsics file named `_intr.yaml` OpenARK SLAM heavily utilizes the open source packages DBoW2, Okvis, and Ceres. Please respect their Licences and credit/cite when appropriate. +## Using OpenARK 3D Reconstruction + +Currently OpenARK only supports the Intel Realsense D435i Camera for use with OpenARK 3D Reconstruction. OpenARK 3D Reconstruction requires two configuration files to run, a camera intrinsics file, and a vocab file. Examples of both files can be found in the config folder. It also accepts a frame output directory, which defaults to `/frames/`. To run the OpenARK 3D Reconstruction Demo run: + +`3dRecon_Data_Recording.exe ` + +While the vocab file provided should work for most purposes, for best performance a custom intrinsics file should be generated for each camera. You can generate a custom intrinsics file for your camera by running + +`d435i_intrinsics_writer ` + +This will generate a complete intrinsics file named `_intr.yaml` + +Offline reconstruction can be performed on the recorded data output of the application using the Python script located in `/scripts/OfflineReconstruction.py`. + +OpenARK 3D Reconstruction heavily utilizes the open source packages DBoW2, Okvis, Open3D, and Ceres. Please respect their Licences and credit/cite when appropriate. + ## Known issues OpenCV prior to 3.2.0 does not offer prebuilt VC14+ binaries. Running VC12 OpenCV binaries with VC14 will result in memories errors in findCountours(). If you are using VC12+ to compile OpenARK, you will need to use CMake to rebuilt OpenCV from source. diff --git a/RS2Camera.cpp b/RS2Camera.cpp index a78e2e07..43ca7d48 100644 --- a/RS2Camera.cpp +++ b/RS2Camera.cpp @@ -16,6 +16,28 @@ namespace ark { badInputFlag = false; rs2::pipeline_profile profile = pipe->start(config); + // Editing laser intensity + rs2::device selected_device = profile.get_device(); + auto depth_sensor = selected_device.first(); + + printf("\nNote: \nSet laser power and exposure in RS2Camera.cpp \n"); + + if (depth_sensor.supports(RS2_OPTION_LASER_POWER)) { + // Use range * (percent float) to adjust range percentage wise + // auto range = depth_sensor.get_option_range(RS2_OPTION_LASER_POWER); + + float laser_power = 225.f; + printf("Setting laser power to -> %f \n", laser_power); + depth_sensor.set_option(RS2_OPTION_LASER_POWER, laser_power); + } + + // Editing exposure + if (depth_sensor.supports(RS2_OPTION_EXPOSURE)) { + float exposure = 6000.f; + printf("Setting exposure to -> %f \n", exposure); + depth_sensor.set_option(RS2_OPTION_EXPOSURE, exposure); + } + // get updated intrinsics rgbIntrinsics = new rs2_intrinsics(); d2rExtrinsics = new rs2_extrinsics(); @@ -102,7 +124,7 @@ namespace ark { } else { rs2::frame depth = frameset.first(RS2_STREAM_DEPTH); - rs2::frame ir = frameset.first(RS2_STREAM_INFRARED); + rs2::frame ir = frameset.get_infrared_frame(1); memcpy(ir_map.data, ir.get_data(), width * height); project(depth, ir, xyz_map, ir_map); } @@ -188,6 +210,6 @@ namespace ark { config.enable_stream(RS2_STREAM_DEPTH, width, height, RS2_FORMAT_Z16); if (useRGBStream) config.enable_stream(RS2_STREAM_COLOR, width, height, RS2_FORMAT_BGR8); - else config.enable_stream(RS2_STREAM_INFRARED, width, height, RS2_FORMAT_Y8); + else config.enable_stream(RS2_STREAM_INFRARED, 1, width, height, RS2_FORMAT_Y8); } } \ No newline at end of file diff --git a/SaveFrame.cpp b/SaveFrame.cpp new file mode 100644 index 00000000..ed8e36c4 --- /dev/null +++ b/SaveFrame.cpp @@ -0,0 +1,147 @@ +// +// Created by yiwen on 2/2/19. +// +/* +Esther commented +- Enables online frame writing to folder for offline reconstruction later +- Enables offline file access +- Requires onKeyFrameAvailable handler in SLAM +- Used in rgbd_realsense_d435, rgbd_realsense_load_gl, rgbd_realsense_load_categorized +- TCW from ORBSLAM is World to Camera Transform. +*/ + +#include +#include +#include +#include +#include + +//#include +//#include +//#include +// #include +#include +#include "SaveFrame.h" +#include "Types.h" + +namespace ark { + + void createFolder(std::string folderPath){ + mkdir(folderPath.c_str()); + std::cout << folderPath << "dir made" << std::endl; + } + + + SaveFrame::SaveFrame(std::string folderPath) { + + createFolder(folderPath); + + rgbPath = folderPath +"RGB/"; + depthPath = folderPath +"depth/"; + tcwPath = folderPath +"tcw/"; + + createFolder(rgbPath); + createFolder(depthPath); + createFolder(tcwPath); + + } + + void SaveFrame::frameWrite(cv::Mat imRGB, cv::Mat depth, Eigen::Matrix4d traj, int frameId){ + + frame_ids.push_back(frameId); + + cv::Mat imBGR; + cv::cvtColor(imRGB, imBGR, CV_RGB2BGR); + cv::imwrite(rgbPath + std::to_string(frameId) + ".jpg", imBGR); + + cv::imwrite(depthPath + std::to_string(frameId) + ".png", depth); + + std::ofstream file(tcwPath + std::to_string(frameId) + ".txt"); + if (file.is_open()) + { + file << traj.matrix() << '\n'; + } + file.close(); + } + + void SaveFrame::updateTransforms(std::map keyframemap) { + + printf("updating transforms inside file\n"); + + for (int frame_id : frame_ids) { + + if (!keyframemap.count(frame_id)) + continue; + + std::ofstream file1(tcwPath + std::to_string(frame_id) + ".txt"); + if (file1.is_open()) + { + file1 << keyframemap[frame_id].matrix() << '\n'; + } + file1.close(); + } + } + + RGBDFrame SaveFrame::frameLoad(int frameId){ + std::cout<<"frameLoad start = "<< frameId <> frame.mTcw.at(i,k); + } + } + file.close(); + + + + if(frame.mTcw.rows == 0) { + std::cout<<"frameLoad tcw fail = "<< frameId <display(); @@ -364,17 +364,17 @@ void Object::del(){ } } -void Object::display(){ - std::lock_guard guard(displayLock_); - if(draw){ - glPushMatrix(); - Eigen::Matrix4d scene_mat; - glGetDoublev(GL_MODELVIEW_MATRIX, scene_mat.data()); - scene_mat = scene_mat*pose.matrix(); - glLoadMatrixd(scene_mat.data()); - draw_obj(); - glPopMatrix(); - } +void Object::display() { + std::lock_guard guard(displayLock_); + if (draw) { + glPushMatrix(); + Eigen::Matrix4d scene_mat; + glGetDoublev(GL_MODELVIEW_MATRIX, scene_mat.data()); + scene_mat = scene_mat*pose.matrix(); + glLoadMatrixd(scene_mat.data()); + draw_obj(); + glPopMatrix(); + } } void Object::hide(){ @@ -564,5 +564,85 @@ void Path::draw_obj() glEnable(GL_LIGHTING); } +void Mesh::draw_obj() +{ + std::lock_guard guard(meshLock_); + + Eigen::Matrix4d scene_mat; + glGetDoublev(GL_MODELVIEW_MATRIX, scene_mat.data()); + scene_mat = scene_mat*pose.matrix().inverse(); + glLoadMatrixd(scene_mat.data()); + + /*Eigen::AngleAxis R(pose.rotation()); + Eigen::Translation3d T(pose.translation()); + + + glRotated(180, 0, 1, 0); + glRotated(180, 0, 0, 1); + + + glTranslated(T.x(), T.y(), T.z()); + glRotated(R.angle() * 180 / 3.14159, R.axis().x(), R.axis().y(), R.axis().z());*/ + + + for (int i = 0; i < mesh_vertices.size(); ++i) { + std::vector vertices = mesh_vertices[i]; + std::vector colors = mesh_colors[i]; + std::vector triangles = mesh_triangles[i]; + Eigen::Affine3d transform(mesh_transforms[i]); + + transform = pose * transform; + //mesh_transforms[i] is c->w, we need w->c + + glPushMatrix(); + + Eigen::AngleAxis R(transform.rotation()); + Eigen::Translation3d T(transform.translation()); + + + glRotated(180, 0, 1, 0); + glRotated(180, 0, 0, 1); + + + glTranslated(T.x(), T.y(), T.z()); + glRotated(R.angle() * 180 / 3.14159, R.axis().x(), R.axis().y(), R.axis().z()); + + + glDisable(GL_LIGHTING); + + glBegin(GL_TRIANGLES); + + for (int i = 0; i < triangles.size(); i++) { + + Eigen::Vector3d vertex1 = vertices[triangles[i][0]]; + Eigen::Vector3d vertex2 = vertices[triangles[i][1]]; + Eigen::Vector3d vertex3 = vertices[triangles[i][2]]; + + Eigen::Vector3d color1 = colors[triangles[i][0]]; + Eigen::Vector3d color2 = colors[triangles[i][1]]; + Eigen::Vector3d color3 = colors[triangles[i][2]]; + + + glColor3f((color1[0] + color2[0] + color3[0]) / 3.0f, + (color1[1] + color2[1] + color3[1]) / 3.0f, + (color1[2] + color2[2] + color3[2]) / 3.0f); + + glVertex3f(vertex1[0], vertex1[1], vertex1[2]); + glVertex3f(vertex2[0], vertex2[1], vertex2[2]); + glVertex3f(vertex3[0], vertex3[1], vertex3[2]); + } + + glEnd(); + + glEnable(GL_LIGHTING); + + + glPopMatrix(); + + + } + +} + }//MyGUI diff --git a/include/D435iCalibWriter.h b/include/D435iCalibWriter.h index 2d88e523..f0a9f809 100644 --- a/include/D435iCalibWriter.h +++ b/include/D435iCalibWriter.h @@ -161,6 +161,13 @@ namespace ark{ << "useDriver" << "false" << "enableLoopClosureDetection" << true + << "Recon_VoxelSize" << 0.01 + << "Recon_BlockSize" << 2.0 + << "Recon_MaxDepth" << 2.0 + << "Recon_SaveFrames" << true + << "Recon_MeshWinWidth" << 1000 + << "Recon_MeshWinHeight" << 1000 + << "publishing_options" << "{" << "publish_rate" << 200 // rate at which odometry updates are published only works properly if imu_rate/publish_rate is an integer!! << "publishLandmarks" << "false" // select, if you want to publish landmarks at all diff --git a/include/D435iCamera.h b/include/D435iCamera.h index 8c6f749b..bc70b72e 100644 --- a/include/D435iCamera.h +++ b/include/D435iCamera.h @@ -56,6 +56,8 @@ namespace ark { bool getImuToTime(double timestamp, std::vector& data_out); + std::vector getColorIntrinsics(); + const rs2_intrinsics &getDepthIntrinsics(); double getDepthScale(); @@ -72,8 +74,10 @@ namespace ark { std::shared_ptr pipe; std::shared_ptr motion_pipe; + std::shared_ptr color_depth_pipe; rs2::config config; rs2::config motion_config; + rs2::config color_depth_config; rs2::depth_sensor* depth_sensor; rs2::device device; rs2_intrinsics depthIntrinsics; @@ -86,5 +90,9 @@ namespace ark { double last_ts_g; bool badInputFlag; std::atomic kill; + + rs2::align * align_to_color; + rs2_intrinsics colorIntrinsics; + }; } diff --git a/include/OkvisSLAMSystem.h b/include/OkvisSLAMSystem.h index ae45d987..86a7dc8b 100644 --- a/include/OkvisSLAMSystem.h +++ b/include/OkvisSLAMSystem.h @@ -62,6 +62,7 @@ namespace ark { void getTrajectory(std::vector& trajOut); + void getMappedTrajectory(std::vector& frameIdOut, std::vector& trajOut); ~OkvisSLAMSystem(); diff --git a/include/SaveFrame.h b/include/SaveFrame.h new file mode 100644 index 00000000..962b3ff8 --- /dev/null +++ b/include/SaveFrame.h @@ -0,0 +1,42 @@ +#ifndef OPENARK_SAVEFRAME_H +#define OPENARK_SAVEFRAME_H + +#include +#include +#include +#include + +#include +#include "Types.h" + + +namespace ark{ + + class SaveFrame{ + public: + SaveFrame(std::string folderPath); + + //void OnKeyFrameAvailable(const RGBDFrame &keyFrame); + + //void OnFrameAvailable(const RGBDFrame &frame); + + void SaveFrame::frameWrite(cv::Mat imRGB, cv::Mat depth, Eigen::Matrix4d traj, int frameId); + void SaveFrame::updateTransforms(std::map keyframemap); + + ark::RGBDFrame SaveFrame::frameLoad(int frameId); + + private: + + //Main Loop thread + std::string folderPath; + std::string rgbPath; + std::string depthPath; + std::string tcwPath; + std::string depth_to_tcw_Path; + std::vector frame_ids; + + + }; +} + +#endif //#define OPENARK_SAVEFRAME_H \ No newline at end of file diff --git a/include/SparseMap.h b/include/SparseMap.h index a8ce7ce7..9d5087fa 100644 --- a/include/SparseMap.h +++ b/include/SparseMap.h @@ -63,6 +63,17 @@ class SparseMap { } } + void getMappedTrajectory(std::vector& frameIdOut, std::vector& trajOut){ + frameIdOut.resize(frameMap_.size()); + trajOut.resize(frameMap_.size()); + size_t i=0; + for(std::map::iterator frame = frameMap_.begin(); + frame!=frameMap_.end(); frame++, i++){ + frameIdOut[i] = frame->first; + trajOut[i] = frame->second->T_WC(3); + } + } + MapKeyFrame::Ptr getCurrentKeyframe(){ return getKeyframe(currentKeyframeId); }; diff --git a/include/Types.h b/include/Types.h index 9028a265..3964228e 100644 --- a/include/Types.h +++ b/include/Types.h @@ -159,6 +159,7 @@ namespace ark{ } void setOptimizedTransform(const Eigen::Matrix4d& T_WS_in){ + T_WS_Optimized_ = T_WS_in; optimized_ = true; } @@ -170,6 +171,14 @@ namespace ark{ return T_WS_; } + Eigen::Matrix4d T_WC(int index) + { + if(index>=0 && index vertices; + std::vector colors; + std::vector triangles; + + std::vector> mesh_vertices; + std::vector> mesh_colors; + std::vector> mesh_triangles; + std::vector mesh_transforms; + + void draw_obj(); + + Mesh(std::string name) + : Object(name), + vertices(), + colors(), + triangles(){ + } + + void update_mesh(std::vector v, + std::vector c, + std::vector t) { + std::lock_guard guard(meshLock_); + vertices = v; + colors = c; + triangles = t; + std::cout << "mesh updated" << std::endl; + } + + void update_mesh_vector(std::vector> mesh_vertices_toadd, + std::vector> mesh_vertex_colors_toadd, + std::vector> mesh_triangles_toadd, + std::vector transforms_toadd) { + std::lock_guard guard(meshLock_); + + mesh_vertices = mesh_vertices_toadd; + mesh_colors = mesh_vertex_colors_toadd; + mesh_triangles = mesh_triangles_toadd; + mesh_transforms = transforms_toadd; + + std::cout << "new meshes append successfully" << std::endl; + /*for (auto transform : mesh_transforms) { + + cout << transform.matrix() << endl; + }*/ + + } + + void update_active_mesh(std::vector updated_active_vertices, + std::vector updated_active_colors, + std::vector updated_active_triangles, + Eigen::Matrix4d updated_active_transform) { + std::lock_guard guard(meshLock_); + + mesh_vertices.pop_back(); + mesh_colors.pop_back(); + mesh_triangles.pop_back(); + mesh_transforms.pop_back(); + + mesh_vertices.push_back(updated_active_vertices); + mesh_colors.push_back(updated_active_colors); + mesh_triangles.push_back(updated_active_triangles); + mesh_transforms.push_back(updated_active_transform); + } + //transforms should be c->w + void update_transforms(std::vector updated_transforms) { + std::lock_guard guard(meshLock_); + mesh_transforms = updated_transforms; + /*for (auto transform : mesh_transforms) { + cout << transform.matrix() << endl; + }*/ + } + int get_number_meshes() { + std::lock_guard guard(meshLock_); + return mesh_vertices.size(); + } + + void clear() { + vertices.clear(); + colors.clear(); + triangles.clear(); + } + +protected: + std::mutex meshLock_; + +}; } //MyGUI diff --git a/scripts/OfflineReconstruction.py b/scripts/OfflineReconstruction.py new file mode 100644 index 00000000..63feb60d --- /dev/null +++ b/scripts/OfflineReconstruction.py @@ -0,0 +1,106 @@ +""" +Reconstructs output data from SaveFrame.cpp using Open3D's Integration +""" + +import open3d as o3d +import numpy as np +import os, sys +import matplotlib.pyplot as plt +from scipy.spatial.transform import Rotation as R + +def main(): + + if (len(sys.argv) != 2): + print("usage: python " + sys.argv[0] + " ") + exit() + + config = dict() + + config.update({"tsdf_cubic_size":0.5}) + config.update({"max_depth": 4.00}) + config.update({"voxel_size": 0.1}) + config.update({"max_depth_diff": 5.00}) + config.update({"tsdf_cubic_size": 3.0}) + config.update({"python_multi_threading": True}) + + + rgb_images = sys.argv[1] + "/RGB/" + depth_images = sys.argv[1] + "/depth/" + transforms = sys.argv[1] + "/tcw/" + + if (not os.path.isdir(rgb_images) or not os.path.isdir(depth_images) or not os.path.isdir(transforms)): + print("Cannot find frames") + print("Check directories: ", rgb_images, " ", depth_images, " ", transforms) + exit() + + cam_intr = np.zeros((3,3)) + cam_intr[0][0] = 612.081 + cam_intr[1][1] = 612.307 + cam_intr[0][2] = 318.254 + cam_intr[1][2] = 237.246 + cam_intr[2][2] = 1.0 + + + intr = o3d.camera.PinholeCameraIntrinsic() + intr.set_intrinsics(640, 480, cam_intr[0][0], cam_intr[1][1], cam_intr[0][2], cam_intr[1][2]) + + print("===================================") + print("===================================") + print("Reconstructing using this intrinsic matrix:") + print(intr.intrinsic_matrix) + print("===================================") + print("===================================") + for key in config.keys(): + print(key, ": ", config[key]) + print("===================================") + print("===================================") + + volume = o3d.integration.ScalableTSDFVolume( + voxel_length= config["voxel_size"], + sdf_trunc=config["voxel_size"] * 5, + color_type=o3d.integration.TSDFVolumeColorType.RGB8) + + lst = [] + + for frame in os.listdir(rgb_images): + frame_id = int(frame[:frame.find(".")]) + lst.append(frame_id) + + lst = sorted(lst) + + max_frameid = lst[-1] + + for frame_id in lst: + + print("processing frame id: ", frame_id, "/", max_frameid, end = '\r') + + frame_id = str(frame_id) + + color_raw = o3d.io.read_image(rgb_images + frame_id + ".jpg") + depth_raw = o3d.io.read_image(depth_images + frame_id + ".png") + + rgbd_image = o3d.geometry.RGBDImage.create_from_color_and_depth( + color_raw, depth_raw, depth_trunc=config["max_depth"], convert_rgb_to_intensity=False) + + cam_pose = np.zeros((4,4)) + + cam_pose = np.loadtxt(transforms + frame_id + ".txt") + + cam_pose = np.linalg.inv(cam_pose) + + volume.integrate(rgbd_image, intr, cam_pose) + + + print("Finished Integrating") + mesh = volume.extract_triangle_mesh() + + mesh.compute_vertex_normals() + + o3d.io.write_triangle_mesh("mesh.ply", mesh) + + print("Saved mesh to mesh.ply") + + + +if __name__ == "__main__": + main() \ No newline at end of file