パースして外部キャリブレーションする

gistfile1.cpp

#include <iostream>
#include <fstream>
#include <sstream>
#include <vector>
#include <regex>

#include <boost/program_options.hpp>
#include <boost/foreach.hpp>
#include <boost/regex.hpp>

#include <opencv/highgui.h>
#include <opencv/cv.h>
#include <opencv/cxcore.h>

namespace dc_parser {

double toDouble(std::string s) { double r = 0; std::istringstream ss(s); ss >> r; return r; }

std::vector <std::string> split(const std::string _s, const std::string del)
{
    std::vector <std::string> ret;
    std::string s = _s;

    while (!s.empty())
    {
        size_t pos = s.find(del);
        std::string sub = "";
        sub = s.substr(0, pos);
        ret.push_back(sub);
        if (pos != std::string::npos)
        pos += del.size();
        s.erase(0, pos);
    }
    return ret;
}

int loadFromLine(std::ifstream& file, std::string& line, const std::string& escape_char){

  if ( !std::getline(file, line) ) return -1; // failed to load

  // escape sequence
  // excape character regexp
  boost::regex pattern("^\\s*" + escape_char);
  if ( regex_search(line, pattern) ) return 0; // skip the line

  return 1; // success
}

cv::Mat lineToMat(const std::string& line) {
  cv::Mat row;
  // load as string array
  std::vector<std::string> tmp  = split(line, " ");
  for (const auto& str : tmp) row.push_back(toDouble(str));
  return row;
}

std::vector<double> lineToVector(const std::string& line) {
  // load as string array
  std::vector<std::string> tmp  = split(line, " ");
  // to double convertion
  std::vector<double> args;
  for (const auto& str : tmp) args.push_back(toDouble(str));

  return args;
}

void loadFromDat(std::string& filename, int& cam_num, int& set_num, std::vector<double>& sets, cv::Mat& data) {
  std::ifstream file(filename.c_str());

  std::string line;
  // comment section
  while (true) {
    int status = dc_parser::loadFromLine(file, line, "#");

    if (status == 1) break;
    assert(status >=0 && "load failed");
  }

  // camera
  cam_num = dc_parser::lineToVector(line)[0];
  int status = dc_parser::loadFromLine(file, line, "#");
  // set-num
  assert(status >=0 && "load failed");
  set_num = dc_parser::lineToVector(line)[0];
  status = dc_parser::loadFromLine(file, line, "#");
  // sets
  assert(status >=0 && "load failed");
  sets = dc_parser::lineToVector(line);

  // data
  // rows: set_points (xd yd x0 y0 x1 y1 x2 y2 ... )
  // cols: points_num
  cv::Mat data_row;
  while (dc_parser::loadFromLine(file, line, "#") > 0) {
    data_row = dc_parser::lineToMat(line);

    // 1つ目
    if (data.empty()) {
      data = data_row;
      continue;
    }

    cv::Mat concat_mat[2];
    concat_mat[0] = data;
    concat_mat[1] = data_row;
    cv::hconcat(concat_mat, 2, data);
  }

  // check
  int row_size = ( set_num + 1 )*2; // display xy + sets xy
  assert(row_size == data.rows && "data row size wrong");

}

}

namespace calib_util {

  void load_inparam(std::string& fname, cv::Mat& intrinsic_matrix, cv::Mat& dist_coeffs) {
    cv::FileStorage cvfs(fname, CV_STORAGE_READ);
    cvfs["intrinsicMat"] >> intrinsic_matrix;
    cvfs["distCoeffs"] >> dist_coeffs;
  }


  void excalib( const cv::Mat& intrinsic_matrix,  const cv::Mat& dist_coeffs, \
                const cv::Mat& image_points_mat,  const cv::Mat& display_points_mat, \
                cv::Mat& rotation_mat,            cv::Mat& tvec)
  {

    cv::vector<cv::Point3f> world_points;
    cv::vector<cv::Point2f> image_points;

    for ( int col=0; col < image_points_mat.cols; col++ ){
      float display_x = display_points_mat.at<float>(0, col);
      float display_y = display_points_mat.at<float>(1, col);

      float image_x = image_points_mat.at<float>(0, col);
      float image_y = image_points_mat.at<float>(1, col);


      world_points.push_back( cv::Point3f( static_cast<float>( display_x ),
                                           static_cast<float>( display_y ),
                                           0.0 ) );
      image_points.push_back( cv::Point2f( static_cast<float>( image_x ),
                                           static_cast<float>( image_y ) ));

    }

    cv::Mat rvec;
    cv::solvePnP(world_points, image_points, intrinsic_matrix, dist_coeffs, rvec, tvec);
    cv::Rodrigues(rvec, rotation_mat);
  }
}


