CeresSolverでH行列を計算する．トライアンギュレーションする．

ceres_cost_function.hpp

#include <cmath>

#include "ceres/ceres.h"
#include "glog/logging.h"

#include <opencv2/opencv.hpp>

using namespace std;

using ceres::AutoDiffCostFunction;
using ceres::CostFunction;
using ceres::Problem;
using ceres::Solver;
using ceres::Solve;
using ceres::CauchyLoss;

struct HomographyCostFunction{
  HomographyCostFunction(cv::Point2d _src_point, cv::Point2d _dst_point){
    src_point = _src_point;
    dst_point = _dst_point;
  }
  template<typename T>
  bool operator () (const T* const h, T* residual) const {
    T projected_point_on_im2[3];
    {
      projected_point_on_im2[0] = h[0]*T(src_point.x) + h[1]*T(src_point.y) + h[2]*T(1.);
      projected_point_on_im2[1] = h[3]*T(src_point.x) + h[4]*T(src_point.y) + h[5]*T(1.);
      projected_point_on_im2[2] = h[6]*T(src_point.x) + h[7]*T(src_point.y) + 1.0*T(1.);
      /*
       *  projected_point_on_im2 * S = H * point_on_im1(homogenious <==> z=1)
       */
    }

    residual[0] = projected_point_on_im2[0]/projected_point_on_im2[2] - T(dst_point.x);
    residual[1] = projected_point_on_im2[1]/projected_point_on_im2[2] - T(dst_point.y);
    return true;
  }
  cv::Point2d src_point, dst_point;
};

struct FundamentalCostFunction{
  FundamentalCostFunction(cv::Point2f _src_point, cv::Point2f _dst_point){
    src_point = _src_point;
    dst_point = _dst_point;
  }
  template<typename T>
  bool operator () (const T* const f, T* residual) const {
    T line[3];

    line[0] = f[0]*T(src_point.x) + f[1]*T(src_point.y) + f[2]*T(1.);
    line[1] = f[3]*T(src_point.x) + f[4]*T(src_point.y) + f[5]*T(1.);
    line[2] = f[6]*T(src_point.x) + f[7]*T(src_point.y) + T(1.);
    /*
     *  line = F * point_on_im1
     *  x.t() * line ---->>> 0 
     */

    residual[0] = line[0]*T(dst_point.x) + line[1]*T(dst_point.y) + line[2]*T(1.);
    return true;
  }
  cv::Point2f src_point, dst_point;
};

struct TriangulationCostFunction{
  TriangulationCostFunction(cv::Point2d _src_point, 
                            cv::Mat _pose1,
                            cv::Point2d _dst_point,
                            cv::Mat _pose2){
    src_point = _src_point;
    dst_point = _dst_point;

    pose1 = _pose1; // == projection matric
    pose2 = _pose2; // == projection matric
  }

  template<typename T>
  bool operator () (const T* const p3d_homo, T* residual) const {
    cv::Mat A(4, 4, CV_64F);

    A.row(0) = src_point.x*pose1.row(2)-pose1.row(0);
    A.row(1) = src_point.y*pose1.row(2)-pose1.row(1);
    A.row(2) = dst_point.x*pose2.row(2)-pose2.row(0);
    A.row(3) = dst_point.y*pose2.row(2)-pose2.row(1);

    residual[0] = T(A.at<double>(0,0))*p3d_homo[0] +
                  T(A.at<double>(0,1))*p3d_homo[1] +
                  T(A.at<double>(0,2))*p3d_homo[2] +
                  T(A.at<double>(0,3))*p3d_homo[3];

    residual[1] = T(A.at<double>(1,0))*p3d_homo[0] +
                  T(A.at<double>(1,1))*p3d_homo[1] +
                  T(A.at<double>(1,2))*p3d_homo[2] +
                  T(A.at<double>(1,3))*p3d_homo[3];

    residual[2] = T(A.at<double>(2,0))*p3d_homo[0] +
                  T(A.at<double>(2,1))*p3d_homo[1] +
                  T(A.at<double>(2,2))*p3d_homo[2] +
                  T(A.at<double>(2,3))*p3d_homo[3];

    residual[3] = T(A.at<double>(3,0))*p3d_homo[0] +
                  T(A.at<double>(3,1))*p3d_homo[1] +
                  T(A.at<double>(3,2))*p3d_homo[2] +
                  T(A.at<double>(3,3))*p3d_homo[3];
    /*
     * AX => 0
     */
    return true;
  }

  cv::Mat pose1, pose2;
  cv::Point2d src_point, dst_point;
};