Add Offline 3D Recon to OpenARK

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<ImuPair, Eigen::aligned_allocator<ImuPair>> 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);
 

CMakeLists.txt

@@ -1,14 +1,14 @@
 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 )
 ]]
 
 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} )

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<ImuPair, Eigen:
     double gyro0, gyro1, gyro2, accel0, accel1, accel2;
     for (; !imuStream.eof() && ts < timestamp;)
     {
-        // TODO: refactor
-        bool allGood = true;
-        allGood = allGood && std::getline(imuStream, line1);
-        allGood = allGood && std::getline(imuStream, line2);
-        allGood = allGood && std::getline(imuStream, line3);
 
-        if (!allGood) {
+        if (!(std::getline(imuStream, line1) && std::getline(imuStream, line2) && std::getline(imuStream, line3))) {
             std::cout << "getImuToTime: unable to read imu data.\n";
             return false;
         }
@@ -79,10 +144,7 @@ bool MockD435iCamera::getImuToTime(double timestamp, std::vector<ImuPair, Eigen:
 };
 
 std::vector<float> 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<float> vect{ -1, -1, -1, -1 };
-    return vect;
+	return std::vector<float>{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<path> pathList{infraredDir, infrared2Dir, depthDir, infraredDir, depthDir};
+    std::vector<path> 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