main.cpp

00001 //
00002 //  odometry.cpp ... Odometry calculation example
00003 //  (deterministic version)
00004 //
00005 //  2016.11.25 ... Y.Kuroda
00006 //
00007 //#include <iostream> I/Oストリームは使わないこと
00008 #include <Eigen/Core.h>
00009 using namespace std;
00010 using namespace Eigen;
00011 
00012 int main()
00013 {
00014     float dt = 0.1;
00015     Vector3f v(1, 0.1, 1);  // Vehicle velocity (v, omega, 1)
00016     Vector3f p(0,0,0);      // Position in world coordinates (x, y, yaw)
00017     Matrix3f M;             // Odometry transition matrix
00018 
00019     for(int i=0; i<1000; i++){
00020 
00021         if(i==500)
00022             v << 1, -0.2, 1;    // just an example of controlling
00023 
00024         printf("%f, %f, %f, %f\n", i*dt, p[0], p[1], p[2]); 
00025 
00026         M << dt*cos(p[2]),  0, p[0],    // create transition matrix
00027              dt*sin(p[2]),  0, p[1],
00028                         0, dt, p[2];
00029     
00030         p = M * v;          // move!!
00031     }
00032 }
00033