path: root/meowpp.test/src/rot_bundle.cpp

#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     > names;
std::vector<Eye<RGBf_Space> >  eyes;

double f_init    =  300;
double threshold =    5;
double r         = 1000;

//////////////////////////////////////////////////////////////////

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",
                stringPrintf("%f", r),
                false);
  usg.optionAdd("o",
                "prefix of output images",
                "pathname",
                "output",
                false);
  usg.optionAdd("f-init",
                "init focal length",
                "floating point",
                stringPrintf("%f", f_init),
                false);
  usg.optionAdd("t",
                "threshold for bundle adjustment",
                "floating point",
                stringPrintf("%f", threshold),
                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;
}

void fmtError(FILE* f) {
  fclose(f);
  fprintf(stderr, "\nFromat error!\n");
  exit(-1);
}

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) fmtError(f);
  names[i].resize(N);
  for (size_t i = 0; i < N; ++i) {
    char s[1000];
    if (fscanf(f, "%s", s) < 1) fmtError(f);
    names[i] = s;
  }
  eyes.resize(N);
  size_t M;
  if (fscanf(f, "%lu", &M) < 1) fmtError(f);
  for (size_t i = 0; i < M; ++M) {
    int a, b;
    double x1, y1, x2, y2;
    if (fscanf(f, "%d %lf %lf %d %lf %lf", &a, &x1, &y1, &b, &x2, &y2) < 6) fmtError(f);
    eyes[a].cameraGet().fixedPoints2DGet().pointAdd(i, Vector2D(x1, y1).matrix());
    eyes[b].cameraGet().fixedPoints2DGet().pointAdd(i, Vector2D(x2, y2).matrix());
  }
  fclose(f);
  messagePrintf(-1, "ok");
}

bool bundle() {
  messagePrintf(1, "boundle adjustment");
  threshold = inRange(0.0005,   1000.0, atof(usg.optionValue("t"     , 0).c_str()));
  f_init    = inRange(0.0005, 100000.0, atof(usg.optionValue("f-init", 0).c_str()));
  BundleAdjustment_LM<RGBf_Space> bdl;
  bdl.threshold(threshold);
  std::vector<SceneInfo<RGBf_Space> > seq;
  for (size_t i = 0, I = eyes.size(); i < I; ++i) {
    eyes[i].cameraGet().photoGet().focal(f_init);
    seq.push_back(SceneInfo<RGBf_Space>(eyes[i], CAN_ROTATE | CAN_ZOOM));
  }
  bdl.adjustEyes(&seq);
  messagePrintf(-1, "ok");
  return true;
}

bool output() {
  r = inRange(10.0, 100000.0, atof(usg.optionValue("output-radius", 0).c_str()));
  Bitmap<RGBf_Space> output;
  Bitmap<double>     alpha;
  for (size_t i = 0, I = eyes.size(); i < I; ++i) {
    messagePrintf(1, "load image");
    cv::Mat img = cv::imread(names[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().bitmap(bmp);
    WatchBall<RGBf_Space> ball;
    ball.cameras(std::vector<Camera<RGBf_Space> >(1, eyes[i].camera()));
    std::pair<Bitmap<RGBf_Space>, Bitmap<double> > p = ball.expandAlpha(r);
    if (output.size() == 0) {
      output = p.first;
      alpha  = p.second;
    }
    else {
      output.matrix(output.matrix() + p.first .matrix());
      alpha .matrix(alpha .matrix() + p.second.matrix());
    }
  }
  for (size_t y = 0, Y = output.height(); y < Y; ++y)
    for (size_t x = 0, X = output.width(); x < X; ++x)
      if (noEPS(alpha.pixel(y, x)) > 0)
        output.pixel(y, x, output.pixel(y, x) / alpha.pixel(y, x));
  messagePrintf(1, "Write images");
  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(output.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) + ".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();
  bundle();
  input();
  output();
  return 0;
}