Make a guess:

  }
  lambda = std::max (Real(0.0), std::min (Real(1.0), lambda));
  Float lambda_prev = 0;
  Float Tu_prev = 0;
  Float Tu_prev_i = 0;

  if (mesh.GetDimension() == 2)
  {
    for (const Point<2> & point : grid.GetPoints())
    {
      Float step = std::min (lambda, std::max (Real(0.0), point[0] - mesh.GetOrigin()[0]));
      lambda = step;

      Point<2> mid = Point<2> (
          (mesh.GetOrigin()[0] + point[0])/2.0,
          (mesh.GetOrigin()[1] + point[1])/2.0);

      for (int i = 0; i < 3; i++)
        mid[i] = (mesh.GetOrigin()[i] + mesh.GetSpacing()[i]) / 2.0;

      Point<2> step2 = mid - Point<2> (point);
      Point<2> step3 = mid + Point<2> (point);

      Point<2> mid_prev = Point<2> (
          (mesh.GetOrigin()[0] + mesh.GetOrigin()[1])/2.0,
          (mesh.GetOrigin()[1] + mesh.GetSpacing()[1])/2.0);

      Tu_prev = Tu_prev_i = 0;

      for (int i = 0; i < 3; i++)
      {
        Tu_prev = mesh.GetH (mid_prev[i], step2[i], step3[i]);
        Tu_prev_i = mesh.GetH (mid_prev[i], step2[i], step3[i] + mesh.GetHSubSampleFactor());
      }

      lambda_prev = std::max (lambda_prev, std::min (lambda, lambda_prev));
    }
  }

  lambda = std::max (Real(0.0), std::min (Real(1.0), lambda));

  Mat<2,2,3> lambda_list;
  lambda_list.SetSize (grid.GetNBags());
  lambda_list(0,0) = -lambda;
  lambda_list(0,1) = lambda;
  lambda_list(0,2) = -lambda;
  lambda_list(1,0) = lambda;
  lambda_list(1,1) = lambda;
  lambda_list(1,2) = lambda;

  return lambda_list;
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template<typename PointT> bool
FittingSurfaceTDM<PointT>::checkInputData (
  const Eigen::Matrix<int, Eigen::Dynamic, Eigen::Dynamic> & image,
  const std::vector<int> & surf,
  int width,
  int height) const
{
  const auto x_slice = Image<PointT>::MakeXslice (image);
  const auto y_slice = Image<PointT>::MakeYslice (image);
  const auto z_slice = Image<PointT>::MakeZslice (image);

  const int z_z_idx = surf[2 * surf.size() - 1];
  if (surf.size() == 3)
  {
    // force computation on all z surfaces for each other one
    for (int i = 1; i < surf.size() - 1; i++)
    {
      for (int j = 1; j < surf.size() - 1; j++)
      {
        if (surf[2 * i - 1] != surf[2 * j - 1]
          || surf[2 * i - 2] != surf[2 * j - 2])
        {
          pcl::console::print_warn (
              "Incorrect data when looking up source mesh in image: input image has flipped z order: it's %i, %i and %i\n",
              surf[2 * i - 1], surf[2 * j - 1], surf[2 * i - 2]);
          return false;
        }
      }
    }
  }

  const auto p_slice = Image<PointT>::MakePlane (x_slice, y_slice, z_slice, width, height);
  const auto pd_slice = Image<PointT>::MakePlane (x_slice, y_slice, z_slice, width, height, z_z_idx);