rename variables

release/4.3a0
Varun Agrawal 2025-02-06 16:39:49 -05:00
parent 67ffd1e1ae
commit c0321443bd
1 changed files with 40 additions and 39 deletions

View File

@ -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;
} }
}; };