0 
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)

既然我设置了雅可比，怎么能说梯度范数这么小呢？

4

1 回答 1

0

两件事情。1. 不能无条件设置雅可比行列式，需要检查求解器是否实际要求，指针是否为非空。2. 你的雅可比 eval 有问题，因为据 Ceres 所知，它看到的是零梯度。要检查的简单事情是在返回之前从 CostFunction 中转出 Jacobian 和 Jacobian'residual。

例如，您确定大小！= 0？

于 2018-03-18T14:21:56.207 回答