Skip to content

Commit 779942f

Browse files
huaizhenghuaizheng
committed
Minor updates
1 parent 8b70a34 commit 779942f

8 files changed

+109
-33
lines changed

‎src/rvio/FeatureDetector.cc

+20
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,23 @@
1+
/**
2+
* This file is part of R-VIO.
3+
*
4+
* Copyright (C) 2019 Zheng Huai <[email protected]> and Guoquan Huang <[email protected]>
5+
* For more information see <http://github.com/rpng/R-VIO>
6+
*
7+
* R-VIO is free software: you can redistribute it and/or modify
8+
* it under the terms of the GNU General Public License as published by
9+
* the Free Software Foundation, either version 3 of the License, or
10+
* (at your option) any later version.
11+
*
12+
* R-VIO is distributed in the hope that it will be useful,
13+
* but WITHOUT ANY WARRANTY; without even the implied warranty of
14+
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15+
* GNU General Public License for more details.
16+
*
17+
* You should have received a copy of the GNU General Public License
18+
* along with R-VIO. If not, see <http://www.gnu.org/licenses/>.
19+
*/
20+
121
#include <opencv2/opencv.hpp>
222

323
#include "FeatureDetector.h"

‎src/rvio/FeatureDetector.h

+20
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,23 @@
1+
/**
2+
* This file is part of R-VIO.
3+
*
4+
* Copyright (C) 2019 Zheng Huai <[email protected]> and Guoquan Huang <[email protected]>
5+
* For more information see <http://github.com/rpng/R-VIO>
6+
*
7+
* R-VIO is free software: you can redistribute it and/or modify
8+
* it under the terms of the GNU General Public License as published by
9+
* the Free Software Foundation, either version 3 of the License, or
10+
* (at your option) any later version.
11+
*
12+
* R-VIO is distributed in the hope that it will be useful,
13+
* but WITHOUT ANY WARRANTY; without even the implied warranty of
14+
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15+
* GNU General Public License for more details.
16+
*
17+
* You should have received a copy of the GNU General Public License
18+
* along with R-VIO. If not, see <http://www.gnu.org/licenses/>.
19+
*/
20+
121
#ifndef FEATURE_DETECTOR_H
222
#define FEATURE_DETECTOR_H
323

‎src/rvio/InputBuffer.cc

+20
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,23 @@
1+
/**
2+
* This file is part of R-VIO.
3+
*
4+
* Copyright (C) 2019 Zheng Huai <[email protected]> and Guoquan Huang <[email protected]>
5+
* For more information see <http://github.com/rpng/R-VIO>
6+
*
7+
* R-VIO is free software: you can redistribute it and/or modify
8+
* it under the terms of the GNU General Public License as published by
9+
* the Free Software Foundation, either version 3 of the License, or
10+
* (at your option) any later version.
11+
*
12+
* R-VIO is distributed in the hope that it will be useful,
13+
* but WITHOUT ANY WARRANTY; without even the implied warranty of
14+
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15+
* GNU General Public License for more details.
16+
*
17+
* You should have received a copy of the GNU General Public License
18+
* along with R-VIO. If not, see <http://www.gnu.org/licenses/>.
19+
*/
20+
121
#include "InputBuffer.h"
222
#include "../util/Numerics.h"
323

‎src/rvio/InputBuffer.h

+20
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,23 @@
1+
/**
2+
* This file is part of R-VIO.
3+
*
4+
* Copyright (C) 2019 Zheng Huai <[email protected]> and Guoquan Huang <[email protected]>
5+
* For more information see <http://github.com/rpng/R-VIO>
6+
*
7+
* R-VIO is free software: you can redistribute it and/or modify
8+
* it under the terms of the GNU General Public License as published by
9+
* the Free Software Foundation, either version 3 of the License, or
10+
* (at your option) any later version.
11+
*
12+
* R-VIO is distributed in the hope that it will be useful,
13+
* but WITHOUT ANY WARRANTY; without even the implied warranty of
14+
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15+
* GNU General Public License for more details.
16+
*
17+
* You should have received a copy of the GNU General Public License
18+
* along with R-VIO. If not, see <http://www.gnu.org/licenses/>.
19+
*/
20+
121
#ifndef INPUTBUFFER_H
222
#define INPUTBUFFER_H
323

‎src/rvio/PreIntegrator.cc

