Skip to content

Commit

Permalink
changed stod to stod98. Fixed logic bug in CMakeLists.txt
Browse files Browse the repository at this point in the history
  • Loading branch information
@RossHartley
RossHartley committed Sep 28, 2018
1 parent 981e64f commit 374e356
Show file tree
Hide file tree
Showing 5 changed files with 62 additions and 56 deletions.
12 changes: 9 additions & 3 deletions CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,11 +4,15 @@ cmake_minimum_required(VERSION 2.8.3)
project(inekf)

option(USE_CPP11 "Use flag -std=c++11" ON)
option(USE_MUTEX "Use mutex in code" ON)
option(USE_MUTEX "Use mutex in code" OFF)

if (USE_CPP11 EQUAL OFF AND USE_MUTEX EQUAL ON)
message("USE_CPP11: " ${USE_CPP11})
message("USE_MUTEX: " ${USE_MUTEX})

if (USE_MUTEX AND NOT USE_CPP11)
message("test")
message(FATAL_ERROR "You cannot have USE_CPP11 = OFF AND USE_MUTEX = ON")
endif (USE_CPP11 EQUAL OFF AND USE_MUTEX EQUAL ON)
endif (USE_MUTEX AND NOT USE_CPP11)

# Set compiler flags
if (USE_CPP11)
Expand All @@ -23,6 +27,8 @@ endif(USE_MUTEX)
SET(CMAKE_CXX_COMPILER /usr/bin/g++)

message("CMAKE_CXX_FLAGS: " ${CMAKE_CXX_FLAGS})
unset(USE_CPP11)
unset(USE_MUTEX)

# Set the output folder where your program will be created
set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/bin)
Expand Down
32 changes: 16 additions & 16 deletions src/examples/kinematics.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,11 +26,11 @@
using namespace std;
using namespace inekf;

