аналитическая производная решателя Цереры не работает

template<typename ConcreteOccGridMapUtil>
class getResidual : public ceres::SizedCostFunction<1,3>
{
public:
    ConcreteOccGridMapUtil* occ;
    DataContainer dataPoints;


    getResidual(ConcreteOccGridMapUtil* occ, const DataContainer& dataPoints)
    {
        this->occ = occ;
        this->dataPoints = dataPoints;
    }
    virtual ~getResidual() {}
    virtual bool Evaluate(double const* const* parameters,
                          double* residuals,
                          double** jacobians) const
    {
        Eigen::Matrix<double, 3, 1> pose1(parameters[0][0],parameters[0][1],parameters[0][2]);
        Eigen::Vector3f pose = pose1.cast<float>();
        Eigen::Affine2f transform(occ->getTransformForState(pose)); // transform: rotation->translation


        float sinRot = std::sin(pose[2]);
        float cosRot = std::cos(pose[2]);

        int size = dataPoints.getSize();

        residuals[0] = 0;
        jacobians[0][0]=0;
        jacobians[0][1]=0;
        jacobians[0][2]=0;
        for (int i = 0; i < size; ++i)
        {
            const Eigen::Vector2f& currPoint (dataPoints.getVecEntry(i));   /// lidar point
            Eigen::Vector3f transformedPointData(occ->interpMapValueWithDerivatives(transform * currPoint));  /// {M,dM/dx,dM/dy}

            float funVal = 1.0f - transformedPointData[0];
            //      float weight=util::WeightValue(funVal);
            float weight=1.0;

            residuals[0] += static_cast<double>(funVal);

            jacobians[0][0] += static_cast<double>(transformedPointData[1]);
            jacobians[0][1] += static_cast<double>(transformedPointData[2]);

            double rotDeriv = ((-sinRot * currPoint.x() - cosRot * currPoint.y()) * transformedPointData[1] + (cosRot * currPoint.x() - sinRot * currPoint.y()) * transformedPointData[2]);

            jacobians[0][2] += static_cast<double>(rotDeriv);

        }
        return true;
    }
};

мой параметр для оптимизации - pose = [x,y,theta]

моя целевая функция состоит в том, чтобы свести к минимуму значение занятости относительно позы и лазерной точки. И здесь я добавляю их вручную вместе в residuals[0]

У меня есть 3 параметра [x,y,theta], поэтому мои якобианцы имеют 3 измерения в jocobians[0]

Но когда я запускаю программу, отчет выглядит следующим образом:

Solver Summary (v 1.12.0-eigen-(3.2.0)-lapack-suitesparse-(4.2.1)-openmp)

                                     Original                  Reduced
Parameter blocks                            1                        1
Parameters                                  3                        3
Residual blocks                             1                        1
Residual                                    1                        1

Minimizer                        TRUST_REGION

Dense linear algebra library            EIGEN
Trust region strategy     LEVENBERG_MARQUARDT

                                        Given                     Used
Linear solver                        DENSE_QR                 DENSE_QR
Threads                                     1                        1
Linear solver threads                       1                        1
Linear solver ordering              AUTOMATIC                        1

Cost:
Initial                          8.569800e+04
Final                            8.569800e+04
Change                           0.000000e+00

Minimizer iterations                        1
Successful steps                            1
Unsuccessful steps                          0

Time (in seconds):
Preprocessor                           0.0001

  Residual evaluation                  0.0000
  Jacobian evaluation                  0.0050
  Linear solver                        0.0000
Minimizer                              0.0051

Postprocessor                          0.0000
Total                                  0.0052

Termination:                      CONVERGENCE (Gradient tolerance reached. Gradient max norm: 0.000000e+00 <= 1.000000e-10)

Поскольку я установил якобианы, как можно сказать, что норма градиента такая маленькая?