55 {
56 std::string line;
57 std::getline(stream, line);
58
59 std::regex re_header("\\s*X\\s*,\\s*Y\\s*,\\s*Z\\s*,\\s*Yaw\\s*,\\s*Pitch\\s*,\\s*Roll\\s*");
60 if (!std::regex_match(line, re_header)) {
61 throw std::runtime_error("Format error in the pose trace header");
62 }
63
64 PoseTrace trace;
65 std::regex re_row("([^,]+),([^,]+),([^,]+),([^,]+),([^,]+),([^,]+)");
66 std::regex re_empty("\\s*");
67 bool trailing_empty_lines = false;
68
69 while (std::getline(stream, line)) {
70 std::smatch match;
71 if (!trailing_empty_lines && std::regex_match(line, match, re_row)) {
72 trace.push_back({
73 cv::Vec3f(
74 std::stof(match[1].str()),
75 std::stof(match[2].str()),
76 std::stof(match[3].str())),
77 cv::Vec3f(
78 std::stof(match[4].str()),
79 std::stof(match[5].str()),
80 std::stof(match[6].str()))
81 });
82 }
83 else if (std::regex_match(line, re_empty)) {
84 trailing_empty_lines = true;
85 }
86 else {
87 throw std::runtime_error("Format error in a pose trace row");
88 }
89 }
90
91 return trace;
92 }