ceres优化参数的传递、残差构建时状态量的传递、evaluate函数的使用

误差函数中的参数包括： 已知参数和待优化参数两部分，

1. 添加待优化参数

ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
problem.AddParameterBlock(para_Pose[i], SIZE_POSE, local_parameterization);

2. 添加残差及需要对待优化参数变来进行雅可比矩阵的计算（解析求导）

先加入残差项
IMU残差项

  IMUFactor* imu_factor = new IMUFactor(pre_integrations[j]);
  problem.AddResidualBlock(imu_factor, NULL, para_Pose[i], para_SpeedBias[i], para_Pose[j], para_SpeedBias[j]);

视觉残差项
同一个相机前后两帧间的残差项
class ProjectionTwoFrameOneCamFactor : public ceres::SizedCostFunction<2, 7, 7, 7, 1, 1>
该继承的类说明残差的维度是2，输入的第一个待优化参数的维度是7，第二个待优化参数的维度是7，第三个待优化参数的维度是7，第四个待优化参数的维度是1，第五个待优化参数的维度是1。

 ProjectionTwoFrameOneCamFactor *f_td = new ProjectionTwoFrameOneCamFactor(pts_i, pts_j, it_per_id.feature_per_frame[0].velocity, it_per_frame.velocity,
                                                                 it_per_id.feature_per_frame[0].cur_td, it_per_frame.cur_td);//需要用来构建残差、雅可比矩阵的量，除了传入的优化参数外其他的已知参数
                problem.AddResidualBlock(f_td, loss_function, para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index], para_Td[0]);//需要求的关于哪些变量的雅可比矩阵，哪些变量是优化变量

 problem.AddResidualBlock()的时候要求传进来的是一维数组para_Pose[i]，和
 virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const;
   //注意这里的Evaluate()是一个虚函数，当ceres在优化求解的时候会调用Evaluate()函数，由于ceres里面把这个设计成了虚函数，就会调用到我们继承的类。

即parameters是指向二维数组的指针，针对一组残差，para_Pose[imu_i]赋值给了parameters[0]，维度是7；para_Pose[imu_j]赋值给parameters[1]，维度是7；
可以看出Evaluate()通过传入的待优化参数parameters计算残差residuals和雅可比矩阵jacobians
说明： 定义的视觉残差子类继承于ceres::SizedCostFunction，new生成f_td我们自定义的残差，放到了problem.AddResidualBlock()中残差函数对应的位置上，在ceres进行优化的时候通过虚函数的调用，调用了自己编写的Evaluate()函数进行残差、雅可比矩阵的计算。

3. 利用ceres的自动求导方式计算残差和雅可比矩阵

一个点在两帧之间的重投影误差
关于自动求导的求导方式:需要构造函数、重载操作符()、工厂函数create()；
构造函数是非必须的，重载操作符()、工厂函数create()是必须的。
生成了AutoDiffCostFunction能够自动进行求导

struct ReprojectionError3D
{
	ReprojectionError3D(double observed_u, double observed_v)
		:observed_u(observed_u), observed_v(observed_v)
		{}

	template <typename T>
	bool operator()(const T* const camera_R, const T* const camera_T, const T* point, T* residuals) const
	{
		T p[3];
		ceres::QuaternionRotatePoint(camera_R, point, p);
		p[0] += camera_T[0]; p[1] += camera_T[1]; p[2] += camera_T[2];
		T xp = p[0] / p[2];
    	T yp = p[1] / p[2];
    	residuals[0] = xp - T(observed_u);
    	residuals[1] = yp - T(observed_v);
    	return true;
	}

	static ceres::CostFunction* Create(const double observed_x,
	                                   const double observed_y) 
	{
	  return (new ceres::AutoDiffCostFunction<
	          ReprojectionError3D, 2, 4, 3, 3>(
	          	new ReprojectionError3D(observed_x,observed_y)));
	}

	double observed_u;
	double observed_v;
};

