OpenCV-Ima3D学习日志：ProjectPoints分析与实现

  1 #include <opencv2/opencv.hpp>
  2 #include <spdlog/spdlog.h>
  3 using namespace std;
  4 using namespace cv;
  5 
  6 class ProjectPointsEx
  7 {
  8 public:
  9     using Mat121d = Matx<double, 12, 1>;
 10     static void TestMe(int argc = 0, char** argv = 0)
 11     {
 12         int N = 999;
 13         for (int k = 0; k < N; ++k)
 14         {
 15             Mat_<Vec3d> PWs(999, 1); cv::randu(PWs, 1111, 9999);
 16             Matx33d K = K.randu(111, 999); K(0, 1) = K(1, 0) = K(2, 0) = K(2, 1) = 0.; K(2, 2) = 1;
 17             Mat121d D = D.randu(0.1, 0.9);
 18             Matx31d r = r.randu(11, 99);
 19             Matx31d t = t.randu(111, 999);
 20 
 21             //2.CalcByOpenCV
 22             Mat p1s;
 23             Mat_<double> dpdKDT1;
 24             cv::projectPoints(PWs, r, t, K, D, p1s, dpdKDT1);
 25 
 26             //3.CalcByDIY
 27             Mat_<Matx21d> p2s(PWs.size());
 28             Mat_<double> _dpdKDT2[5], dpdKDT2;//dpdf1/dpdc1/dpdd1/dpdr1/dpdt1;
 29             ProjectPointsEx::run(PWs, r, t, K, D, p2s, _dpdKDT2, _dpdKDT2 + 1, _dpdKDT2 + 2, _dpdKDT2 + 3, _dpdKDT2 + 4);
 30             hconcat(_dpdKDT2, 5, dpdKDT2);
 31 
 32             //4.AnalyzeError
 33             double infps1ps2 = norm(p1s, p2s, NORM_INF);
 34             double infdpdKDT1dpdKDT2 = norm(dpdKDT1, dpdKDT2, NORM_INF);
 35 
 36             //5.PrintError
 37             cout << endl << "LoopCount: " << k << endl;
 38             if (infps1ps2 > 0 || infdpdKDT1dpdKDT2 > 0)
 39             {
 40                 cout << endl << "5.1PrintError" << endl;
 41                 cout << endl << "infps1ps2: " << infps1ps2 << endl;
 42                 cout << endl << "infdpdKDT1dpdKDT2: " << infdpdKDT1dpdKDT2 << endl;
 43                 if (0)
 44                 {
 45                     cout << endl << "5.2PrintDiff" << endl;
 46                     cout << endl << "dpdKDT1: " << endl << dpdKDT1.t() << endl;
 47                     cout << endl << "dpdKDT2: " << endl << dpdKDT2.t() << endl;
 48                     cout << endl << "p1s: " << endl << p1s << endl;
 49                     cout << endl << "p2s: " << endl << p2s << endl;
 50                     cout << endl << "5.3PrintOthers" << endl;
 51                     cout << endl << "PWs: " << endl << PWs << endl;
 52                     cout << endl << "K: " << endl << K << endl;
 53                     cout << endl << "D: " << endl << D.t() << endl;
 54                     //cout << endl << "r-degree: " << endl << r.t() << endl << degree.t() << endl;
 55                     cout << endl << "t: " << endl << t.t() << endl;
 56                 }
 57                 cout << endl << "Press any key to continue" << endl;
 58                 std::getchar();
 59             }
 60         }
 61     }
 62 
 63 public:
 64     static void run(Mat_<Matx31d> PWs, Matx31d r, Matx31d t, Matx33d K, Mat121d D, Mat_<Matx21d>& pDs,
 65         Mat_<double>* dpdr = 0, Mat_<double>* dpdt = 0, Mat_<double>* dpdF = 0, Mat_<double>* dpdC = 0, Mat_<double>* dpdD = 0, Mat_<double>* dpdPW = 0,
 66         bool fixFxFyRatio = false, int derivRotWay = 0)
 67     {
 68         //1.Projection params
 69         double fx = K.val[0];
 70         double fy = K.val[4];
 71         double cx = K.val[2];
 72         double cy = K.val[5];
 73 
 74         //2.Distortion params
 75         double k1 = D.val[0];
 76         double k2 = D.val[1];
 77         double p1 = D.val[2];
 78         double p2 = D.val[3];
 79         double k3 = D.val[4];
 80         double k4 = D.val[5];
 81         double k5 = D.val[6];
 82         double k6 = D.val[7];
 83         double s1 = D.val[8];
 84         double s2 = D.val[9];
 85         double s3 = D.val[10];
 86         double s4 = D.val[11];
 87         //double a = D.val[12],  b = D.val[13];
 88 
 89         //3.Translation params
 90         double tx = t.val[0];
 91         double ty = t.val[1];
 92         double tz = t.val[2];
 93 
 94         //4.Rotation params
 95         Matx33d R;
 96         Mat_<double> _dRdr; cv::Rodrigues(r, R, dpdr ? _dRdr : noArray());
 97         _dRdr = _dRdr.t(); Matx<double, 9, 3> dRdr; if (dpdr) memcpy(dRdr.val, _dRdr.ptr<double>(), sizeof(dRdr));
 98 
 99         //5.ReProjection
100         for (int k = 0; k < PWs.total(); ++k)
101         {
102             //5.1 WorldCoordinate
103             double X = PWs(k)(0);
104             double Y = PWs(k)(1);
105             double Z = PWs(k)(2);
106 
107             //5.2 CameraCoordinate
108             double x = R.val[0] * X + R.val[1] * Y + R.val[2] * Z + tx;
109             double y = R.val[3] * X + R.val[4] * Y + R.val[5] * Z + ty;
110             double z = R.val[6] * X + R.val[7] * Y + R.val[8] * Z + tz;
111 
112             //5.3 StandardPhysicsCoordinate
113             double iz = z ? 1. / z : 1;
114             double xc = x * iz;
115             double yc = y * iz;
116 
117             //5.4 DistortionPhysicsCoordinate
118             double xc2 = xc * xc;
119             double yc2 = yc * yc;
120             double d2 = xc2 + yc2;
121             double xcyc = 2 * xc * yc;
122             double d4 = d2 * d2;
123             double d6 = d2 * d4;
124             double d2xc2 = d2 + 2 * xc2;
125             double d2yc2 = d2 + 2 * yc2;
126             double nu = 1 + k1 * d2 + k2 * d4 + k3 * d6;
127             double de = 1. / (1 + k4 * d2 + k5 * d4 + k6 * d6);
128             double xd = xc * nu * de + p1 * xcyc + p2 * d2xc2 + s1 * d2 + s2 * d4;
129             double yd = yc * nu * de + p2 * xcyc + p1 * d2yc2 + s3 * d2 + s4 * d4;
130 
131             //5.5 DistortionPixelCoordinate
132             pDs(k) = Matx21d(xd * fx + cx, yd * fy + cy);
133 
134             //5.6 dpdF: derivatives with respect to the focal lengths
135             if (dpdF)
136             {
137                 if (dpdF->empty()) dpdF->create(PWs.rows << 1, 2);
138                 double* ptr = dpdF->ptr<double>(k << 1);
139                 ptr[0] = xd; ptr[1] = 0;
140                 ptr[2] = 0; ptr[3] = yd;
141             }
142 
143             //5.7 dpdC: derivatives with respect to the principal point
144             if (dpdC)
145             {
146                 if (dpdC->empty()) dpdC->create(PWs.rows << 1, 2);
147                 double* ptr = dpdC->ptr<double>(k << 1);
148                 ptr[0] = 1.; ptr[1] = 0;
149                 ptr[2] = 0; ptr[3] = 1.;
150             }
151 
152             //5.8 dpdD: derivatives with respect to the distortion coefficients
153             if (dpdD)
154             {
155                 if (dpdD->empty()) dpdD->create(PWs.rows << 1, 12);
156                 double* ptr = dpdD->ptr<double>(k << 1);
157 
158                 Matx22d dpDdpCD; dpDdpCD <<
159                     fx, 0,
160                     0, fy;
161 
162                 Matx<double, 2, 12> dpCDdD; dpCDdD <<
163                     xc * d2 * de, xc* d4* de, xcyc, d2xc2, xc* d6* de, -xc * d2 * nu * de * de, -xc * d4 * nu * de * de, -xc * d6 * nu * de * de, d2, d4, 0, 0,
164                     yc* d2* de, yc* d4* de, d2yc2, xcyc, yc* d6* de, -yc * d2 * nu * de * de, -yc * d4 * nu * de * de, -yc * d6 * nu * de * de, 0, 0, d2, d4;
165 
166                 Matx<double, 2, 12> dpDdD = dpDdpCD * dpCDdD;
167                 memcpy(ptr, dpDdD.val, 24 * sizeof(double));
168             }
169 
170             //5.9 dpdr&dpdt: derivatives with respect to the pose
171             if (dpdr || dpdt || dpdPW)
172             {
173                 Matx22d dpDdpCD;
174                 dpDdpCD << fx, 0, 0, fy;
175 
176                 Matx<double, 2, 5> dpCDdh; dpCDdh <<
177                     xc * de, -xc * nu * de * de, p2 + s1 + 2 * s2 * d2, nu* de + 2 * p1 * yc + 4 * p2 * xc, 2 * p1 * xc,
178                     yc* de, -yc * nu * de * de, p1 + s3 + 2 * s4 * d2, 2 * p2 * yc, nu* de + 2 * p2 * xc + 4 * p1 * yc;
179 
180                 Matx<double, 5, 2> dhdpC; dhdpC <<
181                     xc * (2 * k1 + 4 * k2 * d2 + 6 * k3 * d4), yc* (2 * k1 + 4 * k2 * d2 + 6 * k3 * d4),
182                     xc* (2 * k4 + 4 * k5 * d2 + 6 * k6 * d4), yc* (2 * k4 + 4 * k5 * d2 + 6 * k6 * d4),
183                     2 * xc, 2 * yc,
184                     1, 0,
185                     0, 1;
186 
187                 Matx23d dpCdPC; dpCdPC <<
188                     iz, 0, -xc * iz,
189                     0, iz, -yc * iz;
190 
191                 if (dpdt)//dpdt: derivatives with respect to the translation vector
192                 {
193                     if (dpdt->empty()) dpdt->create(PWs.rows << 1, 3);
194                     double* ptr = dpdt->ptr<double>(k << 1);
195 
196                     Matx23d dpDdt = dpDdpCD * dpCDdh * dhdpC * dpCdPC;
197                     memcpy(ptr, dpDdt.val, 6 * sizeof(double));
198                 }
199 
200                 if (dpdr)//dpdr: derivatives with respect to the rotation vector
201                 {
202                     if (dpdr->empty()) dpdr->create(PWs.rows << 1, 3);
203                     double* ptr = dpdr->ptr<double>(k << 1);
204 
205                     Matx23d dpDdr;
206                     if (derivRotWay == 0)//(1)by exponential map
207                     {
208                         Matx<double, 3, 9> dPCdR; dPCdR <<
209                             X, Y, Z, 0, 0, 0, 0, 0, 0,
210                             0, 0, 0, X, Y, Z, 0, 0, 0,
211                             0, 0, 0, 0, 0, 0, X, Y, Z;
212                         dpDdr = dpDdpCD * dpCDdh * dhdpC * dpCdPC * dPCdR * dRdr;
213                     }
214                     else
215                     {
216                         Matx33d _skewPC
217                         (
218                             0, (z - tz), -(y - ty),
219                             -(z - tz), 0, (x - tx),
220                             (y - ty), -(x - tx), 0
221                         );
222                         if (derivRotWay == 1)//(2)by BCH formula
223                         {
224                             double theta = sqrt(r.val[0] * r.val[0] + r.val[1] * r.val[1] + r.val[2] * r.val[2]);
225                             double itheta = 1 / theta;
226                             double sn = sin(theta);
227                             double cs1 = 1 - cos(theta);
228                             Matx31d n(r.val[0] * itheta, r.val[1] * itheta, r.val[2] * itheta);
229                             Matx33d nskew(0, -n.val[2], n.val[1], n.val[2], 0, -n.val[0], -n.val[1], n.val[0], 0);
230                             Matx33d Jl = itheta * sn * Matx33d::eye() + itheta * cs1 * nskew + (1 - itheta * sn) * n * n.t();
231                             dpDdr = dpDdpCD * dpCDdh * dhdpC * dpCdPC * _skewPC * Jl;
232                         }
233                         else if (derivRotWay == 2) dpDdr = dpDdpCD * dpCDdh * dhdpC * dpCdPC * _skewPC; //(3)by perturbance for reduced LocalParameterization of ceresSolver
234                     }
235 
236                     memcpy(ptr, dpDdr.val, 6 * sizeof(double));
237 
238                     if (derivRotWay == 3) //(4)by perturbance for general LocalParameterization of ceresSolver
239                     {
240                         if (dpdr->empty() || dpdr->cols != 9) dpdr->create(PWs.rows << 1, 9);
241                         double* ptr = dpdr->ptr<double>(k << 1);
242                         Matx<double, 3, 9> dPCdR; dPCdR <<
243                             X, Y, Z, 0, 0, 0, 0, 0, 0,
244                             0, 0, 0, X, Y, Z, 0, 0, 0,
245                             0, 0, 0, 0, 0, 0, X, Y, Z;
246                         Matx<double, 2, 9> dpDdR = dpDdpCD * dpCDdh * dhdpC * dpCdPC * dPCdR;
247                         memcpy(ptr, dpDdR.val, 18 * sizeof(double));
248                     }
249                 }
250 
251                 if (dpdPW)//dpdPW: derivatives with respect to the worldcoordinate
252                 {
253                     if (dpdPW->empty()) dpdPW->create(PWs.rows << 1, 3);
254                     double* ptr = dpdPW->ptr<double>(k << 1);
255 
256                     Matx23d dpDdPW = dpDdpCD * dpCDdh * dhdpC * dpCdPC * R;
257                     memcpy(ptr, dpDdPW.val, 6 * sizeof(double));
258                 }
259             }
260         }
261     }
262 };
263 
264 int main(int argc, char** argv) { ProjectPointsEx::TestMe(argc, argv); return 0; }