ceres solver analytical derivative doesn't work - optimization

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?

Answer 1

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?