ceres::AutoDiffCostFunction<CostFunctor, int residualDim, int paramDim>(CostFunctor* functor);

		ceres::CostFunction* cost_function = ReprojectionError3D::Create(
												sfm_f[i].observation[j].second.x(),
												sfm_f[i].observation[j].second.y());

    		problem.AddResidualBlock(cost_function, NULL, c_rotation[l], c_translation[l], 
    								sfm_f[i].position);	 

模板参数依次为仿函数（functor）类型CostFunctor，残差维数residualDim和参数维数paramDim，接受参数类型为仿函数指针CostFunctor*。
说明： 在ceres源码中进行回溯，自动求导中一样调用了Evaluate()函数。
优化过程中先调用Evaluate()函数,然后调用了Call(const Functor& functor, T const *const input, T output) ，return functor(input, output);//调用了重载的()函数

  virtual bool Evaluate(double const* const* parameters,
                        double* residuals,
                        double** jacobians) const {
    if (!jacobians) {
      return internal::VariadicEvaluate<
          CostFunctor, double, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>
          ::Call(*functor_, parameters, residuals);//在这里进入call调用
    }
    return internal::AutoDiff<CostFunctor, double,
           N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>::Differentiate(
               *functor_,
               parameters,
               SizedCostFunction<kNumResiduals,
                                 N0, N1, N2, N3, N4,
                                 N5, N6, N7, N8, N9>::num_residuals(),
               residuals,
               jacobians);
  }

// Template instantiation for dynamically-sized functors.
template<typename Functor, typename T>
struct VariadicEvaluate<Functor, T, ceres::DYNAMIC, ceres::DYNAMIC,
                        ceres::DYNAMIC, ceres::DYNAMIC, ceres::DYNAMIC,
                        ceres::DYNAMIC, ceres::DYNAMIC, ceres::DYNAMIC,
                        ceres::DYNAMIC, ceres::DYNAMIC> {
  static bool Call(const Functor& functor, T const *const *input, T* output) {
    return functor(input, output);//调用了functor的仿函数
  }
};

ceres源码回溯

1、
solver.cc
464行，根据不同类型的优化器，生成了最小化cost_function的指针， minimizer->Minimize是基类的虚函数，会到不同优化器类的内部，

  const Vector original_reduced_parameters = pp->reduced_parameters;
  scoped_ptr<Minimizer> minimizer(
      Minimizer::Create(pp->options.minimizer_type));
  minimizer->Minimize(pp->minimizer_options,
                      pp->reduced_parameters.data(),
                      summary);

Minimizer* Minimizer::Create(MinimizerType minimizer_type) {
  if (minimizer_type == TRUST_REGION) {
    return new TrustRegionMinimizer;
  }

  if (minimizer_type == LINE_SEARCH) {
    return new LineSearchMinimizer;
  }

  LOG(FATAL) << "Unknown minimizer_type: " << minimizer_type;
  return NULL;
}

生成指向子类的指针，赋值给指向基类的指针， minimizer->Minimize设置成虚函数，从而达到程序编译时生成虚函数表没有定下到底执行哪个函数，在运行的时候执行子类对象的函数。
这样就通过虚函数的实现，选择不同类型的优化器。
2、如到TrustRegionMinimizer::Minimize内部
会调用TrustRegionMinimizer::EvaluateGradientAndJacobian计算雅可比矩阵，会调用到Evaluator->evaluate()函数，这也是一个虚函数。在program_evaluator.h中有调用，然后是residual_block.cc中调用ResidualBlock::Evaluate(),最后调用到cost_function_->Evaluate()，就是自己残差定义中的Evaluate函数中了。

补充：关于仿函数的说明

仿函数的本质为结构体struct或者类class，由于重载了()运算符，使得其能够具有和函数一样的调用行为，因此被称为仿函数。
在类中实现一个operator()，并给予合适的参数类型，这个类就有了类似于函数的行为，就是一个仿函数类了。

避免函数中用到的一些变量被迫成为全局变量，用到的一些参数可以通过仿函数传进来。

bool operator()(const T* const camera_R, const T* const camera_T, const T* point, T* residuals) const

通过仿函数的实现把要优化的参数传递进来，应该类似于解析求导中的Evalute()函数