aboutsummaryrefslogtreecommitdiffstats
path: root/meowpp.test/src/rot_bundle.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'meowpp.test/src/rot_bundle.cpp')
-rw-r--r--meowpp.test/src/rot_bundle.cpp319
1 files changed, 319 insertions, 0 deletions
diff --git a/meowpp.test/src/rot_bundle.cpp b/meowpp.test/src/rot_bundle.cpp
new file mode 100644
index 0000000..d626e8e
--- /dev/null
+++ b/meowpp.test/src/rot_bundle.cpp
@@ -0,0 +1,319 @@
+#include <cstdio>
+#include <string>
+#include <cstdlib>
+#include <algorithm>
+#include <vector>
+
+#include <opencv/cv.h>
+#include <opencv/highgui.h>
+
+#include "meowpp/dsa/DisjointSet.h"
+#include "meowpp/Usage.h"
+#include "meowpp/gra/Eye.h"
+#include "meowpp/colors/RGB_Space.h"
+#include "meowpp/gra/Bitmap.h"
+#include "meowpp/utility.h"
+#include "meowpp/math/utility.h"
+#include "meowpp/geo/Vectors.h"
+#include "meowpp/gra/WatchBall.h"
+
+#include "meowpp/gra/BundleAdjustment_LM.h"
+
+using namespace meow;
+
+//////////////////////////////////////////////////////////////////
+
+Usage usg("rot_bundle");
+
+std::vector<std::string> bitmap_name;
+std::vector<Eye<RGBf_Space> > eyes;
+std::vector<std::vector<Vector<double> > > fpsv;
+std::vector<std::vector<std::vector<PairToPair<size_t> > > > pairs;
+std::vector<std::vector<size_t > > groups;
+std::vector<WatchBall<RGBf_Space> > balls;
+std::vector<Vector2D<double> > center;
+
+double f_init;
+double threshold;
+
+//////////////////////////////////////////////////////////////////
+
+bool setup(int argc, char** argv) {
+ usg.optionAdd("f",
+ "Input text file",
+ "filename",
+ "",
+ true);
+ usg.optionAdd("h",
+ "help docu");
+ usg.optionAdd("output-radius",
+ "...",
+ "floating point",
+ "1000",
+ false);
+ usg.optionAdd("o",
+ "prefix of output images",
+ "pathname",
+ "output",
+ false);
+ usg.optionAdd("f-init",
+ "init focal length",
+ "floating point",
+ "300.0",
+ false);
+ usg.optionAdd("t",
+ "threshold for bundle adjustment",
+ "floating point",
+ "5.0",
+ false);
+ std::string s;
+ bool ok = usg.arguments(argc, argv, &s);
+ if (usg.hasOptionSetup("h")) {
+ printf("%s\n", usg.usage().c_str());
+ exit(0);
+ }
+ if (!ok) {
+ fprintf(stderr, "%s\n", s.c_str());
+ exit(1);
+ }
+ return true;
+}
+
+bool read() {
+ messagePrintf(1, "read file");
+ FILE* f = fopen(usg.optionValue("f", 0).c_str(), "r");
+ size_t N;
+ if (fscanf(f, "%lu", &N) < 1) {
+ fprintf(stderr, "format error!\n");
+ exit(-1);
+ }
+ std::vector<std::vector<int> > fps_id;
+ int id_max = 0;
+ bitmap_name.resize(N);
+ eyes.resize(N);
+ fpsv.resize(N);
+ fps_id.resize(N);
+ pairs.resize(N);
+ center.resize(N);
+ for (size_t i = 0; i < N; ++i) {
+ pairs[i].resize(N);
+ char s[1000];
+ if (fscanf(f, "%s", s) < 1) {
+ fprintf(stderr, "format error!\n");
+ exit(-1);
+ }
+ size_t h, w;
+ if (fscanf(f, "%lu %lu", &h, &w) < 2) {
+ fprintf(stderr, "format error!\n");
+ exit(-1);
+ }
+ center[i].y(1.0 * h / 2);
+ center[i].x(1.0 * w / 2);
+ bitmap_name[i] = s;
+ size_t M;
+ if (fscanf(f, "%lu", &M) < 1) {
+ fprintf(stderr, "format error!\n");
+ exit(-1);
+ }
+ Vector<double> v(2, 0.0);
+ for (size_t j = 0; j < M; ++j) {
+ double x, y;
+ if (fscanf(f, "%lf %lf", &x, &y) < 2) {
+ fprintf(stderr, "format error!\n");
+ exit(-1);
+ }
+ v.entry(0, x - center[i].x());
+ v.entry(1, -(y - center[i].y()));
+ fpsv[i].push_back(v);
+ fps_id[i].push_back(id_max++);
+ }
+ for (size_t j = 0; j < N; ++j) {
+ size_t O;
+ if (fscanf(f, "%lu", &O) < 1) {
+ fprintf(stderr, "format error!\n");
+ exit(-1);
+ }
+ for (size_t k = 0; k < O; ++k) {
+ size_t a, b, c, d;
+ if (fscanf(f, "%lu %lu %lu %lu", &a, &b, &c, &d) < 4) {
+ fprintf(stderr, "format error!\n");
+ exit(-1);
+ }
+ pairs[i][j].push_back(PairToPair<size_t>(a, b, c, d));
+ }
+ }
+ }
+ fclose(f);
+ messagePrintf(-1, "ok");
+ messagePrintf(1, "merge fixed points");
+ DisjointSet st(id_max);
+ for (size_t i = 0, I = eyes.size(); i < I; ++i) {
+ for (size_t j = 0, J = pairs[i].size(); j < J; ++j) {
+ for (size_t k = 0, K = pairs[i][j].size(); k < K; ++k) {
+ st.merge(fps_id[pairs[i][j][k].from.first][pairs[i][j][k].from.second],
+ fps_id[pairs[i][j][k].to .first][pairs[i][j][k].to .second]);
+ }
+ }
+ }
+ for (size_t i = 0, I = eyes.size(); i < I; ++i) {
+ std::string s(stringPrintf("eye %lu: ", i));
+ for (size_t j = 0, J = fps_id[i].size(); j < J; ++j) {
+ int id = st.root(fps_id[i][j]);
+ s += stringPrintf("%d ", id);
+ eyes[i].cameraGet().fixedPoints2DGet().identityPointAdd(id, fpsv[i][j]);
+ }
+ messagePrintf(0, "%s", s.c_str());
+ }
+ messagePrintf(-1, "ok");
+ messagePrintf(0, "Number of Eyes: %lu", eyes.size());
+ for (size_t i = 0, I = eyes.size(); i < I; ++i) {
+ messagePrintf(0, "eyes[%lu] have %lu fixed points", i, fpsv[i].size());
+ }
+ messagePrintf(0, "relation:");
+ for (size_t i = 0, I = eyes.size(); i < I; ++i) {
+ std::string s;
+ for (size_t j = 0; j < I; ++j) {
+ s += stringPrintf("%3lu ", pairs[i][j].size());
+ }
+ messagePrintf(0, "%s", s.c_str());
+ }
+ return true;
+}
+
+bool group() {
+ size_t N = eyes.size();
+ messagePrintf(1, "group");
+ // union
+ DisjointSet dsj(N);
+ for (size_t i = 0; i < N; i++) {
+ for (size_t j = 0; j < N; j++) {
+ if(pairs[i][j].empty()) continue;
+ dsj.merge(i, j);
+ }
+ }
+ std::vector<size_t> root;
+ for (size_t i = 0, I = eyes.size(); i < I; i++) {
+ if (dsj.root(i) == i) {
+ root.push_back(i);
+ }
+ }
+ // split into groups
+ groups.resize(root.size());
+ for (size_t i = 0, I = root.size(); i < I; i++) {
+ messagePrintf(1, "Group %lu", i);
+ std::vector<size_t> ids;
+ for (size_t j = 0; j < N; ++j) {
+ if (dsj.root(j) != root[i]) continue;
+ groups[i].push_back(j);
+ }
+ for (size_t j = 0, J = groups[i].size(); j < J; ++j) {
+ messagePrintf(0, "eye %lu", groups[i][j]);
+ }
+ messagePrintf(-1, "");
+ }
+ messagePrintf(-1, "ok");
+ return true;
+}
+
+bool bundle() {
+ threshold = inRange(0.0005, 1000.0, atof(usg.optionValue("t", 0).c_str()));
+ f_init = inRange(0.005, 100000.0, atof(usg.optionValue("f-init", 0).c_str()));
+ BundleAdjustment_LM<RGBf_Space> bdl;
+ for (size_t i = 0, I = groups.size(); i < I; ++i) {
+ messagePrintf(1, "bundle adjust for group %lu", i);
+ std::vector<SceneInfo<RGBf_Space> > seq;
+ for (size_t j = 0, J = groups[i].size(); j < J; ++j) {
+ eyes[groups[i][j]].cameraGet().photoGet().focal(f_init);
+ seq.push_back(SceneInfo<RGBf_Space>(&(eyes[groups[i][j]]),
+ CAN_ROTATE | CAN_ZOOM));
+ }
+ bdl.threshold(threshold);
+ bdl.adjustEye(&seq);
+ messagePrintf(-1, "ok");
+ }
+ return true;
+}
+
+bool input() {
+ messagePrintf(1, "loading images");
+ for (size_t i = 0, I = eyes.size(); i < I; ++i) {
+ messagePrintf(1, "%s", bitmap_name[i].c_str());
+ cv::Mat img = cv::imread(bitmap_name[i], CV_LOAD_IMAGE_COLOR);
+ if (!img.data) {
+ messagePrintf(-1, "opencv read error!, ignore");
+ continue;
+ }
+ size_t width = img.size().width ;
+ size_t height = img.size().height;
+ Bitmap<RGBf_Space> bmp;
+ bmp.size(height, width, RGBf_Space(0));
+ for (size_t x = 0; x < width; x++) {
+ for (size_t y = 0; y < height; y++) {
+ RGBi_Space tmp(Vector3D<int>(
+ img.at<cv::Vec3b>(y, x)[2],
+ img.at<cv::Vec3b>(y, x)[1],
+ img.at<cv::Vec3b>(y, x)[0]));
+ RGBf_Space p;
+ colorTransformate(tmp, &p);
+ bmp.pixel(y, x, p);
+ }
+ }
+ eyes[i].cameraGet().photoGet().reset(bmp, eyes[i].camera().photo().focal());
+ messagePrintf(-1, "ok");
+ }
+ messagePrintf(-1, "ok");
+ return true;
+}
+
+bool output() {
+ for (size_t i = 0, I = groups.size(); i < I; ++i) {
+ WatchBall<RGBf_Space> ball;
+ std::vector<Camera<RGBf_Space> > cs(groups[i].size());
+ for (size_t j = 0, J = groups[i].size(); j < J; ++j) {
+ cs[j].referenceFrom(eyes[groups[i][j]].camera());
+ }
+ ball.cameras(cs);
+ balls.push_back(ball);
+ }
+ double r = inRange(10.0, 100000.0, atof(usg.optionValue("output-radius", 0).c_str()));
+ messagePrintf(1, "Write images");
+ Bitmap<RGBf_Space> bmp;
+ for (size_t i = 0; i < balls.size(); i++) {
+ bmp = balls[i].expand(r);
+ size_t width = bmp.width ();
+ size_t height = bmp.height();
+ cv::Mat img(height, width, CV_8UC3);
+ for (size_t x = 0; x < width; x++) {
+ for (size_t y = 0; y < height; y++) {
+ RGBi_Space tmp;
+ colorTransformate(bmp.pixel(y, x), &tmp);
+ img.at<cv::Vec3b>(y, x)[0] = tmp.b();
+ img.at<cv::Vec3b>(y, x)[1] = tmp.g();
+ img.at<cv::Vec3b>(y, x)[2] = tmp.r();
+ }
+ }
+ std::string output_name(usg.optionValue("o", 0)
+ + (balls.size() > 1 ? stringPrintf("%lu", i) : "")
+ + ".jpg");
+ messagePrintf(1, "Write to file '%s'", output_name.c_str());
+ if (imwrite(output_name, img) == false) {
+ messagePrintf(-1, "opencv fail, ignore");
+ }
+ else {
+ messagePrintf(-1, "%lux%lu, ok", width, height);
+ }
+ }
+ messagePrintf(-1, "ok");
+ return true;
+}
+
+int main(int argc, char** argv) {
+ setup(argc, argv);
+ read();
+ group();
+ bundle();
+ input();
+ output();
+ return 0;
+}
+
generated by cgit v1.2.3 (git 2.25.1) at 2025-08-27 01:34:01 +0800