ceres solver analytical derivative doesn't work

Question

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;
    }
};

my parameter to optimize is the pose = [x,y,theta]

my objective function is to minimize the occupancy value about pose and laser point. And here I add them manually together into residuals[0]

I have 3 parameters [x,y,theta] so my jacobians have 3 dimensions in jocobians[0]

But when I run the program, the report is like below:

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)

Since I have set the jacobians, how can it say that the gradient norm is so small?

score 0 · Answer 1 · answered Mar 18 '18 at 14:21

Two things. 1. You cannot unconditionally set the Jacobian, you need to check if the solver is actually asking for and the pointers are non-null. 2. There is something wrong with your Jacobian eval, because as far as Ceres can tell it is seeing a zero gradient. Simple thing to check would be to dump out the Jacobian and Jacobian'residual from the CostFunction before returning.

for example are you sure size != 0?

ceres solver analytical derivative doesn't work

1 Answers1