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

26
src/msckf_vio.cpp
View File

@ -451,13 +451,9 @@ void MsckfVio::imageCallback(
//cout << "1" << endl;
// Augment the state vector.
start_time = ros::Time::now();
if(PHOTOMETRIC)
{
truePhotometricStateAugmentation(feature_msg->header.stamp.toSec());
PhotometricStateAugmentation(feature_msg->header.stamp.toSec());
}
else
stateAugmentation(feature_msg->header.stamp.toSec());
//truePhotometricStateAugmentation(feature_msg->header.stamp.toSec());
PhotometricStateAugmentation(feature_msg->header.stamp.toSec());
double state_augmentation_time = (
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).
MatrixXd H_thin;
VectorXd r_thin;
int augmentationSize = 6;
if(PHOTOMETRIC)
augmentationSize = 7;
int augmentationSize = 7;
// QR decomposition to make stuff faster
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();
for (int i = 0; i < state_server.cam_states.size();
++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>());
cam_state_iter->second.orientation = quaternionMultiplication(
dq_cam, cam_state_iter->second.orientation);
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.
@ -1827,9 +1823,7 @@ void MsckfVio::removeLostFeatures() {
// Remove the features that lost track.
// BTW, find the size the final Jacobian matrix and residual vector.
int jacobian_row_size = 0;
int augmentationSize = 6;
if(PHOTOMETRIC)
augmentationSize = 7;
int augmentationSize = 7;
vector<FeatureIDType> invalid_feature_ids(0);
@ -2133,9 +2127,7 @@ void MsckfVio::pruneCamStateBuffer() {
// Find the size of the Jacobian matrix.
int jacobian_row_size = 0;
int augmentationSize = 6;
if(PHOTOMETRIC)
augmentationSize = 7;
int augmentationSize = 7;
for (auto& item : map_server) {
auto& feature = item.second;