#include<stdio.h>
#include<math.h>
#include <exception>
#include <iostream>
#include <vector>
#include <fstream>
#include<sstream>
#include <stdlib.h>
#include <Eigen/Core>
#include <Eigen/Dense>
#include<Eigen/Eigen>
#include<Eigen/Geometry>
#include<math.h>
#include <exception>
#include <iostream>
#include <vector>
#include <fstream>
#include <boost/concept_check.hpp>
using namespace Eigen;
using namespace std;
int main()
{
/* string imu_file,end_file;
imu_file="/home/xs/SimlateIMU/indoor/end_3939.txt";
end_file="/home/xs/SimlateIMU/indoor/end.txt";
ifstream imuptr(imu_file.c_str());//读取imu
ofstream sampleptr(end_file);
string imuStr;
int countLine=0;
int tag=0;
std::string tempValue[7];
for(int i=0;i<7;i++)
tempValue[i]="";
while(getline(imuptr,imuStr))//以空格区分
{ countLine++;
if(countLine%12==0)
continue;
// if(tag==0)
// {
// if(countLine%3==0)
// {
// tag=1;
// continue;
// }
// }
// else
// {
// if(countLine%4==0)
// {
// tag=0;
// countLine;
// }
// }
std::stringstream linestream(imuStr);
std::string value;
int valueNum=0;
while(getline(linestream,value,','))
{
if(valueNum>=0&&valueNum<7)
tempValue[valueNum]=value.c_str();
++valueNum;
}
sampleptr<<tempValue[0]<<","<<tempValue[1]<<","<<tempValue[2]<<","<<tempValue[3]<<","<<tempValue[4]<<","<<tempValue[5]<<","<<tempValue[6]<<endl;
//countLine++;
}
sampleptr.close();
imuptr.close();
// timeptr.close();
return 0;*/
//添加新的时间戳
string time_file, imu_file,end_file;
time_file="/home/xs/SimlateIMU/indoor/imunew.csv";
imu_file="/home/xs/SimlateIMU/indoor/end_3593.txt";
end_file="/home/xs/SimlateIMU/indoor/end.txt";
ifstream timeptr(time_file.c_str());//读取时间
ifstream imuptr(imu_file.c_str());//读取imu
ofstream sampleptr(end_file);
// sampleptr<<"% timestamp(sec), $a_{is}^s$(m/s^2), $\Omega_{is}^s$(rad/sec), $v_{ws}^w$(m/s), $gravity^e$."<<endl;
string imuStr,timeStr;
int countLine=0;
std::string tempValue[7];
for(int i=0;i<7;i++)
tempValue[i]="";
while(getline(imuptr,imuStr)&&getline(timeptr,timeStr))
{
//std::stringstream linestream(imuStr);
stringstream linestream(imuStr),timestream(timeStr);
std::string value,tempTime;
int valueNum=0;
while(getline(linestream,value,',')&&getline(timestream,tempTime,','))
{
if(0==valueNum)
{
// timeptr>>tempTime;
tempValue[valueNum]=tempTime.c_str();
// tempValue[valueNum]=timeStr[0];
// if(==0)//从文件读时间戳
// {
// timeptr>>tempTime;
// cerr<< "read:value+"<<tempTime<<"+"<<endl;;
// tempValue[valueNum]=tempTime;
// }
// else
// {
// tempValue[valueNum]=value.c_str();
// }
}
else if(valueNum<7&&valueNum>0)
{
tempValue[valueNum]=value.c_str();
}
++valueNum;
}
// countLine++;
//在这里开始生成结合后的文件
sampleptr<<tempValue[0]<<","<<tempValue[1]<<","<<tempValue[2]<<","<<tempValue[3]<<","<<tempValue[4]<<","<<tempValue[5]<<","<<tempValue[6]<<endl;
}
sampleptr.close();
imuptr.close();
timeptr.close();
/*
//imu文件与原始时间戳的匹配,保持差距一致
string time_file, imu_file,end_file;
time_file="/home/xs/SimlateIMU/indoor/time.txt";
imu_file="/home/xs/SimlateIMU/indoor/noise.txt";
end_file="/home/xs/SimlateIMU/indoor/end.txt";
ifstream timeptr(time_file.c_str());//读取时间
ifstream imuptr(imu_file.c_str());//读取imu
ofstream sampleptr(end_file);
sampleptr<<"% timestamp(sec), $a_{is}^s$(m/s^2), $\Omega_{is}^s$(rad/sec), $v_{ws}^w$(m/s), $gravity^e$."<<endl;
string imuStr;
int countLine=0;
std::string tempValue[7];
for(int i=0;i<7;i++)
tempValue[i]="";
int valueNum=0;
int lasttime=0;
int nowtime=0;
int i;
std::string value,tempTime;
while(!timeptr.eof())
{
timeptr>>tempTime;
nowtime=atoi(tempTime.c_str());
nowtime/=10;
if(nowtime!=lasttime)
{
getline(imuptr,imuStr);
stringstream linestream(imuStr);
valueNum=0;
while(getline(linestream,value,' '))
{
if(0<=valueNum&&valueNum<7)
{
tempValue[valueNum]=value.c_str();
}
++valueNum;
}
lasttime=nowtime;
//在这里开始生成结合后的文件
sampleptr<<tempValue[0]<<","<<tempValue[1]<<","<<tempValue[2]<<","<<tempValue[3]<<","<<tempValue[4]<<","<<tempValue[5]<<","<<tempValue[6]<<endl;
getline(imuptr,imuStr);
getline(imuptr,imuStr);
getline(imuptr,imuStr);
}
else
{
getline(imuptr,imuStr);getline(imuptr,imuStr);getline(imuptr,imuStr);getline(imuptr,imuStr);
}
}
sampleptr.close();
imuptr.close();
timeptr.close();
return 0;*/
//去掉最左边的逗号
/* string imu_file,end_file;
imu_file="/home/xs/SimlateIMU/indoor/samples.txt";
end_file="/home/xs/SimlateIMU/indoor/imuRight.txt";
ifstream imuptr(imu_file.c_str());//读取imu
ofstream sampleptr(end_file);
// sampleptr<<"% timestamp(sec), $a_{is}^s$(m/s^2), $\Omega_{is}^s$(rad/sec), $v_{ws}^w$(m/s), $gravity^e$."<<endl;
string imuStr;
int countLine=0;
std::string tempValue[13];
for(int i=0;i<13;i++)
tempValue[i]="";
while(getline(imuptr,imuStr))
{
//std::stringstream linestream(imuStr);
stringstream linestream(imuStr);
std::string value;
int valueNum=0;
while(getline(linestream,value,','))
{
if(0<valueNum&&valueNum<14)
{
tempValue[valueNum-1]=value.c_str();
// if(==0)//从文件读时间戳
// {
// timeptr>>tempTime;
// cerr<< "read:value+"<<tempTime<<"+"<<endl;;
// tempValue[valueNum]=tempTime;
// }
// else
// {
// tempValue[valueNum]=value.c_str();
// }
}
++valueNum;
}
// countLine++;
//在这里开始生成结合后的文件
sampleptr<<tempValue[0]<<","<<tempValue[1]<<","<<tempValue[2]<<","<<tempValue[3]<<","<<tempValue[4]<<","<<tempValue[5]<<","<<tempValue[6]<<","<<tempValue[7]<<","<<tempValue[8]<<","<<tempValue[9]<<","<<tempValue[10]<<","<<tempValue[11]<<","<<tempValue[12]<<endl;
}
sampleptr.close();
imuptr.close();
//timeptr.close();
return 0;*/
/*
string imu_file,end_file;
imu_file="/home/xs/SimlateIMU/tim.txt";
// end_file="/home/xs/SimlateIMU/tim.txt";
ifstream imuptr(imu_file.c_str());//
ofstream sampleptr(end_file);
//sampleptr<<"% timestamp(sec), $a_{is}^s$(m/s^2), $\Omega_{is}^s$(rad/sec), $v_{ws}^w$(m/s), $gravity^e$."<<endl;
string imuStr;
int countLine=0;
std::string tempValue[4];
for(int i=0;i<4;i++)
tempValue[i]="";
double ex=0.0,ey=0.0,ez=0.0,ew=0.0;
while(getline(imuptr,imuStr))
{
//std::stringstream linestream(imuStr);
stringstream linestream(imuStr);
std::string value;
int valueNum=0;
while(getline(linestream,value,','))
{
if(valueNum==0)
{
ew=atof(value.c_str());
// tempValue[valueNum-4]=value.c_str();
}
else if(valueNum==1)
{
ex=atof(value.c_str());
}
else if(valueNum==2)
{
ey=atof(value.c_str());
}
else if(valueNum==3)
{
ez=atof(value.c_str());;
Eigen::Quaterniond q;
q.x() = ex;
q.y() =ey;
q.z() = ez;
q.w() = ew;
Eigen::Vector3d euler = q.toRotationMatrix().eulerAngles(2, 1, 0);
// ex=euler[2];
// ey=euler[1];
// ez=euler[0];
cout << "Quaterniond2Euler result is:" <<endl;
cout << "x = "<< euler[2] << endl ;
cout << "y = "<< euler[1] << endl ;
cout << "z = "<< euler[0] << endl << endl;
// sampleptr<<euler[2]<<","<<tempValue[1]<<","<<tempValue[2]<<","<<tempValue[3]<<endl;
}
++valueNum;
}
countLine++;
//在这里开始生成结合后的文件
// sampleptr<<tempValue<<endl;
sampleptr<<tempValue[0]<<","<<tempValue[1]<<","<<tempValue[2]<<","<<tempValue[3]<<endl;
}
sampleptr.close();
imuptr.close();
//timeptr.close();
return 0;
*/
/* string imu_file,end_file;
imu_file="/home/xs/SimlateIMU/nb/samples.txt";
end_file="/home/xs/SimlateIMU/nb/imuRight.txt";
ifstream imuptr(imu_file.c_str());//读取imu
ofstream sampleptr(end_file);
// sampleptr<<"% timestamp(sec), $a_{is}^s$(m/s^2), $\Omega_{is}^s$(rad/sec), $v_{ws}^w$(m/s), $gravity^e$."<<endl;
string imuStr;
int countLine=0;
std::string tempValue[13];
for(int i=0;i<13;i++)
tempValue[i]="";
while(getline(imuptr,imuStr))
{
//std::stringstream linestream(imuStr);
stringstream linestream(imuStr);
std::string value;
int valueNum=0;
while(getline(linestream,value,','))
{
if(0<valueNum)
{
tempValue[valueNum-1]=value.c_str();
// if(==0)//从文件读时间戳
// {
// timeptr>>tempTime;
// cerr<< "read:value+"<<tempTime<<"+"<<endl;;
// tempValue[valueNum]=tempTime;
// }
// else
// {
// tempValue[valueNum]=value.c_str();
// }
}
++valueNum;
}
// countLine++;
//在这里开始生成结合后的文件
sampleptr<<tempValue[0]<<","<<tempValue[1]<<","<<tempValue[2]<<","<<tempValue[3]<<","<<tempValue[4]<<","<<tempValue[5]<<","<<tempValue[6]<<","<<tempValue[7]<<","<<tempValue[8]<<","<<tempValue[9]<<","<<tempValue[10]<<","<<tempValue[11]<<","<<tempValue[12]<<endl;
}
sampleptr.close();
imuptr.close();
//timeptr.close();
return 0;*/
}
cmakelisttxt:
SET (PROJECT_NAME change)
PROJECT(${PROJECT_NAME})
CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
set(CMAKE_BUILD_TYPE "Debug")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O3 -march=native ")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3 -march=native")
SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH}
"${PROJECT_SOURCE_DIR}/cmake_modules/")
find_package(Eigen3 REQUIRED)
include_directories(
${EIGEN3_INCLUDE_DIR}
#${Geographic_INCLUDE_DIR}/usr/local/include/GeographicLib
)
ADD_EXECUTABLE(${PROJECT_NAME} change.cpp)
TARGET_LINK_LIBRARIES (${PROJECT_NAME} ${LINK_LIBS})