C++ Problem::AddResidualBlock方法代码示例

int main(int argc, char** argv){

	std::string help = 
		"Locates the 3D position of a sound source\n"
		"Arguments: <timestamp1> <timestamp2> <timestamp3> <timestamp4>\n"
		"Note: \n\ttimestamps must be in the correct order to obtain meaningful result\n";
	
	if(std::strcmp(argv[1], "-h") == 0){
		std::cout << help << std::endl;
	} else if(argc < 5){
		std::cout << "Usage:\n" << argv[0] 
			<< " <timestamp1> <timestamp2> <timestamp3> <timestamp4>"
			<< std::endl;
	}

	double ts1 = std::atof(argv[1]);
	double ts2 = std::atof(argv[2]);
	double ts3 = std::atof(argv[3]);
	double ts4 = std::atof(argv[4]);

	const double initial_x = 10;
	const double initial_y = 0;
	const double initial_z = 0;
	const double initial_t = ts1;
	double x = initial_x;
	double y = initial_y;
	double z = initial_z;
	double t = initial_t;

	Problem problem;
	CostFunction* h1cost = new AutoDiffCostFunction<Hydrophone1Cost ,1 ,1, 1, 1, 1>(new Hydrophone1Cost(ts1));
	CostFunction* h2cost = new AutoDiffCostFunction<Hydrophone2Cost ,1 ,1, 1, 1, 1>(new Hydrophone2Cost(ts2));
	CostFunction* h3cost = new AutoDiffCostFunction<Hydrophone3Cost ,1 ,1, 1, 1, 1>(new Hydrophone3Cost(ts3));
	CostFunction* h4cost = new AutoDiffCostFunction<Hydrophone4Cost ,1 ,1, 1, 1, 1>(new Hydrophone4Cost(ts4));
	problem.AddResidualBlock(h1cost, NULL, &x, &y, &z, &t);
	problem.AddResidualBlock(h2cost, NULL, &x, &y, &z, &t);
	problem.AddResidualBlock(h3cost, NULL, &x, &y, &z, &t);
	problem.AddResidualBlock(h4cost, NULL, &x, &y, &z, &t);

	Solver::Options options;
	options.max_num_iterations = 100;
	options.linear_solver_type = ceres::DENSE_QR;
	options.minimizer_progress_to_stdout = true;
	std::cout << "Initial x = " << x
	          << ", y = " << y
	          << ", z = " << z
	          << ", t = " << t
	          << "\n";
	// Run the solver!
	Solver::Summary summary;
	Solve(options, &problem, &summary);
	std::cout << summary.FullReport() << "\n";
	std::cout << "Final x = " << x
	          << ", y = " << y
	          << ", z = " << z
	          << ", t = " << t
	          << "\n";
	return 0;
}

int main(int argc, char** argv) {
	google::InitGoogleLogging(argv[0]);

	// The variable to solve for with its initial value. It will be
	// mutated in place by the solver.
	double x = 0.5;
	const double initial_x = x;

	// Build the problem.
	Problem problem;

	// Set up the only cost function (also known as residual). This uses
	// auto-differentiation to obtain the derivative (jacobian).
	CostFunction* cost_function =
		new AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor);
	problem.AddResidualBlock(cost_function, NULL, &x);

	// Run the solver!
	Solver::Options options;
	options.minimizer_progress_to_stdout = true;
	Solver::Summary summary;
	Solve(options, &problem, &summary);

	std::cout << summary.BriefReport() << "\n";
	std::cout << "x : " << initial_x
		<< " -> " << x << "\n";
	return 0;
}

