Read camera parameter from config file

WeiyanZhu · Feb 5, 2020 · 3c3e0a7 · 3c3e0a7
1 parent 9542b05
commit 3c3e0a7
Show file tree

Hide file tree

Showing 6 changed files with 47 additions and 18 deletions.
diff --git a/D435iCamera.cpp b/D435iCamera.cpp
@@ -10,22 +10,27 @@
 
 /** RealSense SDK2 Cross-Platform Depth Camera Backend **/
 namespace ark {
-    D435iCamera::D435iCamera():
-        last_ts_g(0), kill(false) {
+
+    D435iCamera::D435iCamera(): D435iCamera(CameraParameter()){}
+
+    D435iCamera::D435iCamera(const CameraParameter &parameter): 
+        cameraParameter(parameter), last_ts_g(0), kill(false) {
         //Setup camera
         //TODO: Make read from config file
         rs2::context ctx;
         device = ctx.query_devices().front();
-        width = 640;
-        height = 480;
+        width = cameraParameter.width;
+        height = cameraParameter.height;
         //Setup configuration
-        config.enable_stream(RS2_STREAM_DEPTH,-1,width, height,RS2_FORMAT_Z16,30);
-        config.enable_stream(RS2_STREAM_INFRARED, 1, width, height, RS2_FORMAT_Y8, 30);
-        config.enable_stream(RS2_STREAM_INFRARED, 2, width, height, RS2_FORMAT_Y8, 30);
-        config.enable_stream(RS2_STREAM_COLOR, -1, width, height, RS2_FORMAT_RGB8, 30);
-        motion_config.enable_stream(RS2_STREAM_ACCEL, RS2_FORMAT_MOTION_XYZ32F,250);
-        motion_config.enable_stream(RS2_STREAM_GYRO, RS2_FORMAT_MOTION_XYZ32F,200);
-        imu_rate=200; //setting imu_rate to be the gyro rate since we are using the gyro timestamps
+        const auto irDepthFps = cameraParameter.irDepthFps;
+        config.enable_stream(RS2_STREAM_DEPTH,-1,width, height,RS2_FORMAT_Z16,irDepthFps);
+        config.enable_stream(RS2_STREAM_INFRARED, 1, width, height, RS2_FORMAT_Y8, irDepthFps);
+        config.enable_stream(RS2_STREAM_INFRARED, 2, width, height, RS2_FORMAT_Y8, irDepthFps);
+        config.enable_stream(RS2_STREAM_COLOR, -1, width, height, RS2_FORMAT_RGB8, irDepthFps);
+        const auto imuFps = cameraParameter.imuFps;
+        // TODO: check if fps of acc is neccesary to be different from gyro
+        motion_config.enable_stream(RS2_STREAM_ACCEL, RS2_FORMAT_MOTION_XYZ32F,imuFps+50);
+        motion_config.enable_stream(RS2_STREAM_GYRO, RS2_FORMAT_MOTION_XYZ32F,imuFps);
         //Need to get the depth sensor specifically as it is the one that controls the sync funciton
         depth_sensor = new rs2::depth_sensor(device.first<rs2::depth_sensor>());
 
@@ -52,7 +57,7 @@ namespace ark {
     void D435iCamera::start(){
         //enable sync
         //depth_sensor->set_option(RS2_OPTION_INTER_CAM_SYNC_MODE,1);
-        //depth_sensor->set_option(RS2_OPTION_EMITTER_ENABLED, 1.f);
+        depth_sensor->set_option(RS2_OPTION_EMITTER_ENABLED, cameraParameter.emitterPower);
         //start streaming
         pipe = std::make_shared<rs2::pipeline>();
         rs2::pipeline_profile selection = pipe->start(config);
@@ -132,6 +137,7 @@ namespace ark {
     bool D435iCamera::getImuToTime(double timestamp, std::vector<ImuPair>& data_out){
         ImuPair imu_data;
         imu_data.timestamp=0;
+        float imu_rate = static_cast<float>(cameraParameter.imuFps);
         while((imu_data.timestamp+1e9/imu_rate)<timestamp){
             if(imu_queue_.try_dequeue(&imu_data)){
                 data_out.push_back(imu_data);

diff --git a/SlamDemo435i.cpp b/SlamDemo435i.cpp
@@ -33,6 +33,7 @@ int main(int argc, char **argv)
     else vocabFilename = util::resolveRootPath("config/brisk_vocab.bn");
 
     OkvisSLAMSystem slam(vocabFilename, configFilename);
+    cv::FileStorage configFile(configFilename, cv::FileStorage::READ);
 
     //setup display
     if (!MyGUI::Manager::init())
@@ -44,7 +45,11 @@ int main(int argc, char **argv)
 
     printf("Camera initialization started...\n");
     fflush(stdout);
-    D435iCamera camera;
+    CameraParameter cameraParameter;
+    if (configFile["emitterPower"].isReal()) {
+        configFile["emitterPower"] >> cameraParameter.emitterPower;
+    }
+    D435iCamera camera(cameraParameter);
     camera.start();
 
     printf("Camera-IMU initialization complete\n");

diff --git a/SlamRecording.cpp b/SlamRecording.cpp
@@ -57,6 +57,7 @@ int main(int argc, char **argv)
 
     const path directory_path =
         argc > 1 ? argv[1] : std::string("./data_path_") + getTimeTag(); // modify this
+    const std::string configFilename = argc > 2 ? argv[2] : util::resolveRootPath("config/d435i_intr.yaml");
     path depth_path = directory_path / "depth/";
     path infrared_path = directory_path / "infrared/";
     path infrared2_path = directory_path / "infrared2/";
@@ -77,7 +78,12 @@ int main(int argc, char **argv)
     std::vector<MultiCameraFrame> frameList;
     std::vector<ImuPair> imuList;
 
-    D435iCamera camera;
+    cv::FileStorage configFile(configFilename, cv::FileStorage::READ);
+    CameraParameter cameraParameter;
+    if (configFile["emitterPower"].isReal()) {
+        configFile["emitterPower"] >> cameraParameter.emitterPower;
+    }
+    D435iCamera camera(cameraParameter);
     camera.start();
 
     std::vector<ImuPair> imuBuffer;
@@ -152,9 +158,10 @@ int main(int argc, char **argv)
         camera.update(*frame);
 
         const auto rgb = frame->images_[3].clone();
+        const auto ir1 = frame->images_[0].clone();
 
         cv::cvtColor(rgb, rgb, CV_RGB2BGR);
-        cv::imshow(camera.getModelName() + " RGB", rgb);
+        cv::imshow(camera.getModelName() + " IR", ir1);
 
         if (paused)
         {

diff --git a/config/d435i_intr.yaml b/config/d435i_intr.yaml
@@ -73,6 +73,9 @@ imageDelay: 0.
 displayImages: false
 useDriver: false
 enableLoopClosureDetection: 1
+numKeypointsResetThreshold: 15
+durationResetThreshold: 2.0
+emitterPower: 1.
 publishing_options:
    publish_rate: 200
    publishLandmarks: false

diff --git a/include/D435iCamera.h b/include/D435iCamera.h
@@ -27,6 +27,8 @@ namespace ark {
         */
         explicit D435iCamera();
 
+        D435iCamera(const CameraParameter &parameter);
+
         /**
         * Destructor for the RealSense Camera.
         */
@@ -78,10 +80,10 @@ namespace ark {
         std::thread imuReaderThread_;
         single_consumer_queue<ImuPair> imu_queue_;
 
+        CameraParameter cameraParameter;
+        int width, height;
         double scale;
         double last_ts_g;
-        float imu_rate;
-        int width, height;
         bool badInputFlag;
         std::atomic<bool> kill;
     };

diff --git a/include/Types.h b/include/Types.h
@@ -308,5 +308,11 @@ namespace ark{
 
     };
 
-
+    struct CameraParameter {
+        int width = 640;
+        int height = 480;
+        float emitterPower = 0.5f;
+        int irDepthFps = 30;
+        int imuFps = 200;
+    };
 }