mmr/tkDNN
Archived
1
0
Fork 0
Code Issues 46 Pull Requests 12 Releases 4 Activity

SEP model

Browse Source
This commit is contained in:
Francesco Gatti
2020-05-03 15:54:39 +02:00
parent 0ef8aaf616
commit 8d45259b15
3 changed files with 22 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
include/tkDNN/ImuOdom.h
+14 -3
View File
@@ -35,7 +35,8 @@ class ImuOdom {
// output eigen CPU // output eigen CPU
Eigen::MatrixXf deltaP, deltaQ; Eigen::MatrixXf deltaP, deltaQ;
Eigen::MatrixXd odomPOS, odomROT; Eigen::MatrixXd odomPOS, odomEULER;
Eigen::Matrix3d odomROT;
Eigen::Isometry3f tf = Eigen::Isometry3f::Identity(); Eigen::Isometry3f tf = Eigen::Isometry3f::Identity();
ImuOdom() {} ImuOdom() {}
@@ -109,7 +110,7 @@ class ImuOdom {
odomPOS = Eigen::MatrixXd::Zero(3, 1); odomPOS = Eigen::MatrixXd::Zero(3, 1);
odomROT = Eigen::MatrixXd::Identity(3, 3); odomROT = Eigen::MatrixXd::Identity(3, 3);
odomEULER = Eigen::MatrixXd::Zero(3, 1);
return true; return true;
} }
@@ -132,8 +133,18 @@ class ImuOdom {
q.x() = deltaQ(1); q.x() = deltaQ(1);
q.y() = deltaQ(2); q.y() = deltaQ(2);
q.z() = deltaQ(3); q.z() = deltaQ(3);
odomPOS = odomPOS + odomROT*deltaP.cast<double>(); odomPOS = odomPOS + deltaP.cast<double>();
odomROT = odomROT * q.normalized().toRotationMatrix(); odomROT = odomROT * q.normalized().toRotationMatrix();
// compute euler
auto newEULER = odomROT.eulerAngles(0, 1, 2);
for(int i=0; i<3; i++) {
while( fabs(newEULER(i) - odomEULER(i)) > M_PI_2 ) {
newEULER(i) += newEULER(i) - odomEULER(i) > 0 ? -M_PI : +M_PI;
//std::cout<<newEULER(i)<<" "<<odomEULER(i)<<"\n";
}
}
odomEULER = newEULER;
// compose tf // compose tf
tf.matrix().block(0, 0, 3, 3) = odomROT.cast<float>(); tf.matrix().block(0, 0, 3, 3) = odomROT.cast<float>();
tests/imuodom/imuodom.cpp
+6 -15
View File
@@ -7,18 +7,6 @@ const char *i2_bin = "imuodom/layers/input2.bin";
const char *o0_bin = "imuodom/layers/output0.bin"; const char *o0_bin = "imuodom/layers/output0.bin";
const char *o1_bin = "imuodom/layers/output1.bin"; const char *o1_bin = "imuodom/layers/output1.bin";
const char *c0_bin = "imuodom/layers/conv1d_7.bin";
const char *c1_bin = "imuodom/layers/conv1d_8.bin";
const char *c2_bin = "imuodom/layers/conv1d_9.bin";
const char *c3_bin = "imuodom/layers/conv1d_10.bin";
const char *c4_bin = "imuodom/layers/conv1d_11.bin";
const char *c5_bin = "imuodom/layers/conv1d_12.bin";
const char *l0_bin = "imuodom/layers/bidirectional_3.bin";
const char *l1_bin = "imuodom/layers/bidirectional_4.bin";
const char *d0_bin = "imuodom/layers/dense_3.bin";
const char *d1_bin = "imuodom/layers/dense_4.bin";
int main() { int main() {
downloadWeightsifDoNotExist(i0_bin, "imuodom", "https://cloud.hipert.unimore.it/s/ZAy34K5w2ixED6x/download"); downloadWeightsifDoNotExist(i0_bin, "imuodom", "https://cloud.hipert.unimore.it/s/ZAy34K5w2ixED6x/download");
@@ -26,7 +14,7 @@ int main() {
tk::dnn::ImuOdom ImuNet; tk::dnn::ImuOdom ImuNet;
ImuNet.init("imuodom/layers/"); ImuNet.init("imuodom/layers/");
const int N = 10000; //19513; const int N = 19513;
// Network layout // Network layout
tk::dnn::dataDim_t dim0(1, 4, 1, 100); tk::dnn::dataDim_t dim0(1, 4, 1, 100);
@@ -60,7 +48,9 @@ int main() {
//TIMER_STOP //TIMER_STOP
// log path // log path
path<<ImuNet.odomPOS(0)<<" "<<ImuNet.odomPOS(1)<<" "<< ImuNet.odomPOS(2)<<"\n"; path<<ImuNet.odomPOS(0)<<" "<<ImuNet.odomPOS(1)<<" "<< ImuNet.odomPOS(2)<<" ";
path<<ImuNet.odomEULER(0)<<" "<<ImuNet.odomEULER(1)<<" "<< ImuNet.odomEULER(2)<<"\n";
path.flush(); path.flush();
// Print real test // Print real test
@@ -77,6 +67,7 @@ int main() {
out1 += ImuNet.odim1.tot(); out1 += ImuNet.odim1.tot();
} }
system("cat path.txt | gnuplot -p -e \"set datafile separator ' '; plot '-'\""); system("cat path.txt | cut -d\" \" -f1,2 | gnuplot -p -e \"set datafile separator ' '; plot '-'\"");
system("cat path.txt | cut -d\" \" -f6 | gnuplot -p -e \"set datafile separator ' '; plot '-'\"");
return ret_cudnn; return ret_cudnn;
} }
tests/imuodom/infer.py
+2 -2
View File
@@ -16,9 +16,9 @@ def bin_write(f, data):
bin = struct.pack(fmt, *data) bin = struct.pack(fmt, *data)
f.write(bin) f.write(bin)
# USE weight_exporter to generare wgs bins
if __name__ == '__main__': if __name__ == '__main__':
print("DATA FORMAT: ", keras.backend.image_data_format()) print("DATA FORMAT: ", keras.backend.image_data_format())