// ================================================================================================
// =============================== FUNCTIONS of CLASS BALOptimizer ================================
// ================================================================================================
void BALOptimizer::runBAL()
{
    int num_cams = visibility->rows();
    int num_features = visibility->cols();
    int step_tr = translation_and_intrinsics->rows();
    int step_st = structure->rows();
    double cost;
    quaternion_vector2eigen_vector( *quaternion, q_vector );
    
    Problem problem;
    ceres::LossFunction* loss_function = new ceres::HuberLoss(1.0);
    
    Solver::Options options;
    options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
    options.minimizer_progress_to_stdout = true;
    options.gradient_tolerance = 1e-16;
    options.function_tolerance = 1e-16;
    options.num_threads = 8;
    options.max_num_iterations = 50;
    
    for (register int cam = 0; cam < num_cams; ++cam)
    {
        for (register int ft = 0; ft < num_features ; ++ft)
        {
	  if( (*visibility)(cam,ft) == true )
	  {
	      CostFunction* cost_function = new AutoDiffCostFunction<Snavely_RE_KDQTS, 2, 4, 6, 3>( 
		new Snavely_RE_KDQTS( (*coordinates)(cam,ft)(0), (*coordinates)(cam,ft)(1)) );
	      problem.AddResidualBlock(cost_function, loss_function, q_vector[cam].data(), 
				(translation_and_intrinsics->data()+step_tr*cam), (structure->data()+step_st*ft) );
	  }
        }
    }
    
    cost = 0;
    problem.Evaluate(Problem::EvaluateOptions(), &cost, NULL, NULL, NULL);
    std::cout << "Initial RMS Reprojection Error is : " << std::sqrt(double(cost/num_features)) << "\n";
    
    Solver::Summary summary;
    Solve(options, &problem, &summary);
    std::cout << summary.BriefReport() << "\n";
    
    cost = 0;
    problem.Evaluate(Problem::EvaluateOptions(), &cost, NULL, NULL, NULL);
    std::cout << "RMS Reprojection Error is : " << std::sqrt(double(cost/num_features)) << "\n\n";
    
    update(); // update quaternion; normaliza translation 1
    return;
}

int main(int argc, char *argv[]) {
  FLAGS_log_dir = "logs/";
  google::InitGoogleLogging(argv[0]);
  google::ParseCommandLineFlags(&argc, &argv, true);

  dataset::dataSet<double> data;
  data.residual_type = "quadratic";
  data.numPoints = 100;
  data.range["begin"] = -5.0;
  data.range["end"] = 5.0;

  double A = 1.0;
  double B = 0.0;
  double C = 0.0;
  double D = 1.0;
  double E = 0.0;

  dataset::makeSet(&data, {&A, &B, &C});
  plot::plotData(&data);

  double Ap = 3.45;

  Problem problem;

  for (int i = 0; i < 100; i++){
    double x = data.xdata[i];
    double y = data.ydata[i];
    double r = 0.0;

    CostFunction* cost = residual<double>::Create(x, y, "quadratic");
    problem.AddResidualBlock(cost, NULL, &Ap, &B, &C);

  }

  Solver::Options options;

  Solver::Summary summary;
  Solve(options, &problem, &summary);

  std::cout << summary.FullReport() << std::endl;

  return 0;
}

void Odometry::computeTransformation()
{
    assert(observationVec.size() == cloud.size());
    assert(observationVec.size() == inlierMask.size());
    Problem problem;
    for (unsigned int i = 0; i < cloud.size(); i++)
    {
        
        if (not inlierMask[i]) continue;
        CostFunction * costFunc = new OdometryError(cloud[i],
                                        observationVec[i], TbaseCam, camera);
        problem.AddResidualBlock(costFunc, NULL,
                    TorigBase.transData(), TorigBase.rotData());
    }
    
    Solver::Options options;
    options.linear_solver_type = ceres::DENSE_SCHUR;
    Solver::Summary summary;
    Solve(options, &problem, &summary);
}

