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

imuodom fix

Browse Source
This commit is contained in:
Francesco Gatti
2020-03-09 22:05:34 +01:00
parent 406f8cc9b3
commit 8944778dcf
2 changed files with 34 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
include/tkDNN/ImuOdom.h
+22 -3
View File
@@ -35,6 +35,9 @@ class ImuOdom {
// output eigen CPU
Eigen::MatrixXf deltaP, deltaQ;
Eigen::MatrixXd odomPOS, odomROT;
Eigen::Isometry3f tf = Eigen::Isometry3f::Identity();
ImuOdom() {}
virtual ~ImuOdom() {}
@@ -101,8 +104,11 @@ class ImuOdom {
odim0 = d0->output_dim;
odim1 = d1->output_dim;
deltaP.resize(1, odim0.tot());
deltaQ.resize(1, odim1.tot());
deltaP.resize(odim0.tot(), 1);
deltaQ.resize(odim1.tot(), 1);
odomPOS = Eigen::MatrixXd::Zero(3, 1);
odomROT = Eigen::MatrixXd::Identity(3, 3);
}
void update(dnnType *x0, dnnType *x1, dnnType *x2) {
@@ -116,7 +122,20 @@ class ImuOdom {
net->infer(dim, nullptr);
checkCuda( cudaMemcpy(deltaP.data(), o0_d, odim0.tot()*sizeof(dnnType), cudaMemcpyDeviceToHost) );
checkCuda( cudaMemcpy(deltaQ.data(), o1_d, odim1.tot()*sizeof(dnnType), cudaMemcpyDeviceToHost) );
checkCuda( cudaMemcpy(deltaQ.data(), o1_d, odim1.tot()*sizeof(dnnType), cudaMemcpyDeviceToHost) );
// compute odom
Eigen::Quaterniond q;
q.w() = deltaQ(0);
q.x() = deltaQ(1);
q.y() = deltaQ(2);
q.z() = deltaQ(3);
odomPOS = odomPOS + odomROT*deltaP.cast<double>();
odomROT = odomROT * q.normalized().toRotationMatrix();
// compose tf
tf.matrix().block(0, 0, 3, 3) = odomROT.cast<float>();
tf.matrix().block(0, 3, 3, 1) = odomPOS.cast<float>();
}
};
tests/imuodom/imuodom.cpp
+12 -7
View File
@@ -47,21 +47,26 @@ int main() {
readBinaryFile(o0_bin, ImuNet.odim0.tot()*N, &out0_h, &out0);
readBinaryFile(o1_bin, ImuNet.odim1.tot()*N, &out1_h, &out1);
for(int i=0; i<N; i++) {
TIMER_START
std::ofstream path("path.txt");
for(int i=0; i<N; i++) {
std::cout<<"i: "<<i<<"\n";
//TIMER_START
// Inference
ImuNet.update(i0_h, i1_h, i2_h);
//TIMER_STOP
TIMER_STOP
// log path
path<<ImuNet.odomPOS(0)<<" "<<ImuNet.odomPOS(1)<<" "<< ImuNet.odomPOS(2)<<"\n";
path.flush();
// Print real test
printCenteredTitle( (std::string(" CHECK RESULT ") + std::to_string(i) + " ").c_str() , '=');
//printCenteredTitle( (std::string(" CHECK RESULT ") + std::to_string(i) + " ").c_str() , '=');
//ImuNet.odim0.print();
checkResult(ImuNet.odim0.tot(), out0, ImuNet.o0_d);
//checkResult(ImuNet.odim0.tot(), out0, ImuNet.o0_d);
//ImuNet.odim1.print();
checkResult(ImuNet.odim0.tot(), out1, ImuNet.o1_d);
//checkResult(ImuNet.odim0.tot(), out1, ImuNet.o1_d);
i0_h += ImuNet.dim0.tot();
i1_h += ImuNet.dim1.tot();