1/* ----------------------------------------------------------------------------
2
3 * GTSAM Copyright 2010-2020, Georgia Tech Research Corporation,
4 * Atlanta, Georgia 30332-0415
5 * All Rights Reserved
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
8 * See LICENSE for the license information
9
10 * -------------------------------------------------------------------------- */
11
12/**
13 * @file City10000.h
14 * @brief Class for City10000 dataset
15 * @author Varun Agrawal
16 * @date February 3, 2025
17 */
18
19#include <gtsam/geometry/Pose2.h>
20
21#include <fstream>
22
23using namespace gtsam;
24
25using symbol_shorthand::X;
26
27auto kOpenLoopModel = noiseModel::Diagonal::Sigmas(sigmas: Vector3::Ones() * 10);
28const double kOpenLoopConstant = kOpenLoopModel->negLogConstant();
29
30auto kPriorNoiseModel = noiseModel::Diagonal::Sigmas(
31 sigmas: (Vector(3) << 0.0001, 0.0001, 0.0001).finished());
32
33auto kPoseNoiseModel = noiseModel::Diagonal::Sigmas(
34 sigmas: (Vector(3) << 1.0 / 30.0, 1.0 / 30.0, 1.0 / 100.0).finished());
35const double kPoseNoiseConstant = kPoseNoiseModel->negLogConstant();
36
37class City10000Dataset {
38 std::ifstream in_;
39
40 /// Read a `line` from the dataset, separated by the `delimiter`.
41 std::vector<std::string> readLine(const std::string& line,
42 const std::string& delimiter = " ") const {
43 std::vector<std::string> parts;
44 auto start = 0U;
45 auto end = line.find(str: delimiter);
46 while (end != std::string::npos) {
47 parts.push_back(x: line.substr(pos: start, n: end - start));
48 start = end + delimiter.length();
49 end = line.find(str: delimiter, pos: start);
50 }
51 return parts;
52 }
53
54 public:
55 City10000Dataset(const std::string& filename) : in_(filename) {
56 if (!in_.is_open()) {
57 std::cerr << "Failed to open file: " << filename << std::endl;
58 }
59 }
60
61 /// Parse line from file
62 std::pair<std::vector<Pose2>, std::pair<size_t, size_t>> parseLine(
63 const std::string& line) const {
64 std::vector<std::string> parts = readLine(line);
65
66 size_t keyS = stoi(str: parts[1]);
67 size_t keyT = stoi(str: parts[3]);
68
69 int numMeasurements = stoi(str: parts[5]);
70 std::vector<Pose2> poseArray(numMeasurements);
71 for (int i = 0; i < numMeasurements; ++i) {
72 double x = stod(str: parts[6 + 3 * i]);
73 double y = stod(str: parts[7 + 3 * i]);
74 double rad = stod(str: parts[8 + 3 * i]);
75 poseArray[i] = Pose2(x, y, rad);
76 }
77 return {poseArray, {keyS, keyT}};
78 }
79
80 /// Read and parse the next line.
81 bool next(std::vector<Pose2>* poseArray, std::pair<size_t, size_t>* keys) {
82 std::string line;
83 if (getline(is&: in_, str&: line)) {
84 std::tie(args&: *poseArray, args&: *keys) = parseLine(line);
85 return true;
86 } else
87 return false;
88 }
89};
90
91/**
92 * @brief Write the result of optimization to file.
93 *
94 * @param result The Values object with the final result.
95 * @param num_poses The number of poses to write to the file.
96 * @param filename The file name to save the result to.
97 */
98void writeResult(const Values& result, size_t numPoses,
99 const std::string& filename = "Hybrid_city10000.txt") {
100 std::ofstream outfile;
101 outfile.open(s: filename);
102
103 for (size_t i = 0; i < numPoses; ++i) {
104 Pose2 outPose = result.at<Pose2>(j: X(j: i));
105 outfile << outPose.x() << " " << outPose.y() << " " << outPose.theta()
106 << std::endl;
107 }
108 outfile.close();
109 std::cout << "Output written to " << filename << std::endl;
110}
111