bool solve_translations_problem_l2_chordal
(
  const int* edges,
  const double* poses,
  const double* weights,
  int num_edges,
  double loss_width,
  double* X,
  double function_tolerance,
  double parameter_tolerance,
  int max_iterations
)
{
  // seed the random number generator
  std::srand( std::time( NULL ) );

  // re index the edges to be a sequential set
  std::vector<int> reindexed_edges(edges, edges+2*num_edges);
  std::vector<int> reindexed_lookup;
  reindex_problem(&reindexed_edges[0], num_edges, reindexed_lookup);
  const int num_nodes = reindexed_lookup.size();

  // Init with a random guess solution
  std::vector<double> x(3*num_nodes);
  for (int i=0; i<3*num_nodes; ++i)
    x[i] = (double)rand() / RAND_MAX;

  // add the parameter blocks (a 3-vector for each node)
  Problem problem;
  for (int i=0; i<num_nodes; ++i)
    problem.AddParameterBlock(&x[3*i], 3);

  // set the residual function (chordal distance for each edge)
  for (int i=0; i<num_edges; ++i) {
    CostFunction* cost_function =
      new AutoDiffCostFunction<ChordFunctor, 3, 3, 3>(
      new ChordFunctor(poses+3*i, weights[i]));

    if (loss_width == 0.0) {
      // No robust loss function
      problem.AddResidualBlock(cost_function, NULL, &x[3*reindexed_edges[2*i+0]], &x[3*reindexed_edges[2*i+1]]);
    } else {
      problem.AddResidualBlock(cost_function, new ceres::HuberLoss(loss_width), &x[3*reindexed_edges[2*i+0]], &x[3*reindexed_edges[2*i+1]]);
    }
  }

  // Fix first camera in {0,0,0}: fix the translation ambiguity
  x[0] = x[1] = x[2] = 0.0;
  problem.SetParameterBlockConstant(&x[0]);

  // solve
  Solver::Options options;
#ifdef OPENMVG_USE_OPENMP
  options.num_threads = omp_get_max_threads();
  options.num_linear_solver_threads = omp_get_max_threads();
#endif // OPENMVG_USE_OPENMP
  options.minimizer_progress_to_stdout = false;
  options.logging_type = ceres::SILENT;
  options.max_num_iterations = max_iterations;
  options.function_tolerance = function_tolerance;
  options.parameter_tolerance = parameter_tolerance;
  
  // Since the problem is sparse, use a sparse solver iff available
  if (ceres::IsSparseLinearAlgebraLibraryTypeAvailable(ceres::SUITE_SPARSE))
  {
    options.sparse_linear_algebra_library_type = ceres::SUITE_SPARSE;
    options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
  }
  else if (ceres::IsSparseLinearAlgebraLibraryTypeAvailable(ceres::CX_SPARSE))
  {
    options.sparse_linear_algebra_library_type = ceres::CX_SPARSE;
    options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
  }
  else if (ceres::IsSparseLinearAlgebraLibraryTypeAvailable(ceres::EIGEN_SPARSE))
  {
    options.sparse_linear_algebra_library_type = ceres::EIGEN_SPARSE;
    options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
  }
  else
  {
    options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY;
  }

  Solver::Summary summary;
  Solve(options, &problem, &summary);

  std::cout << summary.FullReport() << "\n";

  if (summary.IsSolutionUsable())
  {
    // undo the re indexing
    for (int i=0; i<num_nodes; ++i) {
      const int j = reindexed_lookup[i];
      X[3*j+0] = x[3*i+0];
      X[3*j+1] = x[3*i+1];
      X[3*j+2] = x[3*i+2];
    }
  }
  return summary.IsSolutionUsable();
}

