// camera
        std::map<std::string, size_t> img2pos; // image key -> image id
        std::map<size_t, std::string> pos2img; // image id -> image key
        std::vector<std::string> cams_filenames; 
        std::vector<Eigen::Matrix3d> cams_rotation; // 摄影测量常用的规范, 实际对应 R_c_M
        std::vector<Eigen::Matrix3d> cams_intrinsic; // K
        std::vector<Eigen::MatrixXd> cams_projection; // camera model m = K [R|-Rt] X
        std::vector<Eigen::Vector3d> cams_translation; // 摄影测量常用的规范, 实际对应 r_c_cM
        std::vector<Eigen::Vector3d> cams_radial_dist; // 径向畸变
        std::vector<Eigen::Vector2d> cams_tangential_dist; // 切向畸变

        std::vector<Eigen::Vector3d> r_M_c;
        std::vector<Eigen::Quaterniond> q_M_c;
        Eigen::Vector3d offset;

		// feature observation
        std::map<std::string, std::list<unsigned int> > featuresPerCam; // 当前相机key -> list<特征点id>
        std::map<std::string, unsigned int> feat_key2id; // featurekey -> 特征点id
        std::map<unsigned int, std::string> feat_id2key; // 特征点id -> featurekey 
        std::map<unsigned int, bool> feat_valid;
        std::map<unsigned int, Eigen::Vector3d> feat_pos3D; // 特征点id , 坐标 
        std::vector<std::list<std::pair<size_t, Eigen::Vector2d> > > feat_observations; // 特征点id： <相机key, <2d 观测>>

void daisy::set_parameters( double rad, int rad_q_no, int th_q_no, int hist_th_q_no )
{
   m_rad = rad;                   // radius of the descriptor at the initial scale
   m_rad_q_no = rad_q_no;         // how many pieces shall I divide the radial range ?
   m_th_q_no = th_q_no;           // how many pieces shall I divide the angular range  ?
   m_hist_th_q_no = hist_th_q_no; // how many pieces shall I divide the grad_hist
   m_grid_point_number = m_rad_q_no * m_th_q_no + 1; // +1 is for center pixel
   m_descriptor_size = m_grid_point_number * m_hist_th_q_no;

   for( int i=0; i<360; i++ )
   {
      m_orientation_shift_table[i] = i/360.0 * m_hist_th_q_no;
   }
   m_layer_size = m_h*m_w;
   m_cube_size = m_layer_size*m_hist_th_q_no; //原始图片八个方向的梯度图

   compute_cube_sigmas();
   compute_grid_points();

}

计算每一个bin的中心位置
void daisy::compute_grid_points()
{
   double r_step = m_rad / m_rad_q_no;
   double t_step = 2*PI/ m_th_q_no;

   if( m_grid_points )
      deallocate( m_grid_points, m_grid_point_number );

   m_grid_points = allocate<double>(m_grid_point_number, 2);
   for( int y=0; y<m_grid_point_number; y++ )
   {
      m_grid_points[y][0] = 0;
      m_grid_points[y][1] = 0;
   }

   for( int r=0; r<m_rad_q_no; r++ )
   {
      int region = r*m_th_q_no+1;
      for( int t=0; t<m_th_q_no; t++ )
      {
         double y, x;
         polar2cartesian( (r+1)*r_step, t*t_step, y, x );
         m_grid_points[region+t][0] = y;
         m_grid_points[region+t][1] = x;
      }
   }
}

    float* gradient_layers = new float[h * w * 8];
    kutility::layered_gradient(img_data, h, w, 8, gradient_layers);

   /// computes the descriptor at homography-warped grid. (y,x) is not the
   /// coordinates of this image but the coordinates of the original grid where
   /// the homography will be applied. Meaning that the grid is somewhere else
   /// and we warp this grid with H and compute the descriptor on this warped
   /// grid; returns null/false if centers falls outside the image; allocate
   /// 'descriptor' memory first. descriptor is normalized.

   inline bool get_descriptor(double y, double x, int orientation, double* H, float* descriptor );

   //使用到的单应变换
   void point_transform_via_homography( double* H, double x, double y, double &u, double &v )
   {
      double kxp = H[0]*x + H[1]*y + H[2];
      double kyp = H[3]*x + H[4]*y + H[5];
      double kp  = H[6]*x + H[7]*y + H[8];
      u = kxp / kp;
      v = kyp / kp;
   }

	Image::Cphoto photo;
	photo.init(name, mname, ename, cname, 3); // 图片名，mask名，edge名，camera名，尺度
	photo.alloc(); // alloc 之后才会加载图片，对应free

	std::vector<unsigned char>& imagedata0 = photo.getImage(0); // 加载不同尺度，存储方式为RGB，RGB
	std::vector<unsigned char>& imagedata1 = photo.getImage(1); 

	Image::CphotoSetS pss; // photo 的集合
	std::vector<int>images;
	for (int i = 0; i < 5; i++)
	{
		images.push_back(i);
	}
	int level = 3;
	int windowSize = 7;
	pss.init(images, prefix, level, windowSize, 1); // 初始化一个photo集合