+12-13
Original file line numberDiff line numberDiff line change
@@ -29,23 +29,22 @@ namespace RVIO
2929

3030
PreIntegrator::PreIntegrator(const cv::FileStorage& fsSettings)
3131
{
32-
msigmaGyroNoise = fsSettings["IMU.sigma_g"];
33-
msigmaGyroBias = fsSettings["IMU.sigma_wg"];
34-
msigmaAccelNoise = fsSettings["IMU.sigma_a"];
35-
msigmaAccelBias = fsSettings["IMU.sigma_wa"];
36-
32+
mnGravity = fsSettings["IMU.nG"];
3733
mnSmallAngle = fsSettings["IMU.nSmallAngle"];
3834

39-
mnGravity = fsSettings["IMU.nG"];
35+
mnGyroNoiseSigma = fsSettings["IMU.sigma_g"];
36+
mnGyroRandomWalkSigma = fsSettings["IMU.sigma_wg"];
37+
mnAccelNoiseSigma = fsSettings["IMU.sigma_a"];
38+
mnAccelRandomWalkSigma = fsSettings["IMU.sigma_wa"];
39+
40+
ImuNoiseMatrix.setIdentity();
41+
ImuNoiseMatrix.block<3,3>(0,0) *= pow(mnGyroNoiseSigma,2);
42+
ImuNoiseMatrix.block<3,3>(3,3) *= pow(mnGyroRandomWalkSigma,2);
43+
ImuNoiseMatrix.block<3,3>(6,6) *= pow(mnAccelNoiseSigma,2);
44+
ImuNoiseMatrix.block<3,3>(9,9) *= pow(mnAccelRandomWalkSigma,2);
4045

4146
xk1k.setZero(26,1);
4247
Pk1k.setZero(24,24);
43-
44-
Sigma.setIdentity();
45-
Sigma.block<3,3>(0,0) *= pow(msigmaGyroNoise,2);
46-
Sigma.block<3,3>(3,3) *= pow(msigmaGyroBias,2);
47-
Sigma.block<3,3>(6,6) *= pow(msigmaAccelNoise,2);
48-
Sigma.block<3,3>(9,9) *= pow(msigmaAccelBias,2);
4948
}
5049

5150

@@ -138,7 +137,7 @@ void PreIntegrator::propagate(Eigen::VectorXd& xkk,
138137
G.block<3,3>(15,6) = -I;
139138
G.block<3,3>(18,3) = I;
140139
G.block<3,3>(21,9) = I;
141-
Q = dt*G*Sigma*(G.transpose());
140+
Q = dt*G*ImuNoiseMatrix*(G.transpose());
142141

143142
Pkk.block(0,0,24,24) = Phi*(Pkk.block(0,0,24,24))*(Phi.transpose())+Q;
144143

‎src/rvio/PreIntegrator.h

+7-8
Original file line numberDiff line numberDiff line change
@@ -52,14 +52,13 @@ class PreIntegrator
5252
double mnGravity;
5353
double mnSmallAngle;
5454

55-
// Sigma{g,a,wg,wa}
56-
double msigmaGyroNoise;
57-
double msigmaAccelNoise;
58-
double msigmaGyroBias;
59-
double msigmaAccelBias;
60-
61-
// Noise covariance matrix
62-
Eigen::Matrix<double,12,12> Sigma;
55+
// Sigma{g,wg,a,wa}
56+
double mnGyroNoiseSigma;
57+
double mnGyroRandomWalkSigma;
58+
double mnAccelNoiseSigma;
59+
double mnAccelRandomWalkSigma;
60+
61+
Eigen::Matrix<double,12,12> ImuNoiseMatrix;
6362
};
6463

6564
} // namespace RVIO

‎src/rvio/Updater.cc

