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