Skip to content

Commit 4f78e92

Browse files
mcm001mcm001
committed
plot sparsity for fun
1 parent aaf8e32 commit 4f78e92

File tree

3 files changed

+270
-1
lines changed

3 files changed

+270
-1
lines changed

‎spy.py‎

Lines changed: 118 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,118 @@
1+
#!/usr/bin/env python3
2+
3+
import argparse
4+
import struct
5+
import sys
6+
7+
import matplotlib.pyplot as plt
8+
import matplotlib.ticker as mticker
9+
import numpy as np
10+
from matplotlib import animation
11+
from matplotlib.colors import ListedColormap
12+
13+
14+
def plot_csv(filename: str) -> animation.FuncAnimation:
15+
print(f"Loading sparsity pattern {filename}...", end="")
16+
sys.stdout.flush()
17+
18+
xs = []
19+
ys = []
20+
vs = []
21+
22+
x = []
23+
y = []
24+
v = []
25+
26+
with open(filename, mode="rb") as f:
27+
size = struct.unpack("<i", f.read(4))[0]
28+
title = struct.unpack(f"{size}s", f.read(size))[0].decode("utf-8")
29+
30+
size = struct.unpack("<i", f.read(4))[0]
31+
row_label = struct.unpack(f"{size}s", f.read(size))[0].decode("utf-8")
32+
33+
size = struct.unpack("<i", f.read(4))[0]
34+
col_label = struct.unpack(f"{size}s", f.read(size))[0].decode("utf-8")
35+
36+
rows = struct.unpack("<i", f.read(4))[0]
37+
cols = struct.unpack("<i", f.read(4))[0]
38+
39+
try:
40+
while True:
41+
num_coords = struct.unpack("<i", f.read(4))[0]
42+
for _ in range(num_coords):
43+
y.append(struct.unpack("<i", f.read(4))[0])
44+
x.append(struct.unpack("<i", f.read(4))[0])
45+
sign = struct.unpack("c", f.read(1))[0]
46+
if sign == b"+":
47+
v.append(1.0)
48+
elif sign == b"-":
49+
v.append(-1.0)
50+
else:
51+
v.append(0.0)
52+
xs.append(x)
53+
ys.append(y)
54+
vs.append(v)
55+
x = []
56+
y = []
57+
v = []
58+
except:
59+
pass
60+
61+
print(" done.")
62+
sys.stdout.flush()
63+
64+
fig = plt.figure()
65+
ax = fig.add_subplot()
66+
67+
# Display scatter plot
68+
cmap = ListedColormap(["red", "black", "green"])
69+
sc = ax.scatter(xs[0], ys[0], s=1, c=vs[0], marker=".", cmap=cmap, vmin=-1, vmax=1)
70+
iteration = ax.text(0.02, 0.95, "", transform=ax.transAxes)
71+
72+
# Display colorbar
73+
colorbar = fig.colorbar(sc)
74+
ticklabels = ["Negative", "Positive"]
75+
colorbar.ax.yaxis.set_major_locator(mticker.FixedLocator(ticklabels))
76+
colorbar.ax.set_yticks([-1, 1])
77+
colorbar.ax.set_yticklabels(ticklabels)
78+
79+
ax.set_title(title)
80+
ax.set_xlabel(col_label)
81+
ax.set_ylabel(row_label)
82+
83+
ax.set_xlim(0, cols)
84+
ax.set_ylim(0, rows)
85+
ax.invert_yaxis()
86+
ax.set_aspect(1.0)
87+
88+
def animate(i: int):
89+
sc.set_offsets(np.c_[xs[i], ys[i]])
90+
sc.set_array(vs[i])
91+
iteration.set_text(f"iter {i}/{max(0, len(vs) - 1)}")
92+
return (sc, iteration)
93+
94+
return animation.FuncAnimation(
95+
fig=fig,
96+
func=animate,
97+
frames=len(vs),
98+
interval=1.0 / 30.0,
99+
repeat_delay=1000,
100+
repeat=True,
101+
blit=True,
102+
)
103+
104+
105+
def main():
106+
parser = argparse.ArgumentParser(
107+
description="Displays sparsity pattern (.spy) files."
108+
)
109+
parser.add_argument("filename", nargs="+")
110+
args = parser.parse_args()
111+
112+
# pragma pylint: disable=unused-variable
113+
animations = [plot_csv(filename) for filename in args.filename] # noqa
114+
plt.show()
115+
116+
117+
if __name__ == "__main__":
118+
main()

‎src/mrcal-uncertainty.hpp‎

Lines changed: 31 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -10,6 +10,7 @@
1010
#include <Eigen/Sparse>
1111
#include <unsupported/Eigen/SparseExtra>
1212
#include <cholmod.h>
13+
#include "spy.hpp"
1314

1415
using namespace cv;
1516

@@ -602,8 +603,21 @@ void compute_uncertainty()
602603
c_observations_board, reinterpret_cast<const mrcal_point3_t *>(observations_board.data()),
603604
rt_ref_frames.size(), 10, 10, 0.0254);
604605

606+
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
607+
slp::Spy<double> spy_J_observations("J_observations.spy",
608+
"J_observations Matrix", "Measurements", "State",
609+
context.Nmeasurements_boards, context.Nstate);
610+
spy_J_observations.add(context.J_observations);
611+
612+
auto JtJ = context.J_observations.transpose() * context.J_observations;
613+
slp::Spy<double> spy_JtJ("JtJ.spy",
614+
"JtJ Matrix (Normal Equations)", "State", "State",
615+
context.Nstate, context.Nstate);
616+
spy_JtJ.add(JtJ);
617+
#endif
618+
605619
// hard code some stuff
606-
auto q = sample_imager({60, 40}, {1280, 720});
620+
auto q = sample_imager({500, 500}, {1280, 720});
607621

