public/vio
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

correct frame

Browse Source
This commit is contained in:
gfuse
2021-05-08 17:26:27 +02:00
parent 9dbc540a5d
commit 2d0f81ebe0
16 changed files with 360 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CPU_version/svo/src/imu_integration.cpp
+4 -2
View File
@@ -16,7 +16,9 @@
Imu_Integration::Imu_Integration(Sophus::SE3& SE_init){
graphPtr=std::make_shared<gtsam::NonlinearFactorGraph>();
valuesPtr=std::make_shared<gtsam::Values>();
parameterPtr = gtsam::PreintegratedCombinedMeasurements::Params::MakeSharedD();// TODO check the IMU frame
gtsam::Vector4 g=SE_init.matrix()*gtsam::Vector4(0, 0, 9.8,1.0);
parameterPtr = boost::shared_ptr<gtsam::PreintegrationCombinedParams>(new gtsam::PreintegrationCombinedParams(gtsam::Vector3(g.x(),g.y(),g.z())));
parameterPtr->setBodyPSensor(gtsam::Pose3(SE_init.matrix()));// The pose of the sensor in the body frame
parameterPtr->accelerometerCovariance=gtsam::I_3x3 * svo::Config::ACC_Noise(); // acc white noise in continuous
parameterPtr->gyroscopeCovariance=gtsam::I_3x3 * svo::Config::GYO_Noise();// gyro white noise in continuous
parameterPtr->integrationCovariance=gtsam::I_3x3 * svo::Config::IUC(); // integration uncertainty continuous
@@ -148,4 +150,4 @@ bool Imu_Integration::predict(boost::shared_ptr<svo::Frame>& new_frame,std::size
}
}
return true;
}
}