#include "tinycthread/tinycthread.h"
#include "tinycthread/rwmutex.h"

using namespace std;
mtx_t m_rwlock;
static int count_i = 0;

int thread_func(void * argc)
{
	int * threadId = (int*)argc;
	for (int i = 0; i < 1000; i++)
	{
		mtx_lock(&m_rwlock);
		count_i = count_i + 1;
		std::cout << "hello: " << count_i << " from thread "<< *threadId <<"\n";
		mtx_unlock(&m_rwlock);
	}	
	return 0;
}

int main(int argc, char** argv)
{
	int m_CPU = 6;
	mtx_init(&m_rwlock, mtx_plain | mtx_recursive);
	vector<thrd_t> threads(m_CPU);
	for (int i = 0; i < m_CPU; ++i)
		thrd_create(&threads[i], thread_func, (void*)(&i));
	for (int i = 0; i < m_CPU; ++i)
		thrd_join(threads[i], NULL);

	mtx_destroy(&m_rwlock);
}

	CdetectFeatures df;
	const int fcsize = 16;

	int detectLevel = 0; //detectLevel 越大，指的是降采样的图像上检测
	df.run(pss, 5, fcsize, detectLevel, 6);

void collectCandidates(const int index, const std::vector<int>& indexes,
                         const Cpoint& point, std::vector<Ppoint>& vcp); 


///  reference image id
///  target image ids
///  reference point
///  candidate points

template<class T>
void setF(const Image::Ccamera& lhs, const Image::Ccamera& rhs,
	  TMat3<T>& F, const int level = 0) {
  const TVec4<T>& p00 = lhs.m_projection[level][0];
  const TVec4<T>& p01 = lhs.m_projection[level][1];
  const TVec4<T>& p02 = lhs.m_projection[level][2];

  const TVec4<T>& p10 = rhs.m_projection[level][0];
  const TVec4<T>& p11 = rhs.m_projection[level][1];
  const TVec4<T>& p12 = rhs.m_projection[level][2];

  F[0][0] = det(TMat4<T>(p01, p02, p11, p12));
  F[0][1] = det(TMat4<T>(p01, p02, p12, p10));
  F[0][2] = det(TMat4<T>(p01, p02, p10, p11));

  F[1][0] = det(TMat4<T>(p02, p00, p11, p12));
  F[1][1] = det(TMat4<T>(p02, p00, p12, p10));
  F[1][2] = det(TMat4<T>(p02, p00, p10, p11));

  F[2][0] = det(TMat4<T>(p00, p01, p11, p12));
  F[2][1] = det(TMat4<T>(p00, p01, p12, p10));
  F[2][2] = det(TMat4<T>(p00, p01, p10, p11));
 };

// make sorted array of feature points in images, that satisfy the
// epipolar geometry coming from point in image
void Cseed::collectCandidates(const int index, const std::vector<int>& indexes,
                              const Cpoint& point, std::vector<Ppoint>& vcp) 

void Cseed::collectCells(const int index0, const int index1,
                         const Cpoint& p0, std::vector<Vec2i>& cells) 

double myfunc(unsigned n, const double* x, double* grad, void* my_func_data)
{
    if (grad) {
        grad[0] = 0.0;
        grad[1] = 0.5 / sqrt(x[1]);
    }
    return sqrt(x[1]);
}

typedef struct {
    double a, b;
} my_constraint_data;

double myconstraint(unsigned n, const double* x, double* grad, void* data)
{
    my_constraint_data* d = (my_constraint_data*)data;
    double a = d->a, b = d->b;
    if (grad) {
        grad[0] = 3 * a * (a * x[0] + b) * (a * x[0] + b);
        grad[1] = -1.0;
    }
    return ((a * x[0] + b) * (a * x[0] + b) * (a * x[0] + b) - x[1]);
}

int main() {
    double lb[2] = { -HUGE_VAL, 0 }; /* lower bounds */
    double ub[2] = { HUGE_VAL, HUGE_VAL }; /* lower bounds */
    nlopt_opt opt;

    opt = nlopt_create(NLOPT_LD_MMA, 2); /* algorithm and dimensionality */
    nlopt_set_lower_bounds(opt, lb);
    nlopt_set_upper_bounds(opt, ub);
    nlopt_set_min_objective(opt, myfunc, NULL);

    my_constraint_data data[2] = { {2,0}, {-1,1} };

    nlopt_add_inequality_constraint(opt, myconstraint, &data[0], 1e-8);
    nlopt_add_inequality_constraint(opt, myconstraint, &data[1], 1e-8);

    nlopt_set_xtol_rel(opt, 1e-4);

    double x[2] = { 1.234, 5.678 };  /* `*`some` `initial` `guess`*` */
    double minf; /* `*`the` `minimum` `objective` `value,` `upon` `return`*` */
    if (nlopt_optimize(opt, x, &minf) < 0) {
        printf("nlopt failed!\n");
    }
    else {
        printf("found minimum at f(%g,%g) = %0.10g\n", x[0], x[1], minf);
    }

    nlopt_destroy(opt);
}

