ConjGradSolver_.cpp

Go to the documentation of this file.

#include "mexopencv.hpp"
using namespace std;
using namespace cv;

// Persistent objects
namespace {
int last_id = 0;
map<int,Ptr<ConjGradSolver> > obj_;

class MatlabFunction : public cv::MinProblemSolver::Function
{
public:
    MatlabFunction(int num_dims, const string &func, const string &grad_func = "", double h = 1e-3)
    : dims(num_dims), fun_name(func), grad_fun_name(grad_func), gradeps(h)
    {}

    double calc(const double *x) const
    {
        // create input to evaluate objective function
        mxArray *lhs, *rhs[2];
        rhs[0] = MxArray(fun_name);
        rhs[1] = MxArray(vector<double>(x, x + dims));

        // evaluate specified function in MATLAB as:
        // val = feval("fun_name", x)
        double val;
        if (mexCallMATLAB(1, &lhs, 2, rhs, "feval") == 0) {
            MxArray res(lhs);
            CV_Assert(res.isDouble() && !res.isComplex() && res.numel() == 1);
            val = res.at<double>(0);
        }
        else {
            //TODO: error
            val = 0;
        }

        // cleanup
        mxDestroyArray(lhs);
        mxDestroyArray(rhs[0]);
        mxDestroyArray(rhs[1]);

        // return scalar value of objective function evaluated at x
        return val;
    }

    void getGradient(const double* x, double* grad) /*const*/
    {
        // if no function is specified, approximate the gradient using
        // finite difference method: F'(x) = (F(x+h) - F(x-h)) / 2*h
        if (grad_fun_name.empty()) {
            cv::MinProblemSolver::Function::getGradient(x, grad);
            return;
        }

        // create input to evaluate gradient function
        mxArray *lhs, *rhs[2];
        rhs[0] = MxArray(grad_fun_name);
        rhs[1] = MxArray(vector<double>(x, x + dims));

        // evaluate specified function in MATLAB as:
        // grad = feval("grad_fun_name", x)
        if (mexCallMATLAB(1, &lhs, 2, rhs, "feval") == 0) {
            MxArray res(lhs);
            CV_Assert(res.isDouble() && !res.isComplex() && res.ndims() == 2);
            vector<double> v(res.toVector<double>());
            CV_Assert(v.size() == dims);
            std::copy(v.begin(), v.end(), grad);
        }
        else {
            //TODO: error
            std::fill(grad, grad + dims, 0.0);
        }

        // cleanup
        mxDestroyArray(lhs);
        mxDestroyArray(rhs[0]);
        mxDestroyArray(rhs[1]);
    }

    double getGradientEps() const
    {
        return gradeps;
    }

    int getDims() const
    {
        return dims;
    }

    MxArray toStruct() const
    {
        MxArray s(MxArray::Struct());
        s.set("dims",    dims);
        s.set("fun",     fun_name);
        s.set("gradfun", grad_fun_name);
        s.set("gradeps", gradeps);
        return s;
    }

