fix little bugs

kongdaqing · Sep 28, 2021 · 4ff2c3a · 4ff2c3a
1 parent e359ec9
commit 4ff2c3a
Show file tree

Hide file tree

Showing 3 changed files with 8 additions and 6 deletions.
diff --git a/SlamCodes/viz_scene/config/config.yaml b/SlamCodes/viz_scene/config/config.yaml
@@ -4,10 +4,10 @@ imuFreq: 200
 imageFreq: 30
 simStart: 0
 simEnd: 5
-gyr_noise_sigma: 0.0001
-acc_noise_sigma: 0.001
-gyr_bias_sigma: 0.00001
-acc_bias_sigma: 0.0001
+gyr_noise_sigma: 0.001
+acc_noise_sigma: 0.01
+gyr_bias_sigma: 0.0001
+acc_bias_sigma: 0.001
 pixel_noise: 2
 add_pixel_noise: 0
 camera_yaml: "/home/kdq/Workspace/SLAM/SLAM-KDQ/SlamCodes/viz_scene/config/cam0_pinhole.yaml"

diff --git a/SlamCodes/viz_scene/src/imu_motion/imu_motion.cpp b/SlamCodes/viz_scene/src/imu_motion/imu_motion.cpp
@@ -5,8 +5,7 @@ ImuMotionState ImuMotion::addImuNoise(const ImuMotionState& data) {
   std::random_device rd;
   std::default_random_engine rdEngine(rd());
   std::normal_distribution<double> noise(0.0,1.0);
-  ImuMotionState outData;
-
+  ImuMotionState outData = data;
   Eigen::Vector3d gyro_noise(noise(rdEngine),noise(rdEngine),noise(rdEngine));
   Eigen::Matrix3d gyro_sqrt_cov  = para_->gyr_noise_sigma_ * Eigen::Matrix3d::Identity();
   outData.gyr_ = data.gyr_ + gyro_sqrt_cov * gyro_noise / sqrt(para_->imuInterval_) + gyr_bias_;
@@ -22,6 +21,7 @@ ImuMotionState ImuMotion::addImuNoise(const ImuMotionState& data) {
   Eigen::Vector3d acc_bias_noise(noise(rdEngine),noise(rdEngine),noise(rdEngine));
   acc_bias_ += para_->acc_bias_sigma_ * sqrt(para_->imuInterval_) * acc_bias_noise;
   outData.acc_bias_ = acc_bias_;
+  return outData;
 }
 
 

diff --git a/SlamCodes/viz_scene/src/testCameraMotion.cpp b/SlamCodes/viz_scene/src/testCameraMotion.cpp
@@ -23,6 +23,8 @@ int main(int argc,char** argv){
   VioDatasInterface::recordImuMotionState(ImuMotionState(),"log/test.csv",true);
   VioDatasInterface::recordImuMotionState(ImuMotionState(),"log/noise_test.csv",true);
   VioDatasInterface::recordPoseAsTum(ImuMotionState(),"log/imu_pose.csv",true);
+  VioDatasInterface::recordPoseAsTum(ImuMotionState(),"log/real_pose.csv",true);
+
   while(1) {
     //vizScene.testIncreasePoints("test plane");
     if(t  < para->end_t_) {