608622
// and unproject
609623
std::vector<mrcal_point3_t> pcam;
@@ -649,5 +663,21 @@ void compute_uncertainty()
649663
auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end - start);
650664

651665
fmt::print("{}, {}, {}\n", qi.x, qi.y, uncertainty);
666+
667+
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
668+
if (i < 5) {
669+
int istate_frames0 = mrcal_state_index_frames(
670+
0, 1, 0, 6, 0, 0, 6, problem_selections, &lensmodel);
671+
672+
auto dq_db = _dq_db_projection_uncertainty(pi, lensmodel, rt_ref_frames,
673+
context.Nstate, istate_frames0, intrinsics);
674+
675+
Eigen::SparseMatrix<double> dq_db_sparse = dq_db.sparseView();
676+
slp::Spy<double> spy_gradient(fmt::format("gradient_{}.spy", i),
677+
fmt::format("Gradient dq_db Point {}", i), "Output", "State",
678+
2, context.Nstate);
679+
spy_gradient.add(dq_db_sparse);
680+
}
681+
#endif
652682
}
653683
}

‎src/spy.hpp‎

Lines changed: 121 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,121 @@
1+
// Copyright (c) Sleipnir contributors
2+
3+
#pragma once
4+
5+
#ifndef SLEIPNIR_DISABLE_DIAGNOSTICS
6+
7+
#include <stdint.h>
8+
9+
#include <bit>
10+
#include <fstream>
11+
#include <string>
12+
#include <string_view>
13+
14+
#include <Eigen/SparseCore>
15+
16+
namespace slp {
17+
18+
/// Writes the sparsity pattern of a sparse matrix to a file.
19+
///
20+
/// Each file represents the sparsity pattern of one matrix over time. <a
21+
/// href="https://github.com/SleipnirGroup/Sleipnir/blob/main/tools/spy.py">spy.py</a>
22+
/// can display it as an animation.
23+
///
24+
/// The file starts with the following header:
25+
/// <ol>
26+
/// <li>Plot title (length as a little-endian int32, then characters)</li>
27+
/// <li>Row label (length as a little-endian int32, then characters)</li>
28+
/// <li>Column label (length as a little-endian int32, then characters)</li>
29+
/// </ol>
30+
///
31+
/// Then, each sparsity pattern starts with:
32+
/// <ol>
33+
/// <li>Number of coordinates as a little-endian int32</li>
34+
/// </ol>
35+
///
36+
/// followed by that many coordinates in the following format:
37+
/// <ol>
38+
/// <li>Row index as a little-endian int32</li>
39+
/// <li>Column index as a little-endian int32</li>
40+
/// <li>Sign as a character ('+' for positive, '-' for negative, or '0' for
41+
/// zero)</li>
42+
/// </ol>
43+
///
44+
/// @tparam Scalar Scalar type.
45+
template <typename Scalar>
46+
class Spy {
47+
public:
48+
/// Constructs a Spy instance.
49+
///
50+
/// @param filename The filename.
51+
/// @param title Plot title.
52+
/// @param row_label Row label.
53+
/// @param col_label Column label.
54+
/// @param rows The sparse matrix's number of rows.
55+
/// @param cols The sparse matrix's number of columns.
56+
Spy(std::string_view filename, std::string_view title,
57+
std::string_view row_label, std::string_view col_label, int rows,
58+
int cols)
59+
: m_file{std::string{filename}, std::ios::binary} {
60+
// Write title
61+
write32le(title.size());
62+
m_file.write(title.data(), title.size());
63+
64+
// Write row label
65+
write32le(row_label.size());
66+
m_file.write(row_label.data(), row_label.size());
67+
68+
// Write column label
69+
write32le(col_label.size());
70+
m_file.write(col_label.data(), col_label.size());
71+
72+
// Write row and column counts
73+
write32le(rows);
74+
write32le(cols);
75+
}
76+
77+
/// Adds a matrix to the file.
78+
///
79+
/// @param mat The matrix.
80+
void add(const Eigen::SparseMatrix<Scalar>& mat) {
81+
// Write number of coordinates
82+
write32le(mat.nonZeros());
83+
84+
// Write coordinates
85+
for (int k = 0; k < mat.outerSize(); ++k) {
86+
for (typename Eigen::SparseMatrix<Scalar>::InnerIterator it{mat, k}; it;
87+
++it) {
88+
write32le(it.row());
89+
write32le(it.col());
90+
if (it.value() > Scalar(0)) {
91+
m_file << '+';
92+
} else if (it.value() < Scalar(0)) {
93+
m_file << '-';
94+
} else {
95+
m_file << '0';
96+
}
97+
}
98+
}
99+
}
100+
101+
private:
102+
std::ofstream m_file;
103+
104+
/// Writes a 32-bit signed integer to the file as little-endian.
105+
///
106+
/// @param num A 32-bit signed integer.
107+
void write32le(int32_t num) {
108+
if constexpr (std::endian::native != std::endian::little) {
109+
// Manual byteswap for 32-bit integer
110+
num = ((num & 0xFF000000) >> 24) |
111+
((num & 0x00FF0000) >> 8) |
112+
((num & 0x0000FF00) << 8) |
113+
((num & 0x000000FF) << 24);
114+
}
115+
m_file.write(reinterpret_cast<char*>(&num), sizeof(num));
116+
}
117+
};
118+
119+
} // namespace slp
120+
121+
#endif

0 commit comments

Comments
 (0)