void Coptim::encode(const Vec4f& coord,  // 深度，注意是m_raysT是参考相机观测到patch的光线
		    double* const vect, const int id) const {
  vect[0] = (coord - m_centersT[id]) * m_raysT[id] / m_dscalesT[id];
}

void Coptim::encode(const Vec4f& coord, const Vec4f& normal,
		    double* const vect, const int id) const {
  encode(coord, vect, id);
  
  const int image = m_indexesT[id][0];
  const float fx = m_xaxes[image] * proj(normal); // projects from 4D to 3D, divide by last value
  const float fy = m_yaxes[image] * proj(normal);
  const float fz = m_zaxes[image] * proj(normal); // 计算法向量到相机三个轴的投影

  vect[2] = asin(max(-1.0f, min(1.0f, fy))); // 可以理解为计算经纬度
  const float cosb = cos(vect[2]);

  if (cosb == 0.0)
    vect[1] = 0.0;
  else {
    const float sina = fx / cosb;
    const float cosa = - fz / cosb;
    vect[1] = acos(max(-1.0f, min(1.0f, cosa)));
    if (sina < 0.0)
      vect[1] = - vect[1];
  }

  vect[1] = vect[1] / m_ascalesT[id];
  vect[2] = vect[2] / m_ascalesT[id];  
}

//----------------------------------------------------------------------
// BFGS functions
//----------------------------------------------------------------------

double Coptim::my_f(unsigned n, const double *x, double *grad, void *my_func_data)

// get x and y axis to collect textures given reference image and normal
void Coptim::getPAxes(const int index, const Vec4f& coord, const Vec4f& normal,
		      Vec4f& pxaxis, Vec4f& pyaxis) const{  
  // yasu changed here for fpmvs
  const float pscale = getUnit(index, coord);

  Vec3f normal3(normal[0], normal[1], normal[2]);
  Vec3f yaxis3 = cross(normal3, m_xaxes[index]);
  unitize(yaxis3);
  Vec3f xaxis3 = cross(yaxis3, normal3);
  pxaxis[0] = xaxis3[0];  pxaxis[1] = xaxis3[1];  pxaxis[2] = xaxis3[2];  pxaxis[3] = 0.0;
  pyaxis[0] = yaxis3[0];  pyaxis[1] = yaxis3[1];  pyaxis[2] = yaxis3[2];  pyaxis[3] = 0.0;

  pxaxis *= pscale;
  pyaxis *= pscale;
  const float xdis = norm(m_fm.m_pss.project(index, coord + pxaxis, m_fm.m_level) -
                          m_fm.m_pss.project(index, coord, m_fm.m_level));
  const float ydis = norm(m_fm.m_pss.project(index, coord + pyaxis, m_fm.m_level) -
                          m_fm.m_pss.project(index, coord, m_fm.m_level));
  pxaxis /= xdis;
  pyaxis /= ydis;
}

int Coptim::grabTex(const Vec4f& coord, const Vec4f& pxaxis, const Vec4f& pyaxis,
		    const Vec4f& pzaxis, const int index, const int size,
                    std::vector<float>& tex) const {
  tex.clear();

  Vec4f ray = m_fm.m_pss.m_photos[index].m_center - coord;
  unitize(ray);
  const float weight = max(0.0f, ray * pzaxis);

  //???????
  //if (weight < cos(m_fm.m_angleThreshold0))
  if (weight < cos(m_fm.m_angleThreshold1))
    return 1;

  const int margin = size / 2;

  Vec3f center = m_fm.m_pss.project(index, coord, m_fm.m_level);  
  Vec3f dx = m_fm.m_pss.project(index, coord + pxaxis, m_fm.m_level) - center;
  Vec3f dy = m_fm.m_pss.project(index, coord + pyaxis, m_fm.m_level) - center;
  
  const float ratio = (norm(dx) + norm(dy)) / 2.0f;
  //int leveldif = (int)floor(log(ratio) / log(2.0f) + 0.5f);
  int leveldif = (int)floor(log(ratio) / Log2 + 0.5f);

  // Upper limit is 2
  leveldif = max(-m_fm.m_level, min(2, leveldif));

  //const float scale = pow(2.0f, (float)leveldif);

  const float scale = MyPow2(leveldif);
  const int newlevel = m_fm.m_level + leveldif; // 这里的采样方式是先确定尺度，再把dx，dy，center归一化

  center /= scale;  dx /= scale;  dy /= scale;
  
  if (grabSafe(index, size, center, dx, dy, newlevel) == 0) // 检查投影的patch最大最小值是否在图像范围内
    return 1;
  
  Vec3f left = center - dx * margin - dy * margin;

  tex.resize(3 * size * size);
  float* texp = &tex[0] - 1;
  for (int y = 0; y < size; ++y) {
    Vec3f vftmp = left;
    left += dy;
    for (int x = 0; x < size; ++x) {
      Vec3f color = m_fm.m_pss.getColor(index, vftmp[0], vftmp[1], newlevel);
      *(++texp) = color[0];
      *(++texp) = color[1];
      *(++texp) = color[2];
      vftmp += dx;
    }
  }
  
  return 0;
}