diff --git a/.idea/.gitignore b/.idea/.gitignore
new file mode 100644
index 0000000..73f69e0
--- /dev/null
+++ b/.idea/.gitignore
@@ -0,0 +1,8 @@
+# Default ignored files
+/shelf/
+/workspace.xml
+# Datasource local storage ignored files
+/dataSources/
+/dataSources.local.xml
+# Editor-based HTTP Client requests
+/httpRequests/
diff --git a/.idea/misc.xml b/.idea/misc.xml
new file mode 100644
index 0000000..339cee9
--- /dev/null
+++ b/.idea/misc.xml
@@ -0,0 +1,6 @@
+
+
+
+
+
+
\ No newline at end of file
diff --git a/.idea/modules.xml b/.idea/modules.xml
new file mode 100644
index 0000000..1ef27e7
--- /dev/null
+++ b/.idea/modules.xml
@@ -0,0 +1,8 @@
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/.idea/rpg_svo.iml b/.idea/rpg_svo.iml
new file mode 100644
index 0000000..f08604b
--- /dev/null
+++ b/.idea/rpg_svo.iml
@@ -0,0 +1,2 @@
+
+
\ No newline at end of file
diff --git a/.idea/vcs.xml b/.idea/vcs.xml
new file mode 100644
index 0000000..94a25f7
--- /dev/null
+++ b/.idea/vcs.xml
@@ -0,0 +1,6 @@
+
+
+
+
+
+
\ No newline at end of file
diff --git a/CPU_version/svo/doc/notation.ipe b/CPU_version/svo/doc/notation.ipe
new file mode 100644
index 0000000..f8c1efe
--- /dev/null
+++ b/CPU_version/svo/doc/notation.ipe
@@ -0,0 +1,312 @@
+
+
+
+
+
+
+
+0 0 m
+-1 0.333 l
+-1 -0.333 l
+h
+
+
+
+
+0 0 m
+-1 0.333 l
+-1 -0.333 l
+h
+
+
+
+
+0.6 0 0 0.6 0 0 e
+0.4 0 0 0.4 0 0 e
+
+
+
+
+0.6 0 0 0.6 0 0 e
+
+
+
+
+
+0.6 0 0 0.6 0 0 e
+0.4 0 0 0.4 0 0 e
+
+
+0.4 0 0 0.4 0 0 e
+
+
+
+
+
+-0.6 -0.6 m
+0.6 -0.6 l
+0.6 0.6 l
+-0.6 0.6 l
+h
+-0.4 -0.4 m
+0.4 -0.4 l
+0.4 0.4 l
+-0.4 0.4 l
+h
+
+
+
+
+-0.6 -0.6 m
+0.6 -0.6 l
+0.6 0.6 l
+-0.6 0.6 l
+h
+
+
+
+
+
+-0.6 -0.6 m
+0.6 -0.6 l
+0.6 0.6 l
+-0.6 0.6 l
+h
+-0.4 -0.4 m
+0.4 -0.4 l
+0.4 0.4 l
+-0.4 0.4 l
+h
+
+
+-0.4 -0.4 m
+0.4 -0.4 l
+0.4 0.4 l
+-0.4 0.4 l
+h
+
+
+
+
+
+
+-0.43 -0.57 m
+0.57 0.43 l
+0.43 0.57 l
+-0.57 -0.43 l
+h
+
+
+-0.43 0.57 m
+0.57 -0.43 l
+0.43 -0.57 l
+-0.57 0.43 l
+h
+
+
+
+
+
+0 0 m
+-1 0.333 l
+-1 -0.333 l
+h
+
+
+
+
+0 0 m
+-1 0.333 l
+-0.8 0 l
+-1 -0.333 l
+h
+
+
+
+
+0 0 m
+-1 0.333 l
+-0.8 0 l
+-1 -0.333 l
+h
+
+
+
+
+-1 0.333 m
+0 0 l
+-1 -0.333 l
+
+
+
+
+0 0 m
+-1 0.333 l
+-1 -0.333 l
+h
+-1 0 m
+-2 0.333 l
+-2 -0.333 l
+h
+
+
+
+
+0 0 m
+-1 0.333 l
+-1 -0.333 l
+h
+-1 0 m
+-2 0.333 l
+-2 -0.333 l
+h
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+$_w\mathtt{p}$
+$\overrightarrow{\mathcal{F}}_w$
+
+64 576 m
+96 576 l
+
+
+64 576 m
+64 608 l
+
+
+64 576 m
+80 592 l
+
+
+144 688 m
+176 688 l
+
+
+144 688 m
+144 720 l
+
+
+144 688 m
+160 704 l
+
+
+285.262 759.615 m
+162.017 735.456
+129.232 628.801 s
+
+
+$\mathtt{T}_{f,w}$
+
+128.441 626.745 m
+320.625 647.224 l
+
+
+331.681 587.399 m
+331.681 587.399 l
+
+$x$
+$z$
+$y$
+
+256 736 m
+224 704 l
+320 704 l
+352 736 l
+h
+
+
+285.262 759.265 m
+322.176 651.119 l
+
+$\mathtt{px}$
+
+285.612 758.915 m
+315.723 669.632 l
+
+$\mathtt{f}$
+$\overrightarrow{\mathcal{F}}_f$
+
+255.668 719.918 m
+274.728 719.957 l
+
+
+256.107 720.028 m
+246.655 710.527 l
+
+$v$
+$u$
+
+$\mathbf{p}$
+$_f\mathtt{p}$
+
+
diff --git a/CPU_version/svo/doc/notation.pdf b/CPU_version/svo/doc/notation.pdf
new file mode 100644
index 0000000..3bc3fcd
Binary files /dev/null and b/CPU_version/svo/doc/notation.pdf differ
diff --git a/CPU_version/svo/doc/notation.png b/CPU_version/svo/doc/notation.png
new file mode 100644
index 0000000..d613b70
Binary files /dev/null and b/CPU_version/svo/doc/notation.png differ
diff --git a/CPU_version/svo/src/imu_integration.cpp b/CPU_version/svo/src/imu_integration.cpp
index c2a720f..a66223c 100644
--- a/CPU_version/svo/src/imu_integration.cpp
+++ b/CPU_version/svo/src/imu_integration.cpp
@@ -16,7 +16,9 @@
Imu_Integration::Imu_Integration(Sophus::SE3& SE_init){
graphPtr=std::make_shared();
valuesPtr=std::make_shared();
- parameterPtr = gtsam::PreintegratedCombinedMeasurements::Params::MakeSharedD();// TODO check the IMU frame
+ gtsam::Vector4 g=SE_init.matrix()*gtsam::Vector4(0, 0, 9.8,1.0);
+ parameterPtr = boost::shared_ptr(new gtsam::PreintegrationCombinedParams(gtsam::Vector3(g.x(),g.y(),g.z())));
+ parameterPtr->setBodyPSensor(gtsam::Pose3(SE_init.matrix()));// The pose of the sensor in the body frame
parameterPtr->accelerometerCovariance=gtsam::I_3x3 * svo::Config::ACC_Noise(); // acc white noise in continuous
parameterPtr->gyroscopeCovariance=gtsam::I_3x3 * svo::Config::GYO_Noise();// gyro white noise in continuous
parameterPtr->integrationCovariance=gtsam::I_3x3 * svo::Config::IUC(); // integration uncertainty continuous
@@ -148,4 +150,4 @@ bool Imu_Integration::predict(boost::shared_ptr& new_frame,std::size
}
}
return true;
-}
\ No newline at end of file
+}
diff --git a/GPU_version/svo/.idea/.name b/GPU_version/svo/.idea/.name
new file mode 100644
index 0000000..4b0f94b
--- /dev/null
+++ b/GPU_version/svo/.idea/.name
@@ -0,0 +1 @@
+gpu_svo
\ No newline at end of file
diff --git a/GPU_version/svo/.idea/svo.iml b/GPU_version/svo/.idea/svo.iml
new file mode 100644
index 0000000..f08604b
--- /dev/null
+++ b/GPU_version/svo/.idea/svo.iml
@@ -0,0 +1,2 @@
+
+
\ No newline at end of file
diff --git a/GPU_version/svo/.idea/vcs.xml b/GPU_version/svo/.idea/vcs.xml
index 6c0b863..64713b8 100644
--- a/GPU_version/svo/.idea/vcs.xml
+++ b/GPU_version/svo/.idea/vcs.xml
@@ -1,6 +1,7 @@
+
\ No newline at end of file
diff --git a/GPU_version/svo/src/imu_integration.cpp b/GPU_version/svo/src/imu_integration.cpp
index 2238f7c..b3f80ab 100644
--- a/GPU_version/svo/src/imu_integration.cpp
+++ b/GPU_version/svo/src/imu_integration.cpp
@@ -16,7 +16,12 @@
Imu_Integration::Imu_Integration(Sophus::SE3& SE_init){
graphPtr=std::make_shared();
valuesPtr=std::make_shared();
- parameterPtr = gtsam::PreintegratedCombinedMeasurements::Params::MakeSharedD();// TODO check the IMU frame
+ // Example: pitch and roll of aircraft in an ENU Cartesian frame.
+ // If pitch and roll are zero for an aerospace frame,
+ // that means Z is pointing down, i.e., direction of Z = (0,0,-1)
+ gtsam::Vector4 g=SE_init.matrix()*gtsam::Vector4(0, 0, 9.8,1.0);
+ parameterPtr = boost::shared_ptr(new gtsam::PreintegrationCombinedParams(gtsam::Vector3(g.x(),g.y(),g.z())));
+ parameterPtr->setBodyPSensor(gtsam::Pose3(SE_init.matrix()));// The pose of the sensor in the body frame
parameterPtr->accelerometerCovariance=gtsam::I_3x3 * svo::Config::ACC_Noise(); // acc white noise in continuous
parameterPtr->gyroscopeCovariance=gtsam::I_3x3 * svo::Config::GYO_Noise();// gyro white noise in continuous
parameterPtr->integrationCovariance=gtsam::I_3x3 * svo::Config::IUC(); // integration uncertainty continuous
@@ -122,14 +127,6 @@ bool Imu_Integration::predict(boost::shared_ptr& new_frame,std::size
++num_obs;
}
// Now optimize.
- //gtsam::ISAM2 optimizer(*optimizerParamPtr);
- //optimizer.update(*graphPtr, *valuesPtr);
- // Each call to iSAM2 update(*) performs one iteration of the iterative
- // nonlinear solver. If accuracy is desired at the expense of time,
- // update(*) can be called additional times to perform multiple optimizer
- // iterations every step.
- //optimizer.update();
- // optimizer.print();
gtsam::LevenbergMarquardtOptimizer optimizer(*graphPtr, *valuesPtr);
reset(optimizer,new_frame);
diff --git a/GPU_version/vio/.idea/.name b/GPU_version/vio/.idea/.name
new file mode 100644
index 0000000..06a578d
--- /dev/null
+++ b/GPU_version/vio/.idea/.name
@@ -0,0 +1 @@
+vio_gpu
\ No newline at end of file
diff --git a/GPU_version/vio/.idea/svo_ros.iml b/GPU_version/vio/.idea/svo_ros.iml
new file mode 100644
index 0000000..f08604b
--- /dev/null
+++ b/GPU_version/vio/.idea/svo_ros.iml
@@ -0,0 +1,2 @@
+
+
\ No newline at end of file
diff --git a/GPU_version/vio/.idea/vcs.xml b/GPU_version/vio/.idea/vcs.xml
index 6c0b863..64713b8 100644
--- a/GPU_version/vio/.idea/vcs.xml
+++ b/GPU_version/vio/.idea/vcs.xml
@@ -1,6 +1,7 @@
+
\ No newline at end of file