int main(int argc, char const* argv[])
{
  boost::program_options::options_description cmdline("Command line options");
  cmdline.add_options()
    ("help,h",      "show help messages")
    ("camidx,c",    boost::program_options::value<int>(), "camera idx")
    ("inparam,i",   boost::program_options::value<std::string>(), "intrinsic matrix param .xml")
    ("output,o",   boost::program_options::value<std::string>(), "output filename")
    ("filename,f",  boost::program_options::value<std::string>(), "file name cam0-x-x-x-x-x.dat");

  boost::program_options::variables_map variables_map;
  try {
      // parse_command_line
      boost::program_options::store(boost::program_options::parse_command_line(argc, argv, cmdline), variables_map);
  } catch (std::exception &e) {
      std::cerr << e.what() << std::endl;
      return -1;
  }

  if (argc==1 || variables_map.count("help")){
      std::cerr << " USAGE: $ ./excalib --camidx 0 --inparam camera0.xml --filename camera0-1-2-3-4.dat " << std::endl;
      std::cerr << cmdline << std::endl;
      return 0;
  }

  std::string filename = variables_map["filename"].as<std::string>();

  int cam_num, set_num;
  std::vector<double> sets;
  cv::Mat data;

  dc_parser::loadFromDat(filename, cam_num, set_num, sets, data);

  std::string inparam_fpath = variables_map["inparam"].as<std::string>();
  cv::Mat intrinsic_matrix, dist_coeffs;
  calib_util::load_inparam(inparam_fpath, intrinsic_matrix, dist_coeffs);
  std::cout << intrinsic_matrix << std::endl;
  std::cout << dist_coeffs << std::endl;

//   std::cout << points << std::endl;
//   std::cout << points.rows << " " << points.cols << std::endl;
//   range [0, 2)
  std::string output_fname= variables_map["output"].as<std::string>();
  std::ofstream ofs(output_fname.c_str());

  ofs << "#cam_num 0\n" \
      << "#set_num 4\n" \
      << "#sets 1 2 3 4\n" \
      << "#\n" \
      << "#set 1\n"\
      << "#rot_mat 3x3\n"\
      << "#tvec 1x3\n" << std::endl;
  ofs << cam_num << std::endl;
  ofs << set_num << std::endl;
  for (auto& set : sets) ofs << set << " ";
  ofs << "\n" << std::endl;

  cv::Mat image_points_mat = data(cv::Range(0,2), cv::Range(0,data.cols));
  for ( int setidx=1; setidx<=set_num; setidx++ ) {
    int row_begin = 1+setidx;
    int row_end   = row_begin+2;
    cv::Mat display_points_mat = data(cv::Range(row_begin, row_end), cv::Range(0, data.cols));


    cv::Mat rotation_mat, tvec;
    calib_util::excalib(  intrinsic_matrix, dist_coeffs,
                          image_points_mat, display_points_mat,
                          rotation_mat,     tvec);

    ofs << sets[setidx-1] << std::endl;
    ofs << cv::format(rotation_mat, "csv") << std::endl;
    ofs << tvec << std::endl;

    ofs << cv::format(tvec, "csv") << std::endl;

  }
  return 0;
}

CXX=g++ -std=c++11
BINDIR=bin
SRCDIR=src
OBJDIR=obj
CXXFLAGS=-O2 -MMD -Wall -Wextra -std=c++11

CXXFLAGS+=`pkg-config --cflags opencv`
LDFLAGS=`pkg-config --libs opencv`

CXXFLAGS+= -lboost_program_options -lboost_regex

TARGETS=excalib

SRCS=$(wildcard $(SRCDIR)/*.cpp)
OBJS=$(SRCS:$(SRCDIR)/%.cpp=$(OBJDIR)/%.o)
DEPS=$(SRCS:$(SRCDIR)/%.cpp=$(OBJDIR)/%.d)

$(BINDIR)/$(TARGETS):   $(OBJS)
    $(CXX) -o $@ $(OBJS) $(CXXFLAGS) $(LDFLAGS)
    @echo 'Linking Complete.'

#$(OBJS):$(OBJDIR)/%.o:$(SRCDIR)/%.cpp
$(OBJDIR)/%.o:$(SRCDIR)/%.cpp
    $(CXX) -c $< $(CXXFLAGS) -o $@
    @echo 'Compiled' $< 'Successfully.'
    @echo $(OBJS)

clean:
    rm -rf $(BINDIR)/$(TARGETS) $(OBJS) $(DEPS)

-include $(DEPS)

makefileおかしかった