raphael/msckf_vio
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

changed structure for sublime folding

Browse Source
This commit is contained in:
Raphael Maenle 2019-04-26 10:45:10 +02:00
parent e8489dbd06
commit e3ac604dbf
1 changed files with 22 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
src/msckf_vio.cpp
View File

@ -292,7 +292,7 @@ bool MsckfVio::initialize() {
for (int i = 1; i < 100; ++i) { for (int i = 1; i < 100; ++i) {
boost::math::chi_squared chi_squared_dist(i); boost::math::chi_squared chi_squared_dist(i);
chi_squared_test_table[i] = chi_squared_test_table[i] =
boost::math::quantile(chi_squared_dist, 0.2); boost::math::quantile(chi_squared_dist, 0.05);
} }
if (!createRosIO()) return false; if (!createRosIO()) return false;
@ -301,9 +301,7 @@ bool MsckfVio::initialize() {
return true; return true;
} }
void MsckfVio::imuCallback( void MsckfVio::imuCallback(const sensor_msgs::ImuConstPtr& msg){
const sensor_msgs::ImuConstPtr& msg)
{
// IMU msgs are pushed backed into a buffer instead of // IMU msgs are pushed backed into a buffer instead of
// being processed immediately. The IMU msgs are processed // being processed immediately. The IMU msgs are processed
@ -491,7 +489,8 @@ void MsckfVio::initializeGravityAndBias() {
bool MsckfVio::resetCallback( bool MsckfVio::resetCallback(
std_srvs::Trigger::Request& req, std_srvs::Trigger::Request& req,
std_srvs::Trigger::Response& res) { std_srvs::Trigger::Response& res)
{
cout << "reset" << endl; cout << "reset" << endl;
ROS_WARN("Start resetting msckf vio..."); ROS_WARN("Start resetting msckf vio...");
@ -559,17 +558,16 @@ bool MsckfVio::resetCallback(
imu_sub = nh.subscribe("imu", 100, imu_sub = nh.subscribe("imu", 100,
&MsckfVio::imuCallback, this); &MsckfVio::imuCallback, this);
// feature_sub = nh.subscribe("features", 40, // feature_sub = nh.subscribe("features", 40,
// &MsckfVio::featureCallback, this); // &MsckfVio::featureCallback, this);
// TODO: When can the reset fail? // TODO: When can the reset fail?
res.success = true; res.success = true;
ROS_WARN("Resetting msckf vio completed..."); ROS_WARN("Resetting msckf vio completed...");
return true; return true;
} }
void MsckfVio::mocapOdomCallback( void MsckfVio::mocapOdomCallback(const nav_msgs::OdometryConstPtr& msg) {
const nav_msgs::OdometryConstPtr& msg) {
static bool first_mocap_odom_msg = true; static bool first_mocap_odom_msg = true;
// If this is the first mocap odometry messsage, set // If this is the first mocap odometry messsage, set
@ -668,7 +666,8 @@ void MsckfVio::batchImuProcessing(const double& time_bound) {
void MsckfVio::processModel(const double& time, void MsckfVio::processModel(const double& time,
const Vector3d& m_gyro, const Vector3d& m_gyro,
const Vector3d& m_acc) { const Vector3d& m_acc)
{
// Remove the bias from the measured gyro and acceleration // Remove the bias from the measured gyro and acceleration
IMUState& imu_state = state_server.imu_state; IMUState& imu_state = state_server.imu_state;
@ -739,7 +738,8 @@ void MsckfVio::processModel(const double& time,
void MsckfVio::predictNewState(const double& dt, void MsckfVio::predictNewState(const double& dt,
const Vector3d& gyro, const Vector3d& gyro,
const Vector3d& acc) { const Vector3d& acc)
{
// TODO: Will performing the forward integration using // TODO: Will performing the forward integration using
// the inverse of the quaternion give better accuracy? // the inverse of the quaternion give better accuracy?
@ -802,7 +802,8 @@ void MsckfVio::predictNewState(const double& dt,
return; return;
} }
void MsckfVio::stateAugmentation(const double& time) { void MsckfVio::stateAugmentation(const double& time)
{
const Matrix3d& R_i_c = state_server.imu_state.R_imu_cam0; const Matrix3d& R_i_c = state_server.imu_state.R_imu_cam0;
const Vector3d& t_c_i = state_server.imu_state.t_cam0_imu; const Vector3d& t_c_i = state_server.imu_state.t_cam0_imu;
@ -866,7 +867,8 @@ void MsckfVio::stateAugmentation(const double& time) {
return; return;
} }
void MsckfVio::PhotometricStateAugmentation(const double& time) { void MsckfVio::PhotometricStateAugmentation(const double& time)
{
const Matrix3d& R_i_c = state_server.imu_state.R_imu_cam0; const Matrix3d& R_i_c = state_server.imu_state.R_imu_cam0;
const Vector3d& t_c_i = state_server.imu_state.t_cam0_imu; const Vector3d& t_c_i = state_server.imu_state.t_cam0_imu;
@ -936,10 +938,9 @@ void MsckfVio::PhotometricStateAugmentation(const double& time) {
return; return;
} }
void MsckfVio::addFeatureObservations( void MsckfVio::addFeatureObservations(
const CameraMeasurementConstPtr& msg) { const CameraMeasurementConstPtr& msg)
{
StateIDType state_id = state_server.imu_state.id; StateIDType state_id = state_server.imu_state.id;
int curr_feature_num = map_server.size(); int curr_feature_num = map_server.size();
@ -973,7 +974,8 @@ void MsckfVio::addFeatureObservations(
void MsckfVio::PhotometricMeasurementJacobian( void MsckfVio::PhotometricMeasurementJacobian(
const StateIDType& cam_state_id, const StateIDType& cam_state_id,
const FeatureIDType& feature_id, const FeatureIDType& feature_id,
MatrixXd& H_x, MatrixXd& H_y, VectorXd& r) { MatrixXd& H_x, MatrixXd& H_y, VectorXd& r)
{
// Prepare all the required data. // Prepare all the required data.
const CAMState& cam_state = state_server.cam_states[cam_state_id]; const CAMState& cam_state = state_server.cam_states[cam_state_id];
@ -1125,7 +1127,8 @@ void MsckfVio::PhotometricMeasurementJacobian(
void MsckfVio::PhotometricFeatureJacobian( void MsckfVio::PhotometricFeatureJacobian(
const FeatureIDType& feature_id, const FeatureIDType& feature_id,
const std::vector<StateIDType>& cam_state_ids, const std::vector<StateIDType>& cam_state_ids,
MatrixXd& H_x, VectorXd& r) { MatrixXd& H_x, VectorXd& r)
{
const auto& feature = map_server[feature_id]; const auto& feature = map_server[feature_id];