Problem of using SetParameterUpperBound

[weisongwen]

Hi @sandwichmaker Thanks again for your great sharing on this package. In my project, I wish to estimate the position of the vehicle based on the sensor data. Since I already know the velocity of the vehicle is smaller. Therefore, I wish to set an upper and lower bound to the velocity state using the following code:

problem.SetParameterUpperBound(state_array_vel[i], 0, 2.0);
problem.SetParameterUpperBound(state_array_vel[i], 1, 2.0);
problem.SetParameterUpperBound(state_array_vel[i], 2, 2.0);

In my implementation, I correlate the velocity and the position using a constant velocity model, in other words, the velocity can also constrain the position. After I use the function SetParameterUpperBound, I found the velocity is well-bounded. But it seems that the position (x, y, z) is not affected by the bounds of the velocity. In other words, the position estimation is not affected by the velocity bounds.

Do you have any ideas on this?

The connection between the position and the velocity is as follows:

/* Motion model FACTORS */
struct motionModelFactor
{
    motionModelFactor(double dt, double var)
                :dt(dt), var(var){}

    template <typename T>
    bool operator()(const T* statePi,const T* statePj,const T* stateVi, T* residuals) const
    {
        T est_v_x = (statePj[0] - statePi[0])/ dt;
        T est_v_y = (statePj[1] - statePi[1])/ dt;
        T est_v_z = (statePj[2] - statePi[2])/ dt;

        // est_pseudorange = sqrt(delta_x+ delta_y + delta_z);

        residuals[0] = (est_v_x - stateVi[0]) / T(var);
        residuals[1] = (est_v_y - stateVi[1]) / T(var);
        residuals[2] = (est_v_z - stateVi[2]) / T(var);

        return true;
    }

    double var;
    double dt;
};

Best, Weisong,

[sandwichmaker]

Weisong thats hard to say. your position has to push against the velocity bounds to be affected by it and that will depend on the full optimization problem and the specific values. It is hard to say anything more by just looking at this cost function.

[weisongwen]

Hi @sandwichmaker ,

Thanks for your kind reply, I really like this Ceres-solver community!

Yes, I agree. But overall, you mean that the bound of the velocity can definitely affect the estimation of the position although the final impacts are determined by the overall optimization problem, am I correct?

One more thing, I really interested in how you achieve this optimization problem with bound constraints? Could you please kindly recommend any papers on solving this?

Thanks a lot for your kind suggestions in advance,

Best, Weisong,

[sandwichmaker]

Yes you are correct. The correct implementation of bounds constrained optimization is basically a bounds constrained line search after computing the trust region step. The reference we use is

C. Kanzow, N. Yamashita and M. Fukushima, Levenberg-Marquardt methods with strong local convergence properties for solving nonlinear equations with convex constraints, Journal of Computational and Applied Mathematics, 177(2):375-397, 2005.

but our implementation is not entirely correct. You can see it in trust_region_minimizer.cc

[weisongwen]

hi @sandwichmaker

Thanks for the reply and the recommendation of the paper. I will look into the code you recommend first.

Thanks, Weisong,