feat(calib): add process impl code

This commit is contained in:
TinyOh 2019-01-04 15:09:21 +08:00
parent d505b3b647
commit b40723e819
4 changed files with 95 additions and 15 deletions

View File

@ -464,15 +464,8 @@ struct MYNTEYE_API IntrinsicsEquidistant : public IntrinsicsBase {
std::uint16_t width;
/** The height of the image in pixels */
std::uint16_t height;
/** The distortion coefficients */
double k2;
double k3;
double k4;
double k5;
double mu;
double mv;
double u0;
double v0;
/** The distortion coefficients k2,k3,k4,k5,mu,mv,u0,v0*/
double coeffs[8];
};
MYNTEYE_API

View File

@ -43,10 +43,20 @@ std::string RectifyProcessor::Name() {
void RectifyProcessor::NotifyImageParamsChanged() {
auto in_left = device_->GetIntrinsics(Stream::LEFT);
auto in_right = device_->GetIntrinsics(Stream::RIGHT);
if (in_left->calib_model == CalibrationModel::CALIB_MODEL_PINHOLE) {
InitParams(
*std::dynamic_pointer_cast<IntrinsicsPinhole>(in_left),
*std::dynamic_pointer_cast<IntrinsicsPinhole>(in_right),
device_->GetExtrinsics(Stream::RIGHT, Stream::LEFT));
} else if (in_left->calib_model ==
CalibrationModel::CALIB_MODEL_KANNALA_BRANDT) {
InitParams(
*std::dynamic_pointer_cast<IntrinsicsEquidistant>(in_left),
*std::dynamic_pointer_cast<IntrinsicsEquidistant>(in_right),
device_->GetExtrinsics(Stream::RIGHT, Stream::LEFT));
} else {
// toido
}
}
Object *RectifyProcessor::OnCreateOutput() {
@ -106,4 +116,78 @@ void RectifyProcessor::InitParams(
cv::initUndistortRectifyMap(M2, D2, R2, P2, size, CV_16SC2, map21, map22);
}
// enum class CalibrationModel : std::uint8_t {
// /** Unknow */
// CALIB_MODEL_UNKNOW = 0,
// /** Pinhole */
// CALIB_MODEL_PINHOLE = 1,
// /** Equidistant: KANNALA_BRANDT */
// CALIB_MODEL_KANNALA_BRANDT = 2,
// // CALIB_MODEL_SCARAMUZZA,
// // CALIB_MODEL_MEI,
// };
// /**
// * @ingroup calibration
// * Stream intrinsics (Equidistant: KANNALA_BRANDT)
// */
// struct MYNTEYE_API IntrinsicsEquidistant : public IntrinsicsBase {
// IntrinsicsEquidistant() {
// calib_model = CalibrationModel::CALIB_MODEL_KANNALA_BRANDT;
// }
// /** The width of the image in pixels */
// std::uint16_t width;
// /** The height of the image in pixels */
// std::uint16_t height;
// /** The distortion coefficients */
// double k2;
// double k3;
// double k4;
// double k5;
// double mu;
// double mv;
// double u0;
// double v0;
// };
void RectifyProcessor::InitParams(
IntrinsicsEquidistant in_left,
IntrinsicsEquidistant in_right,
Extrinsics ex_right_to_left) {
cv::Size size{in_left.width, in_left.height};
cv::Mat M1 =
(cv::Mat_<double>(3, 3) << 1.0, 0, 1.0, 0, 1.0,
1.0, 0, 0, 1);
cv::Mat M2 =
(cv::Mat_<double>(3, 3) << 1.0, 0, 1.0, 0, 1.0,
1.0, 0, 0, 1);
cv::Mat D1(1, 8, CV_64F, in_left.coeffs);
cv::Mat D2(1, 8, CV_64F, in_right.coeffs);
cv::Mat R =
(cv::Mat_<double>(3, 3) << ex_right_to_left.rotation[0][0],
ex_right_to_left.rotation[0][1], ex_right_to_left.rotation[0][2],
ex_right_to_left.rotation[1][0], ex_right_to_left.rotation[1][1],
ex_right_to_left.rotation[1][2], ex_right_to_left.rotation[2][0],
ex_right_to_left.rotation[2][1], ex_right_to_left.rotation[2][2]);
cv::Mat T(3, 1, CV_64F, ex_right_to_left.translation);
VLOG(2) << "InitParams size: " << size;
VLOG(2) << "M1: " << M1;
VLOG(2) << "M2: " << M2;
VLOG(2) << "D1: " << D1;
VLOG(2) << "D2: " << D2;
VLOG(2) << "R: " << R;
VLOG(2) << "T: " << T;
cv::Rect left_roi, right_roi;
cv::stereoRectify(
M1, D1, M2, D2, size, R, T, R1, R2, P1, P2, Q, cv::CALIB_ZERO_DISPARITY,
0, size, &left_roi, &right_roi);
cv::initUndistortRectifyMap(M1, D1, R1, P1, size, CV_16SC2, map11, map12);
cv::initUndistortRectifyMap(M2, D2, R2, P2, size, CV_16SC2, map21, map22);
}
MYNTEYE_END_NAMESPACE

View File

@ -50,6 +50,8 @@ class RectifyProcessor : public Processor {
private:
void InitParams(IntrinsicsPinhole in_left,
IntrinsicsPinhole in_right, Extrinsics ex_right_to_left);
void InitParams(IntrinsicsEquidistant in_left,
IntrinsicsEquidistant in_right, Extrinsics ex_right_to_left);
std::shared_ptr<Device> device_;
};

View File

@ -198,14 +198,15 @@ std::ostream &operator<<(std::ostream &os, const IntrinsicsPinhole &in) {
os << in.coeffs[i] << ", ";
return os << in.coeffs[4] << "]";
}
/** The distortion coefficients k2,k3,k4,k5,mu,mv,u0,v0*/
double coeffs[8];
std::ostream &operator<<(std::ostream &os, const IntrinsicsEquidistant &in) {
os << "equidistant, " << FULL_PRECISION
<< "width: " << in.width << ", height: " << in.height
<< ", k2: " << in.k2 << ", k3: " << in.k3
<< ", k4: " << in.k4 << ", k5: " << in.k5
<< ", mu: " << in.mu << ", mv: " << in.mv
<< ", u0: " << in.u0 << ", v0: " << in.v0;
<< ", k2: " << in.coeffs[0] << ", k3: " << in.coeffs[1]
<< ", k4: " << in.coeffs[2] << ", k5: " << in.coeffs[3]
<< ", mu: " << in.coeffs[4] << ", mv: " << in.coeffs[5]
<< ", u0: " << in.coeffs[6] << ", v0: " << in.coeffs[7];
return os;
}