+9-10
Original file line numberDiff line numberDiff line change
@@ -39,8 +39,9 @@ Updater::Updater(const cv::FileStorage& fsSettings)
3939
{
4040
mnCamRate = fsSettings["Camera.fps"];
4141

42-
msigmaImageNoiseX = fsSettings["Camera.sigma_px"];
43-
msigmaImageNoiseY = fsSettings["Camera.sigma_py"];
42+
const float nImageNoiseSigmaX = fsSettings["Camera.sigma_px"];
43+
const float nImageNoiseSigmaY = fsSettings["Camera.sigma_py"];
44+
mnImageNoiseSigma = std::max(nImageNoiseSigmaX, nImageNoiseSigmaY);
4445

4546
cv::Mat T(4,4,CV_32F);
4647
fsSettings["Camera.T_BC0"] >> T;
@@ -170,8 +171,8 @@ void Updater::update(Eigen::VectorXd& xk1k,
170171
-sin(phi)*cos(psi), -cos(phi)*sin(psi);
171172

172173
Eigen::Matrix2d Rinv;
173-
Rinv << 1/pow(msigmaImageNoiseX,2), 0,
174-
0, 1/pow(msigmaImageNoiseY,2);
174+
Rinv << 1./pow(mnImageNoiseSigma,2), 0,
175+
0, 1./pow(mnImageNoiseSigma,2);
175176

176177
int maxIter = 10;
177178
double lambda = 0.01;
@@ -201,7 +202,6 @@ void Updater::update(Eigen::VectorXd& xk1k,
201202
e1 << (ptFirst-pt1).x, (ptFirst-pt1).y;
202203

203204
cost += e1.transpose()*Rinv*e1;
204-
205205
HTRinvH.noalias() += H1.transpose()*Rinv*H1;
206206
HTRinve.noalias() += H1.transpose()*Rinv*e1;
207207

@@ -228,7 +228,6 @@ void Updater::update(Eigen::VectorXd& xk1k,
228228
e << ((*lit)-pt).x, ((*lit)-pt).y;
229229

230230
cost += e.transpose()*Rinv*e;
231-
232231
HTRinvH.noalias() += H.transpose()*Rinv*H;
233232
HTRinve.noalias() += H.transpose()*Rinv*e;
234233
}
@@ -249,7 +248,7 @@ void Updater::update(Eigen::VectorXd& xk1k,
249248
cos(phi), 0,
250249
-sin(phi)*cos(psi), -cos(phi)*sin(psi);
251250

252-
if (fabs(lastCost-cost)<1e-6 || dpfinv.norm()<1e-6)
251+
if (fabs(lastCost-cost)<1e-6 && dpfinv(2)<1e-6)
253252
break;
254253

255254
lambda *= .1;
@@ -411,7 +410,7 @@ void Updater::update(Eigen::VectorXd& xk1k,
411410

412411
Eigen::VectorXd tempR;
413412
tempR.setOnes(nDOF,1);
414-
tempR *= pow(msigmaImageNoiseX>msigmaImageNoiseY ? msigmaImageNoiseX:msigmaImageNoiseY,2);
413+
tempR *= pow(mnImageNoiseSigma, 2);
415414

416415
Eigen::MatrixXd tempS;
417416
tempS = tempHx_*Pk1k.block(24,24,6*nCloneStates,6*nCloneStates)*(tempHx_.transpose());
@@ -465,7 +464,7 @@ void Updater::update(Eigen::VectorXd& xk1k,
465464

466465
Eigen::VectorXd Ro;
467466
Ro.setOnes(nRowCount,1);
468-
Ro *= pow(msigmaImageNoiseX>msigmaImageNoiseY ? msigmaImageNoiseX:msigmaImageNoiseY,2);
467+
Ro *= pow(mnImageNoiseSigma, 2);
469468

470469
// Model compression
471470
Eigen::VectorXd rn;
@@ -526,7 +525,7 @@ void Updater::update(Eigen::VectorXd& xk1k,
526525
rn = ro.block(0,0,nRank,1);
527526
Hn = Ho.block(0,0,nRank,Ho.cols());
528527
Rn.setOnes(nRank,1);
529-
Rn *= pow(msigmaImageNoiseX>msigmaImageNoiseY ? msigmaImageNoiseX:msigmaImageNoiseY,2);
528+
Rn *= pow(mnImageNoiseSigma, 2);
530529
}
531530
else
532531
{

‎src/rvio/Updater.h

+1-2
Original file line numberDiff line numberDiff line change
@@ -56,8 +56,7 @@ class Updater
5656
double mnCamRate;
5757

5858
// Sigma{pixel}
59-
double msigmaImageNoiseX;
60-
double msigmaImageNoiseY;
59+
double mnImageNoiseSigma;
6160

6261
// Extrinsics
6362
Eigen::Matrix3d mRic;

0 commit comments

Comments
 (0)