double stod(const std::string &s) {
double stod98(const std::string &s) {
return atof(s.c_str());
}

int stoi(const std::string &s) {
int stoi98(const std::string &s) {
return atoi(s.c_str());
}

Expand Down Expand Up @@ -87,12 +87,12 @@ int main() {
assert((measurement.size()-2) == 6);
t = atof(measurement[1].c_str());
// Read in IMU data
imu_measurement << stod(measurement[2]),
stod(measurement[3]),
stod(measurement[4]),
stod(measurement[5]),
stod(measurement[6]),
stod(measurement[7]);
imu_measurement << stod98(measurement[2]),
stod98(measurement[3]),
stod98(measurement[4]),
stod98(measurement[5]),
stod98(measurement[6]),
stod98(measurement[7]);

// Propagate using IMU data
double dt = t - t_prev;
Expand All @@ -107,11 +107,11 @@ int main() {
vector<pair<int,bool> > contacts;
int id;
bool indicator;
t = stod(measurement[1]);
t = stod98(measurement[1]);
// Read in contact data
for (int i=2; i<measurement.size(); i+=2) {
id = stoi(measurement[i]);
indicator = bool(stod(measurement[i+1]));
id = stoi98(measurement[i]);
indicator = bool(stod98(measurement[i+1]));
contacts.push_back(pair<int,bool> (id, indicator));
}
// Set filter's contact state
Expand All @@ -126,18 +126,18 @@ int main() {
Eigen::Matrix4d pose = Eigen::Matrix4d::Identity();
Eigen::Matrix<double,6,6> covariance;
vectorKinematics measured_kinematics;
t = stod(measurement[1]);
t = stod98(measurement[1]);
// Read in kinematic data
for (int i=2; i<measurement.size(); i+=44) {
id = stoi(measurement[i]);
q = Eigen::Quaternion<double> (stod(measurement[i+1]),stod(measurement[i+2]),stod(measurement[i+3]),stod(measurement[i+4]));
id = stoi98(measurement[i]);
q = Eigen::Quaternion<double> (stod98(measurement[i+1]),stod98(measurement[i+2]),stod98(measurement[i+3]),stod98(measurement[i+4]));
q.normalize();
p << stod(measurement[i+5]),stod(measurement[i+6]),stod(measurement[i+7]);
p << stod98(measurement[i+5]),stod98(measurement[i+6]),stod98(measurement[i+7]);
pose.block<3,3>(0,0) = q.toRotationMatrix();
pose.block<3,1>(0,3) = p;
for (int j=0; j<6; ++j) {
for (int k=0; k<6; ++k) {
covariance(j,k) = stod(measurement[i+8 + j*6+k]);
covariance(j,k) = stod98(measurement[i+8 + j*6+k]);
}
}
Kinematics frame(id, pose, covariance);
Expand Down
28 changes: 14 additions & 14 deletions src/examples/landmarks.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,11 +22,11 @@
#define DT_MIN 1e-6
#define DT_MAX 1

double stod(const std::string &s) {
double stod98(const std::string &s) {
return atof(s.c_str());
}

int stoi(const std::string &s) {
int stoi98(const std::string &s) {
return atoi(s.c_str());
}

Expand Down Expand Up @@ -108,13 +108,13 @@ int main() {
if (measurement[0].compare("IMU")==0){
cout << "Received IMU Data, propagating state\n";
assert((measurement.size()-2) == 6);
t = stod(measurement[1]);
imu_measurement << stod(measurement[2]),
stod(measurement[3]),
stod(measurement[4]),
stod(measurement[5]),
stod(measurement[6]),
stod(measurement[7]);
t = stoi98(measurement[1]);
imu_measurement << stoi98(measurement[2]),
stoi98(measurement[3]),
stoi98(measurement[4]),
stoi98(measurement[5]),
stoi98(measurement[6]),
stoi98(measurement[7]);

// Propagate using IMU data
double dt = t - t_prev;
Expand All @@ -125,14 +125,14 @@ int main() {
else if (measurement[0].compare("LANDMARK")==0){
cout << "Received LANDMARK observation, correcting state\n";
assert((measurement.size()-2)%4 == 0);
t = stod(measurement[1]);
t = stoi98(measurement[1]);
vectorLandmarks measured_landmarks;
for (int i=2; i<measurement.size(); i+=4) {
int id = stod(measurement[i]);
int id = stoi98(measurement[i]);
Eigen::Vector3d p_bl;
p_bl << stod(measurement[i+1]),
stod(measurement[i+2]),
stod(measurement[i+3]);
p_bl << stoi98(measurement[i+1]),
stoi98(measurement[i+2]),
stoi98(measurement[i+3]);
Landmark landmark(id, p_bl);
measured_landmarks.push_back(landmark);
}
Expand Down
28 changes: 14 additions & 14 deletions src/tests/correction_speed.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,11 +30,11 @@ using namespace boost::posix_time;
typedef vector<pair<double,Eigen::Matrix<double,6,1> > > vectorPairIntVector6d;
typedef vector<pair<double,Eigen::Matrix<double,6,1> > >::const_iterator vectorPairIntVector6dIterator;

double stod(const std::string &s) {
double stod98(const std::string &s) {
return atof(s.c_str());
}

int stoi(const std::string &s) {
int stoi98(const std::string &s) {
return atoi(s.c_str());
}

Expand All @@ -60,24 +60,24 @@ int main() {
boost::split(measurement,line,boost::is_any_of(" "));
// Handle measurements
if (measurement[0].compare("IMU")==0){
t = stod(measurement[1]);
m << stod(measurement[2]),
stod(measurement[3]),
stod(measurement[4]),
stod(measurement[5]),
stod(measurement[6]),
stod(measurement[7]);
t = stod98(measurement[1]);
m << stod98(measurement[2]),
stod98(measurement[3]),
stod98(measurement[4]),
stod98(measurement[5]),
stod98(measurement[6]),
stod98(measurement[7]);
measurements_vec.push_back(pair<double,Eigen::Matrix<double,6,1> > (t, m));

}
else if (measurement[0].compare("LANDMARK")==0){
t = stod(measurement[1]);
t = stod98(measurement[1]);
for (int i=2; i<measurement.size(); i+=4) {
int id = stod(measurement[i]);
int id = stod98(measurement[i]);
Eigen::Vector3d p_bl;
p_bl << stod(measurement[i+1]),
stod(measurement[i+2]),
stod(measurement[i+3]);
p_bl << stod98(measurement[i+1]),
stod98(measurement[i+2]),
stod98(measurement[i+3]);
Landmark landmark(id, p_bl);
measured_landmarks.push_back(landmark);
}
Expand Down
18 changes: 9 additions & 9 deletions src/tests/propagation_speed.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,11 +31,11 @@ using namespace boost::posix_time;
typedef vector<pair<double,Eigen::Matrix<double,6,1> > > vectorPairIntVector6d;
typedef vector<pair<double,Eigen::Matrix<double,6,1> > >::const_iterator vectorPairIntVector6dIterator;

double stod(const std::string &s) {
double stod98(const std::string &s) {
return atof(s.c_str());
}

int stoi(const std::string &s) {
int stoi98(const std::string &s) {
return atoi(s.c_str());
}

Expand Down Expand Up @@ -64,13 +64,13 @@ int main()
boost::split(measurement,line,boost::is_any_of(" "));
// Handle measurements
if (measurement[0].compare("IMU")==0){
t = stod(measurement[1]);
m << stod(measurement[2]),
stod(measurement[3]),
stod(measurement[4]),
stod(measurement[5]),
stod(measurement[6]),
stod(measurement[7]);
t = stod98(measurement[1]);
m << stod98(measurement[2]),
stod98(measurement[3]),
stod98(measurement[4]),
stod98(measurement[5]),
stod98(measurement[6]),
stod98(measurement[7]);
measurements_vec.push_back(pair<double,Eigen::Matrix<double,6,1> > (t, m));
}
}
Expand Down

0 comments on commit 374e356

Please sign in to comment.