I am building pos graph and trying to use ceres to solve the optimization problem. But the result keeps zero iteration and the gradient max norm reach zero immediately. I looked up online and think its jacobian is zero at the initial but do not know why. I attached the summary as following:
Solver Summary (v 1.11.0-eigen-(3.2.4)-no_lapack-eigensparse-openmp)
Original Reduced
Parameter blocks 137857 137665
Parameters 413571 412995
Effective parameters 138241 137665
Residual blocks 789264 788425
Residual 1578528 1576850
Minimizer TRUST_REGION
Sparse linear algebra library EIGEN_SPARSE
Trust region strategy LEVENBERG_MARQUARDT
Given Used
Linear solver SPARSE_NORMAL_CHOLESKY SPARSE_NORMAL_CHOLESKY
Threads 1 1
Linear solver threads 8 8
Cost:
Initial 6.515367e+011
Final 6.515367e+011
Change 0.000000e+000
Minimizer iterations 0
Successful steps 0
Unsuccessful steps 0
Time (in seconds):
Preprocessor 1.0601
Residual evaluation 0.0000
Jacobian evaluation 9.3984
Linear solver 0.0000
Minimizer 9.4583
Postprocessor 0.0538
Total 10.5721
Termination: CONVERGENCE (Gradient tolerance reached. Gradient max norm: 0.000000e+000 <= 0.000000e+000)
here is the costfunction:
struct xyerror_trail{
const double x_diff_;
const double y_diff_;
const double weight_;
xyerror_trail(const double& x_diff, const double& y_diff, const double& weight) :
x_diff_(x_diff),
y_diff_(y_diff),
weight_(weight)
{
}
static ceres::CostFunction* Create(const double& x_diff, const double& y_diff, const double& weight) {
return (new ceres::AutoDiffCostFunction<xyerror_trail, 2, 3, 3>(new xyerror_trail(x_diff, y_diff, weight)));
}
template <typename T>
bool operator()(const T* const veh_translation, const T* const veh_translation_dst, T* residuals) const {
// Map T* to Eigen Vector3 with correct Scalar type
Eigen::Matrix<T, 3, 1> t = Eigen::Map<const Eigen::Matrix<T, 3, 1> >(veh_translation);
Eigen::Matrix<T, 3, 1> tDst = Eigen::Map<const Eigen::Matrix<T, 3, 1> >(veh_translation_dst);
// The error is the difference between the predicted and observed position.
residuals[0] = T(weight_) * (t[0] - tDst[0] - T(double(x_diff_)));
residuals[1] = T(weight_) * (t[1] - tDst[1] - T(double(y_diff_ )));
std::cout << residuals[0] << " " << residuals[1] << std::endl;
return true;
}
};
basically, I have two types of xyerror_trail to build. First it is the trail itself and second is the relationship between trails. Since the first is trail self, most residuals are zeros at begining. Second ones are not zeros but have fewer residuals. Most residuals come from the first type. I thought maybe because most residuals are zeros, i made them not zeros but still not work. Anyone can help me ? Thanks.