findEssentialMat gives wrong results in android

asked 2016-04-17 13:08:06 -0500

Hi, I am trying to create a visual odometry app for android using NDK. I am using FAST and KLT and then using findEssentialMat to find the essential matrix. However, it is giving erroneous results. Here's some debug output:

04-17 23:23:28.313 2630-2903/com.example.shaswat.testopencv D/OCVSample::SDK: Essential Matrix = 
04-17 23:23:28.313 2630-2903/com.example.shaswat.testopencv D/OCVSample::SDK: -0.000000
04-17 23:23:28.313 2630-2903/com.example.shaswat.testopencv D/OCVSample::SDK: -1.655305
04-17 23:23:28.313 2630-2903/com.example.shaswat.testopencv D/OCVSample::SDK: 29303136256.000000
04-17 23:23:28.313 2630-2903/com.example.shaswat.testopencv D/OCVSample::SDK: -182220068263779929277612730010501644288.000000
04-17 23:23:28.313 2630-2903/com.example.shaswat.testopencv D/OCVSample::SDK: 1.771581
04-17 23:23:28.314 2630-2903/com.example.shaswat.testopencv D/OCVSample::SDK: 0.000000
04-17 23:23:28.314 2630-2903/com.example.shaswat.testopencv D/OCVSample::SDK: -598.520691
04-17 23:23:28.314 2630-2903/com.example.shaswat.testopencv D/OCVSample::SDK: 1.350371
04-17 23:23:28.314 2630-2903/com.example.shaswat.testopencv D/OCVSample::SDK: 152428905045575845543936.000000

The essential matrix values are displayed column-wise. My code is as follows: Java part - onCameraFrame()

public Mat onCameraFrame(CameraBridgeViewBase.CvCameraViewFrame inputFrame) {
        this.mCurr = inputFrame.rgba();    
        if(this.firstRun == 1){ 
            this.mCurr = inputFrame.rgba();
            this.mPrev = inputFrame.rgba();
            this.mDisplay = inputFrame.rgba();
            this.firstRun = 0;

            this.mRf = new Mat(3, 3, CvType.CV_32F);
            this.mRf.put(0,0,0,0,0,0,0,0,0,0,0);

            this.mTf = new Mat(3, 1, CvType.CV_32F);
            this.mTf.put(0,0,0,0,0);
        }

        this.controlFrameRate++;

        if(this.controlFrameRate<2)
            return null;

        this.controlFrameRate = 0;

        if(this.firstRun == 0) {

            this.mDisplay = this.mPrev;

            convertNativeGray(mCurr.getNativeObjAddr(), mPrev.getNativeObjAddr(), mDisplay.getNativeObjAddr(), mRf.getNativeObjAddr(), mTf.getNativeObjAddr());

            this.mPrev = this.mCurr;
        }

        return mDisplay;
    }

Native part:

    JNIEXPORT jint JNICALL Java_com_example_shaswat_testopencv_MainActivity_convertNativeGray(JNIEnv*, jobject, jlong addrCurr, jlong addrPrev, jlong addrDisplay, jlong addrRF, jlong addrTF) {

        __android_log_print(ANDROID_LOG_DEBUG, "OCVSample::SDK","In Native Code");

        Mat& mCurr = *(Mat*)addrCurr;
        Mat& mPrev = *(Mat*)addrPrev;
        Mat& mDisplay = *(Mat*)addrDisplay;
        Mat& R_f = *(Mat*)addrRF;
        Mat& t_f = *(Mat*)addrTF;

        int conv = 0;
        jint retVal;

        cvtColor(mCurr, mCurr, CV_RGBA2BGR);
        cvtColor(mPrev, mPrev, CV_RGBA2BGR);
        cvtColor(mDisplay, mDisplay, CV_RGBA2BGR);

        //mDisplay = cv::Mat::zeros(mCurr.rows, mCurr.cols, mCurr.type());

        Mat cameraMatrix = (Mat_<float>(3,3) << 1097.1547, 0.0, 403.9075, 0.0, 821.5675, 298.8437, 0.0, 0.0, 1.0);

        VO::featureOperations odometry(cameraMatrix, mCurr, mPrev, mDisplay, R_f, t_f, 1);
        odometry.calcOdometry(cameraMatrix, mCurr, mPrev, mDisplay, R_f, t_f, 1);

        cvtColor(mCurr, mCurr, CV_BGR2RGBA);
        cvtColor(mPrev, mPrev, CV_BGR2RGBA);
        cvtColor(mDisplay, mDisplay, CV_BGR2RGBA);

        retVal = (jint)conv;

        return retVal;

    }

calcOdometry function:

void VO::featureOperations::calcOdometry(cv::Mat cameraMatrix, cv::Mat currImage, cv::Mat prevImage, cv::Mat& trajectory, cv::Mat& R_f, cv::Mat& t_f, int enableHomography){
        // Change these accordingly. Intrinsics
        double focal = cameraMatrix.at<float>(0,0);
        cv::Point2d pp(cameraMatrix.at<float>(0,2), cameraMatrix.at<float>(1,2));

        // recovering the pose and the essential matrix
        cv::Mat E ...
(more)
edit retag flag offensive close merge delete