Proper optimisation of nonlinear terms (example: curvature of a 2D curve)

[maximilia...@gmail.com]

Hi there,

I am working on a trajectoryplanning optimisation problem for autonomous cars. The trajectory consists of 40 points, each point has two coordinates (x,y). And there
is a temporal spacing between this points (in my case 0.25s).
The costfunctions consists of 4 linear terms, on which the solver reaches the optimum nicely. Now, i want to add a yawrate term (see code below). For the initial solution, i chosed a a curved trajectory and set all weights except the one for the yawrate to 0 (just optimise the yawrate). Of course i would except a straight curve, but the optimiser terminates with the message "NO_CONVERGENCE".
Here is the relevant code snippet:




//////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////


//setting up the costfunction (of course i have to do this for all points in the trajectory)

CostFunction * cost_function = new AutoDiffCostFunction<YawRateCostsFunctor, 1, 1, 1, 1, 1, 1, 1>(
                                              new YawRateCostsFunctor(this));

         ProblemObject.AddResidualBlock(cost_function,
                                                                             NULL,
                                                                              &x0,&y0,&x1,&y1,&x2,&y2)

//The functor

template<typename T>
bool YawRateCostsFunctor::operator()(const T* x0,
                                     const T* y0,
                                     const T* x1,
                                     const T* y1,
                                     const T* x2,
                                     const T* y2,
                                              T* value) const
{
    T TemporalSpacing = T(0.25);

    T vx2 = (x2[0] - x1[0]) / TemporalSpacing;
    T vy2 = (y2[0] - y1[0]) / TemporalSpacing;

    T vx1 = (x1[0] - x0[0]) / TemporalSpacing;
    T vy1 = (y1[0] - y0[0]) / TemporalSpacing;

    T yaw2 = ceres::atan2(vy2, vx2);
    T yaw1 = ceres::atan2(vy1, vx1);

    T yawdiff = (yaw2 - yaw1);

    T yawrate = yawdiff / TemporalSpacing;

    value[0] = yawrate;

    return true;
}

//////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////

 Am i doing anything wrong?

thx four your help in advance and if you have questions, dont hesitate to contact me!

kind regards,
Max

[Sameer Agarwal]

NO_CONVERGENCE only means that the solver did not converge in the given number of iterations, it does not mean you did anything wrong.

--
You received this message because you are subscribed to the Google Groups "Ceres Solver" group.
To unsubscribe from this group and stop receiving emails from it, send an email to ceres-solver...@googlegroups.com.
To view this discussion on the web visit https://groups.google.com/d/msgid/ceres-solver/1fe12b9d-eb98-41db-a7e0-92222fd16ef3%40googlegroups.com.
For more options, visit https://groups.google.com/d/optout.

[graf]

Yes i know! but the results are not statisfieing. So i am not sure if i did everything correct :/. What do you say? Would it be better to split it into two residuals for example?

kind regards and thx,

Max

You received this message because you are subscribed to a topic in the Google Groups "Ceres Solver" group.
To unsubscribe from this topic, visit https://groups.google.com/d/topic/ceres-solver/jbrMKCycBU8/unsubscribe.
To unsubscribe from this group and all its topics, send an email to ceres-solver...@googlegroups.com.
To view this discussion on the web visit https://groups.google.com/d/msgid/ceres-solver/CABqdRUBFgdqEZcxJqddK8DNCLABK5%2BubmwuC-TRRVmCeKf47Aw%40mail.gmail.com.

[Sameer Agarwal]

Run the optimization longer  by increasing the maximum number of iterations.

To view this discussion on the web visit https://groups.google.com/d/msgid/ceres-solver/3ef31d14-1440-e7d9-f86c-ef36d0efb915%40gmail.com.