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

minor changes in launch file and edited augmentationsize

Browse Source
This commit is contained in:
Raphael Maenle 2019-06-19 09:59:53 +02:00
parent ecab936c62
commit c79fc173b3
1 changed files with 16 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
src/msckf_vio.cpp
View File

@ -451,9 +451,13 @@ void MsckfVio::imageCallback(
//cout << "1" << endl; //cout << "1" << endl;
// Augment the state vector. // Augment the state vector.
start_time = ros::Time::now(); start_time = ros::Time::now();
//truePhotometricStateAugmentation(feature_msg->header.stamp.toSec()); if(PHOTOMETRIC)
PhotometricStateAugmentation(feature_msg->header.stamp.toSec()); {
truePhotometricStateAugmentation(feature_msg->header.stamp.toSec());
PhotometricStateAugmentation(feature_msg->header.stamp.toSec());
}
else
stateAugmentation(feature_msg->header.stamp.toSec());
double state_augmentation_time = ( double state_augmentation_time = (
ros::Time::now()-start_time).toSec(); ros::Time::now()-start_time).toSec();
@ -1702,7 +1706,9 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
// complexity as in Equation (28), (29). // complexity as in Equation (28), (29).
MatrixXd H_thin; MatrixXd H_thin;
VectorXd r_thin; VectorXd r_thin;
int augmentationSize = 7; int augmentationSize = 6;
if(PHOTOMETRIC)
augmentationSize = 7;
// QR decomposition to make stuff faster // QR decomposition to make stuff faster
if (H.rows() > H.cols()) { if (H.rows() > H.cols()) {
@ -1823,7 +1829,9 @@ void MsckfVio::removeLostFeatures() {
// Remove the features that lost track. // Remove the features that lost track.
// BTW, find the size the final Jacobian matrix and residual vector. // BTW, find the size the final Jacobian matrix and residual vector.
int jacobian_row_size = 0; int jacobian_row_size = 0;
int augmentationSize = 7; int augmentationSize = 6;
if(PHOTOMETRIC)
augmentationSize = 7;
vector<FeatureIDType> invalid_feature_ids(0); vector<FeatureIDType> invalid_feature_ids(0);
@ -2127,7 +2135,9 @@ void MsckfVio::pruneCamStateBuffer() {
// Find the size of the Jacobian matrix. // Find the size of the Jacobian matrix.
int jacobian_row_size = 0; int jacobian_row_size = 0;
int augmentationSize = 7; int augmentationSize = 6;
if(PHOTOMETRIC)
augmentationSize = 7;
for (auto& item : map_server) { for (auto& item : map_server) {
auto& feature = item.second; auto& feature = item.second;