double CeresSolverPoissonImageEditing::solve(const NamedParameters& solverParameters, const NamedParameters& problemParameters, bool profileSolve, std::vector<SolverIteration>& iters)
{

    Problem problem;

    auto getPixel = [=](int x, int y) {
        return y * m_dims[0] + x;
    };

    const int pixelCount = m_dims[0] * m_dims[1];
    std::vector<float4> h_unknownFloat(pixelCount);
    std::vector<float4> h_target(pixelCount);
    std::vector<float> h_mask(pixelCount);

    findAndCopyArrayToCPU("X", h_unknownFloat, problemParameters);
    findAndCopyArrayToCPU("T", h_target, problemParameters);
    findAndCopyArrayToCPU("M", h_mask, problemParameters);

    std::vector<double4> h_unknownDouble(pixelCount);

    for (int i = 0; i < pixelCount; i++)
    {
        h_unknownDouble[i].x = h_unknownFloat[i].x;
        h_unknownDouble[i].y = h_unknownFloat[i].y;
        h_unknownDouble[i].z = h_unknownFloat[i].z;
        h_unknownDouble[i].w = h_unknownFloat[i].w;
    }

    vector< pair<int, int> > edges;
    for (int y = 0; y < m_dims[1] - 1; y++)
    {
        for (int x = 0; x < m_dims[0] - 1; x++)
        {
            int pixel00 = getPixel(x + 0, y + 0);
            int pixel10 = getPixel(x + 1, y + 0);
            int pixel01 = getPixel(x + 0, y + 1);
            int pixel11 = getPixel(x + 1, y + 1);
            edges.push_back(make_pair(pixel00, pixel10));
            edges.push_back(make_pair(pixel00, pixel01));

            edges.push_back(make_pair(pixel11, pixel10));
            edges.push_back(make_pair(pixel11, pixel01));
        }
    }

    cout << "Edges: " << edges.size() << endl;

    int edgesAdded = 0;
    // add all edge constraints
    for (auto &e : edges)
    {
        const float mask = h_mask[e.first];
        if (mask == 0.0f)
        {
            const vec4f targetA = toVec(h_target[e.first]);
            const vec4f targetB = toVec(h_target[e.second]);

            vec4f targetDelta = targetA - targetB;
            ceres::CostFunction* costFunction = EdgeTerm::Create(targetA - targetB, 1.0f);
            double4 *varStartA = h_unknownDouble.data() + e.first;
            double4 *varStartB = h_unknownDouble.data() + e.second;

            problem.AddResidualBlock(costFunction, NULL, (double*)varStartA, (double*)varStartB);
            edgesAdded++;
        }
    }
    cout << "Edges added: " << edgesAdded << endl;

    // add all fit constraints
    set<int> addedEdges;
    for (auto &e : edges)
    {
        const float mask = h_mask[e.first];
        if (mask != 0.0f && addedEdges.count(e.first) == 0)
        {
            addedEdges.insert(e.first);
            const vec4f target = toVec(h_unknownFloat[e.first]);

            ceres::CostFunction* costFunction = FitTerm::Create(target, 1.0f);
            double4 *varStart = h_unknownDouble.data() + e.first;

            problem.AddResidualBlock(costFunction, NULL, (double*)varStart);
            edgesAdded++;
        }
    }

    cout << "Solving..." << endl;

    Solver::Summary summary;
    unique_ptr<Solver::Options> options = initializeOptions(solverParameters);
    options->function_tolerance = 0.01;
    options->gradient_tolerance = 1e-4 * options->function_tolerance;

    double cost = launchProfiledSolveAndSummary(options, &problem, profileSolve, iters);
    m_finalCost = cost;

    for (int i = 0; i < pixelCount; i++)
    {
        h_unknownFloat[i].x = (float)h_unknownDouble[i].x;
        h_unknownFloat[i].y = (float)h_unknownDouble[i].y;
//.........这里部分代码省略.........

int main( int argc, char** argv )
{
  google::InitGoogleLogging(argv[0]);

  if ((argc < 3) || (argc > 4)) {
    std::cerr << "usage: gettruth <measurement_file> <camera file> [<output file>]\n";
    return 1;
  }

  FILE* file;
  if (argc == 3) {
    file = stdout;
  } else {
    file = fopen(argv[3], "w");
  }

  // get camera model
  calibu::Rig<double> rig;
  calibu::LoadRig( std::string(argv[2]), &rig );
  if (rig.cameras_.size() == 0) {
    fprintf(stderr, "No cameras in this rig or no camera file provided\n");
    exit(0);
  }
  calibu::CameraInterface<double>* cam = rig.cameras_[0];

  LocalizationProblem ctx;
  if (!ctx.LoadFile(argv[1])) {
    std::cerr << "ERROR: unable to open file " << argv[1] << "\n";
    return 1;
  }

  // Build the problem.
  Problem problem;

  for (int i = 0; i < ctx.num_observations(); ++i) {
    const Vector2d& measurement = ctx.observation(i);
    const Vector3d& landmark = ctx.landmark_for_observation(i);
    ceres::CostFunction* cost_function = ProjectionCost( landmark, measurement, cam );
    problem.AddResidualBlock( cost_function, NULL, ctx.pose_data_for_observation(i) );
  }

  fprintf(stdout, "\n\nThe problem has been set up with %d residuals and %d residual blocks\n", problem.NumResiduals(), problem.NumResidualBlocks());

  // Make Ceres automatically detect the bundle structure. Note that the
  // standard solver, SPARSE_NORMAL_CHOLESKY, also works fine but it is slower
  // for standard bundle adjustment problems.
  ceres::Solver::Options options;
  options.linear_solver_type = ceres::DENSE_SCHUR;
  options.minimizer_progress_to_stdout = true;  

  ceres::Solver::Summary summary;
  ceres::Solve(options, &problem, &summary);

  for (int ii = 0; ii < ctx.num_observations(); ii++) {
    fprintf(file, "%f, %f, %f, %f, %f, %f\n", ii + 1, ctx.pose_for_observation(ii)(0), ctx.pose_for_observation(ii)(1),
            ctx.pose_for_observation(ii)(2), ctx.pose_for_observation(ii)(3), ctx.pose_for_observation(ii)(4), ctx.pose_for_observation(ii)(5));
  }
  fclose(file);

  return 0;
}

void Odometry::Ransac()
{
    assert(observationVec.size() == cloud.size());
    int numPoints = observationVec.size();
    
    inlierMask.resize(numPoints);
    
    const int numIterMax = 25;
    const Transformation<double> initialPose = TorigBase;
    int bestInliers = 0;
    //TODO add a termination criterion
    for (unsigned int iteration = 0; iteration < numIterMax; iteration++)
    {
        Transformation<double> pose = initialPose;
        int maxIdx = observationVec.size();
        //choose three points at random
	    int idx1m = rand() % maxIdx;
	    int idx2m, idx3m;
	    do 
	    {
		    idx2m = rand() % maxIdx;
	    } while (idx2m == idx1m);
	
	    do 
	    {
		    idx3m = rand() % maxIdx;
	    } while (idx3m == idx1m or idx3m == idx2m);
        
        
        //solve an optimization problem 
        
        Problem problem;
        for (auto i : {idx1m, idx2m, idx3m})
        {
            CostFunction * costFunc = new OdometryError(cloud[i],
                                        observationVec[i], TbaseCam, camera);
            problem.AddResidualBlock(costFunc, NULL,
                        pose.transData(), pose.rotData());
        }
        
        Solver::Options options;
        options.linear_solver_type = ceres::DENSE_SCHUR;
        Solver::Summary summary;
        options.max_num_iterations = 5;
        Solve(options, &problem, &summary);
            
        //count inliers
        vector<Vector3d> XcamVec(numPoints);
        Transformation<double> TorigCam = pose.compose(TbaseCam);
        TorigCam.inverseTransform(cloud, XcamVec);
        vector<Vector2d> projVec(numPoints);
        camera.projectPointCloud(XcamVec, projVec);
        vector<bool> currentInlierMask(numPoints, false);
        
        int countInliers = 0;
        for (unsigned int i = 0; i < numPoints; i++)
        {   
            Vector2d err = observationVec[i] - projVec[i];
            if (err.norm() < 2)
            {
                currentInlierMask[i] = true;
                countInliers++;
            }
        }
        //keep the best hypothesis
        if (countInliers > bestInliers)
        {
            //TODO copy in a bettegit lor way
            inlierMask = currentInlierMask;
            bestInliers = countInliers;
            TorigBase = pose;
        }        
    }
}

bool TargetLocatorService::executeCallBack( target_locater::Request &req, target_locater::Response &res)
{
  ros::NodeHandle nh;
  CameraObservations camera_observations;

  ROSCameraObserver camera_observer(image_topic_);
  camera_observer.clearObservations();
  camera_observer.clearTargets();

  // set the roi to the requested
  Roi roi;
  roi.x_min = req.roi.x_offset;
  roi.y_min = req.roi.y_offset;
  roi.x_max = req.roi.x_offset + req.roi.width;
  roi.y_max = req.roi.y_offset + req.roi.height;

  industrial_extrinsic_cal::Cost_function cost_type;
  
  camera_observer.clearTargets();
  camera_observer.clearObservations();

  camera_observer.addTarget(target_, roi, cost_type);
  camera_observer.triggerCamera();
  while (!camera_observer.observationsDone()) ;
  camera_observer.getObservations(camera_observations);
  int num_observations = (int) camera_observations.size();
  if(num_observations != target_rows_* target_cols_){
    ROS_ERROR("Target Locator could not find target %d", num_observations);
    return(false);
  }

  // set initial conditions
  target_->pose_.setQuaternion(req.initial_pose.orientation.x, req.initial_pose.orientation.y, req.initial_pose.orientation.z, req.initial_pose.orientation.w );
  target_->pose_.setOrigin(req.initial_pose.position.x, req.initial_pose.position.y, req.initial_pose.position.z );

  Problem problem;
  for(int i=0; i<num_observations; i++){
    double image_x = camera_observations[i].image_loc_x;
    double image_y = camera_observations[i].image_loc_y;
    Point3d point = target_->pts_[i]; // assume the correct ordering
    CostFunction* cost_function =
      industrial_extrinsic_cal::CameraReprjErrorPK::Create(image_x, image_y, focal_length_x_, focal_length_y_,center_x_, center_y_, point);
    problem.AddResidualBlock(cost_function, NULL , target_->pose_.pb_pose);
  }
  Solver::Options options;
  Solver::Summary summary;
  options.linear_solver_type = ceres::DENSE_SCHUR;
  options.minimizer_progress_to_stdout = false;
  options.max_num_iterations = 1000;
  ceres::Solve(options, &problem, &summary);

  if(summary.termination_type == ceres::USER_SUCCESS
     || summary.termination_type == ceres::FUNCTION_TOLERANCE
     || summary.termination_type == ceres::GRADIENT_TOLERANCE
     || summary.termination_type == ceres::PARAMETER_TOLERANCE
     ){

    double error_per_observation = summary.final_cost/num_observations;
    if(error_per_observation <= req.allowable_cost_per_observation){
      res.final_pose.position.x = target_->pose_.x;
      res.final_pose.position.y = target_->pose_.y;
      res.final_pose.position.z = target_->pose_.z;
      res.final_cost_per_observation  = error_per_observation;
      target_->pose_.getQuaternion(res.final_pose.orientation.x, res.final_pose.orientation.y, res.final_pose.orientation.z, res.final_pose.orientation.w);

      return true;
    }
    else{
      res.final_cost_per_observation  = error_per_observation;
      ROS_ERROR("allowable cost exceeded %f > %f", error_per_observation, req.allowable_cost_per_observation);
      return(false);
    }
  }
}

//.........这里部分代码省略.........
//    Eigen::LevenbergMarquardt<Eigen::NumericalDiff<my_functor>,double> lm(numDiff);
//    lm.parameters.maxfev = 2000;
//    lm.parameters.xtol = 1.0e-10;
//    std::cout << lm.parameters.maxfev << std::endl;

//    int ret = lm.minimize(x);
//    std::cout << lm.iter << std::endl;
//    std::cout << ret << std::endl;

//    std::cout << "x that minimizes the function: " << x << std::endl;

//////    Try to solve lidarboost with Eigen
//      my_functor functor;
//      Eigen::NumericalDiff<my_functor> numDiff(functor);
//      Eigen::LevenbergMarquardt<Eigen::NumericalDiff<my_functor>,double> lm(numDiff);
//      lm.parameters.maxfev = 2000;
//      lm.parameters.xtol = 1.0e-10;
//      std::cout << lm.parameters.maxfev << std::endl;

//    VectorXd val(2);
//    for(int i = 0; i < X.rows(); i++)
//    {
//        for(int j = 0; j < X.cols(); j++)
//        {
//            val = X(i, j);
//            int ret = lm.minimize(val);
//        }
//    }

//    std::cout << lm.iter << std::endl;
//    std::cout << ret << std::endl;

//    std::cout << "x that minimizes the function: " << X << std::endl;

////  Solve example using ceres

//         The variable to solve for with its initial value.
//        double initial_x = 5.0;
//        double x = initial_x;

        MatrixXd X(beta*n, beta*m);// init_X(beta*n, beta*m);
//        X = MatrixXd::Zero(beta*n,beta*m);
        X = up_D;
//        MatrixXd init_X( beta*n, beta*m );
//        init_X = X;
//        int M[2][2], M2[2][2];
//        M[0][0] = 5;
//        M[1][0] = 10;
//        M[0][1] = 20;
//        M[1][1] = 30;

//        M2 = *M;

        // Build the problem.
        Problem problem;

        // Set up the only cost function (also known as residual). This uses
        // auto-differentiation to obtain the derivative (jacobian).

        double val, w, t, d;

        Solver::Options options;
        options.linear_solver_type = ceres::DENSE_QR;
        options.minimizer_progress_to_stdout = false;
        Solver::Summary summary;

        for(int i = 0; i < X.rows(); i++)
        {
            for(int j = 0; j < X.cols(); j++)
            {

                val = X(i, j);
                w = W(i, j);
                t = T(i, j);
                d = up_D(i, j);

                std::cout << "i = " << i << "; j = " << j << std::endl;
                std::cout << "w = " << w << "; t = " << t << "; d = " << d << std::endl;
                CostFunction* cost_function =
                    new AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor(w, t, d));

                problem.AddResidualBlock(cost_function, NULL, &val);
                // Run the solver
                Solve(options, &problem, &summary);
                X(i, j) = val;
            }
        }




        std::cout << summary.BriefReport() << "\n";
//        std::cout << "x : " << init_X
//                  << " -> " << X << "\n";

        cv::Mat M3(beta*n, beta*m, CV_32FC1);
        cv::eigen2cv(X, M3);
        cv::namedWindow("check3", cv::WINDOW_AUTOSIZE );
        cv::imshow("check3", M3);
}