c++opencvceres-solver

pass parameters of double but get Jet<double,6>when using ceres solver


I'm a new learner to Ceres Solver, when adding the residualblock using

problem.AddResidualBlock( new ceres::AutoDiffCostFunction<Opt, 1, 6> (new Opt(Pts[i][j].x, Pts[i][j].y, Pts[i][j].z, Ns[i].at<double>(0, 0), Ns[i].at<double>(1, 0), Ns[i].at<double>(2, 0), Ds[i], weights[i]) ),
                      NULL,
                              param );

where param is double[6];

struct Opt
{
const double ptX, ptY, ptZ, nsX, nsY, nsZ, ds, w;

Opt( double ptx, double pty, double ptz, double nsx, double nsy, double nsz, double ds1, double w1):
         ptX(ptx), ptY(pty), ptZ(ptz), nsX(nsx), nsY(nsy), nsZ(nsz), ds(ds1), w(w1) {}

template<typename T>
bool operator()(const T* const x, T* residual) const
{
    Mat R(3, 3, CV_64F), r(1, 3, CV_64F);
    Mat inverse(3,3, CV_64F);
    T newP[3];
    T xyz[3];
        for (int i = 0; i < 3; i++){
            r.at<T>(i) = T(x[i]);
            cout<<x[i]<<endl;
            }
    Rodrigues(r, R);
    inverse = R.inv();
    newP[0]=T(ptX)-x[3];
    newP[1]=T(ptY)-x[4];
    newP[2]=T(ptZ)-x[5];
    xyz[0]= inverse.at<T>(0, 0)*newP[0] + inverse.at<T>(0, 1)*newP[1] + inverse.at<T>(0, 2)*newP[2];
    xyz[1] = inverse.at<T>(1, 0)*newP[0] + inverse.at<T>(1, 1)*newP[1] + inverse.at<T>(1, 2)*newP[2];
    xyz[2] = inverse.at<T>(2, 0)*newP[0] + inverse.at<T>(2, 1)*newP[1] + inverse.at<T>(2, 2)*newP[2];
    T ds1 = T(nsX) * xyz[0] + T(nsY) * xyz[1] + T(nsZ) * xyz[2];
    residual[0] = (ds1 - T(ds)) * T(w);
}
};

but when I output the x[0], I got this:

[-1.40926 ; 1, 0, 0, 0, 0, 0]

after I change the type of the x to double

I got this error :

note:   no known conversion for argument 1 from ‘const ceres::Jet<double, 6>* const’ to ‘const double*’

in

bool operator()(const double* const x, double* residual) const

what's wrong with my codes? Thanks a lot!


Solution

  • I am guessing you are using cv::Mat.

    The reason the functor is templated is because Ceres evaluates it using doubles when it needs just the residuals, and evaluates with ceres:Jet objects when it needs to compute the Jacobian. So your attempt to fill r as

    for (int i = 0; i < 3; i++){
        r.at<T>(i) = T(x[i]);
        cout<<x[i]<<endl;
    }
    

    are trying to convert a Jet into a double. Which is what the compiler is correctly complaining about.

    you can re-write your code as (I have not compiled it, so there maybe a minor typo or two).

    template<typename T>
    bool operator()(const T* const x, T* residual) const {
        const T inverse_rotation[3] = {-x[0], -x[1], -x[3]};
        const T newP[3] = {ptX - x[3], ptY - x[4]. ptZ - x[5]};
        T xyz[3];
        ceres::AngleAxisRotatePoint(inverse_rotation, newP, xyz);
        const T ds1 = nsX * xyz[0] + nsY * xyz[1] + nsZ * xyz[2];
        residual[0] = (ds1 - ds) * w;
        return true;
    }