12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

get data from dataset

release/4.3a0
Varun Agrawal 2025-02-06 16:34:59 -05:00
parent a18857a117
commit 98904e08bf
1 changed files with 7 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
examples/ISAM2_City10000.cpp
View File

@ -80,7 +80,7 @@ class Experiment {
pose_count++; pose_count++;
// Initial update // Initial update
isam2.update(*graph, initial_); isam2.update(graph, initial_);
graph.resize(0); graph.resize(0);
initial_.clear(); initial_.clear();
results = isam2.calculateBestEstimate(); results = isam2.calculateBestEstimate();
@ -90,22 +90,13 @@ class Experiment {
size_t keyT = 0; size_t keyT = 0;
clock_t start_time = clock(); clock_t start_time = clock();
string str; std::vector<Pose2> poseArray;
while (getline(in, str) && index < max_loop_count) { std::pair<size_t, size_t> keys;
vector<string> parts;
split(parts, str, is_any_of(" "));
keyS = stoi(parts[1]); while (dataset_.next(&poseArray, &keys) && index < maxLoopCount) {
keyT = stoi(parts[3]); keyS = keys.first;
keyT = keys.second;
int num_measurements = stoi(parts[5]); size_t numMeasurements = poseArray.size();
vector<Pose2> pose_array(num_measurements);
for (int i = 0; i < num_measurements; ++i) {
x = stod(parts[6 + 3 * i]);
y = stod(parts[7 + 3 * i]);
rad = stod(parts[8 + 3 * i]);
pose_array[i] = Pose2(x, y, rad);
}
Pose2 odom_pose; Pose2 odom_pose;
if (is_with_ambiguity) { if (is_with_ambiguity) {