1.本文要点说明
(1)测试旋转模型：旋转向量NRot=3(仅加法导数)、旋转四元数NRot=4(先加法导数后乘法导数)、旋转矩阵NRot=9(先加法导数后乘法导数)。
(2)测试成像模型：透视模型RTCM、等距模型KBCM、全向模型MUCM、椭球模型EUCM、双球模型DSCM。
(3)测试径切畸变：仅针对透视模型RTCM，NDist=4、5、8、12。
(4)测试针孔效应：仅针对统一模型XUCM，SIMPin为真时收敛更快，正如DSCM论文所说具有更好的数值性。
(5)测试稀疏效应：USEBatch为真时将每次观察的所有点作为一个残差块，USEBatch为假时将每次观察的每个点作为一个残差块。
测试数据来源opencv/samples/data和opencv_extra/testdata/cv/cameracalibration/fisheye/calib-3_stereo_from_JY，复制到../data/calib/cvrtcm/cam1|cam2|all和../data/calib/cvkbcm/cam1|cam2|all。
2.实验测试代码
依赖于OpenCV、Ceres和Spdlog，封装在类ModelCalibration：
(0)CalibConfigs：NRot、CAMModel、NDist、FIXDist、NModel、FIXModel、SIMPin、USEBatch、{MONO_INTRINSIC_GUESS, BINO_INTRINSIC_FIXED, BINO_EXTRINSIC_GUESS}
(1)CeresModels：旋转矩阵参数化DIY::RotationMatrixParameterization用于NRot=9，单位四元数参数化ceres::QuaternionParameterization用于NRot=4，旋转向量参数化DIY::RotationVectorParameterization未使用(需要自定义代价函数)，对NRot=3，默认即可。
(2)CalibModels：单目成像残差模型MonoVCM，双目成像残差模型BinoVCM。
(3)CalibUtils：calcBestPinCAM、rectAsPinMAP。
(4)MonoCalib：单目成像解算模型，flags/imaSize/point3D/point2D、camSolid/camPoses/rmsMean/rmsView/errPoints、create/calibrate、init/optimize/updateErr、initRTCM/initKBCM/initXUCM、solvePnP。
(5)BinoCalib：双目成像解算模型，calib1/calib2、cam21Pose/rmsMean/rmsView/errPoints、create/calibrate、init/optimize/updateErr。
(6)CalibAPP：单目成像解算样例MonoAPP，双目解算样例BinoAPP。
遗留问题：initXUCM只针对棋盘板、initRTCM和initKBCM未执行，solvePnP未执行，CalibFlags未执行。
1 #ifndef __ModelCalibration_h__ 2 #define __ModelCalibration_h__ 3 4 #include <cscv/Motion2D.h> 5 6 class ModelCalibration 7 { 8 private://CalibConfigs 9 using SolidPose = ModelRotation::SolidPose; 10 using VisionCM = ModelCamera::VisionCM; 11 using CamSolid = ModelCamera::CamSolid; 12 static const int NProject = 4; //cannot be modified and only for clarity 13 static const int NRot = 4; //support 3 4 9 for rotation vector and quaternion and rotation matrix 14 static const int CAMModel = VisionCM::MUCM; 15 //RTCM: NDist=5 NModel>0 FIXDist=0 FIXModel=1 16 //KBCM: NDist=4 NModle>0 FIXDist=0 FIXModel=1 17 //MUCM: NDist=4 NModel=1 FIXDist=0 FIXModel=0 18 //EUCM: NDist>0 NModel=2 FIXDist=1 FIXModel=0 19 //DSCM: NDist>0 NModel=2 FIXDist=1 FIXModel=0 20 static const int NDist = 4, FIXDist = false; 21 static const int NModel = 1, FIXModel = false; 22 static const int SIMPin = false; 23 static const int USEBatch = false; 24 enum { MONO_INTRINSIC_GUESS, BINO_INTRINSIC_FIXED, BINO_EXTRINSIC_GUESS }; //not used yet 25 26 public://CeresModels 27 //RotationMatrixParameterization: Half Addition Derivative + Half Multiplication Derivative + AutoCostFunction 28 //QuaternionParameterization: Half Addition Derivative + Half Multiplication Derivative + AutoCostFunction 29 //RotationVectorParameterization: Full Multiplication Derivative + ManuCostFunction 30 struct RotationVectorParameterization : public ceres::LocalParameterization 31 { 32 bool Plus(const double* x, const double* delta, double* x_plus_delta) const 33 { 34 #if 1 //Coversion by quaternion 35 double q_x[4], q_delta[4], q_x_plus_delta[4]; 36 ceres::AngleAxisToQuaternion(x, q_x); 37 ceres::AngleAxisToQuaternion(delta, q_delta); 38 ceres::QuaternionProduct(q_delta, q_x, q_x_plus_delta); 39 ceres::QuaternionToAngleAxis(q_x_plus_delta, x_plus_delta); 40 return true; 41 #else //Conversion by matrix 42 double m_x[9], m_delta[9], m_x_plus_delta[9]; 43 ceres::AngleAxisToRotationMatrix(x, ceres::RowMajorAdapter3x3(m_x)); 44 ceres::AngleAxisToRotationMatrix(delta, ceres::RowMajorAdapter3x3(m_delta)); 45 m_x_plus_delta[0] = m_delta[0] * m_x[0] + m_delta[1] * m_x[3] + m_delta[2] * m_x[6]; 46 m_x_plus_delta[1] = m_delta[0] * m_x[1] + m_delta[1] * m_x[4] + m_delta[2] * m_x[7]; 47 m_x_plus_delta[2] = m_delta[0] * m_x[2] + m_delta[1] * m_x[5] + m_delta[2] * m_x[8]; 48 m_x_plus_delta[3] = m_delta[3] * m_x[0] + m_delta[4] * m_x[3] + m_delta[5] * m_x[6]; 49 m_x_plus_delta[4] = m_delta[3] * m_x[1] + m_delta[4] * m_x[4] + m_delta[5] * m_x[7]; 50 m_x_plus_delta[5] = m_delta[3] * m_x[2] + m_delta[4] * m_x[5] + m_delta[5] * m_x[8]; 51 m_x_plus_delta[6] = m_delta[6] * m_x[0] + m_delta[7] * m_x[3] + m_delta[8] * m_x[6]; 52 m_x_plus_delta[7] = m_delta[6] * m_x[1] + m_delta[7] * m_x[4] + m_delta[8] * m_x[7]; 53 m_x_plus_delta[8] = m_delta[6] * m_x[2] + m_delta[7] * m_x[5] + m_delta[8] * m_x[8]; 54 ceres::RotationMatrixToAngleAxis(ceres::RowMajorAdapter3x3((const double*)m_x_plus_delta), x_plus_delta); 55 return true; 56 #endif 57 } 58 59 bool ComputeJacobian(const double* x, double* jacobian) const 60 { 61 jacobian[0] = 1; jacobian[1] = 0; jacobian[2] = 0; 62 jacobian[3] = 0; jacobian[4] = 1; jacobian[5] = 0; 63 jacobian[6] = 0; jacobian[7] = 0; jacobian[8] = 1; 64 return true; 65 } 66 67 int GlobalSize() const { return 3; } 68 69 int LocalSize() const { return 3; } 70 }; 71 72 struct RotationMatrixParameterization : public ceres::LocalParameterization 73 { 74 bool Plus(const double* x, const double* delta, double* x_plus_delta) const 75 { 76 double m_delta[9]; 77 ceres::AngleAxisToRotationMatrix(delta, ceres::RowMajorAdapter3x3(m_delta)); 78 x_plus_delta[0] = m_delta[0] * x[0] + m_delta[1] * x[3] + m_delta[2] * x[6]; 79 x_plus_delta[1] = m_delta[0] * x[1] + m_delta[1] * x[4] + m_delta[2] * x[7]; 80 x_plus_delta[2] = m_delta[0] * x[2] + m_delta[1] * x[5] + m_delta[2] * x[8]; 81 x_plus_delta[3] = m_delta[3] * x[0] + m_delta[4] * x[3] + m_delta[5] * x[6]; 82 x_plus_delta[4] = m_delta[3] * x[1] + m_delta[4] * x[4] + m_delta[5] * x[7]; 83 x_plus_delta[5] = m_delta[3] * x[2] + m_delta[4] * x[5] + m_delta[5] * x[8]; 84 x_plus_delta[6] = m_delta[6] * x[0] + m_delta[7] * x[3] + m_delta[8] * x[6]; 85 x_plus_delta[7] = m_delta[6] * x[1] + m_delta[7] * x[4] + m_delta[8] * x[7]; 86 x_plus_delta[8] = m_delta[6] * x[2] + m_delta[7] * x[5] + m_delta[8] * x[8]; 87 return true; 88 } 89 90 bool ComputeJacobian(const double* x, double* jacobian) const 91 { 92 jacobian[0] = 0; jacobian[1] = x[6]; jacobian[2] = -x[3]; 93 jacobian[3] = 0; jacobian[4] = x[7]; jacobian[5] = -x[4]; 94 jacobian[6] = 0; jacobian[7] = x[8]; jacobian[8] = -x[5]; 95 jacobian[9] = -x[6]; jacobian[10] = 0; jacobian[11] = x[0]; 96 jacobian[12] = -x[7]; jacobian[13] = 0; jacobian[14] = x[1]; 97 jacobian[15] = -x[8]; jacobian[16] = 0; jacobian[17] = x[2]; 98 jacobian[18] = x[3]; jacobian[19] = -x[0]; jacobian[20] = 0; 99 jacobian[21] = x[4]; jacobian[22] = -x[1]; jacobian[23] = 0; 100 jacobian[24] = x[5]; jacobian[25] = -x[2]; jacobian[26] = 0; 101 return true; 102 } 103 104 int GlobalSize() const { return 9; } 105 106 int LocalSize() const { return 3; } 107 }; 108 109 public://CalibModels 110 struct MonoVCM 111 { 112 int nPoint; 113 VisionCM vcm; 114 Mat_<Vec2d> point2D; 115 MonoVCM(double* data3D0, double* data2D0, int nPoint0, int nRot0, int nDist0, int nModel0, int camModel0, bool simPin0) : nPoint(nPoint0), 116 vcm(data3D0, nPoint, nRot0, nDist0, nModel0, camModel0, simPin0) { 117 if (data2D0) Mat_<Vec2d>(nPoint, 1, (Vec2d*)data2D0).copyTo(point2D); 118 } 119 MonoVCM(float* data3D0, float* data2D0, int nPoint0, int nRot0, int nDist0, int nModel0, int camModel0, bool simPin0) : nPoint(nPoint0), 120 vcm(data3D0, nPoint, nRot0, nDist0, nModel0, camModel0, simPin0) { 121 if (data2D0) Mat_<Vec2f>(nPoint, 1, (Vec2f*)data2D0).copyTo(point2D); 122 } 123 template <typename tp> double calcErr(const tp* const rot, const tp* const t, const tp* const K, const tp* const D, const tp* const ab, tp* err2D0 = 0) const 124 { 125 //1.CalcResiduals 126 vector<tp> buf; 127 if (err2D0 == 0) { buf.resize(nPoint * 2); err2D0 = buf.data(); } 128 (*this)(rot, t, K, D, ab, err2D0); 129 130 //2.CalcMeanRms 131 Vec<tp, 2>* err2D = (Vec<tp, 2>*)err2D0; 132 double rmsMean = 0; for (int k = 0; k < nPoint; ++k) rmsMean += sqrt(err2D[k][0] * err2D[k][0] + err2D[k][1] * err2D[k][1]); 133 return rmsMean / nPoint; 134 } 135 template <typename tp> bool operator()(const tp* const rot, const tp* const t, const tp* const K, const tp* const D, const tp* const ab, tp* err2D0) const 136 { 137 //1.ProjectPoints 138 vector<Vec<tp, 2>> point2DEva(nPoint); 139 vcm(rot, t, K, D, ab, point2DEva.data()->val); 140 141 //2.CalcResiduals 142 Vec<tp, 2>* eva2D = point2DEva.data(); 143 Vec<tp, 2>* err2D = (Vec<tp, 2>*)err2D0; 144 const Vec2d* data2D = point2D.ptr<Vec2d>(); 145 for (int k = 0; k < nPoint; ++k) err2D[k] = Vec<tp, 2>(eva2D[k][0] - data2D[k][0], eva2D[k][1] - data2D[k][1]); 146 return true; 147 } 148 }; 149 150 struct BinoVCM 151 { 152 MonoVCM mvcm1; 153 MonoVCM mvcm2; 154 BinoVCM(double* data3D0, double* data2D10, double* data2D20, int nPoint, int nRot0, int nDist0, int nModel0, int camModel0, bool simPin0) : 155 mvcm1(data3D0, data2D10, nPoint, nRot0, nDist0, nModel0, camModel0, simPin0), 156 mvcm2(data3D0, data2D20, nPoint, nRot0, nDist0, nModel0, camModel0, simPin0) { } 157 BinoVCM(float* data3D0, float* data2D10, float* data2D20, int nPoint, int nRot0, int nDist0, int nModel0, int camModel0, bool simPin0) : 158 mvcm1(data3D0, data2D10, nPoint, nRot0, nDist0, nModel0, camModel0, simPin0), 159 mvcm2(data3D0, data2D20, nPoint, nRot0, nDist0, nModel0, camModel0, simPin0) { } 160 template <typename tp> double calcErr(const tp* const rot12, const tp* const t12, const tp* const rot1, const tp* const t1, 161 const tp* const K1, const tp* const D1, const tp* const ab1, 162 const tp* const K2, const tp* const D2, const tp* const ab2, tp* err4D0) const 163 { 164 //1.CalcResiduals 165 vector<tp> buf; 166 if (err4D0 == 0) { buf.resize(mvcm1.nPoint * 4); err4D0 = buf.data(); } 167 (*this)(rot12, t12, rot1, t1, K1, D1, ab1, K2, D2, ab2, err4D0); 168 169 //2.CalcMeanRms 170 Vec<tp, 4>* err4D = (Vec<tp, 4>*)err4D0; 171 double rmsMean = 0; for (int k = 0; k < mvcm1.nPoint; ++k) rmsMean += sqrt(err4D[k][0] * err4D[k][0] + err4D[k][1] * err4D[k][1] + err4D[k][2] * err4D[k][2] + err4D[k][3] * err4D[k][3]); 172 return rmsMean / mvcm1.nPoint; 173 } 174 template <typename tp> bool operator()(const tp* const rot21, const tp* const t21, const tp* const rot1, const tp* const t1, 175 const tp* const K1, const tp* const D1, const tp* const ab1, 176 const tp* const K2, const tp* const D2, const tp* const ab2, tp* err4D0) const 177 { 178 //0. 179 int nRot = mvcm1.vcm.nRot; 180 int nPoint = mvcm1.nPoint; 181 182 //1.ProjectCamera1 183 vector<Vec<tp, 2>> point2D1Eva(nPoint); 184 mvcm1.vcm(rot1, t1, K1, D1, ab1, point2D1Eva.data()->val); 185 186 //2.CalcCam21Pose 187 tp rot2[9], t2[3]; 188 if (nRot == 9) 189 { 190 rot2[0] = rot21[0] * rot1[0] + rot21[1] * rot1[3] + rot21[2] * rot1[6]; 191 rot2[1] = rot21[0] * rot1[1] + rot21[1] * rot1[4] + rot21[2] * rot1[7]; 192 rot2[2] = rot21[0] * rot1[2] + rot21[1] * rot1[5] + rot21[2] * rot1[8]; 193 194 rot2[3] = rot21[3] * rot1[0] + rot21[4] * rot1[3] + rot21[5] * rot1[6]; 195 rot2[4] = rot21[3] * rot1[1] + rot21[4] * rot1[4] + rot21[5] * rot1[7]; 196 rot2[5] = rot21[3] * rot1[2] + rot21[4] * rot1[5] + rot21[5] * rot1[8]; 197 198 rot2[6] = rot21[6] * rot1[0] + rot21[7] * rot1[3] + rot21[8] * rot1[6]; 199 rot2[7] = rot21[6] * rot1[1] + rot21[7] * rot1[4] + rot21[8] * rot1[7]; 200 rot2[8] = rot21[6] * rot1[2] + rot21[7] * rot1[5] + rot21[8] * rot1[8]; 201 202 t2[0] = rot21[0] * t1[0] + rot21[1] * t1[1] + rot21[2] * t1[2] + t21[0]; 203 t2[1] = rot21[3] * t1[0] + rot21[4] * t1[1] + rot21[5] * t1[2] + t21[1]; 204 t2[2] = rot21[6] * t1[0] + rot21[7] * t1[1] + rot21[8] * t1[2] + t21[2]; 205 } 206 else if (nRot == 4) 207 { 208 ceres::QuaternionProduct(rot21, rot1, rot2);//R2=R21*R1 209 ceres::QuaternionRotatePoint(rot21, t1, t2);//t2=R21*t1+t21 210 t2[0] += t21[0]; t2[1] += t21[1]; t2[2] += t21[2]; 211 } 212 else if (nRot == 3) 213 { 214 tp tmp[12]; 215 ceres::AngleAxisToQuaternion(rot21, tmp); 216 ceres::AngleAxisToQuaternion(rot1, tmp + 4); 217 ceres::QuaternionProduct(tmp, tmp + 4, tmp + 8); 218 ceres::QuaternionRotatePoint(tmp, t1, t2); 219 t2[0] += t21[0]; t2[1] += t21[1]; t2[2] += t21[2]; 220 ceres::QuaternionToAngleAxis(tmp + 8, rot2); 221 } 222 else spdlog::critical("Invalid rotation configuration"); 223 224 //3.ProjectCamera2 225 vector<Vec<tp, 2>> point2D2Eva(nPoint); 226 mvcm2.vcm(rot2, t2, K2, D2, ab2, point2D2Eva.data()->val); 227 228 //3.CalcResiduals 229 Vec<tp, 2>* eva2D1 = point2D1Eva.data(); 230 Vec<tp, 2>* eva2D2 = point2D2Eva.data(); 231 Vec<tp, 4>* err4D = (Vec<tp, 4>*)err4D0; 232 const Vec2d* data2D1 = mvcm1.point2D.ptr<Vec2d>(); 233 const Vec2d* data2D2 = mvcm2.point2D.ptr<Vec2d>(); 234 for (int k = 0; k < nPoint; ++k) err4D[k] = Vec<tp, 4>(eva2D1[k][0] - data2D1[k][0], eva2D1[k][1] - data2D1[k][1], eva2D2[k][0] - data2D2[k][0], eva2D2[k][1] - data2D2[k][1]); 235 return true; 236 } 237 }; 238 239 public://CalibUtils 240 static void calcBestPinCAM(Mat_<double> K, Mat_<double> D, Mat_<double> ab, int camModel, bool simPin, Size srcSize, Size newSize, Mat_<double> R, Mat_<double>& P) {} 241 242 static void rectAsPinMAP(Mat_<double> K, Mat_<double> D, Mat_<double> ab, int camModel, bool simPin, Mat_<double> R, Mat_<double> P, Size newSize, Mat_<float>& mapx, Mat_<float>& mapy) 243 { 244 //0.FormatInput 245 if (K(0, 1) != 0) { spdlog::critical("Skew must be zero"); assert(0); } 246 if (D.total() != 5) { spdlog::critical("Number of distortion must be five"); assert(0); } 247 if (ab.total() != 2) { spdlog::critical("Number of model must be two"); assert(0); } 248 if (mapx.rows != newSize.height || mapx.cols != newSize.width) mapx.create(newSize); 249 if (mapy.rows != newSize.height || mapy.cols != newSize.width) mapy.create(newSize); 250 double fx = K(0, 0), fy = K(1, 1), cx = K(0, 2), cy = K(1, 2); 251 double a = ab(0), b = ab(1); 252 double k1 = D(0), k2 = D(1), p1 = D(2), p2 = D(3), k3 = D(4); 253 254 //1.MainTask 255 double sumD = k1 + k2 + p1 + p2 + k3; 256 Matx33d iRiK = Mat_<double>((P * R).inv()); 257 for (int v = 0; v < newSize.height; ++v) 258 for (int u = 0; u < newSize.width; ++u) 259 { 260 //CamPoint 261 double x = u * iRiK.val[0] + v * iRiK.val[1] + iRiK.val[2]; 262 double y = u * iRiK.val[3] + v * iRiK.val[4] + iRiK.val[5]; 263 double z = u * iRiK.val[6] + v * iRiK.val[7] + iRiK.val[8]; 264 265 //NormPoint 266 double iz = 0; 267 if (camModel == VisionCM::RTCM || camModel == VisionCM::KBCM) iz = 1. / z; 268 else if (camModel == VisionCM::MUCM || camModel == VisionCM::EUCM) iz = 1. / (a * sqrt(b * (x * x + y * y) + z * z) + (1. - (simPin ? a : 0)) * z); 269 else if (camModel == VisionCM::DSCM) 270 { 271 double d1 = sqrt(x * x + y * y + z * z); 272 double d2 = sqrt(x * x + y * y + (b * d1 + z) * (b * d1 + z)); 273 iz = 1. / (a * d2 + (1. - (simPin ? a : 0)) * (b * d1 + z)); 274 } 275 else spdlog::critical("Invalid model configuration"); 276 277 double xn = x * iz; 278 double yn = y * iz; 279 280 //DistPoint 281 double xd = xn; 282 double yd = yn; 283 if (sumD != 0) 284 { 285 double xn2 = xn * xn; 286 double yn2 = yn * yn; 287 double r2 = xn2 + yn2; 288 double xnyn = 2 * xn * yn; 289 double r4 = r2 * r2; 290 double r6 = r2 * r4; 291 double r2xn2 = r2 + 2 * xn2; 292 double r2yn2 = r2 + 2 * yn2; 293 double an = 1 + k1 * r2 + k2 * r4 + k3 * r6; 294 xd = xn * an + p1 * xnyn + p2 * r2xn2; 295 yd = yn * an + p2 * xnyn + p1 * r2yn2; 296 } 297 298 //PixelPoint 299 mapx(v, u) = float(fx * xd + cx); 300 mapy(v, u) = float(fy * yd + cy); 301 } 302 } 303 304 public://MonoCalib 305 class MonoCalib 306 { 307 public://ForeEndArgs(Variable) 308 Size chSizeCross; 309 310 public://BackEndArgs(Constant) 311 int flags; 312 Size imaSize; 313 vector<vector<Point3f>> point3D; 314 vector<vector<Point2f>> point2D; 315 void create(vector<vector<Point3f>> p3D, vector<vector<Point2f>> p2D, Size imaSz, Size chSz) 316 { 317 point3D = p3D; point2D = p2D; imaSize = imaSz; chSizeCross = chSz; 318 camSolid.nDist = NDist; 319 camSolid.nModel = NModel; 320 camSolid.camModel = CAMModel; 321 camSolid.simPin = SIMPin; 322 camPoses.resize(point3D.size()); 323 rmsView.create(int(point3D.size()), 1); 324 errPoints.resize(point3D.size()); 325 for (size_t k = 0; k < point3D.size(); ++k) errPoints[k].resize(point3D[k].size()); 326 } 327 328 public://FinalResults(Constant) 329 CamSolid camSolid; 330 vector<SolidPose> camPoses; 331 double rmsMean = -1; 332 Mat_<double> rmsView; 333 vector<vector<Vec2d>> errPoints; 334 335 public://CalibFuncs(Constant) 336 void calibrate() { init(); optimize(); } 337 338 void init(/*Based on rotation vector*/) { camSolid.camModel == VisionCM::RTCM ? initRTCM() : camSolid.camModel == VisionCM::KBCM ? initKBCM() : initXUCM(); } 339 340 void optimize(/*Based on given rotation*/) 341 { 342 //1.CreateModel 343 ceres::Problem problem; 344 for (int k = 0; k < point3D.size(); ++k) 345 { 346 if (USEBatch) 347 { 348 ceres::CostFunction* cereCost = 349 new ceres::AutoDiffCostFunction<MonoVCM, ceres::DYNAMIC, NRot, 3, NProject, NDist, NModel>( 350 new MonoVCM((float*)point3D[k].data(), (float*)point2D[k].data(), int(point3D[k].size()), NRot, camSolid.nDist, camSolid.nModel, camSolid.camModel, camSolid.simPin), 351 int(point3D[k].size() * 2), ceres::TAKE_OWNERSHIP); 352 problem.AddResidualBlock(cereCost, new ceres::CauchyLoss(4.431969), camPoses[k].getRot(NRot), camPoses[k].tdata, camSolid.K, camSolid.D, camSolid.ab); 353 } 354 else 355 for (int i = 0; i < point3D[k].size(); ++i) 356 { 357 ceres::CostFunction* cereCost = 358 new ceres::AutoDiffCostFunction<MonoVCM, ceres::DYNAMIC, NRot, 3, NProject, NDist, NModel>( 359 new MonoVCM((float*)(point3D[k].data() + i), (float*)(point2D[k].data() + i), 1, NRot, camSolid.nDist, camSolid.nModel, camSolid.camModel, camSolid.simPin), 360 2, ceres::TAKE_OWNERSHIP); 361 problem.AddResidualBlock(cereCost, new ceres::CauchyLoss(1.0), camPoses[k].getRot(NRot), camPoses[k].tdata, camSolid.K, camSolid.D, camSolid.ab); 362 } 363 if (NRot == 4) problem.SetParameterization(camPoses[k].getRot(NRot), new ceres::QuaternionParameterization); 364 if (NRot == 9) problem.SetParameterization(camPoses[k].getRot(NRot), new RotationMatrixParameterization); 365 } 366 if (FIXDist) problem.SetParameterBlockConstant(camSolid.D); 367 if (FIXModel) problem.SetParameterBlockConstant(camSolid.ab); 368 369 //2.SolveModel 370 ceres::Solver::Options options; 371 options.num_threads = 8; 372 options.max_num_iterations = 256; 373 options.minimizer_progress_to_stdout = true; 374 ceres::Solver::Summary summary; 375 ceres::Solve(options, &problem, &summary); 376 for (size_t k = 0; k < camPoses.size(); ++k) camPoses[k].setRot(camPoses[k].getRot(NRot), NRot); 377 spdlog::info("Optimization done and report:{}", summary.FullReport()); 378 379 //3.UpdateState 380 updateErr(); //0.2279609352654616 381 spdlog::info("Optimization done and reprojection rms: {}", rmsMean); 382 } 383 384 void updateErr(/*Based on rotation matrix*/) 385 { 386 for (int k = 0; k < point3D.size(); ++k) 387 rmsView(k) = MonoVCM((float*)(point3D[k].data()), (float*)(point2D[k].data()), int(point3D[k].size()), 9, camSolid.nDist, camSolid.nModel, camSolid.camModel, camSolid.simPin).calcErr( 388 camPoses[k].mdata, camPoses[k].tdata, camSolid.K, camSolid.D, camSolid.ab, (double*)errPoints[k].data()); 389 rmsMean = cv::sum(rmsView)[0] / point3D.size();//0.2279609352654616 390 } 391 392 public: 393 void initRTCM() {} 394 void initKBCM() {} 395 void initXUCM(/*Based on rotation vector*/) 396 { 397 //1.BasicInit 398 camSolid.ab[0] = 1;//let degraded to UCM 399 camSolid.ab[1] = camSolid.camModel == VisionCM::DSCM ? 0 : 1;//let degraded to UCM 400 camSolid.K[2] = imaSize.width / 2.0; 401 camSolid.K[3] = imaSize.height / 2.0; 402 for (int k = 0; k < camSolid.nDist; ++k) camSolid.D[k] = 0; 403 404 //2.CoreInit 405 rmsMean = DBL_MAX; 406 const double cx = camSolid.K[2]; 407 const double cy = camSolid.K[3]; 408 for (int k = 0; k < point2D.size(); ++k) 409 for (int i = 0; i < chSizeCross.height; ++i) 410 { 411 //2.1 GetEquations 412 Mat_<double> P(chSizeCross.width, 4); 413 for (int j = 0; j < chSizeCross.width; ++j) 414 { 415 Point2f pt = point2D[k][i * chSizeCross.width + j]; 416 double u = pt.x - cx; 417 double v = pt.y - cy; 418 P(j, 0) = u; 419 P(j, 1) = v; 420 P(j, 2) = 0.5; 421 P(j, 3) = -0.5 * (u * u + v * v); 422 } 423 424 //2.2 SolveEquations 425 Mat_<double> C; 426 SVD::solveZ(P, C); 427 double t = C(0) * C(0) + C(1) * C(1) + C(2) * C(3); 428 if (t < 0.0) continue; 429 430 //2.3 CalcCamParam 431 double d = std::sqrt(1.0 / t); 432 double nx = C(0) * d; 433 double ny = C(1) * d; 434 if (std::hypot(nx, ny) > 0.95) continue; 435 double ffk = std::sqrt(C(2) / C(3)); 436 437 //2.4 CalcCamPose 438 double rmsMean0 = 0; 439 Mat_<double> rmsView0(rmsView.size(), 0.); 440 vector<SolidPose> camPoses0(camPoses.size()); 441 vector<vector<Vec2d>> errPoints0(errPoints.size()); 442 for (int kk = 0; kk < point3D.size(); ++kk) 443 { 444 //1.CalcCamPose 445 vector<Point3f> point3DEx; 446 vector<Point2f> point2DEx; 447 for (int ii = 0; ii < point2D[kk].size(); ++ii) 448 { 449 double x = (point2D[kk][ii].x - cx) / ffk; 450 double y = (point2D[kk][ii].y - cy) / ffk; 451 double z = (1 - x * x - y * y) / 2;//because of a=b=1 D=0 452 if (z < 0.01) continue; // { spdlog::warn("Skip point3DRay[{}] ({}, {}, {}) which is close to or more than 180: z<0.01", ii, x, y, z); continue; } 453 point3DEx.push_back(point3D[kk][ii]); 454 point2DEx.push_back(Point2f(float(x / z), float(y / z))); 455 } 456 if (point3DEx.size() < 8) { spdlog::warn("View{}---Line{}---View{}--ValidPoints{}", k, i, kk, point3DEx.size()); continue; } 457 cv::solvePnP(point3DEx, point2DEx, Mat_<double>::eye(3, 3), noArray(), Mat_<double>(3, 1, camPoses0[kk].vdata), Mat_<double>(3, 1, camPoses0[kk].tdata)); 458 459 //2.AnalyzeCamPose 460 errPoints0[kk].resize(point3D[kk].size()); 461 Matx33d R; cv::Rodrigues(Mat_<double>(3, 1, camPoses0[kk].vdata), R); 462 for (int ii = 0; ii < point3D[kk].size(); ++ii) 463 { 464 double x = R.val[0] * point3D[kk][ii].x + R.val[1] * point3D[kk][ii].y + R.val[2] * point3D[kk][ii].z + camPoses0[kk].tdata[0]; 465 double y = R.val[3] * point3D[kk][ii].x + R.val[4] * point3D[kk][ii].y + R.val[5] * point3D[kk][ii].z + camPoses0[kk].tdata[1]; 466 double z = R.val[6] * point3D[kk][ii].x + R.val[7] * point3D[kk][ii].y + R.val[8] * point3D[kk][ii].z + camPoses0[kk].tdata[2]; 467 double d = sqrt(x * x + y * y + z * z);//because of a=b=1 D=0 468 double iz = 1. / (d + z); //if denominator==0 469 double xn = x * iz; 470 double yn = y * iz; 471 errPoints0[kk][ii] = Vec2d(ffk * xn + cx - point2D[kk][ii].x, ffk * yn + cy - point2D[kk][ii].y); 472 rmsView0(kk) += cv::norm(errPoints0[kk][ii]); 473 } 474 rmsView0(kk) = rmsView0(kk) / point3D[kk].size(); 475 } 476 477 //2.5 ChooseBetterPose 478 rmsMean0 = cv::sum(rmsView0)[0] / point3D.size(); 479 if (rmsMean0 < rmsMean) //1.4185722111399552 480 { 481 rmsMean = rmsMean0; 482 rmsView = rmsView0; 483 camPoses = camPoses0; 484 errPoints = errPoints0; 485 camSolid.K[0] = camSolid.K[1] = ffk; 486 } 487 } 488 489 //3.UpdateState 490 for (size_t k = 0; k < camPoses.size(); ++k) camPoses[k].setRot(camPoses[k].vdata, 3);//Here must be roation vector 491 spdlog::info("Initialization done and reprojection rms: {}", rmsMean);//1.3384170570917158 492 } 493 494 public: 495 void sovlePnP() {} 496 }; 497 498 public://BinoCalib 499 class BinoCalib 500 { 501 public://MonoFuncs(Constant) 502 MonoCalib calib1; 503 MonoCalib calib2; 504 505 public://FinalResults(Constant) 506 SolidPose cam21Pose; 507 double rmsMean = -1; 508 Mat_<double> rmsView; 509 vector<vector<Vec4d>> errPoints; 510 void create(vector<vector<Point3f>> p3D, vector<vector<Point2f>> p2D1, vector<vector<Point2f>> p2D2, Size imaSz, Size chSz) 511 { 512 calib1.create(p3D, p2D1, imaSz, chSz); 513 calib2.create(p3D, p2D2, imaSz, chSz); 514 rmsView.create(int(p3D.size()), 1); 515 errPoints.resize(calib1.point3D.size()); 516 for (size_t k = 0; k < calib1.point3D.size(); ++k) errPoints[k].resize(calib1.point3D[k].size()); 517 } 518 519 public://CalibFuncs(Constant) 520 void calibrate() { init(); optimize(); } 521 522 void init(/*Based on unit quaternion*/) 523 { 524 //1.BasicInit 525 const ModelCamera::CamSolid& camSolid = calib1.camSolid; 526 const vector<vector<Point3f>>& point3D = calib1.point3D; 527 const vector<vector<Point2f>>& point2D1 = calib1.point2D; 528 const vector<vector<Point2f>>& point2D2 = calib2.point2D; 529 calib1.calibrate(); //0.228574 530 calib2.calibrate(); //0.273371 531 532 //2.CoreInit 533 this->rmsMean = DBL_MAX; 534 for (int k = 0; k < point3D.size(); ++k) 535 { 536 //2.1 GetCam1PoseAndCam2Pose 537 const SolidPose& cam1Pose = calib1.camPoses[k]; 538 const SolidPose& cam2Pose = calib2.camPoses[k]; 539 540 //2.2 CalcCam21Pose 541 SolidPose cam21Pose0; 542 ceres::QuaternionProduct(cam2Pose.qdata, Vec4d(cam1Pose.qdata[0], -cam1Pose.qdata[1], -cam1Pose.qdata[2], -cam1Pose.qdata[3]).val, cam21Pose0.qdata);//R21=R2*Inv(R1) 543 ceres::QuaternionRotatePoint(cam21Pose0.qdata, cam1Pose.tdata, cam21Pose0.tdata);//t12=-R21*t1+t2 544 for (int t = 0; t < 3; ++t) cam21Pose0.tdata[t] = -cam21Pose0.tdata[t] + cam2Pose.tdata[t]; 545 546 //2.3 CalcCam2Pose 547 vector<SolidPose> cam2Poses0(point3D.size()); 548 for (int i = 0; i < point3D.size(); ++i) 549 { 550 const double* cam1Q = calib1.camPoses[i].qdata; 551 const double* cam1T = calib1.camPoses[i].tdata; 552 ceres::QuaternionProduct(cam21Pose0.qdata, cam1Q, cam2Poses0[i].qdata);//R2=R21*R1 553 ceres::QuaternionRotatePoint(cam21Pose0.qdata, cam1T, cam2Poses0[i].tdata);//t2=R21*t1+t21 554 for (int t = 0; t < 3; ++t) cam2Poses0[i].tdata[t] += cam21Pose0.tdata[t]; 555 } 556 557 //2.4 AnalyzeCam21Pose 558 Mat_<double> rmsView0(rmsView.size(), 0.); 559 vector<vector<Vec2d>> errPoints0(errPoints.size()); 560 for (int i = 0; i < point3D.size(); ++i) 561 rmsView0(i) = MonoVCM((float*)point3D[i].data(), (float*)point2D2[i].data(), int(point3D[i].size()), 4, camSolid.nDist, camSolid.nModel, camSolid.camModel, camSolid.simPin).calcErr( 562 cam2Poses0[i].qdata, cam2Poses0[i].tdata, calib2.camSolid.K, calib2.camSolid.D, calib2.camSolid.ab);//Here must be quaternion 563 564 //2.5 ChooseBetterPose 565 double rmsMean0 = cv::sum(rmsView0)[0] / point3D.size(); 566 if (rmsMean0 < this->rmsMean)//0.814502 567 { 568 this->rmsMean = rmsMean0; 569 this->rmsView = rmsView0; 570 this->cam21Pose = cam21Pose0; 571 } 572 } 573 574 //3.UpdateState 575 this->cam21Pose.setRot(cam21Pose.qdata, 4);//0.999380396759363, -0.0031299638455660334, -0.002043827564730205, -0.0349978380519228 //Here must be quaternion 576 spdlog::info("Initialization done and quat: {}, {}, {}, {}", this->cam21Pose.qdata[0], this->cam21Pose.qdata[1], this->cam21Pose.qdata[2], this->cam21Pose.qdata[3]); 577 spdlog::info("Initialization done and tvec: {}, {}, {}", this->cam21Pose.tdata[0], this->cam21Pose.tdata[1], this->cam21Pose.tdata[2]);//-203.4071041001514, 5.738333250873239, -0.39390447090420366 578 spdlog::info("Initialization done and reprojection rms2: {}", this->rmsMean);//0.5493172947025333 579 } 580 581 void optimize(/*Based on given rotation*/) 582 { 583 //0.GetInput 584 const ModelCamera::CamSolid& camSolid = calib1.camSolid; 585 const vector<vector<Point3f>>& point3D = calib1.point3D; 586 const vector<vector<Point2f>>& point2D1 = calib1.point2D; 587 const vector<vector<Point2f>>& point2D2 = calib2.point2D; 588 589 //1.CreateModel 590 ceres::Problem problem; 591 for (int k = 0; k < point3D.size(); ++k) 592 { 593 if (USEBatch) 594 { 595 ceres::CostFunction* cereCost = 596 new ceres::AutoDiffCostFunction<BinoVCM, ceres::DYNAMIC, NRot, 3, NRot, 3, NProject, NDist, NModel, NProject, NDist, NModel>( 597 new BinoVCM((float*)point3D[k].data(), (float*)point2D1[k].data(), (float*)point2D2[k].data(), int(point3D[k].size()), NRot, camSolid.nDist, camSolid.nModel, camSolid.camModel, camSolid.simPin), 598 int(point3D[k].size() * 4), ceres::TAKE_OWNERSHIP); 599 problem.AddResidualBlock(cereCost, new ceres::CauchyLoss(5.5), 600 this->cam21Pose.getRot(NRot), this->cam21Pose.tdata, calib1.camPoses[k].getRot(NRot), calib1.camPoses[k].tdata, 601 calib1.camSolid.K, calib1.camSolid.D, calib1.camSolid.ab, 602 calib2.camSolid.K, calib2.camSolid.D, calib2.camSolid.ab); 603 } 604 else 605 for (size_t i = 0; i < point3D[k].size(); ++i) 606 { 607 ceres::CostFunction* cereCost = 608 new ceres::AutoDiffCostFunction<BinoVCM, ceres::DYNAMIC, NRot, 3, NRot, 3, NProject, NDist, NModel, NProject, NDist, NModel>( 609 new BinoVCM((float*)(point3D[k].data() + i), (float*)(point2D1[k].data() + i), (float*)(point2D2[k].data() + i), 1, NRot, camSolid.nDist, camSolid.nModel, camSolid.camModel, camSolid.simPin), 610 4, ceres::TAKE_OWNERSHIP); 611 problem.AddResidualBlock(cereCost, new ceres::CauchyLoss(1.0), 612 this->cam21Pose.getRot(NRot), this->cam21Pose.tdata, calib1.camPoses[k].getRot(NRot), calib1.camPoses[k].tdata, 613 calib1.camSolid.K, calib1.camSolid.D, calib1.camSolid.ab, 614 calib2.camSolid.K, calib2.camSolid.D, calib2.camSolid.ab); 615 } 616 if(NRot == 4) problem.SetParameterization(calib1.camPoses[k].getRot(NRot), new ceres::QuaternionParameterization); 617 if(NRot == 9) problem.SetParameterization(calib1.camPoses[k].getRot(NRot), new RotationMatrixParameterization); 618 } 619 if (NRot == 4) problem.SetParameterization(this->cam21Pose.getRot(NRot), new ceres::QuaternionParameterization); 620 if (NRot == 9) problem.SetParameterization(this->cam21Pose.getRot(NRot), new RotationMatrixParameterization); 621 if (FIXDist) problem.SetParameterBlockConstant(calib1.camSolid.D); 622 if (FIXDist) problem.SetParameterBlockConstant(calib2.camSolid.D); 623 if (FIXModel) problem.SetParameterBlockConstant(calib1.camSolid.ab); 624 if (FIXModel) problem.SetParameterBlockConstant(calib2.camSolid.ab); 625 626 //2.SolveModel 627 ceres::Solver::Options options; 628 options.num_threads = 8; 629 options.max_num_iterations = 256; 630 options.minimizer_progress_to_stdout = true; 631 ceres::Solver::Summary summary; 632 ceres::Solve(options, &problem, &summary); 633 this->cam21Pose.setRot(this->cam21Pose.getRot(NRot), NRot); 634 for (int k = 0; k < point3D.size(); ++k)//UpdateCam2Pose 635 { 636 calib1.camPoses[k].setRot(calib1.camPoses[k].getRot(NRot), NRot); 637 ceres::QuaternionProduct(this->cam21Pose.qdata, calib1.camPoses[k].qdata, calib2.camPoses[k].qdata);//R2=R21*R1 638 ceres::QuaternionRotatePoint(this->cam21Pose.qdata, calib1.camPoses[k].tdata, calib2.camPoses[k].tdata);//t2=R21*t1+t12 639 for (int t = 0; t < 3; ++t) calib2.camPoses[k].tdata[t] += this->cam21Pose.tdata[t]; 640 calib2.camPoses[k].setRot(calib2.camPoses[k].qdata, 4);//Here must be quaternion 641 } 642 spdlog::info("Optimization done and report:{}", summary.FullReport()); 643 644 //3.UpdateState 645 this->updateErr(); 646 calib1.updateErr(); 647 calib2.updateErr(); 648 spdlog::info("Optimization done and reprojection rms: {}", this->rmsMean);//0.346291 649 spdlog::info("Optimization done and reprojection rms1: {}", calib1.rmsMean);//0.237126 650 spdlog::info("Optimization done and reprojection rms2: {}", calib2.rmsMean);//0.284363 651 } 652 653 void updateErr(/*Based on rotation matrix*/) 654 { 655 const ModelCamera::CamSolid& camSolid = calib1.camSolid; 656 for (int k = 0; k < calib1.point3D.size(); ++k) 657 rmsView(k) = BinoVCM((float*)(calib1.point3D[k].data()), (float*)(calib1.point2D[k].data()), (float*)(calib2.point2D[k].data()), int(calib1.point3D.size()), 9, camSolid.nDist, camSolid.nModel, camSolid.camModel, camSolid.simPin).calcErr( 658 this->cam21Pose.mdata, this->cam21Pose.tdata, calib1.camPoses[k].mdata, calib1.camPoses[k].tdata, 659 calib1.camSolid.K, calib1.camSolid.D, calib1.camSolid.ab, 660 calib2.camSolid.K, calib2.camSolid.D, calib2.camSolid.ab, (double*)errPoints[k].data()); 661 this->rmsMean = cv::sum(rmsView)[0] / calib1.point3D.size(); 662 } 663 }; 664 665 public://CalibAPP 666 class MonoAPP 667 { 668 public://CalibArgs 669 bool doCalib = true; 670 bool doRect = true; 671 bool doUnd = true; 672 string detDir = "../data/calib/cvkbcm/cam1"; 673 Size calibSize = Size(1280, 800); 674 int chSqrSide = 50; 675 Size chSizeCross = Size(8, 6); 676 int chDetFlag = 3; 677 678 public://RectArgs 679 Size rectSize = calibSize; 680 Vec4d rectPlan = Vec4d(0.4, 30, 0, 0); 681 void resetViewPose(Vec4d rectPlan0) { rectPlan = rectPlan0; rectPath = fmt::format("{}/rst/RectF{}R{}P{}Y{}.yml", detDir, rectPlan[0], rectPlan[1], rectPlan[2], rectPlan[3]); } 682 683 public://AutoParams 684 string calibPath = fmt::format("{}/rst/Calib.yml", detDir); 685 string rectPath = fmt::format("{}/rst/RectF{}R{}P{}Y{}.yml", detDir, rectPlan[0], rectPlan[1], rectPlan[2], rectPlan[3]); 686 687 public://CalibParams 688 int camModel; 689 int simPin; 690 Mat_<double> K = Mat_<double>(3, 3, 0.); 691 Mat_<double> D = Mat_<double>(5, 1, 0.); 692 Mat_<double> ab = Mat_<double>(2, 1, 0.); 693 Mat_<double> poses; 694 Mat_<double> rmsView; 695 double rmsMean; 696 vector<vector<Vec2d>> errPoints; 697 vector<vector<Point3f>> point3D; 698 vector<vector<Point2f>> point2D; 699 700 public://RectParams 701 Mat_<double> P; 702 Mat_<double> R; 703 Mat_<float> mapx; 704 Mat_<float> mapy; 705 706 public: 707 void save(string path) 708 { 709 FileStorage fs(path, FileStorage::WRITE); 710 fs << "camModel" << camModel; 711 fs << "simPin" << simPin; 712 fs << "K" << K; 713 fs << "D" << D; 714 fs << "ab" << ab; 715 fs << "P" << P; 716 fs << "R" << R; 717 fs << "poses" << poses; 718 fs << "rmsMean" << rmsMean; 719 fs << "rmsView" << rmsView; 720 fs << "errPoints" << errPoints; 721 fs << "point3D" << point3D; 722 fs << "point2D" << point2D; 723 fs << "mapx" << mapx; 724 fs << "mapy" << mapy; 725 fs.release(); 726 } 727 728 void read(string path) 729 { 730 FileStorage fs(path, FileStorage::READ); 731 fs["camModel"] >> camModel; 732 fs["simPin"] >> simPin; 733 fs["K"] >> K; 734 fs["D"] >> D; 735 fs["ab"] >> ab; 736 fs["P"] >> P; 737 fs["R"] >> R; 738 fs["poses"] >> poses; 739 fs["rmsMean"] >> rmsMean; 740 fs["rmsView"] >> rmsView; 741 fs["errPoints"] >> errPoints; 742 fs["point3D"] >> point3D; 743 fs["point2D"] >> point2D; 744 fs["mapx"] >> mapx; 745 fs["mapy"] >> mapy; 746 fs.release(); 747 }; 748 749 public: 750 ModelCalibration::MonoCalib monoCalib; 751 void run() 752 { 753 //1.Calib 754 if (doCalib) 755 { 756 //1.1 Detection 757 monoDetCHBD(detDir, chSizeCross, float(chSqrSide), chDetFlag, point2D, point3D); 758 monoCalib.create(point3D, point2D, calibSize, chSizeCross); 759 760 //1.2 Calibration 761 monoCalib.calibrate(); 762 camModel = monoCalib.camSolid.camModel; 763 simPin = monoCalib.camSolid.simPin; 764 K << monoCalib.camSolid.K[0], 0, monoCalib.camSolid.K[2], 0, monoCalib.camSolid.K[1], monoCalib.camSolid.K[3], 0, 0, 1; 765 for (int k = 0; k < monoCalib.camSolid.nDist; ++k) D(k) = monoCalib.camSolid.D[k]; 766 ab(0) = monoCalib.camSolid.ab[0]; ab(1) = monoCalib.camSolid.ab[1]; 767 poses = Mat_<double>(int(monoCalib.camPoses.size()), sizeof(monoCalib.camPoses[0].data) / sizeof(double), (double*)monoCalib.camPoses.data()); 768 rmsView = monoCalib.rmsView; 769 rmsMean = monoCalib.rmsMean; 770 errPoints = monoCalib.errPoints; 771 utils::fs::createDirectories(utils::fs::getParent(calibPath)); save(calibPath); 772 } 773 else read(calibPath); 774 775 //2.Rect 776 if (doRect) 777 { 778 R.create(3, 3); ceres::EulerAnglesToRotationMatrix(Vec3d(rectPlan[1], rectPlan[2], rectPlan[3]).val, 3, R.ptr<double>()); 779 P.create(3, 3); P << K(0, 0) * rectPlan[0] / (1 + (simPin ? 0 : ab(0))), 0, rectSize.width * 0.5, 0, K(1, 1)* rectPlan[0] / (1 + (simPin ? 0 : ab(0))), rectSize.height * 0.5, 0, 0, 1; 780 ModelCalibration::rectAsPinMAP(K, D, ab, camModel, simPin, R, P, rectSize, mapx, mapy); 781 save(rectPath); 782 } 783 784 //3.Undistort 785 if (doUnd) monoUnd(mapx, mapy, detDir); 786 } 787 788 public: 789 static void monoDetCHBD(string detDir, Size chSizeCross, float chSqrSide, int chDetFlag, 790 vector<vector<Point2f>>& point2D, vector<vector<Point3f>>& point3D, int chWinSize = 5, string chPattern = "*", int chShowMS = 10) 791 { 792 //0.Initialize 793 point2D.clear(); 794 point3D.clear(); 795 if (chShowMS >= 0) cv::namedWindow(__FUNCTION__, WINDOW_NORMAL); 796 797 //1.DetectPoint2D 798 int cnt = 0; 799 vector<string> filePaths = Tool2D::globPaths(detDir, chPattern, 0, false); 800 for (size_t fileId = 0; fileId < filePaths.size(); ++fileId) 801 { 802 //1.1 803 Mat_<Vec3b> color = imread(filePaths[fileId]); 804 Mat_<uchar> gray; cvtColor(color, gray, COLOR_BGR2GRAY); 805 806 //1.2 807 vector<Point2f> corner2D; 808 bool found = findChessboardCorners(gray, chSizeCross, corner2D, chDetFlag); 809 if (found == false) 810 { 811 Mat_<uchar> tmpIma; resize(gray, tmpIma, gray.size() / 2); 812 found = findChessboardCorners(tmpIma, chSizeCross, corner2D, chDetFlag); 813 for (size_t k = 0; k < corner2D.size(); ++k) corner2D[k] *= 2; 814 } 815 816 //1.3 817 if (found) cornerSubPix(gray, corner2D, Size(chWinSize, chWinSize), Size(-1, -1), TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 30, 0.01)); 818 if (found) drawChessboardCorners(color, chSizeCross, corner2D, found); 819 if (found) point2D.push_back(corner2D); 820 string tips = fmt::format("{} detect {} and current vaild views {}", found ? "Could" : "Cannot", filePaths[fileId], cnt += found); 821 if (!found) spdlog::warn(tips); 822 if (chShowMS >= 0) cv::displayStatusBar(__FUNCTION__, tips, 1000); 823 824 //1.4 825 if (chShowMS < 0) continue; 826 cv::imshow(__FUNCTION__, color); 827 cv::waitKey(chShowMS); 828 } if (chShowMS >= 0) cv::destroyWindow(__FUNCTION__); 829 830 //2.CalcPoint3D 831 vector<Point3f> corner3D; 832 for (int i = 0; i < chSizeCross.height; ++i) 833 for (int j = 0; j < chSizeCross.width; ++j) 834 corner3D.push_back(Point3f(j * chSqrSide, i * chSqrSide, 0)); 835 point3D.assign(cnt, corner3D); 836 } 837 838 static void monoUnd(Mat_<float> mapx, Mat_<float> mapy, string undDir, string undPattern = "*", int undShowMS = 10) 839 { 840 //0.Initialize 841 string undOut = undDir + "/rst"; 842 utils::fs::createDirectories(undOut); 843 if (undShowMS >= 0) cv::namedWindow(__FUNCTION__, WINDOW_NORMAL); 844 845 //1.UndCamera 846 vector<string> filePaths = Tool2D::globPaths(undDir, undPattern, 0, false); 847 for (size_t fileId = 0; fileId < filePaths.size(); ++fileId) 848 { 849 //1.1 850 Mat_<Vec3b> color = imread(filePaths[fileId], 1); 851 Mat_<uchar> gray; cvtColor(color, gray, COLOR_BGR2GRAY); 852 853 //1.2 854 Mat_<uchar> undIma; 855 remap(gray, undIma, mapx, mapy, INTER_LINEAR); 856 857 //1.3 858 Mat_<uchar> catIma; 859 if (gray.size() != undIma.size()) resize(gray, gray, undIma.size()); 860 hconcat(gray, undIma, catIma); 861 cv::imwrite(fmt::format("{}/und_{}", undOut, Tool2D::pathProps(filePaths[fileId])[4]), catIma); 862 if (undShowMS >= 0) cv::displayStatusBar(__FUNCTION__, "Undistorting " + filePaths[fileId], 1000); 863 864 //1.4 865 if (undShowMS < 0) continue; 866 cv::imshow(__FUNCTION__, catIma); 867 cv::waitKey(undShowMS); 868 } if (undShowMS >= 0) cv::destroyWindow(__FUNCTION__); 869 } 870 }; 871 872 class BinoAPP 873 { 874 public://CalibArgs 875 bool doCalib = true; 876 bool doRect = true; 877 string detDir = "../data/calib/cvkbcm/all"; 878 Size calibSize = Size(1280, 800); 879 int chSqrSide = 50; 880 Size chSizeCross = Size(8, 6); 881 int chDetFlag = 3; 882 883 public://RectArgs 884 Size rectSize = calibSize; 885 Vec4d rectPlan = Vec4d(0.4, 30, 0, 0); 886 string calibPath = fmt::format("{}/rst/Calib.yml", detDir); 887 string rectPath = fmt::format("{}/rst/RectF{}R{}P{}Y{}.yml", detDir, rectPlan[0], rectPlan[1], rectPlan[2], rectPlan[3]); 888 889 public://CalibParams 890 int camModel; 891 int simPin; 892 Mat_<double> K1 = Mat_<double>(3, 3, 0.); 893 Mat_<double> K2 = Mat_<double>(3, 3, 0.); 894 Mat_<double> D1 = Mat_<double>(5, 1, 0.); 895 Mat_<double> D2 = Mat_<double>(5, 1, 0.); 896 Mat_<double> ab1 = Mat_<double>(2, 1, 0.); 897 Mat_<double> ab2 = Mat_<double>(2, 1, 0.); 898 Mat_<double> R; 899 Mat_<double> T; 900 Mat_<double> rmsView; 901 double rmsMean; 902 vector<vector<Vec4d>> errPoints; 903 vector<vector<Point2f>> point2D1; 904 vector<vector<Point2f>> point2D2; 905 vector<vector<Point3f>> point3D; 906 907 public://RectParams 908 Mat_<double> P1; 909 Mat_<double> P2; 910 Mat_<double> R1; 911 Mat_<double> R2; 912 Mat_<float> mapx1; 913 Mat_<float> mapy1; 914 Mat_<float> mapx2; 915 Mat_<float> mapy2; 916 917 public://IOActions 918 void save(string path) 919 { 920 FileStorage fs(path, FileStorage::WRITE); 921 fs << "camModel" << camModel; 922 fs << "simPin" << simPin; 923 fs << "K1" << K1; 924 fs << "K2" << K2; 925 fs << "D1" << D1; 926 fs << "D2" << D2; 927 fs << "ab1" << ab1; 928 fs << "ab2" << ab2; 929 fs << "R" << R; 930 fs << "T" << T; 931 fs << "P1" << P1; 932 fs << "P2" << P2; 933 fs << "R1" << R1; 934 fs << "R2" << R2; 935 fs << "rmsMean" << rmsMean; 936 fs << "rmsView" << rmsView; 937 fs << "errPoints" << errPoints; 938 fs << "point3D" << point3D; 939 fs << "point2D1" << point2D1; 940 fs << "point2D2" << point2D2; 941 fs << "mapx1" << mapx1; 942 fs << "mapy1" << mapy1; 943 fs << "mapx2" << mapx2; 944 fs << "mapy2" << mapy2; 945 fs.release(); 946 } 947 948 void read(string path) 949 { 950 FileStorage fs(path, FileStorage::READ); 951 fs["camModel"] >> camModel; 952 fs["simPin"] >> simPin; 953 fs["K1"] >> K1; 954 fs["K2"] >> K2; 955 fs["D1"] >> D1; 956 fs["D2"] >> D2; 957 fs["ab1"] >> ab1; 958 fs["ab2"] >> ab2; 959 fs["R"] >> R; 960 fs["T"] >> T; 961 fs["P1"] >> P1; 962 fs["P2"] >> P2; 963 fs["R1"] >> R1; 964 fs["R2"] >> R2; 965 fs["rmsMean"] >> rmsMean; 966 fs["rmsView"] >> rmsView; 967 fs["errPoints"] >> errPoints; 968 fs["point3D"] >> point3D; 969 fs["point2D1"] >> point2D1; 970 fs["point2D2"] >> point2D2; 971 fs["mapx1"] >> mapx1; 972 fs["mapy1"] >> mapy1; 973 fs["mapx2"] >> mapx2; 974 fs["mapy2"] >> mapy2; 975 fs.release(); 976 }; 977 978 public://MainTask 979 ModelCalibration::BinoCalib binoCalib; 980 void run() 981 { 982 //1.DetectionAndCalibbration 983 if (doCalib) 984 { 985 //1.1 Detection 986 binoDetCHBD(detDir, chSizeCross, float(chSqrSide), chDetFlag, point2D1, point2D2, point3D); 987 binoCalib.create(point3D, point2D1, point2D2, calibSize, chSizeCross); 988 989 //1.2 Calibbration 990 binoCalib.calibrate(); 991 camModel = binoCalib.calib1.camSolid.camModel; 992 simPin = binoCalib.calib2.camSolid.simPin; 993 K1 << binoCalib.calib1.camSolid.K[0], 0, binoCalib.calib1.camSolid.K[2], 0, binoCalib.calib1.camSolid.K[1], binoCalib.calib1.camSolid.K[3], 0, 0, 1; 994 K2 << binoCalib.calib2.camSolid.K[0], 0, binoCalib.calib2.camSolid.K[2], 0, binoCalib.calib2.camSolid.K[1], binoCalib.calib2.camSolid.K[3], 0, 0, 1; 995 for (int k = 0; k < binoCalib.calib1.camSolid.nDist; ++k) D1(k) = binoCalib.calib1.camSolid.D[k]; 996 for (int k = 0; k < binoCalib.calib2.camSolid.nDist; ++k) D2(k) = binoCalib.calib2.camSolid.D[k]; 997 ab1(0) = binoCalib.calib1.camSolid.ab[0]; ab1(1) = binoCalib.calib1.camSolid.ab[1]; 998 ab2(0) = binoCalib.calib2.camSolid.ab[0]; ab2(1) = binoCalib.calib2.camSolid.ab[1]; 999 R = Mat_<double>(3, 3, binoCalib.cam21Pose.mdata); 1000 T = Mat_<double>(3, 1, binoCalib.cam21Pose.tdata); 1001 rmsView = binoCalib.rmsView; 1002 rmsMean = binoCalib.rmsMean; 1003 errPoints = binoCalib.errPoints; 1004 utils::fs::createDirectories(utils::fs::getParent(calibPath)); save(calibPath); 1005 } 1006 else read(calibPath); 1007 1008 //2.RectificationAndUndistortion 1009 if (doRect) 1010 { 1011 //2.1 Rectification 1012 stereoRectify(Matx33d::eye(), Matx41d(), Matx33d::eye(), Matx41d(), Size(640, 480), R, T, R1, R2, Matx34d(), Matx34d(), Matx44d()); 1013 Mat_<double> viewR(3, 3); ceres::EulerAnglesToRotationMatrix(Vec3d(rectPlan[1], rectPlan[2], rectPlan[3]).val, 3, viewR.ptr<double>()); 1014 R1 = viewR * R1; 1015 R2 = viewR * R2; 1016 P1.create(3, 3); P1 << K1(0, 0) * rectPlan[0] / (1 + (simPin ? 0 : ab1(0))), 0, rectSize.width * 0.5, 0, K1(1, 1)* rectPlan[0] / (1 + (simPin ? 0 : ab1(0))), rectSize.height * 0.5, 0, 0, 1; 1017 P2.create(3, 3); P2 << K2(0, 0) * rectPlan[0] / (1 + (simPin ? 0 : ab2(0))), 0, rectSize.width * 0.5, 0, K2(1, 1)* rectPlan[0] / (1 + (simPin ? 0 : ab2(0))), rectSize.height * 0.5, 0, 0, 1; 1018 P1 = P2 = (P1 + P2) * 0.5; 1019 ModelCalibration::rectAsPinMAP(K1, D1, ab1, camModel, simPin, R1, P1, rectSize, mapx1, mapy1); 1020 ModelCalibration::rectAsPinMAP(K2, D2, ab2, camModel, simPin, R2, P2, rectSize, mapx2, mapy2); 1021 save(rectPath); 1022 1023 //2.2 Undistortion 1024 binoUnd(mapx1, mapy1, mapx2, mapy2, detDir); 1025 } 1026 } 1027 1028 public://Utils 1029 static void binoDetCHBD(string detDir, Size chSizeCross, float chSqrSide, int chDetFlag, 1030 vector<vector<Point2f>>& point2D1, vector<vector<Point2f>>& point2D2, vector<vector<Point3f>>& point3D, int chWinSize = 5, string chPattern = "*", int chShowMS = 10) 1031 { 1032 //0.Initialize 1033 point2D1.clear(); 1034 point2D2.clear(); 1035 point3D.clear(); 1036 if (chShowMS >= 0) cv::namedWindow(__FUNCTION__, WINDOW_NORMAL); 1037 1038 //1.DetectPoint2D 1039 int cnt = 0; 1040 vector<string> filePaths = Tool2D::globPaths(detDir, chPattern, 0, false); 1041 for (size_t fileId = 0; fileId < filePaths.size(); ++fileId) 1042 { 1043 //1.1 1044 Mat_<Vec3b> color = imread(filePaths[fileId]); 1045 Mat_<uchar> gray1; cvtColor(color.colRange(0, color.cols >> 1), gray1, COLOR_BGR2GRAY); 1046 Mat_<uchar> gray2; cvtColor(color.colRange(color.cols >> 1, color.cols), gray2, COLOR_BGR2GRAY); 1047 1048 //1.2 1049 vector<Point2f> corner2D1; 1050 vector<Point2f> corner2D2; 1051 bool found1 = findChessboardCorners(gray1, chSizeCross, corner2D1, chDetFlag); 1052 bool found2 = findChessboardCorners(gray2, chSizeCross, corner2D2, chDetFlag); 1053 if (found1 == false) 1054 { 1055 Mat_<uchar> tmpIma1; resize(gray1, tmpIma1, gray1.size() / 2); 1056 found1 = findChessboardCorners(tmpIma1, chSizeCross, corner2D1, chDetFlag); 1057 for (size_t k = 0; k < corner2D1.size(); ++k) corner2D1[k] *= 2; 1058 } 1059 if (found2 == false) 1060 { 1061 Mat_<uchar> tmpIma2; resize(gray2, tmpIma2, gray2.size() / 2); 1062 found2 = findChessboardCorners(tmpIma2, chSizeCross, corner2D2, chDetFlag); 1063 for (size_t k = 0; k < corner2D2.size(); ++k) corner2D2[k] *= 2; 1064 } 1065 1066 //2.4 1067 if (found1 == true && found2 == true) 1068 { 1069 cornerSubPix(gray1, corner2D1, Size(chWinSize, chWinSize), Size(-1, -1), TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 30, 0.01)); 1070 cornerSubPix(gray2, corner2D2, Size(chWinSize, chWinSize), Size(-1, -1), TermCriteria(TermCriteria::COUNT + TermCriteria::EPS, 30, 0.01)); 1071 drawChessboardCorners(color.colRange(0, color.cols >> 1), chSizeCross, corner2D1, found1); 1072 drawChessboardCorners(color.colRange(color.cols >> 1, color.cols), chSizeCross, corner2D2, found2); 1073 point2D1.push_back(corner2D1); 1074 point2D2.push_back(corner2D2); 1075 } 1076 string tips = fmt::format("{} detect {} and current vaild views {}", found1 && found2 ? "Could" : "Cannot", filePaths[fileId], cnt += (found1 && found2)); 1077 if (!found1 || !found2) spdlog::warn(tips); 1078 if (chShowMS >= 0) cv::displayStatusBar(__FUNCTION__, tips, 1000); 1079 1080 //2.5 1081 if (chShowMS < 0) continue; 1082 cv::imshow(__FUNCTION__, color); 1083 cv::waitKey(chShowMS); 1084 } if (chShowMS >= 0) cv::destroyWindow(__FUNCTION__); 1085 1086 //3. CalcPoint3D 1087 vector<Point3f> corner3D; 1088 for (int i = 0; i < chSizeCross.height; ++i) 1089 for (int j = 0; j < chSizeCross.width; ++j) 1090 corner3D.push_back(Point3f(j * chSqrSide, i * chSqrSide, 0)); 1091 point3D.assign(cnt, corner3D); 1092 } 1093 1094 static void binoUnd(Mat_<float> mapx1, Mat_<float> mapy1, Mat_<float> mapx2, Mat_<float> mapy2, string undDir, string undPattern = "*", int undShowMS = 10) 1095 { 1096 //1.Initialize 1097 string undOut = undDir + "/rst"; 1098 utils::fs::createDirectories(undOut); 1099 if (undShowMS >= 0) cv::namedWindow(__FUNCTION__, WINDOW_NORMAL); 1100 1101 //2.UndCamera 1102 vector<string> filePaths = Tool2D::globPaths(undDir, undPattern, 0, false); 1103 for (size_t fileId = 0; fileId < filePaths.size(); ++fileId) 1104 { 1105 //2.1 1106 Mat_<Vec3b> color = imread(filePaths[fileId], 1); 1107 Mat_<uchar> gray1; cvtColor(color.colRange(0, color.cols >> 1), gray1, COLOR_BGR2GRAY); 1108 Mat_<uchar> gray2; cvtColor(color.colRange(color.cols >> 1, color.cols), gray2, COLOR_BGR2GRAY); 1109 1110 //2.2 1111 Mat_<uchar> undIma1; remap(gray1, undIma1, mapx1, mapy1, INTER_LINEAR); 1112 Mat_<uchar> undIma2; remap(gray2, undIma2, mapx2, mapy2, INTER_LINEAR); 1113 1114 //2.3 1115 Mat_<uchar> catIma; 1116 hconcat(undIma1, undIma2, catIma); 1117 cv::imwrite(fmt::format("{}/und_{}", undOut, Tool2D::pathProps(filePaths[fileId])[4]), catIma); 1118 if (undShowMS >= 0) cv::displayStatusBar(__FUNCTION__, "Undistorting " + filePaths[fileId], 1000); 1119 1120 //2.4 1121 if (undShowMS < 0) continue; 1122 cv::imshow(__FUNCTION__, catIma); 1123 cv::waitKey(undShowMS); 1124 } if (undShowMS >= 0) cv::destroyWindow(__FUNCTION__); 1125 } 1126 }; 1127 }; 1128 1129 #ifdef ModelCalibrationTest 1130 int main(int argc, char** argv) { ModelCalibration::MonoAPP monoApp; monoApp.run(); ModelCalibration::BinoAPP binoApp; binoApp.run(); return 0; } 1131 int main1(int argc, char** argv) { ModelCalibration::MonoAPP monoApp; monoApp.run(); return 0; } 1132 int main2(int argc, char** argv) { ModelCalibration::BinoAPP binoApp; binoApp.run(); return 0; } 1133 #endif 1134 1135 #endif
