rename variables
parent
67ffd1e1ae
commit
c0321443bd
|
@ -49,7 +49,7 @@ class Experiment {
|
||||||
|
|
||||||
// false: run original iSAM2 without ambiguities
|
// false: run original iSAM2 without ambiguities
|
||||||
// true: run original iSAM2 with ambiguities
|
// true: run original iSAM2 with ambiguities
|
||||||
const bool is_with_ambiguity = false;
|
const bool isWithAmbiguity = false;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
ISAM2Params parameters;
|
ISAM2Params parameters;
|
||||||
|
@ -69,7 +69,7 @@ class Experiment {
|
||||||
/// @brief Run the main experiment with a given maxLoopCount.
|
/// @brief Run the main experiment with a given maxLoopCount.
|
||||||
void run() {
|
void run() {
|
||||||
// Initialize local variables
|
// Initialize local variables
|
||||||
size_t pose_count = 0, index = 0;
|
size_t poseCount = 0, index = 0;
|
||||||
|
|
||||||
std::list<double> timeList;
|
std::list<double> timeList;
|
||||||
|
|
||||||
|
@ -77,7 +77,7 @@ class Experiment {
|
||||||
Pose2 priorPose(0, 0, 0);
|
Pose2 priorPose(0, 0, 0);
|
||||||
initial_.insert(X(0), priorPose);
|
initial_.insert(X(0), priorPose);
|
||||||
graph.addPrior<Pose2>(X(0), priorPose, kPriorNoiseModel);
|
graph.addPrior<Pose2>(X(0), priorPose, kPriorNoiseModel);
|
||||||
pose_count++;
|
poseCount++;
|
||||||
|
|
||||||
// Initial update
|
// Initial update
|
||||||
isam2.update(graph, initial_);
|
isam2.update(graph, initial_);
|
||||||
|
@ -88,7 +88,7 @@ class Experiment {
|
||||||
// Start main loop
|
// Start main loop
|
||||||
size_t keyS = 0;
|
size_t keyS = 0;
|
||||||
size_t keyT = 0;
|
size_t keyT = 0;
|
||||||
clock_t start_time = clock();
|
clock_t startTime = clock();
|
||||||
|
|
||||||
std::vector<Pose2> poseArray;
|
std::vector<Pose2> poseArray;
|
||||||
std::pair<size_t, size_t> keys;
|
std::pair<size_t, size_t> keys;
|
||||||
|
@ -98,34 +98,35 @@ class Experiment {
|
||||||
keyT = keys.second;
|
keyT = keys.second;
|
||||||
size_t numMeasurements = poseArray.size();
|
size_t numMeasurements = poseArray.size();
|
||||||
|
|
||||||
Pose2 odom_pose;
|
Pose2 odomPose;
|
||||||
if (is_with_ambiguity) {
|
if (isWithAmbiguity) {
|
||||||
// Get wrong intentionally
|
// Get wrong intentionally
|
||||||
int id = index % num_measurements;
|
int id = index % numMeasurements;
|
||||||
odom_pose = Pose2(pose_array[id]);
|
odomPose = Pose2(poseArray[id]);
|
||||||
} else {
|
} else {
|
||||||
odom_pose = pose_array[0];
|
odomPose = poseArray[0];
|
||||||
}
|
}
|
||||||
|
|
||||||
if (keyS == keyT - 1) { // new X(key)
|
if (keyS == keyT - 1) { // new X(key)
|
||||||
initial_.insert(X(keyT), results.at<Pose2>(X(keyS)) * odom_pose);
|
initial_.insert(X(keyT), results.at<Pose2>(X(keyS)) * odomPose);
|
||||||
graph.add(
|
graph.add(
|
||||||
BetweenFactor<Pose2>(X(keyS), X(keyT), odom_pose, kPoseNoiseModel));
|
BetweenFactor<Pose2>(X(keyS), X(keyT), odomPose, kPoseNoiseModel));
|
||||||
pose_count++;
|
poseCount++;
|
||||||
|
|
||||||
} else { // loop
|
} else { // loop
|
||||||
int id = index % num_measurements;
|
int id = index % numMeasurements;
|
||||||
if (is_with_ambiguity && id % 2 == 0) {
|
if (isWithAmbiguity && id % 2 == 0) {
|
||||||
graph.add(BetweenFactor<Pose2>(X(keyS), X(keyT), odom_pose,
|
graph.add(BetweenFactor<Pose2>(X(keyS), X(keyT), odomPose,
|
||||||
kPoseNoiseModel));
|
kPoseNoiseModel));
|
||||||
} else {
|
} else {
|
||||||
graph.add(BetweenFactor<Pose2>(
|
graph.add(BetweenFactor<Pose2>(
|
||||||
X(keyS), X(keyT), odom_pose,
|
X(keyS), X(keyT), odomPose,
|
||||||
noiseModel::Diagonal::Sigmas(Vector3::Ones() * 10.0)));
|
noiseModel::Diagonal::Sigmas(Vector3::Ones() * 10.0)));
|
||||||
}
|
}
|
||||||
index++;
|
index++;
|
||||||
}
|
}
|
||||||
|
|
||||||
isam2.update(*graph, initial_);
|
isam2.update(graph, initial_);
|
||||||
graph.resize(0);
|
graph.resize(0);
|
||||||
initial_.clear();
|
initial_.clear();
|
||||||
results = isam2.calculateBestEstimate();
|
results = isam2.calculateBestEstimate();
|
||||||
|
@ -133,45 +134,45 @@ class Experiment {
|
||||||
// Print loop index and time taken in processor clock ticks
|
// Print loop index and time taken in processor clock ticks
|
||||||
if (index % 50 == 0 && keyS != keyT - 1) {
|
if (index % 50 == 0 && keyS != keyT - 1) {
|
||||||
std::cout << "index: " << index << std::endl;
|
std::cout << "index: " << index << std::endl;
|
||||||
std::cout << "acc_time: " << timeList.back() / CLOCKS_PER_SEC
|
std::cout << "accTime: " << timeList.back() / CLOCKS_PER_SEC
|
||||||
<< std::endl;
|
<< std::endl;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (keyS == keyT - 1) {
|
if (keyS == keyT - 1) {
|
||||||
clock_t cur_time = clock();
|
clock_t curTime = clock();
|
||||||
timeList.push_back(cur_time - start_time);
|
timeList.push_back(curTime - startTime);
|
||||||
}
|
}
|
||||||
|
|
||||||
if (timeList.size() % 100 == 0 && (keyS == keyT - 1)) {
|
if (timeList.size() % 100 == 0 && (keyS == keyT - 1)) {
|
||||||
string step_file_idx = std::to_string(100000 + timeList.size());
|
string stepFileIdx = std::to_string(100000 + timeList.size());
|
||||||
|
|
||||||
ofstream step_outfile;
|
ofstream stepOutfile;
|
||||||
string step_file_name = "step_files/ISAM2_city10000_S" + step_file_idx;
|
string stepFileName = "step_files/ISAM2_city10000_S" + stepFileIdx;
|
||||||
step_outfile.open(step_file_name + ".txt");
|
stepOutfile.open(stepFileName + ".txt");
|
||||||
for (size_t i = 0; i < (keyT + 1); ++i) {
|
for (size_t i = 0; i < (keyT + 1); ++i) {
|
||||||
Pose2 out_pose = results.at<Pose2>(X(i));
|
Pose2 outPose = results.at<Pose2>(X(i));
|
||||||
step_outfile << out_pose.x() << " " << out_pose.y() << " "
|
stepOutfile << outPose.x() << " " << outPose.y() << " "
|
||||||
<< out_pose.theta() << endl;
|
<< outPose.theta() << endl;
|
||||||
}
|
}
|
||||||
step_outfile.close();
|
stepOutfile.close();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
clock_t end_time = clock();
|
clock_t endTime = clock();
|
||||||
clock_t total_time = end_time - start_time;
|
clock_t totalTime = endTime - startTime;
|
||||||
cout << "total_time: " << total_time / CLOCKS_PER_SEC << endl;
|
cout << "totalTime: " << totalTime / CLOCKS_PER_SEC << endl;
|
||||||
|
|
||||||
/// Write results to file
|
/// Write results to file
|
||||||
writeResult(results, (keyT + 1), "ISAM2_city10000.txt");
|
writeResult(results, (keyT + 1), "ISAM2_city10000.txt");
|
||||||
|
|
||||||
ofstream outfile_time;
|
ofstream outfileTime;
|
||||||
std::string time_file_name = "ISAM2_city10000_time.txt";
|
std::string timeFileName = "ISAM2_city10000_time.txt";
|
||||||
outfile_time.open(time_file_name);
|
outfileTime.open(timeFileName);
|
||||||
for (auto acc_time : timeList) {
|
for (auto accTime : timeList) {
|
||||||
outfile_time << acc_time << std::endl;
|
outfileTime << accTime << std::endl;
|
||||||
}
|
}
|
||||||
outfile_time.close();
|
outfileTime.close();
|
||||||
cout << "Written cumulative time to: " << time_file_name << " file."
|
cout << "Written cumulative time to: " << timeFileName << " file."
|
||||||
<< endl;
|
<< endl;
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
Loading…
Reference in New Issue