diff --git a/src/Calib3D.cc b/src/Calib3D.cc index f223fef..62b56ca 100644 --- a/src/Calib3D.cc +++ b/src/Calib3D.cc @@ -12,6 +12,7 @@ void Calib3D::Init(Handle target) NODE_SET_METHOD(obj, "calibrateCamera", CalibrateCamera); NODE_SET_METHOD(obj, "solvePnP", SolvePnP); NODE_SET_METHOD(obj, "getOptimalNewCameraMatrix", GetOptimalNewCameraMatrix); + NODE_SET_METHOD(obj, "stereoCalibrate", StereoCalibrate); target->Set(NanNew("calib3d"), obj); } @@ -314,7 +315,7 @@ NAN_METHOD(Calib3D::SolvePnP) } } -// cv::solvePnP +// cv::getOptimalNewCameraMAtrix NAN_METHOD(Calib3D::GetOptimalNewCameraMatrix) { NanEscapableScope(); @@ -373,3 +374,180 @@ NAN_METHOD(Calib3D::GetOptimalNewCameraMatrix) NanReturnUndefined(); } } + +// cv::stereoCalibrate +NAN_METHOD(Calib3D::StereoCalibrate) +{ + NanEscapableScope(); + + try { + // Get the arguments + + // Arg 0, the array of object points, an array of arrays + std::vector > objectPoints; + if(args[0]->IsArray()) { + Local objectPointsArray = Local::Cast(args[0]); + + for(unsigned int i = 0; i < objectPointsArray->Length(); i++) + { + std::vector points; + + Local pointsArray = Local::Cast(objectPointsArray->Get(i)); + for(unsigned int j = 0; j < pointsArray->Length(); j++) + { + Local pt = pointsArray->Get(j)->ToObject(); + points.push_back(cv::Point3f(pt->Get(NanNew("x"))->ToNumber()->Value(), + pt->Get(NanNew("y"))->ToNumber()->Value(), + pt->Get(NanNew("z"))->ToNumber()->Value())); + } + + objectPoints.push_back(points); + } + } else { + JSTHROW_TYPE("Must pass array of object points for each frame") + } + + // Arg 1, the image points1, another array of arrays =( + std::vector > imagePoints1; + if(args[1]->IsArray()) { + Local imagePointsArray = Local::Cast(args[1]); + + for(unsigned int i = 0; i < imagePointsArray->Length(); i++) + { + std::vector points; + + Local pointsArray = Local::Cast(imagePointsArray->Get(i)); + for(unsigned int j = 0; j < pointsArray->Length(); j++) + { + Local pt = pointsArray->Get(j)->ToObject(); + points.push_back(cv::Point2f(pt->Get(NanNew("x"))->ToNumber()->Value(), + pt->Get(NanNew("y"))->ToNumber()->Value())); + } + + imagePoints1.push_back(points); + } + } else { + JSTHROW_TYPE("Must pass array of object points for each frame") + } + + // Arg 2, the image points2, another array of arrays =( + std::vector > imagePoints2; + if(args[2]->IsArray()) { + Local imagePointsArray = Local::Cast(args[2]); + + for(unsigned int i = 0; i < imagePointsArray->Length(); i++) + { + std::vector points; + + Local pointsArray = Local::Cast(imagePointsArray->Get(i)); + for(unsigned int j = 0; j < pointsArray->Length(); j++) + { + Local pt = pointsArray->Get(j)->ToObject(); + points.push_back(cv::Point2f(pt->Get(NanNew("x"))->ToNumber()->Value(), + pt->Get(NanNew("y"))->ToNumber()->Value())); + } + + imagePoints2.push_back(points); + } + } else { + JSTHROW_TYPE("Must pass array of object points for each frame") + } + + // Arg 3 is the image size (follows the PYTHON api not the C++ api since all following arguments are optional or outputs) + cv::Size imageSize; + if (args[3]->IsArray()) { + Local v8sz = args[3]->ToObject(); + + imageSize = cv::Size(v8sz->Get(1)->IntegerValue(), v8sz->Get(0)->IntegerValue()); + } else { + JSTHROW_TYPE("Must pass original image size"); + } + + // Arg 4,5,6,7 is the camera matrix and distortion coefficients (optional but must pass all 4 or none) + cv::Mat k1, d1, k2, d2; + if(args.Length() >= 8) + { + Matrix* mk1 = ObjectWrap::Unwrap(args[4]->ToObject()); + Matrix* md1 = ObjectWrap::Unwrap(args[5]->ToObject()); + Matrix* mk2 = ObjectWrap::Unwrap(args[6]->ToObject()); + Matrix* md2 = ObjectWrap::Unwrap(args[7]->ToObject()); + + k1 = mk1->mat; + d1 = md1->mat; + + k2 = mk2->mat; + d2 = md2->mat; + } + + // Last argument is flags, skipping for now + + // Output mats + cv::Mat R, t, E, F; + + // Do the stereo calibration + cv::stereoCalibrate(objectPoints, imagePoints1, imagePoints2, k1, d1, k2, d2, imageSize, R, t, E, F); + + // make the return value + Local ret = NanNew(); + + // Make the output arguments + + // k1 + Local K1MatrixWrap = NanNew(Matrix::constructor)->GetFunction()->NewInstance(); + Matrix *K1Matrix = ObjectWrap::Unwrap(K1MatrixWrap); + K1Matrix->mat = k1; + + // d1 + Local d1MatrixWrap = NanNew(Matrix::constructor)->GetFunction()->NewInstance(); + Matrix *d1Matrix = ObjectWrap::Unwrap(d1MatrixWrap); + d1Matrix->mat = d1; + + // k2 + Local K2MatrixWrap = NanNew(Matrix::constructor)->GetFunction()->NewInstance(); + Matrix *K2Matrix = ObjectWrap::Unwrap(K2MatrixWrap); + K2Matrix->mat = k2; + + // d2 + Local d2MatrixWrap = NanNew(Matrix::constructor)->GetFunction()->NewInstance(); + Matrix *d2Matrix = ObjectWrap::Unwrap(d2MatrixWrap); + d2Matrix->mat = d2; + + // R + Local RMatrixWrap = NanNew(Matrix::constructor)->GetFunction()->NewInstance(); + Matrix *RMatrix = ObjectWrap::Unwrap(RMatrixWrap); + RMatrix->mat = R; + + // t + Local tMatrixWrap = NanNew(Matrix::constructor)->GetFunction()->NewInstance(); + Matrix *tMatrix = ObjectWrap::Unwrap(tMatrixWrap); + tMatrix->mat = t; + + // E + Local EMatrixWrap = NanNew(Matrix::constructor)->GetFunction()->NewInstance(); + Matrix *EMatrix = ObjectWrap::Unwrap(EMatrixWrap); + EMatrix->mat = E; + + // F + Local FMatrixWrap = NanNew(Matrix::constructor)->GetFunction()->NewInstance(); + Matrix *FMatrix = ObjectWrap::Unwrap(FMatrixWrap); + FMatrix->mat = F; + + // Add to return object + ret->Set(NanNew("K1"), K1MatrixWrap); + ret->Set(NanNew("distortion1"), d1MatrixWrap); + ret->Set(NanNew("K2"), K2MatrixWrap); + ret->Set(NanNew("distortion2"), d2MatrixWrap); + ret->Set(NanNew("R"), RMatrixWrap); + ret->Set(NanNew("t"), tMatrixWrap); + ret->Set(NanNew("E"), EMatrixWrap); + ret->Set(NanNew("F"), FMatrixWrap); + + // Return + NanReturnValue(ret); + + } catch (cv::Exception &e) { + const char *err_msg = e.what(); + NanThrowError(err_msg); + NanReturnUndefined(); + } +} diff --git a/src/Calib3D.h b/src/Calib3D.h index ab5669e..1f18894 100644 --- a/src/Calib3D.h +++ b/src/Calib3D.h @@ -18,6 +18,8 @@ public: static NAN_METHOD(SolvePnP); static NAN_METHOD(GetOptimalNewCameraMatrix); + + static NAN_METHOD(StereoCalibrate); }; #endif