    static Ptr<MatlabFunction> create(const MxArray &s)
    {
        if (!s.isStruct() || s.numel()!=1)
            mexErrMsgIdAndTxt("mexopencv:error", "Invalid objective function");
        return makePtr<MatlabFunction>(
            s.at("dims").toInt(),
            s.at("fun").toString(),
            s.isField("gradfun") ? s.at("gradfun").toString() : "",
            s.isField("gradeps") ? s.at("gradeps").toDouble() : 1e-3);
    }

private:
    int dims;              
    string fun_name;       
    string grad_fun_name;  
    double gradeps;        
};
}

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
    // Check the number of arguments
    nargchk(nrhs>=2 && nlhs<=2);

    // Arguments vector
    vector<MxArray> rhs(prhs, prhs+nrhs);
    int id = rhs[0].toInt();
    string method(rhs[1].toString());

    // Constructor is called. Create a new object from argument
    if (method == "new") {
        nargchk(nrhs>=2 && (nrhs%2)==0 && nlhs<=1);
        Ptr<MinProblemSolver::Function> f;
        TermCriteria termcrit(TermCriteria::MAX_ITER+TermCriteria::EPS, 5000, 1e-6);
        for (int i=2; i<nrhs; i+=2) {
            string key(rhs[i].toString());
            if (key == "Function")
                f = MatlabFunction::create(rhs[i+1]);
            else if (key == "TermCriteria")
                termcrit = rhs[i+1].toTermCriteria();
            else
                mexErrMsgIdAndTxt("mexopencv:error",
                    "Unrecognized option %s", key.c_str());
        }
        obj_[++last_id] = ConjGradSolver::create(f, termcrit);
        plhs[0] = MxArray(last_id);
        mexLock();
        return;
    }

    // Big operation switch
    Ptr<ConjGradSolver> obj = obj_[id];
    if (obj.empty())
        mexErrMsgIdAndTxt("mexopencv:error", "Object not found id=%d", id);
    if (method == "delete") {
        nargchk(nrhs==2 && nlhs==0);
        obj_.erase(id);
        mexUnlock();
    }
    else if (method == "clear") {
        nargchk(nrhs==2 && nlhs==0);
        obj->clear();
    }
    else if (method == "load") {
        nargchk(nrhs>=3 && (nrhs%2)==1 && nlhs==0);
        string objname;
        bool loadFromString = false;
        for (int i=3; i<nrhs; i+=2) {
            string key(rhs[i].toString());
            if (key == "ObjName")
                objname = rhs[i+1].toString();
            else if (key == "FromString")
                loadFromString = rhs[i+1].toBool();
            else
                mexErrMsgIdAndTxt("mexopencv:error",
                    "Unrecognized option %s", key.c_str());
        }
        obj_[id] = (loadFromString ?
            Algorithm::loadFromString<ConjGradSolver>(rhs[2].toString(), objname) :
            Algorithm::load<ConjGradSolver>(rhs[2].toString(), objname));
    }
    else if (method == "save") {
        nargchk(nrhs==3 && nlhs==0);
        obj->save(rhs[2].toString());
    }
    else if (method == "empty") {
        nargchk(nrhs==2 && nlhs<=1);
        plhs[0] = MxArray(obj->empty());
    }
    else if (method == "getDefaultName") {
        nargchk(nrhs==2 && nlhs<=1);
        plhs[0] = MxArray(obj->getDefaultName());
    }
    else if (method == "minimize") {
        nargchk(nrhs==3 && nlhs<=2);
        Mat x(rhs[2].toMat(CV_64F));
        double fx = obj->minimize(x);
        plhs[0] = MxArray(x);
        if (nlhs>1)
            plhs[1] = MxArray(fx);
    }
    else if (method == "get") {
        nargchk(nrhs==3 && nlhs<=1);
        string prop(rhs[2].toString());
        if (prop == "Function") {
            Ptr<MinProblemSolver::Function> f(obj->getFunction());
            plhs[0] = (f.empty()) ? MxArray::Struct() :
                (f.dynamicCast<MatlabFunction>())->toStruct();
        }
        else if (prop == "TermCriteria")
            plhs[0] = MxArray(obj->getTermCriteria());
        else
            mexErrMsgIdAndTxt("mexopencv:error",
                "Unrecognized property %s", prop.c_str());
    }
    else if (method == "set") {
        nargchk(nrhs==4 && nlhs==0);
        string prop(rhs[2].toString());
        if (prop == "Function")
            obj->setFunction(MatlabFunction::create(rhs[3]));
        else if (prop == "TermCriteria")
            obj->setTermCriteria(rhs[3].toTermCriteria());
        else
            mexErrMsgIdAndTxt("mexopencv:error",
                "Unrecognized property %s", prop.c_str());
    }
    else
        mexErrMsgIdAndTxt("mexopencv:error",
            "Unrecognized operation %s", method.c_str());
}