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:22 +02:00
parent 2d045660c2
commit ecab936c62
2 changed files with 11 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
launch/msckf_vio_tum.launch
View File

@ -21,7 +21,7 @@
<param name="PHOTOMETRIC" value="false"/> <param name="PHOTOMETRIC" value="false"/>
<!-- Debugging Flaggs --> <!-- Debugging Flaggs -->
<param name="StreamPause" value="true"/> <param name="StreamPause" value="false"/>
<param name="PrintImages" value="false"/> <param name="PrintImages" value="false"/>
<param name="GroundTruth" value="false"/> <param name="GroundTruth" value="false"/>
@ -72,6 +72,6 @@
</node> </node>
</group> </group>
<node name="player" pkg="bagcontrol" type="control.py" /> <!--node name="player" pkg="bagcontrol" type="control.py" /-->
</launch> </launch>

26
src/msckf_vio.cpp
View File

@ -451,13 +451,9 @@ 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();
if(PHOTOMETRIC) //truePhotometricStateAugmentation(feature_msg->header.stamp.toSec());
{ 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();
@ -1706,9 +1702,7 @@ 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 = 6; int augmentationSize = 7;
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()) {
@ -1779,11 +1773,13 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
auto cam_state_iter = state_server.cam_states.begin(); auto cam_state_iter = state_server.cam_states.begin();
for (int i = 0; i < state_server.cam_states.size(); for (int i = 0; i < state_server.cam_states.size();
++i, ++cam_state_iter) { ++i, ++cam_state_iter) {
const VectorXd& delta_x_cam = delta_x.segment(21+i*augmentationSize, augmentationSize); const VectorXd& delta_x_cam = delta_x.segment(21+i*augmentationSize, 6);
const Vector4d dq_cam = smallAngleQuaternion(delta_x_cam.head<3>()); const Vector4d dq_cam = smallAngleQuaternion(delta_x_cam.head<3>());
cam_state_iter->second.orientation = quaternionMultiplication( cam_state_iter->second.orientation = quaternionMultiplication(
dq_cam, cam_state_iter->second.orientation); dq_cam, cam_state_iter->second.orientation);
cam_state_iter->second.position += delta_x_cam.tail<3>(); cam_state_iter->second.position += delta_x_cam.tail<3>();
if(PHOTOMETRIC)
cam_state_iter->second.illumination.frame_bias += delta_x(21+i*augmentationSize+6);
} }
// Update state covariance. // Update state covariance.
@ -1827,9 +1823,7 @@ 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 = 6; int augmentationSize = 7;
if(PHOTOMETRIC)
augmentationSize = 7;
vector<FeatureIDType> invalid_feature_ids(0); vector<FeatureIDType> invalid_feature_ids(0);
@ -2133,9 +2127,7 @@ 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 = 6; int augmentationSize = 7;
if(PHOTOMETRIC)
augmentationSize = 7;
for (auto& item : map_server) { for (auto& item : map_server) {
auto& feature = item.second; auto& feature = item.second;