apex-solver 1.3.0

High-performance nonlinear least squares optimization with Lie group support for SLAM and bundle adjustment
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171

#include "read_g2o.h"
#include <iostream>
#include <fstream>
#include <sstream>

namespace g2o_reader {

GraphType DetectGraphType(const std::string& filename) {
    std::ifstream infile(filename);
    if (!infile.is_open()) {
        std::cerr << "Error: Cannot open file " << filename << std::endl;
        return GraphType::UNKNOWN;
    }

    std::string line;
    while (std::getline(infile, line)) {
        std::istringstream iss(line);
        std::string tag;
        iss >> tag;

        if (tag == "VERTEX_SE2" || tag == "EDGE_SE2") {
            return GraphType::SE2;
        } else if (tag == "VERTEX_SE3:QUAT" || tag == "EDGE_SE3:QUAT") {
            return GraphType::SE3;
        }
    }

    return GraphType::UNKNOWN;
}

bool ReadG2oFile2D(const std::string& filename, Graph2D& graph) {
    std::ifstream infile(filename);
    if (!infile.is_open()) {
        std::cerr << "Error: Cannot open file " << filename << std::endl;
        return false;
    }

    graph.poses.clear();
    graph.constraints.clear();

    std::string line;
    int line_number = 0;

    while (std::getline(infile, line)) {
        line_number++;
        std::istringstream iss(line);
        std::string tag;
        iss >> tag;

        if (tag == "VERTEX_SE2") {
            int id;
            double x, y, theta;
            if (!(iss >> id >> x >> y >> theta)) {
                std::cerr << "Error parsing VERTEX_SE2 at line " << line_number << std::endl;
                continue;
            }
            graph.poses[id] = Pose2D(x, y, theta);
        }
        else if (tag == "EDGE_SE2") {
            Constraint2D constraint;
            double x, y, theta;

            if (!(iss >> constraint.id_begin >> constraint.id_end >> x >> y >> theta)) {
                std::cerr << "Error parsing EDGE_SE2 at line " << line_number << std::endl;
                continue;
            }

            constraint.measurement = Pose2D(x, y, theta);

            // Read information matrix (upper triangular: 6 values for 3x3 symmetric)
            double info[6];
            for (int i = 0; i < 6; ++i) {
                if (!(iss >> info[i])) {
                    std::cerr << "Error reading information matrix at line " << line_number << std::endl;
                    continue;
                }
            }

            // Fill symmetric information matrix
            constraint.information <<
                info[0], info[1], info[2],
                info[1], info[3], info[4],
                info[2], info[4], info[5];

            graph.constraints.push_back(constraint);
        }
    }

    return true;
}

bool ReadG2oFile3D(const std::string& filename, Graph3D& graph) {
    std::ifstream infile(filename);
    if (!infile.is_open()) {
        std::cerr << "Error: Cannot open file " << filename << std::endl;
        return false;
    }

    graph.poses.clear();
    graph.constraints.clear();

    std::string line;
    int line_number = 0;

    while (std::getline(infile, line)) {
        line_number++;
        std::istringstream iss(line);
        std::string tag;
        iss >> tag;

        if (tag == "VERTEX_SE3:QUAT") {
            int id;
            double tx, ty, tz, qx, qy, qz, qw;

            if (!(iss >> id >> tx >> ty >> tz >> qx >> qy >> qz >> qw)) {
                std::cerr << "Error parsing VERTEX_SE3:QUAT at line " << line_number << std::endl;
                continue;
            }

            Eigen::Quaterniond q(qw, qx, qy, qz);
            q.normalize();
            Eigen::Vector3d t(tx, ty, tz);
            graph.poses[id] = Pose3D(q, t);
        }
        else if (tag == "EDGE_SE3:QUAT") {
            Constraint3D constraint;
            double tx, ty, tz, qx, qy, qz, qw;

            if (!(iss >> constraint.id_begin >> constraint.id_end
                     >> tx >> ty >> tz >> qx >> qy >> qz >> qw)) {
                std::cerr << "Error parsing EDGE_SE3:QUAT at line " << line_number << std::endl;
                continue;
            }

            Eigen::Quaterniond q(qw, qx, qy, qz);
            q.normalize();
            Eigen::Vector3d t(tx, ty, tz);
            constraint.measurement = Pose3D(q, t);

            // Read information matrix (upper triangular: 21 values for 6x6 symmetric)
            double info[21];
            bool info_read_success = true;
            for (int i = 0; i < 21; ++i) {
                if (!(iss >> info[i])) {
                    std::cerr << "Error reading information matrix at line " << line_number << std::endl;
                    info_read_success = false;
                    break;
                }
            }
            if (!info_read_success) {
                continue;
            }

            // Fill symmetric 6x6 information matrix from upper triangular
            int idx = 0;
            for (int i = 0; i < 6; ++i) {
                for (int j = i; j < 6; ++j) {
                    constraint.information(i, j) = info[idx];
                    constraint.information(j, i) = info[idx];
                    idx++;
                }
            }

            graph.constraints.push_back(constraint);
        }
    }

   return true;
}

}  // namespace g2o_reader