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

added tiny tum

Browse Source
This commit is contained in:
Raphael Maenle
2019-06-25 12:03:08 +02:00
parent c565033d7f
commit 6f7f8b7892
4 changed files with 39 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
config/camchain-imucam-tum-scaled.yaml
View File

@@ -5,11 +5,11 @@ cam0:
-0.029453096117288798, -0.9989836729399656, 0.034119442089241274, -0.0697294754693238, -0.029453096117288798, -0.9989836729399656, 0.034119442089241274, -0.0697294754693238,
0.0, 0.0, 0.0, 1.0] 0.0, 0.0, 0.0, 1.0]
camera_model: pinhole camera_model: pinhole
distortion_coeffs: [0.010171079892421483, -0.010816440029919381, 0.005942781769412756, distortion_coeffs: [0.0034823894022493434, 0.0007150348452162257, -0.0020532361418706202,
-0.001662284667857643] 0.00020293673591811182]
distortion_model: equidistant distortion_model: equidistant
intrinsics: [95.2026071784, 95.2029854486, 127.573663262, 128.582615763] intrinsics: [190.97847715128717, 190.9733070521226, 254.93170605935475, 256.8974428996504]
resolution: [256, 256] resolution: [512, 512]
rostopic: /cam0/image_raw rostopic: /cam0/image_raw
cam1: cam1:
T_cam_imu: T_cam_imu:
@@ -23,11 +23,11 @@ cam1:
0.0006997790813734836, 0.04690628718225568, 0.9988990492196964, -0.0014663556043866572, 0.0006997790813734836, 0.04690628718225568, 0.9988990492196964, -0.0014663556043866572,
0.0, 0.0, 0.0, 1.0] 0.0, 0.0, 0.0, 1.0]
camera_model: pinhole camera_model: pinhole
distortion_coeffs: [0.01371679169245271, -0.015567360615942622, 0.00905043103315326, distortion_coeffs: [0.0034003170790442797, 0.001766278153469831, -0.00266312569781606,
-0.002347858896562788] 0.0003299517423931039]
distortion_model: equidistant distortion_model: equidistant
intrinsics: [94.8217471066, 94.8164593555, 126.391667581, 127.571024044] intrinsics: [190.44236969414825, 190.4344384721956, 252.59949716835982, 254.91723064636983]
resolution: [256, 256] resolution: [512, 512]
rostopic: /cam1/image_raw rostopic: /cam1/image_raw
T_imu_body: T_imu_body:
[1.0000, 0.0000, 0.0000, 0.0000, [1.0000, 0.0000, 0.0000, 0.0000,

4
launch/image_processor_tinytum.launch
View File

@@ -25,8 +25,8 @@
<param name="stereo_threshold" value="5"/> <param name="stereo_threshold" value="5"/>
<remap from="~imu" to="/imu0"/> <remap from="~imu" to="/imu0"/>
<remap from="~cam0_image" to="/cam0/new_image_raw"/> <remap from="~cam0_image" to="/cam0/image_raw"/>
<remap from="~cam1_image" to="/cam1/new_image_raw"/> <remap from="~cam1_image" to="/cam1/image_raw"/>
</node> </node>
</group> </group>

6
launch/msckf_vio_tum.launch
View File

@@ -18,11 +18,11 @@
output="screen"> output="screen">
<!-- Photometry Flag--> <!-- Photometry Flag-->
<param name="PHOTOMETRIC" value="true"/> <param name="PHOTOMETRIC" value="false"/>
<!-- Debugging Flaggs --> <!-- Debugging Flaggs -->
<param name="StreamPause" value="true"/> <param name="StreamPause" value="true"/>
<param name="PrintImages" value="true"/> <param name="PrintImages" value="false"/>
<param name="GroundTruth" value="false"/> <param name="GroundTruth" value="false"/>
<param name="patch_size_n" value="5"/> <param name="patch_size_n" value="5"/>
@@ -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>

33
src/msckf_vio.cpp
View File

@@ -1664,10 +1664,29 @@ void MsckfVio::featureJacobian(
{ {
ofstream myfile; ofstream myfile;
myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt"); myfile.open("/home/raphael/dev/octave/org2octave");
myfile << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x.colPivHouseholderQr().solve(r) << endl; myfile << "# Created by Octave 3.8.1, Wed Jun 12 14:36:37 2019 CEST <raphael@raphael-desktop>\n"
<< "# name: Hx\n"
<< "# type: matrix\n"
<< "# rows: " << H_xj.rows() << "\n"
<< "# columns: " << H_xj.cols() << "\n"
<< H_xj << endl;
myfile << "# name: Hy\n"
<< "# type: matrix\n"
<< "# rows: " << H_fj.rows() << "\n"
<< "# columns: " << H_fj.cols() << "\n"
<< H_fj << endl;
myfile << "# name: r\n"
<< "# type: matrix\n"
<< "# rows: " << r_j.rows() << "\n"
<< "# columns: " << 1 << "\n"
<< r_j << endl;
myfile.close(); myfile.close();
cout << "---------- LOGGED -------- " << endl; cout << "---------- LOGGED ORG -------- " << endl;
} }
return; return;
@@ -1716,8 +1735,8 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
MatrixXd K = K_transpose.transpose(); MatrixXd K = K_transpose.transpose();
// Compute the error of the state. // Compute the error of the state.
VectorXd delta_x = K * r; VectorXd delta_x = K * r;
cout << "reg rotate: " << delta_x[0] << ", " << delta_x[1] << ", " << delta_x[2] << endl; // cout << "reg rotate: " << delta_x[0] << ", " << delta_x[1] << ", " << delta_x[2] << endl;
cout << "reg update: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; // cout << "reg update: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl;
// Update the IMU state. // Update the IMU state.
if(PHOTOMETRIC) return; if(PHOTOMETRIC) return;
@@ -1821,8 +1840,8 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r
// Compute the error of the state. // Compute the error of the state.
VectorXd delta_x = K * r; VectorXd delta_x = K * r;
cout << "msc rotate: " << delta_x[0] << ", " << delta_x[1] << ", " << delta_x[2] << endl; // cout << "msc rotate: " << delta_x[0] << ", " << delta_x[1] << ", " << delta_x[2] << endl;
cout << "msc update: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl; // cout << "msc update: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl;
// Update the IMU state. // Update the IMU state.
if (not PHOTOMETRIC) return; if (not PHOTOMETRIC) return;