findEssentialMat gives wrong results in android
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 ...