Ceres中的扰动导数主要通过是继承ceres::LocalParameterization来实现。当然,LocalParameterization本身具有更远大的使命或者说具有更广义的功能,扰动导数只是其特定的应用,也是其典型应用。
谈到扰动求导自然避不开李代数的知识,但这不属于本文的范围,可参见相关理论或我之前的文章,本文假设你已具备了李代数的知识。如果实在没有,也不是不行,正如前面所说,扰动导数只是LocalParameterization的特定应用,本文虽然题为扰动导数巴拉巴拉,但大部分内容是在阐述链式求导、广义导数及如何使用LocalParameterization。
1.链式求导
以二维为例,以x为自变量,y为自变量,Y为两者之间的映射关系,则世间二维关系都可以建模为Y(x,y)=0。
如果映射关系很简单,则根据多元微分理论,可得y对x的导数为dy/dx=∂Y(x,y)/∂y / ∂Y(x,y)/∂x。
然而,实际中的二维关系或者说几何物理模型通常并不这么简单,而是存在多个中间关系,或者说通过多个中间关系可以更好地表达实际意义。
于是,实际的二维关系可能是这样的:A(x,a)=0, B(a, b)=0, C(b, c)=0, Y(c, y)=0。式中,小写字母是中间变量,大写字母是映射关系。根据链式链式求导方法,可得dy/dx=dy/dc*dc/db*db/da*da/dx。这里是四个链式结点。
Ceres提供了一种实现,可以将所有链式结点分成前后两部分实现,前一部分由残差类(对自动求导而言)或CostFunction::Evaluate(对手动求导而言)实现,后一分部分由LocalParameterization::ComuputeJacobian实现,前一部分导数命名为GlobalJacobian,后一部分命名为Jacobian,整个导数命名为LocalJacobian,即LocalJacobian=GlobaJacobian*Jacobian。
当然,你也可以在前一部分实现整个导数,则Jacobian为单位矩阵。或在后一部分实现全部导数,则GloabalJacobian为单位矩阵。
2.广义导数
以上链式环节中,默认都是常规求导,即因变量增量与自变量增量的比值的极限,我们暂且称之为加法导数,以与后面的乘法导数(即扰动导数)呼应。
自近代物理后,加法导数以不能满足物理学求解的需求,于是出了乘法导数(即扰动导数),具体怎么回事,参见李代数相关理论。这里,主要是想说以上链式结点中,每个结点都可以是不同的求导方式,尽管当前数学主是加法导数和乘法导数,但我们可以根据实际需求自定义某种导数(这是要成为数学家的节奏啦),我这里就称之为广义导数。
Ceres将链式求导分为两段的目的就是为了能使用两种导数,从而减小计算量。因为不同的结点使用不同的求导方式可以减小计算,如旋转矩阵对旋转向量的乘法导数的计算量远小于加法导数。
此外,利用不同的求导方式所得的雅可比矩阵来计算函数的下降方向的增量(相关理论参见梯度下降法、牛顿法、高斯牛顿法及LM法),此增量与初值的合并方式也因求导方式而异,默认是加法。Ceres提供了LocalParameterization::Plus来实现非加法的合并方式。所以,无论前一部分是什么求导方式,只要最终求导方式(即因变量对自变量的导数不是加法导数)不是加法导数,则一定要重载LocalParameterization::Plus。
3.曲切空间
有没有发现,上面所举例的中间关系,也是二维关系,而实际中通常是更高维的关系或更低维的关系。如自变量是10,因变量是5维,中间变量可能有3维、7维、30维、700维,这都取决于实际物理几何模型。如针孔成像模型中,世界坐标作为自变量是3维,图像坐标作为因变量是2维的,中间变量归一化物理坐标是2维的,中间变量相机坐标是3维的。或者将世界坐标当作已知量,将旋转当作未知量,则旋转向量作为自变量是3维,中间变量旋转矩阵是9维的,中间变量相机坐标是3维的,中间变量归一化物理坐标是2维的。
可见,自变量和因变量的维度与中间变量的维度没有太大关系,关键还是取决于如何建模型。在不影响几何物理意义的情况下,当然是越少越好,这可以极大的减少计算量,也符合大道至简的规则。
于是,根据变量的维度,我们相对地可以定义曲空间和切空间。在表示能表示同一物理几何意义的情况下,将最低维度的空间或更低维度的空间称为切空间,将最高维度的空间或更高维度的空间称为曲空间。
典型地,对旋转向量和旋转四元数,旋转向量位于切空间,旋转四元数位于曲空间;对旋转向量和旋转矩阵,旋转向量位于切空间,旋转矩阵位于曲空间;对旋转四元数和旋转矩阵,旋转四元数位于切空间,旋转矩阵位于曲空间。
4.使用样例
提供三个使用样例,封装为两个类:
OptimizeRt:基于单次观察,优化此次观察的外参,定义了ProjectionModelRt、ProjectionCostRt、LocalParamRWithGeneralJ、LocalParamRWithIdentJ四个类。
OptimizeKDRt:基于多次观察,优化相机内参和所有观察的外参,定义了ProjectionModelKDRt和ProjectionCostKDRt两个类并使用了OptimizeRt:: LocalParamRWithGeneralJ和OptimizeRt::LocalParamRWithIdentJ两个类。
OptimzedRtWithNormazelized: 正如其名,基归一化坐标来优化外参,比较这两个类可发现它们的联系和区别,但归一化坐标是用cv::projectPoints生成而没用cv::undistortPoints(用之不能收敛),以排除数据本身仿真且cv::undistortPoints非闭环实现的干扰。
其中类MotionSim用于生成仿真数据,使用说明参见《CV学习日志:CV开发之三维仿真器》。
存在可能不收敛的测试结果,属于正常现象,可修改初值精度来增加收敛性。
以下是详细代码,依赖于C++14、OpenCV4.x、Ceres和Spdlog。
![](https://images.cnblogs.com/OutliningIndicators/ContractedBlock.gif)
1 #include <opencv2/opencv.hpp> 2 #include <opencv2/viz.hpp> 3 #include <spdlog/spdlog.h> 4 #include <ceres/ceres.h> 5 using namespace std; 6 using namespace cv; 7 8 class MotionSim 9 { 10 public: 11 static void TestMe(int argc, char** argv) 12 { 13 MotionSim motionSim(false); 14 motionSim.camFovX = 45; 15 motionSim.camFovY = 30; 16 motionSim.camRand = 10; 17 motionSim.enableVerbose = false; 18 motionSim.runMotion(false, false, 7); 19 motionSim.visMotion(); 20 } 21 22 public: 23 struct MotionView 24 { 25 Mat_<double> r = Mat_<double>(3, 1); 26 Mat_<double> t = Mat_<double>(3, 1); 27 Mat_<double> q = Mat_<double>(4, 1); 28 Mat_<double> rt = Mat_<double>(6, 1); 29 Mat_<double> radian = Mat_<double>(3, 1); 30 Mat_<double> degree = Mat_<double>(3, 1); 31 Mat_<double> R = Mat_<double>(3, 3); 32 Mat_<double> T = Mat_<double>(3, 4); 33 Mat_<double> K; 34 Mat_<double> D; 35 Mat_<Vec3d> point3D; 36 Mat_<Vec2d> point2D; 37 Mat_<int> point3DIds; 38 string print(string savePath = "") 39 { 40 string str; 41 str += fmt::format("r: {}\n", cvarr2str(r.t())); 42 str += fmt::format("t: {}\n", cvarr2str(t.t())); 43 str += fmt::format("q: {}\n", cvarr2str(q.t())); 44 str += fmt::format("rt: {}\n", cvarr2str(rt.t())); 45 str += fmt::format("radian: {}\n", cvarr2str(radian.t())); 46 str += fmt::format("degree: {}\n", cvarr2str(degree.t())); 47 str += fmt::format("R: {}\n", cvarr2str(R)); 48 str += fmt::format("T: {}\n", cvarr2str(T)); 49 str += fmt::format("K: {}\n", cvarr2str(K)); 50 str += fmt::format("D: {}\n", cvarr2str(D.t())); 51 if (savePath.empty() == false) { FILE* out = fopen(savePath.c_str(), "w"); fprintf(out, str.c_str()); fclose(out); } 52 return str; 53 } 54 }; 55 static string cvarr2str(InputArray v) 56 { 57 Ptr<Formatted> fmtd = cv::format(v, Formatter::FMT_DEFAULT); 58 string dst; fmtd->reset(); 59 for (const char* str = fmtd->next(); str; str = fmtd->next()) dst += string(str); 60 return dst; 61 } 62 static void euler2matrix(double e[3], double R[9], bool forward = true, int argc = 0, char** argv = 0) 63 { 64 if (argc > 0) 65 { 66 int N = 999; 67 for (int k = 0; k < N; ++k)//OpenCV not better than DIY 68 { 69 //1.GenerateData 70 Matx31d radian0 = radian0.randu(-3.14159265358979323846, 3.14159265358979323846); 71 Matx33d R; euler2matrix(radian0.val, R.val, true); 72 const double deg2rad = 3.14159265358979323846 * 0.0055555555555555556; 73 const double rad2deg = 180 * 0.3183098861837906715; 74 75 //2.CalcByOpenCV 76 Matx31d radian1 = cv::RQDecomp3x3(R, Matx33d(), Matx33d()) * deg2rad; 77 78 //3.CalcByDIY 79 Matx31d radian2; euler2matrix(R.val, radian2.val, false); 80 81 //4.AnalyzeError 82 double infRadian0Radian1 = norm(radian0, radian1, NORM_INF); 83 double infRadian1Radian2 = norm(radian1, radian2, NORM_INF); 84 85 //5.PrintError 86 cout << endl << "LoopCount: " << k << endl; 87 if (infRadian0Radian1 > 0 || infRadian1Radian2 > 0) 88 { 89 cout << endl << "5.1PrintError" << endl; 90 cout << endl << "infRadian0Radian1: " << infRadian0Radian1 << endl; 91 cout << endl << "infRadian1Radian2: " << infRadian1Radian2 << endl; 92 if (0) 93 { 94 cout << endl << "5.2PrintDiff" << endl; 95 cout << endl << "radian0-degree0:" << endl << radian0.t() << endl << radian0.t() * rad2deg << endl; 96 cout << endl << "radian1-degree1:" << endl << radian1.t() << endl << radian1.t() * rad2deg << endl; 97 cout << endl << "radian2-degree2:" << endl << radian2.t() << endl << radian2.t() * rad2deg << endl; 98 cout << endl << "5.3PrintOthers" << endl; 99 cout << endl << "R:" << endl << R << endl; 100 } 101 cout << endl << "Press any key to continue" << endl; std::getchar(); 102 } 103 } 104 return; 105 } 106 if (forward)//check with 3D Rotation Converter 107 { 108 double sinR = std::sin(e[0]); 109 double sinP = std::sin(e[1]); 110 double sinY = std::sin(e[2]); 111 double cosR = std::cos(e[0]); 112 double cosP = std::cos(e[1]); 113 double cosY = std::cos(e[2]); 114 115 //RPY indicates: first Yaw aroundZ, second Pitch aroundY, third Roll aroundX 116 R[0] = cosY * cosP; R[1] = cosY * sinP * sinR - sinY * cosR; R[2] = cosY * sinP * cosR + sinY * sinR; 117 R[3] = sinY * cosP; R[4] = sinY * sinP * sinR + cosY * cosR; R[5] = sinY * sinP * cosR - cosY * sinR; 118 R[6] = -sinP; R[7] = cosP * sinR; R[8] = cosP * cosR; 119 } 120 else 121 { 122 double vs1 = std::abs(R[6] - 1.); 123 double vs_1 = std::abs(R[6] + 1.); 124 if (vs1 > 1E-9 && vs_1 > 1E-9) 125 { 126 e[2] = std::atan2(R[3], R[0]); //Yaw aroundZ 127 e[1] = std::asin(-R[6]);//Pitch aroundY 128 e[0] = std::atan2(R[7], R[8]); //Roll aroundX 129 } 130 else if (vs_1 <= 1E-9) 131 { 132 e[2] = 0; //Yaw aroundZ 133 e[1] = 3.14159265358979323846 * 0.5;//Pitch aroundY 134 e[0] = e[2] + atan2(R[1], R[2]); //Roll aroundX 135 } 136 else 137 { 138 e[2] = 0; //Yaw aroundZ 139 e[1] = -3.14159265358979323846 * 0.5;//Pitch aroundY 140 e[0] = -e[2] + atan2(-R[1], -R[2]); //Roll aroundX 141 } 142 } 143 }; 144 static void quat2matrix(double q[4], double R[9], bool forward = true) 145 { 146 if (forward)//refer to qglviwer 147 { 148 double L1 = std::sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); 149 if (std::abs(L1 - 1) > 1E-9) { std::printf("Not uint quaternion: NormQ=%.9f\n", L1); abort(); } 150 151 double xx = 2.0 * q[1] * q[1]; 152 double yy = 2.0 * q[2] * q[2]; 153 double zz = 2.0 * q[3] * q[3]; 154 155 double xy = 2.0 * q[1] * q[2]; 156 double xz = 2.0 * q[1] * q[3]; 157 double wx = 2.0 * q[1] * q[0]; 158 159 double yz = 2.0 * q[2] * q[3]; 160 double wy = 2.0 * q[2] * q[0]; 161 162 double wz = 2.0 * q[3] * q[0]; 163 164 R[0] = 1.0 - yy - zz; 165 R[4] = 1.0 - xx - zz; 166 R[8] = 1.0 - xx - yy; 167 168 R[1] = xy - wz; 169 R[3] = xy + wz; 170 171 R[2] = xz + wy; 172 R[6] = xz - wy; 173 174 R[5] = yz - wx; 175 R[7] = yz + wx; 176 } 177 else 178 { 179 double onePlusTrace = 1.0 + R[0] + R[4] + R[8];// Compute one plus the trace of the matrix 180 if (onePlusTrace > 1E-9) 181 { 182 double s = sqrt(onePlusTrace) * 2.0; 183 double is = 1 / s; 184 q[0] = 0.25 * s; 185 q[1] = (R[7] - R[5]) * is; 186 q[2] = (R[2] - R[6]) * is; 187 q[3] = (R[3] - R[1]) * is; 188 } 189 else 190 { 191 std::printf("1+trace(R)=%.9f is too small and (R11,R22,R33)=(%.9f,%.9f,%.9f)\n", onePlusTrace, R[0], R[4], R[8]); 192 if ((R[0] > R[4]) && (R[0] > R[8]))//max(R00, R11, R22)=R00 193 { 194 double s = sqrt(1.0 + R[0] - R[4] - R[8]) * 2.0; 195 double is = 1 / s; 196 q[0] = (R[5] - R[7]) * is; 197 q[1] = 0.25 * s; 198 q[2] = (R[1] + R[3]) * is; 199 q[3] = (R[2] + R[6]) * is; 200 } 201 else if (R[4] > R[8])//max(R00, R11, R22)=R11 202 { 203 double s = sqrt(1.0 - R[0] + R[4] - R[8]) * 2.0; 204 double is = 1 / s; 205 q[0] = (R[2] - R[6]) * is; 206 q[1] = (R[1] + R[3]) * is; 207 q[2] = 0.25 * s; 208 q[3] = (R[5] + R[7]) * is; 209 } 210 else//max(R00, R11, R22)=R22 211 { 212 double s = sqrt(1.0 - R[0] - R[4] + R[8]) * 2.0; 213 double is = 1 / s; 214 q[0] = (R[1] - R[3]) * is; 215 q[1] = (R[2] + R[6]) * is; 216 q[2] = (R[5] + R[7]) * is; 217 q[3] = 0.25 * s; 218 } 219 } 220 double L1 = std::sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); 221 if (L1 < 1e-9) { std::printf("Wrong rotation matrix: NormQ=%.9f\n", L1); abort(); } 222 else { L1 = 1 / L1; q[0] *= L1; q[1] *= L1; q[2] *= L1; q[3] *= L1; } 223 } 224 } 225 static void vec2quat(double r[3], double q[4], bool forward = true) 226 { 227 if (forward)//refer to qglviwer 228 { 229 double theta = std::sqrt(r[0] * r[0] + r[1] * r[1] + r[2] * r[2]); 230 if (std::abs(theta) < 1E-9) 231 { 232 q[0] = 1; q[1] = q[2] = q[3] = 0; 233 std::printf("Rotation approximates zero: Theta=%.9f\n", theta); 234 }; 235 236 q[0] = std::cos(theta * 0.5); 237 double ss = std::sin(theta * 0.5) / theta; 238 q[1] = r[0] * ss; 239 q[2] = r[1] * ss; 240 q[3] = r[2] * ss; 241 } 242 else 243 { 244 double L1 = std::sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); 245 if (std::abs(L1 - 1) > 1E-9) { std::printf("Not uint quaternion: NormQ=%.9f\n", L1); abort(); } 246 247 double theta = 2 * acos(q[0]); 248 if (theta > 3.14159265358979323846) theta = 2 * 3.14159265358979323846 - theta; 249 double thetaEx = theta / std::sin(theta * 0.5); 250 r[0] = q[1] * thetaEx; 251 r[1] = q[2] * thetaEx; 252 r[2] = q[3] * thetaEx; 253 } 254 } 255 static void vec2matrix(double r[3], double R[9], bool forward = true, int argc = 0, char** argv = 0) 256 { 257 if (argc > 0) 258 { 259 int N = 999; 260 for (int k = 0; k < N; ++k) //refer to the subsequent article for more details 261 { 262 //1.GenerateData 263 Matx31d r0 = r0.randu(-999, 999); 264 Matx33d R0; cv::Rodrigues(r0, R0); 265 266 //2.CalcByOpenCV 267 Matx33d R1; 268 Matx31d r1; 269 cv::Rodrigues(r0, R1); 270 cv::Rodrigues(R0, r1); 271 272 //3.CalcByDIY 273 Matx33d R2; 274 Matx31d r2; 275 vec2matrix(r0.val, R2.val, true); 276 vec2matrix(r2.val, R0.val, false); 277 278 //4.AnalyzeError 279 double infR1R2 = norm(R1, R2, NORM_INF); 280 double infr1r2 = norm(r1, r2, NORM_INF); 281 282 //5.PrintError 283 cout << endl << "LoopCount: " << k << endl; 284 if (infR1R2 > 1E-12 || infr1r2 > 1E-12) 285 { 286 cout << endl << "5.1PrintError" << endl; 287 cout << endl << "infR1R2: " << infR1R2 << endl; 288 cout << endl << "infr1r2: " << infr1r2 << endl; 289 if (0) 290 { 291 cout << endl << "5.2PrintDiff" << endl; 292 cout << endl << "R1: " << endl << R1 << endl; 293 cout << endl << "R2: " << endl << R2 << endl; 294 cout << endl; 295 cout << endl << "r1: " << endl << r1.t() << endl; 296 cout << endl << "r2: " << endl << r2.t() << endl; 297 cout << endl << "5.3PrintOthers" << endl; 298 } 299 cout << endl << "Press any key to continue" << endl; std::getchar(); 300 } 301 } 302 return; 303 } 304 305 if (forward) 306 { 307 double theta = std::sqrt(r[0] * r[0] + r[1] * r[1] + r[2] * r[2]); 308 if (theta < 1E-9) 309 { 310 R[0] = R[4] = R[8] = 1.0; 311 R[1] = R[2] = R[3] = R[5] = R[6] = R[7] = 0.0; 312 std::printf("Rotation approximates zero: Theta=%.9f\n", theta); 313 return; 314 } 315 double cs = cos(theta); 316 double sn = sin(theta); 317 double itheta = 1. / theta; 318 double cs1 = 1 - cs; 319 double nx = r[0] * itheta; 320 double ny = r[1] * itheta; 321 double nz = r[2] * itheta; 322 323 double nxnx = nx * nx, nyny = ny * ny, nznz = nz * nz; 324 double nxny = nx * ny, nxnz = nx * nz, nynz = ny * nz; 325 double nxsn = nx * sn, nysn = ny * sn, nzsn = nz * sn; 326 327 R[0] = nxnx * cs1 + cs; 328 R[3] = nxny * cs1 + nzsn; 329 R[6] = nxnz * cs1 - nysn; 330 331 R[1] = nxny * cs1 - nzsn; 332 R[4] = nyny * cs1 + cs; 333 R[7] = nynz * cs1 + nxsn; 334 335 R[2] = nxnz * cs1 + nysn; 336 R[5] = nynz * cs1 - nxsn; 337 R[8] = nznz * cs1 + cs; 338 339 if (0) 340 { 341 Mat_<double> dRdu({ 9, 4 }, { 342 2 * nx * cs1, 0, 0, (nxnx - 1) * sn, 343 ny * cs1, nx * cs1, -sn, nxny * sn - nz * cs, 344 nz * cs1, sn, nx * cs1, nxnz * sn + ny * cs, 345 ny * cs1, nx * cs1, sn, nxny * sn + nz * cs, 346 0, 2 * ny * cs1, 0, (nyny - 1) * sn, 347 -sn, nz * cs1, ny * cs1, nynz * sn - nx * cs, 348 nz * cs1, -sn, nx * cs1, nxnz * sn - ny * cs, 349 sn, nz * cs1, ny * cs1, nynz * sn + nx * cs, 350 0, 0, 2 * nz * cs1, (nznz - 1) * sn }); 351 352 Mat_<double> dudv({ 4, 4 }, { 353 itheta, 0, 0, -nx * itheta, 354 0, itheta, 0, -ny * itheta, 355 0, 0, itheta, -nz * itheta, 356 0, 0, 0, 1 }); 357 358 Mat_<double> dvdr({ 4, 3 }, { 359 1, 0, 0, 360 0, 1, 0, 361 0, 0, 1, 362 nx, ny, nz }); 363 364 Mat_<double> Jacobian = dRdu * dudv * dvdr;//rows=9 cols=3 365 } 366 } 367 else 368 { 369 double sx = R[7] - R[5]; 370 double sy = R[2] - R[6]; 371 double sz = R[3] - R[1]; 372 double sn = sqrt(sx * sx + sy * sy + sz * sz) * 0.5; 373 double cs = (R[0] + R[4] + R[8] - 1) * 0.5; 374 double theta = acos(cs); 375 double ss = 2 * sn; 376 double iss = 1. / ss; 377 double tss = theta * iss; 378 r[0] = tss * sx; 379 r[1] = tss * sy; 380 r[2] = tss * sz; 381 382 if (0) 383 { 384 Mat_<double> drdu({ 3, 4 }, { 385 tss, 0, 0, (sn - theta * cs) * iss * iss * sx * 2, 386 0, tss, 0, (sn - theta * cs) * iss * iss * sy * 2, 387 0, 0, tss, (sn - theta * cs) * iss * iss * sz * 2 }); 388 389 Mat_<double> dudR({ 4, 9 }, { 390 0, 0, 0, 0, 0, -1, 0, 1, 0, 391 0, 0, 1, 0, 0, 0, -1, 0, 0, 392 0, -1, 0, 1, 0, 0, 0, 0, 0, 393 -iss, 0, 0, 0, -iss, 0, 0, 0, -iss }); 394 395 Mat_<double> Jacobian = drdu * dudR;//rows=3 cols=9 396 } 397 } 398 } 399 400 private: 401 const int nHorPoint3D = 100; 402 const int nVerPoint3D = 100; 403 const double varPoint3DXY = 10.; 404 const double minPoint3DZ = 1.; 405 const double maxPoint3DZ = 99.; 406 const double minCamZ = 101.; 407 const double maxCamZ = 150.; 408 const double varCamDegree = 10.; 409 Mat_<Vec3d> allPoint3D = Mat_<Vec3d>(nVerPoint3D * nHorPoint3D, 1); 410 Mat_<double> allPoint3DZ = Mat_<double>(nVerPoint3D * nHorPoint3D, 1); 411 Mat_<double> K; 412 Mat_<double> D; 413 const double deg2rad = 3.14159265358979323846 * 0.0055555555555555556; 414 const double rad2deg = 180 * 0.3183098861837906715; 415 416 public: 417 int camRows = 480; 418 int camCols = 640; 419 int camFovY = 90; 420 int camFovX = 90; 421 int camRand = 10;//append random[0,camRand] to camera intrinsics 422 int nCamDist = 5;//refer to opencv for value domain 423 int nMinMotion = 32; // no less than X motion views 424 int nMaxMotion = INT_MAX; // no more than X motion views 425 int nPoint2DThenExit = 32;//exit when less than X pixies 426 int rotMode = 1 + 2 + 4;//0=noRot 1=xAxis 2=yAxis 4=zAxis 427 bool noTrans = false;//translate or not while motion 428 bool world2D = false;//planar world or not 429 bool rndSeek = true;//use random seek or not 430 bool enableVerbose = false;//check motions one by one or not 431 vector<MotionView> motionViews;//World Information: RightX, FrontY, DownZ 432 MotionSim(bool run = true, bool world2D0 = false, bool noTrans0 = false, int rotMode0 = 7) { if (run) runMotion(world2D0, noTrans0, rotMode0); } 433 434 public: 435 void runMotion(bool world2D0 = false, bool noTrans0 = false, int rotMode0 = 7) 436 { 437 world2D = world2D0; 438 noTrans = noTrans0; 439 rotMode = rotMode0; 440 motionViews.clear(); 441 if (rndSeek) cv::setRNGSeed(clock()); 442 while (motionViews.size() < nMinMotion) 443 { 444 //1.GetAllPoint3D 445 if (world2D) allPoint3DZ = 0.; 446 else cv::randu(allPoint3DZ, -maxPoint3DZ, -minPoint3DZ);//DownZ 447 for (int i = 0, k = 0; i < nVerPoint3D; ++i) 448 for (int j = 0; j < nHorPoint3D; ++j, ++k) 449 allPoint3D(k) = Vec3d((j + cv::randu<double>()) * varPoint3DXY, (i + cv::randu<double>()) * varPoint3DXY, allPoint3DZ(i, j)); 450 451 //2.GetCamParams 452 double camFx = camCols / 2. / std::tan(camFovX / 2. * deg2rad) + cv::randu<double>() * camRand; 453 double camFy = camRows / 2. / std::tan(camFovY / 2. * deg2rad) + cv::randu<double>() * camRand; 454 double camCx = camCols / 2. + cv::randu<double>() * camRand; 455 double camCy = camRows / 2. + cv::randu<double>() * camRand; 456 K.create(3, 3); K << camFx, 0, camCx, 0, camFy, camCy, 0, 0, 1; 457 D.create(nCamDist, 1); cv::randu(D, -1.0, 1.0); 458 459 //3.GetAllMotionView 460 motionViews.clear(); 461 for (int64 k = 0; ; ++k) 462 { 463 //3.1 JoinCamParams 464 MotionView view; 465 view.K = K.clone(); 466 view.D = D.clone(); 467 468 //3.2 GetCamTrans 469 if (k == 0) view.t(0) = view.t(1) = 0; 470 else 471 { 472 view.t(0) = motionViews[k - 1].t(0) + cv::randu<double>() * varPoint3DXY; 473 view.t(1) = motionViews[k - 1].t(1) + cv::randu<double>() * varPoint3DXY; 474 } 475 view.t(2) = minCamZ + cv::randu<double>() * (maxCamZ - minCamZ); 476 view.t(2) = -view.t(2);//DownZ 477 if (noTrans && k != 0) { view.t(0) = motionViews[0].t(0); view.t(1) = motionViews[0].t(1); view.t(2) = motionViews[0].t(2); } 478 479 //3.3 GetCamRot: degree-->radian-->matrix-->vector&quaternion 480 view.degree = 0.; 481 if (rotMode & 1) view.degree(0) = cv::randu<double>() * varCamDegree; 482 if (rotMode & 2) view.degree(1) = cv::randu<double>() * varCamDegree; 483 if (rotMode & 4) view.degree(2) = cv::randu<double>() * varCamDegree; 484 view.radian = view.degree * deg2rad; 485 euler2matrix(view.radian.ptr<double>(), view.R.ptr<double>()); 486 cv::Rodrigues(view.R, view.r); 487 quat2matrix(view.q.ptr<double>(), view.R.ptr<double>(), false); 488 cv::hconcat(view.R, view.t, view.T); 489 cv::vconcat(view.r, view.t, view.rt); 490 491 //3.4 GetPoint3DAndPoint2D 492 Mat_<Vec2d> allPoint2D; 493 cv::projectPoints(allPoint3D, -view.r, -view.R.t() * view.t, view.K, view.D, allPoint2D); 494 for (int k = 0; k < allPoint2D.total(); ++k) 495 if (allPoint2D(k)[0] > 0 && allPoint2D(k)[0] < camCols && allPoint2D(k)[1] > 0 && allPoint2D(k)[1] < camRows) 496 { 497 view.point2D.push_back(allPoint2D(k)); 498 view.point3D.push_back(allPoint3D(k)); 499 view.point3DIds.push_back(k); 500 } 501 502 //3.5 PrintDetails 503 motionViews.push_back(view); 504 if (enableVerbose) 505 { 506 cout << endl << view.print(); 507 cout << fmt::format("view={} features={}\n", k, view.point2D.rows); 508 double minV = 0, maxV = 0;//Distortion makes some minV next to maxV 509 int minId = 0, maxId = 0; 510 cv::minMaxIdx(allPoint2D.reshape(1, int(allPoint2D.total()) * allPoint2D.channels()), &minV, &maxV, &minId, &maxId); 511 cout << fmt::format("minInfo:({}, {})", minId, minV) << allPoint3D(minId / 2) << allPoint2D(minId / 2) << endl; 512 cout << fmt::format("maxInfo:({}, {})", maxId, maxV) << allPoint3D(maxId / 2) << allPoint2D(maxId / 2) << endl; 513 cout << "Press any key to continue" << endl; std::getchar(); 514 } 515 if (view.point2D.rows < nPoint2DThenExit || motionViews.size() > nMaxMotion) break; 516 } 517 } 518 } 519 void visMotion() 520 { 521 //1.CreateWidgets 522 Size2d validSize(nHorPoint3D * varPoint3DXY, nVerPoint3D * varPoint3DXY); 523 Mat_<cv::Affine3d> camPoses(int(motionViews.size()), 1); for (int k = 0; k < camPoses.rows; ++k) camPoses(k) = cv::Affine3d(motionViews[k].T); 524 viz::WText worldInfo(fmt::format("nMotionView: {}\nK: {}\nD: {}", motionViews.size(), cvarr2str(K), cvarr2str(D)), Point(10, 240), 10); 525 viz::WCoordinateSystem worldCSys(1000); 526 viz::WPlane worldGround(Point3d(validSize.width / 2, validSize.height / 2, 0), Vec3d(0, 0, 1), Vec3d(0, 1, 0), validSize); 527 viz::WCloud worldPoints(allPoint3D, Mat_<Vec3b>(allPoint3D.size(), Vec3b(0, 255, 0))); 528 viz::WTrajectory camTraj1(camPoses, viz::WTrajectory::FRAMES, 8); 529 viz::WTrajectorySpheres camTraj2(camPoses, 100, 2); 530 viz::WTrajectoryFrustums camTraj3(camPoses, Matx33d(K), 4., viz::Color::yellow()); 531 worldCSys.setRenderingProperty(viz::OPACITY, 0.1); 532 worldGround.setRenderingProperty(viz::OPACITY, 0.1); 533 camTraj2.setRenderingProperty(viz::OPACITY, 0.6); 534 535 //2.ShowWidgets 536 static viz::Viz3d viz3d(__FUNCTION__); 537 viz3d.showWidget("worldInfo", worldInfo); 538 viz3d.showWidget("worldCSys", worldCSys); 539 viz3d.showWidget("worldGround", worldGround); 540 viz3d.showWidget("worldPoints", worldPoints); 541 viz3d.showWidget("camTraj1", camTraj1); 542 viz3d.showWidget("camTraj2", camTraj2); 543 viz3d.showWidget("camTraj3", camTraj3); 544 545 //3.UpdateWidghts 546 static const vector<MotionView>& views = motionViews; 547 viz3d.registerKeyboardCallback([](const viz::KeyboardEvent& keyboarEvent, void* pVizBorad)->void 548 { 549 if (keyboarEvent.action != viz::KeyboardEvent::KEY_DOWN) return; 550 static int pos = 0; 551 if (keyboarEvent.code == ' ') 552 { 553 size_t num = views.size(); 554 size_t ind = pos % num; 555 double xmin3D = DBL_MAX, ymin3D = DBL_MAX, xmin2D = DBL_MAX, ymin2D = DBL_MAX; 556 double xmax3D = -DBL_MAX, ymax3D = -DBL_MAX, xmax2D = -DBL_MAX, ymax2D = -DBL_MAX; 557 for (size_t k = 0; k < views[ind].point3D.rows; ++k) 558 { 559 Vec3d pt3 = views[ind].point3D(int(k)); 560 Vec2d pt2 = views[ind].point2D(int(k)); 561 if (pt3[0] < xmin3D) xmin3D = pt3[0]; 562 if (pt3[0] > xmax3D) xmax3D = pt3[0]; 563 if (pt3[1] < ymin3D) ymin3D = pt3[1]; 564 if (pt3[1] > ymax3D) ymax3D = pt3[1]; 565 if (pt2[0] < xmin2D) xmin2D = pt2[0]; 566 if (pt2[0] > xmax2D) xmax2D = pt2[0]; 567 if (pt2[1] < ymin2D) ymin2D = pt2[1]; 568 if (pt2[1] > ymax2D) ymax2D = pt2[1]; 569 } 570 if (pos != 0) 571 { 572 for (int k = 0; k < views[ind == 0 ? num - 1 : ind - 1].point3D.rows; ++k) viz3d.removeWidget("active" + std::to_string(k)); 573 viz3d.removeWidget("viewInfo"); 574 viz3d.removeWidget("camSolid"); 575 } 576 for (int k = 0; k < views[ind].point3D.rows; ++k) viz3d.showWidget("active" + std::to_string(k), viz::WSphere(views[ind].point3D(k), 5, 10)); 577 viz3d.showWidget("viewInfo", viz::WText(fmt::format("CurrentMotion: {}\nValidPoints: {}\nMin3DXY_Min2DXY: {}, {}, {}, {}\nMax3DXY_Max2DXY: {}, {}, {}, {}\nRot_Trans_Euler: {}\n", 578 ind, views[ind].point3D.rows, xmin3D, ymin3D, xmin2D, ymin2D, xmax3D, ymax3D, xmax2D, ymax2D, 579 cvarr2str(views[ind].r.t()) + cvarr2str(views[ind].t.t()) + cvarr2str(views[ind].degree.t())), Point(10, 10), 10)); 580 viz3d.showWidget("camSolid", viz::WCameraPosition(Matx33d(views[ind].K), 10, viz::Color::yellow()), cv::Affine3d(views[ind].T)); 581 ++pos; 582 } 583 }, 0); 584 viz3d.spin(); 585 } 586 }; 587 588 class OptimizeRt 589 { 590 public: 591 using MotionView = MotionSim::MotionView; 592 static void TestMe(int argc = 0, char** argv = 0) 593 { 594 int N = 99; 595 for (int k = 0; k < N; ++k) 596 { 597 //0.GenerateData 598 bool world2D = k % 2; 599 int rotMode = k % 7 + 1; 600 MotionSim motionSim(false); 601 motionSim.camFovX = 90; 602 motionSim.camFovY = 90; 603 motionSim.camRand = 10; 604 motionSim.nMinMotion = 16;//2 605 motionSim.nMaxMotion = 32;//4 606 motionSim.rndSeek = false; 607 motionSim.nCamDist = 5; 608 motionSim.runMotion(world2D, false, rotMode); 609 //motionSim.visMotion(); 610 int rndInd = int(motionSim.motionViews.size() * cv::randu<double>()); 611 Mat_<double> r0 = -motionSim.motionViews[rndInd].r; 612 Mat_<double> t0 = -motionSim.motionViews[rndInd].R.t() * motionSim.motionViews[rndInd].t; 613 const MotionView& motionView = motionSim.motionViews[rndInd]; 614 double errRatio = 0.9; 615 616 //1.CalcByCeresWithAutoDeriv 617 Mat_<double> R1; cv::Rodrigues(r0 * errRatio, R1); 618 Mat_<double> t1 = t0 * errRatio; 619 ceres::Problem problem1; 620 problem1.AddResidualBlock(new ceres::AutoDiffCostFunction<ProjectionModelRt, ceres::DYNAMIC, 9, 3>( 621 new ProjectionModelRt(motionView, motionSim.nCamDist), motionView.point3D.rows * 2), NULL, R1.ptr<double>(), t1.ptr<double>()); 622 problem1.SetParameterization(R1.ptr<double>(), new LocalParamRWithGeneralJ); 623 ceres::Solver::Options options1; 624 ceres::Solver::Summary summary1; 625 ceres::Solve(options1, &problem1, &summary1); 626 Mat_<double> r1; cv::Rodrigues(R1, r1); 627 int nIter1 = (int)summary1.iterations.size(); 628 629 //2.CalcByCeresWithGeneralJ 630 Mat_<double> R2; cv::Rodrigues(r0 * errRatio, R2); 631 Mat_<double> t2 = t0 * errRatio; 632 ceres::Problem problem2; 633 problem2.AddResidualBlock(new ProjectionCostRt(motionView, motionSim.nCamDist, 9), NULL, R2.ptr<double>(), t2.ptr<double>()); 634 problem2.SetParameterization(R2.ptr<double>(), new LocalParamRWithGeneralJ); 635 ceres::Solver::Options options2; 636 ceres::Solver::Summary summary2; 637 ceres::Solve(options2, &problem2, &summary2); 638 Mat_<double> r2; cv::Rodrigues(R2, r2); 639 int nIter2 = (int)summary2.iterations.size(); 640 641 //3.CalcByCeresWithGeneralJ 642 Mat_<double> r3 = r0 * errRatio; 643 Mat_<double> t3 = t0 * errRatio; 644 ceres::Problem problem3; 645 problem3.AddResidualBlock(new ProjectionCostRt(motionView, motionSim.nCamDist, 3), NULL, r3.ptr<double>(), t3.ptr<double>()); 646 problem3.SetParameterization(r3.ptr<double>(), new LocalParamRWithIdentJ); 647 ceres::Solver::Options options3; 648 ceres::Solver::Summary summary3; 649 ceres::Solve(options3, &problem3, &summary3); 650 int nIter3 = (int)summary3.iterations.size(); 651 652 //4.AnalyzeError 653 double infr0r0 = norm(r0, r0 * errRatio, NORM_INF); 654 double infr0r1 = norm(r0, r1, NORM_INF); 655 double infr0r2 = norm(r0, r2, NORM_INF); 656 double infr0r3 = norm(r0, r3, NORM_INF); 657 double inft0t0 = norm(t0, t0 * errRatio, NORM_INF); 658 double inft0t1 = norm(t0, t1, NORM_INF); 659 double inft0t2 = norm(t0, t2, NORM_INF); 660 double inft0t3 = norm(t0, t3, NORM_INF); 661 662 //5.PrintError 663 cout << fmt::format("LoopCount: {} AutoDeriv.iters: {} GeneralJ.iters: {} IdentJ.iters: {}\n", k, nIter1, nIter2, nIter3); 664 if (infr0r1 > 1e-8 || infr0r2 > 1e-8 || infr0r3 > 1e-8 || inft0t1 > 1e-8 || inft0t2 > 1e-8 || inft0t3 > 1e-8) 665 { 666 cout << fmt::format("infr0r1: {:<15.9}\t\t{:<15.9}\n", infr0r1, infr0r0); 667 cout << fmt::format("infr0r2: {:<15.9}\t\t{:<15.9}\n", infr0r2, infr0r0); 668 cout << fmt::format("infr0r3: {:<15.9}\t\t{:<15.9}\n", infr0r3, infr0r0); 669 cout << fmt::format("inft0t1: {:<15.9}\t\t{:<15.9}\n", inft0t1, inft0t0); 670 cout << fmt::format("inft0t2: {:<15.9}\t\t{:<15.9}\n", inft0t2, inft0t0); 671 cout << fmt::format("inft0t3: {:<15.9}\t\t{:<15.9}\n", inft0t3, inft0t0); 672 cout << "Press any key to continue" << endl; std::getchar(); 673 } 674 } 675 } 676 677 public: 678 struct ProjectionModelRt 679 { 680 const int nDist; 681 double K[4]; 682 const double* D; 683 Mat_<Vec2d> point2D; 684 Mat_<Vec3d> point3D; 685 ProjectionModelRt(const MotionView& motionView0, const int nDist0) : nDist(nDist0) 686 { 687 K[0] = motionView0.K(0, 0); 688 K[1] = motionView0.K(1, 1); 689 K[2] = motionView0.K(0, 2); 690 K[3] = motionView0.K(1, 2); 691 D = motionView0.D.ptr<double>(); 692 point2D = motionView0.point2D; 693 point3D = motionView0.point3D; 694 } 695 template <typename tp> bool operator()(const tp* const rot, const tp* const t, tp* errPoint2D) const 696 { 697 //1.Projection params 698 double fx = K[0]; 699 double fy = K[1]; 700 double cx = K[2]; 701 double cy = K[3]; 702 703 //2.Distortion params 704 double k1 = D[0]; 705 double k2 = D[1]; 706 double p1 = D[2]; 707 double p2 = D[3]; 708 double k3, k4, k5, k6; 709 double s1, s2, s3, s4; 710 if (nDist > 4) k3 = D[4]; 711 if (nDist > 5) { k4 = D[5]; k5 = D[6]; k6 = D[7]; } 712 if (nDist > 8) { s1 = D[8]; s2 = D[9]; s3 = D[10]; s4 = D[11]; } 713 714 //3.Translation params 715 tp tx = t[0]; 716 tp ty = t[1]; 717 tp tz = t[2]; 718 719 //4.Rotation params 720 tp R11, R12, R13, R21, R22, R23, R31, R32, R33; 721 { 722 R11 = rot[0]; R12 = rot[1]; R13 = rot[2]; 723 R21 = rot[3]; R22 = rot[4]; R23 = rot[5]; 724 R31 = rot[6]; R32 = rot[7]; R33 = rot[8]; 725 } 726 727 //5.ReProjection 728 const Vec2d* data2D = point2D.ptr<Vec2d>(); 729 const Vec3d* data3D = point3D.ptr<Vec3d>(); 730 Vec<tp, 2>* err2D = (Vec<tp, 2>*)errPoint2D; 731 for (int k = 0; k < point3D.rows; ++k) 732 { 733 //5.1 WorldCoordinate 734 double X = data3D[k][0]; 735 double Y = data3D[k][1]; 736 double Z = data3D[k][2]; 737 738 //5.2 CameraCoordinate 739 tp x = R11 * X + R12 * Y + R13 * Z + tx; 740 tp y = R21 * X + R22 * Y + R23 * Z + ty; 741 tp z = R31 * X + R32 * Y + R33 * Z + tz; 742 743 //5.3 StandardPhysicsCoordinate 744 tp iz = 1. / z; //if denominator==0 745 tp xc = x * iz; 746 tp yc = y * iz; 747 748 //5.4 DistortionPhysicsCoordinate 749 tp xc2 = xc * xc; 750 tp yc2 = yc * yc; 751 tp d2 = xc2 + yc2; 752 tp xcyc = 2. * xc * yc; 753 tp d4 = d2 * d2; 754 tp d6 = d2 * d4; 755 tp d2xc2 = d2 + 2. * xc2; 756 tp d2yc2 = d2 + 2. * yc2; 757 tp nu, de, xd, yd; 758 if (nDist < 5) 759 { 760 nu = 1. + k1 * d2 + k2 * d4; 761 xd = xc * nu + p1 * xcyc + p2 * d2xc2; 762 yd = yc * nu + p2 * xcyc + p1 * d2yc2; 763 } 764 else if (nDist < 8) 765 { 766 nu = 1. + k1 * d2 + k2 * d4 + k3 * d6; 767 xd = xc * nu + p1 * xcyc + p2 * d2xc2; 768 yd = yc * nu + p2 * xcyc + p1 * d2yc2; 769 } 770 else if (nDist < 12) 771 { 772 nu = 1. + k1 * d2 + k2 * d4 + k3 * d6; 773 de = 1. / (1. + k4 * d2 + k5 * d4 + k6 * d6);//if denominator==0 774 xd = xc * nu * de + p1 * xcyc + p2 * d2xc2; 775 yd = yc * nu * de + p2 * xcyc + p1 * d2yc2; 776 } 777 else if (nDist < 14) 778 { 779 nu = 1. + k1 * d2 + k2 * d4 + k3 * d6; 780 de = 1. / (1. + k4 * d2 + k5 * d4 + k6 * d6);//if denominator==0 781 xd = xc * nu * de + p1 * xcyc + p2 * d2xc2 + s1 * d2 + s2 * d4; 782 yd = yc * nu * de + p2 * xcyc + p1 * d2yc2 + s3 * d2 + s4 * d4; 783 } 784 err2D[k][0] = xd * fx + cx - data2D[k][0]; 785 err2D[k][1] = yd * fy + cy - data2D[k][1]; 786 } 787 return true; 788 } 789 }; 790 struct LocalParamRWithGeneralJ : public ceres::LocalParameterization 791 { 792 bool Plus(const double* preParam, const double* deltaParam, double* newParam) const 793 { 794 Matx33d ambientPre(preParam); 795 Matx33d ambientDelta; cv::Rodrigues(Matx31d(deltaParam), ambientDelta); 796 Matx33d ambientNew = ambientDelta * ambientPre; 797 memcpy(newParam, ambientNew.val, sizeof(double) * 9); 798 return true; 799 } 800 801 bool ComputeJacobian(const double* param, double* jacobian) const 802 { 803 Mat_<double> dRdr(9, 3, jacobian); 804 dRdr << 0, param[6], -param[3], 0, param[7], -param[4], 0, param[8], -param[5], 805 -param[6], 0, param[0], -param[7], 0, param[1], -param[8], 0, param[2], 806 param[3], -param[0], 0, param[4], -param[1], 0, param[5], -param[2], 0; 807 return true; 808 809 Matx33d R(param); 810 Mat_<double> dRdrT({ 3, 9 }, { 811 0, 0, 0, -R(2, 0), -R(2, 1), -R(2, 2), R(1, 0), R(1, 1), R(1, 2), 812 R(2, 0), R(2, 1), R(2, 2), 0, 0, 0, -R(0, 0), -R(0, 1), -R(0, 2), 813 -R(1, 0), -R(1, 1), -R(1, 2), R(0, 0), R(0, 1), R(0, 2), 0, 0, 0 }); 814 cout << endl << cv::norm(dRdr, dRdrT.t(), NORM_INF) << endl; 815 } 816 817 int GlobalSize() const { return 9; } 818 int LocalSize() const { return 3; } 819 }; 820 struct LocalParamRWithIdentJ : public ceres::LocalParameterization 821 { 822 bool Plus(const double* preParam, const double* deltaParam, double* newParam) const 823 { 824 Matx33d ambientPre; cv::Rodrigues(Matx31d(preParam), ambientPre); 825 Matx33d ambientDelta; cv::Rodrigues(Matx31d(deltaParam), ambientDelta); 826 Matx31d tangentNew; cv::Rodrigues(ambientDelta * ambientPre, tangentNew); 827 memcpy(newParam, tangentNew.val, sizeof(double) * 3); 828 return true; 829 } 830 831 bool ComputeJacobian(const double* param, double* jacobian) const 832 { 833 Mat_<double>(3, 3, jacobian) = Mat_<double>::eye(3, 3); 834 return true; 835 } 836 837 int GlobalSize() const { return 3; } 838 int LocalSize() const { return 3; } 839 }; 840 struct ProjectionCostRt : public ceres::CostFunction 841 { 842 const int nDist; 843 double K[4]; 844 const double* D; 845 Mat_<Vec2d> point2D; 846 Mat_<Vec3d> point3D; 847 const int nRot; 848 const MotionView& motionViewForDBG;//only use once 849 ProjectionCostRt(const MotionView& motionView0, const int nDist0, const int nRot0) : nDist(nDist0), nRot(nRot0), motionViewForDBG(motionView0) 850 { 851 K[0] = motionView0.K(0, 0); 852 K[1] = motionView0.K(1, 1); 853 K[2] = motionView0.K(0, 2); 854 K[3] = motionView0.K(1, 2); 855 D = motionView0.D.ptr<double>(); 856 point2D = motionView0.point2D; 857 point3D = motionView0.point3D; 858 this->set_num_residuals(point2D.rows * 2); 859 this->mutable_parameter_block_sizes()->push_back(nRot); 860 this->mutable_parameter_block_sizes()->push_back(3); 861 } 862 bool Evaluate(double const* const* params, double* residuals, double** jacobians) const 863 { 864 //0.FormatInput 865 const double* rot = params[0]; 866 const double* t = params[1]; 867 Vec2d* err2D = (Vec2d*)(residuals); 868 Mat_<double> dpdR; if (jacobians && jacobians[0]) dpdR = Mat_<double>(point2D.rows * 2, nRot, jacobians[0]); 869 Mat_<double> dpdt; if (jacobians && jacobians[1]) dpdt = Mat_<double>(point2D.rows * 2, 3, jacobians[1]); 870 871 //1.Projection params 872 double fx = K[0]; 873 double fy = K[1]; 874 double cx = K[2]; 875 double cy = K[3]; 876 877 //2.Distortion params 878 double k1 = D[0]; 879 double k2 = D[1]; 880 double p1 = D[2]; 881 double p2 = D[3]; 882 double k3 = 0, k4 = 0, k5 = 0, k6 = 0; 883 double s1 = 0, s2 = 0, s3 = 0, s4 = 0; 884 if (nDist > 4) k3 = D[4]; 885 if (nDist > 5) { k4 = D[5]; k5 = D[6]; k6 = D[7]; } 886 if (nDist > 8) { s1 = D[8]; s2 = D[9]; s3 = D[10]; s4 = D[11]; } 887 888 //3.Translation params 889 double tx = t[0]; 890 double ty = t[1]; 891 double tz = t[2]; 892 893 //4.Rotation params 894 double R11, R12, R13, R21, R22, R23, R31, R32, R33; 895 if (nRot == 9) 896 { 897 R11 = rot[0]; R12 = rot[1]; R13 = rot[2]; 898 R21 = rot[3]; R22 = rot[4]; R23 = rot[5]; 899 R31 = rot[6]; R32 = rot[7]; R33 = rot[8]; 900 } 901 if (nRot == 3) 902 { 903 Matx33d R; cv::Rodrigues(Matx31d(rot), R); 904 rot = R.val; 905 R11 = rot[0]; R12 = rot[1]; R13 = rot[2]; 906 R21 = rot[3]; R22 = rot[4]; R23 = rot[5]; 907 R31 = rot[6]; R32 = rot[7]; R33 = rot[8]; 908 rot = params[0]; 909 } 910 911 //5.ReProjection 912 const Vec2d* data2D = point2D.ptr<Vec2d>(); 913 const Vec3d* data3D = point3D.ptr<Vec3d>(); 914 for (int k = 0; k < point3D.rows; ++k) 915 { 916 //5.1 WorldCoordinate 917 double X = data3D[k][0]; 918 double Y = data3D[k][1]; 919 double Z = data3D[k][2]; 920 921 //5.2 CameraCoordinate 922 double x = R11 * X + R12 * Y + R13 * Z + tx; 923 double y = R21 * X + R22 * Y + R23 * Z + ty; 924 double z = R31 * X + R32 * Y + R33 * Z + tz; 925 926 //5.3 StandardPhysicsCoordinate 927 double iz = 1. / z; //if denominator==0 928 double xc = x * iz; 929 double yc = y * iz; 930 931 //5.4 DistortionPhysicsCoordinate 932 double xc2 = xc * xc; 933 double yc2 = yc * yc; 934 double d2 = xc2 + yc2; 935 double xcyc = 2. * xc * yc; 936 double d4 = d2 * d2; 937 double d6 = d2 * d4; 938 double d2xc2 = d2 + 2. * xc2; 939 double d2yc2 = d2 + 2. * yc2; 940 double nu = 0, de = 1., xd = 0, yd = 0; 941 if (nDist < 5) 942 { 943 nu = 1. + k1 * d2 + k2 * d4; 944 xd = xc * nu + p1 * xcyc + p2 * d2xc2; 945 yd = yc * nu + p2 * xcyc + p1 * d2yc2; 946 } 947 else if (nDist < 8) 948 { 949 nu = 1. + k1 * d2 + k2 * d4 + k3 * d6; 950 xd = xc * nu + p1 * xcyc + p2 * d2xc2; 951 yd = yc * nu + p2 * xcyc + p1 * d2yc2; 952 } 953 else if (nDist < 12) 954 { 955 nu = 1. + k1 * d2 + k2 * d4 + k3 * d6; 956 de = 1. / (1. + k4 * d2 + k5 * d4 + k6 * d6);//if denominator==0 957 xd = xc * nu * de + p1 * xcyc + p2 * d2xc2; 958 yd = yc * nu * de + p2 * xcyc + p1 * d2yc2; 959 } 960 else if (nDist < 14) 961 { 962 nu = 1. + k1 * d2 + k2 * d4 + k3 * d6; 963 de = 1. / (1. + k4 * d2 + k5 * d4 + k6 * d6);//if denominator==0 964 xd = xc * nu * de + p1 * xcyc + p2 * d2xc2 + s1 * d2 + s2 * d4; 965 yd = yc * nu * de + p2 * xcyc + p1 * d2yc2 + s3 * d2 + s4 * d4; 966 } 967 err2D[k][0] = xd * fx + cx - data2D[k][0]; 968 err2D[k][1] = yd * fy + cy - data2D[k][1]; 969 if (!dpdR.empty() || !dpdt.empty()) 970 { 971 Mat_<double> dpDdpCD({ 2, 2 }, { fx, 0, 0, fy }); 972 973 Mat_<double> dpCDdh({ 2, 5 }, { 974 xc * de, -xc * nu * de * de, p2 + s1 + 2 * s2 * d2, nu * de + 2 * p1 * yc + 4 * p2 * xc, 2 * p1 * xc, 975 yc * de, -yc * nu * de * de, p1 + s3 + 2 * s4 * d2, 2 * p2 * yc, nu * de + 2 * p2 * xc + 4 * p1 * yc }); 976 977 Mat_<double> dhdpC({ 5, 2 }, { 978 xc * (2 * k1 + 4 * k2 * d2 + 6 * k3 * d4), yc * (2 * k1 + 4 * k2 * d2 + 6 * k3 * d4), 979 xc * (2 * k4 + 4 * k5 * d2 + 6 * k6 * d4), yc * (2 * k4 + 4 * k5 * d2 + 6 * k6 * d4), 980 2 * xc, 2 * yc, 981 1, 0, 982 0, 1 }); 983 984 Mat_<double> dpCdPC({ 2, 3 }, { 985 iz, 0, -xc * iz, 986 0, iz, -yc * iz }); 987 988 if (!dpdt.empty()) dpdt.rowRange(2 * k, 2 * k + 2) = dpDdpCD * dpCDdh * dhdpC * dpCdPC; 989 if (!dpdR.empty() && nRot == 9) 990 { 991 Mat_<double> dPCdR({ 3, 9 }, { 992 X, Y, Z, 0, 0, 0, 0, 0, 0, 993 0, 0, 0, X, Y, Z, 0, 0, 0, 994 0, 0, 0, 0, 0, 0, X, Y, Z }); 995 dpdR.rowRange(2 * k, 2 * k + 2) = (dpDdpCD * dpCDdh * dhdpC * dpCdPC * dPCdR); 996 } 997 if (!dpdR.empty() && nRot == 3) 998 { 999 Mat_<double> _skewPC({ 3, 3 }, { 1000 0, (z - tz), -(y - ty), 1001 -(z - tz), 0, (x - tx), 1002 (y - ty), -(x - tx), 0 }); 1003 dpdR.rowRange(2 * k, 2 * k + 2) = dpDdpCD * dpCDdh * dhdpC * dpCdPC * _skewPC; 1004 } 1005 } 1006 } 1007 1008 return true; 1009 if (nRot == 9) 1010 { 1011 ceres::AutoDiffCostFunction<ProjectionModelRt, ceres::DYNAMIC, 9, 3> autoDeriv(new ProjectionModelRt(motionViewForDBG, nDist), motionViewForDBG.point3D.rows * 2); 1012 Mat_<double> dpdRDBG(point3D.rows * 2, 9); 1013 Mat_<double> dpdtDBG(point3D.rows * 2, 3); 1014 Mat_<Vec2d> err2DDBG(point3D.rows, 1); 1015 const double* parametersDBG[2] = { rot, t }; 1016 double* jacobiansDBG[2] = { dpdRDBG.ptr<double>(), dpdtDBG.ptr<double>() }; 1017 double* residualsDBG = err2DDBG.ptr<double>(); 1018 autoDeriv.Evaluate(parametersDBG, residualsDBG, jacobiansDBG); 1019 1020 if (!dpdR.empty()) cout << endl << norm(dpdR, dpdRDBG, NORM_INF) << endl; 1021 if (!dpdt.empty()) cout << endl << norm(dpdt, dpdtDBG, NORM_INF) << endl; 1022 cout << endl << norm(Mat_<Vec2d>(point2D.rows, 1, err2D), err2DDBG, NORM_INF) << endl; 1023 std::getchar(); 1024 } 1025 return true; 1026 } 1027 }; 1028 }; 1029 1030 class OptimizeKDRt 1031 { 1032 public: 1033 using MotionView = MotionSim::MotionView; 1034 static void TestMe(int argc = 0, char** argv = 0) 1035 { 1036 int N = 99; 1037 for (int k = 0; k < N; ++k) 1038 { 1039 //0.GenerateData 1040 bool world2D = k % 2; 1041 int rotMode = k % 7 + 1; 1042 MotionSim motionSim(false); 1043 motionSim.camFovX = 90; 1044 motionSim.camFovY = 90; 1045 motionSim.camRand = 10; 1046 motionSim.nMinMotion = 16;//2 1047 motionSim.nMaxMotion = 32;//4 1048 motionSim.rndSeek = false; 1049 static const int nDist = 5; 1050 motionSim.nCamDist = nDist; 1051 motionSim.runMotion(world2D, false, rotMode); 1052 //motionSim.visMotion(); 1053 Mat_<double> rs0; for (size_t k = 0; k < motionSim.motionViews.size(); ++k) rs0.push_back(-motionSim.motionViews[k].r); 1054 Mat_<double> ts0; for (size_t k = 0; k < motionSim.motionViews.size(); ++k) ts0.push_back(-motionSim.motionViews[k].R.t() * motionSim.motionViews[k].t); 1055 Mat_<double> K0({ 4, 1 }, { motionSim.motionViews[0].K(0, 0), motionSim.motionViews[0].K(1, 1), motionSim.motionViews[0].K(0, 2), motionSim.motionViews[0].K(1, 2) }); 1056 Mat_<double> D0 = motionSim.motionViews[0].D.clone(); 1057 double errRatio = 0.9; 1058 double errRatioTrans = 0.99; 1059 1060 //1.CalcByCeresWithAutoDeriv 1061 Mat_<double> Rs1; for (int k = 0; k < rs0.rows; k += 3) { Mat_<double> _R; cv::Rodrigues(rs0.rowRange(k, k + 3) * errRatio, _R); Rs1.push_back(_R); } 1062 Mat_<double> ts1 = ts0 * errRatioTrans; 1063 Mat_<double> K1 = K0 * errRatio; 1064 Mat_<double> D1 = D0 * errRatio; 1065 ceres::Problem problem1; 1066 for (int k = 0; k < motionSim.motionViews.size(); ++k) 1067 problem1.AddResidualBlock(new ceres::AutoDiffCostFunction<ProjectionModelKDRt, ceres::DYNAMIC, 9, 3, 4, nDist>( 1068 new ProjectionModelKDRt(motionSim.motionViews[k], motionSim.nCamDist), motionSim.motionViews[k].point3D.rows * 2), NULL, 1069 Rs1.ptr<double>(k * 3), ts1.ptr<double>(k * 3), 1070 K1.ptr<double>(), D1.ptr<double>()); 1071 for (int k = 0; k < motionSim.motionViews.size(); ++k) problem1.SetParameterization(Rs1.ptr<double>(k * 3), new OptimizeRt::LocalParamRWithGeneralJ); 1072 ceres::Solver::Options options; 1073 ceres::Solver::Summary summary; 1074 ceres::Solve(options, &problem1, &summary); 1075 Mat_<double> rs1; for (int k = 0; k < Rs1.rows; k += 3) { Mat_<double> _r; cv::Rodrigues(Rs1.rowRange(k, k + 3), _r); rs1.push_back(_r); } 1076 int nIter1 = (int)summary.iterations.size(); 1077 1078 //2.CalcByCeresWithGeneralJ 1079 Mat_<double> Rs2; for (int k = 0; k < rs0.rows; k += 3) { Mat_<double> _R; cv::Rodrigues(rs0.rowRange(k, k + 3) * errRatio, _R); Rs2.push_back(_R); } 1080 Mat_<double> ts2 = ts0 * errRatioTrans; 1081 Mat_<double> K2 = K0 * errRatio; 1082 Mat_<double> D2 = D0 * errRatio; 1083 ceres::Problem problem2; 1084 for (int k = 0; k < motionSim.motionViews.size(); ++k) 1085 problem2.AddResidualBlock(new ProjectionCostKDRt(motionSim.motionViews[k], motionSim.nCamDist, 9), NULL, 1086 Rs2.ptr<double>(k * 3), ts2.ptr<double>(k * 3), K2.ptr<double>(), D2.ptr<double>()); 1087 for (int k = 0; k < motionSim.motionViews.size(); ++k) problem2.SetParameterization(Rs2.ptr<double>(k * 3), new OptimizeRt::LocalParamRWithGeneralJ); 1088 ceres::Solver::Options options2; 1089 ceres::Solver::Summary summary2; 1090 ceres::Solve(options2, &problem2, &summary2); 1091 Mat_<double> rs2; for (int k = 0; k < Rs2.rows; k += 3) { Mat_<double> _r; cv::Rodrigues(Rs2.rowRange(k, k + 3), _r); rs2.push_back(_r); } 1092 int nIter2 = (int)summary2.iterations.size(); 1093 1094 //3.CalcByCeresWithIdentJ 1095 Mat_<double> rs3; for (int k = 0; k < rs0.rows; k += 3) rs3.push_back(rs0.rowRange(k, k + 3) * errRatio); 1096 Mat_<double> ts3 = ts0 * errRatioTrans; 1097 Mat_<double> K3 = K0 * errRatio; 1098 Mat_<double> D3 = D0 * errRatio; 1099 ceres::Problem problem3; 1100 for (int k = 0; k < motionSim.motionViews.size(); ++k) 1101 problem3.AddResidualBlock(new ProjectionCostKDRt(motionSim.motionViews[k], motionSim.nCamDist, 3), NULL, 1102 rs3.ptr<double>(k * 3), ts3.ptr<double>(k * 3), K3.ptr<double>(), D3.ptr<double>()); 1103 for (int k = 0; k < motionSim.motionViews.size(); ++k) problem3.SetParameterization(rs3.ptr<double>(k * 3), new OptimizeRt::LocalParamRWithIdentJ); 1104 ceres::Solver::Options options3; 1105 ceres::Solver::Summary summary3; 1106 ceres::Solve(options3, &problem3, &summary3); 1107 int nIter3 = (int)summary3.iterations.size(); 1108 1109 //4.AnalyzeError 1110 double infrs0rs0 = norm(rs0, rs0 * errRatio, NORM_INF); 1111 double infrs0rs1 = norm(rs0, rs1, NORM_INF); 1112 double infrs0rs2 = norm(rs0, rs2, NORM_INF); 1113 double infrs0rs3 = norm(rs0, rs3, NORM_INF); 1114 double infts0ts0 = norm(ts0, ts0 * errRatioTrans, NORM_INF); 1115 double infts0ts1 = norm(ts0, ts1, NORM_INF); 1116 double infts0ts2 = norm(ts0, ts2, NORM_INF); 1117 double infts0ts3 = norm(ts0, ts3, NORM_INF); 1118 double infK0K0 = norm(K0, K0 * errRatio, NORM_INF); 1119 double infK0K1 = norm(K0, K1, NORM_INF); 1120 double infK0K2 = norm(K0, K2, NORM_INF); 1121 double infK0K3 = norm(K0, K3, NORM_INF); 1122 double infD0D0 = norm(D0, D0 * errRatio, NORM_INF); 1123 double infD0D1 = norm(D0, D1, NORM_INF); 1124 double infD0D2 = norm(D0, D2, NORM_INF); 1125 double infD0D3 = norm(D0, D3, NORM_INF); 1126 1127 //5.PrintError 1128 cout << fmt::format("LoopCount: {} AutoDeriv.iters: {} GeneralJ.iters: {} IdentJ.iters: {}\n", k, nIter1, nIter2, nIter3); 1129 if (infrs0rs1 > 1e-8 || infrs0rs2 > 1e-8 || infrs0rs3 > 1e-8 || infts0ts1 > 1e-8 || infts0ts2 > 1e-8 || infts0ts3 > 1e-8 || 1130 infK0K1 > 1e-8 || infK0K2 > 1e-8 || infK0K3 > 1e-8 || infD0D1 > 1e-8 || infD0D2 > 1e-8 || infD0D3 > 1e-8) 1131 { 1132 cout << fmt::format("infrs0rs1: {:<15.9}\t\t{:<15.9}\n", infrs0rs1, infrs0rs0); 1133 cout << fmt::format("infrs0rs2: {:<15.9}\t\t{:<15.9}\n", infrs0rs2, infrs0rs0); 1134 cout << fmt::format("infrs0rs3: {:<15.9}\t\t{:<15.9}\n", infrs0rs3, infrs0rs0); 1135 cout << fmt::format("infts0ts1: {:<15.9}\t\t{:<15.9}\n", infts0ts1, infts0ts0); 1136 cout << fmt::format("infts0ts2: {:<15.9}\t\t{:<15.9}\n", infts0ts2, infts0ts0); 1137 cout << fmt::format("infts0ts3: {:<15.9}\t\t{:<15.9}\n", infts0ts3, infts0ts0); 1138 cout << fmt::format("infK0K1 : {:<15.9}\t\t{:<15.9}\n", infK0K1, infK0K0); 1139 cout << fmt::format("infK0K2 : {:<15.9}\t\t{:<15.9}\n", infK0K2, infK0K0); 1140 cout << fmt::format("infK0K3 : {:<15.9}\t\t{:<15.9}\n", infK0K3, infK0K0); 1141 cout << fmt::format("infD0D1 : {:<15.9}\t\t{:<15.9}\n", infD0D1, infD0D0); 1142 cout << fmt::format("infD0D2 : {:<15.9}\t\t{:<15.9}\n", infD0D2, infD0D0); 1143 cout << fmt::format("infD0D3 : {:<15.9}\t\t{:<15.9}\n", infD0D3, infD0D0); 1144 cout << "Press any key to continue" << endl; std::getchar(); 1145 } 1146 } 1147 } 1148 1149 public: 1150 struct ProjectionModelKDRt 1151 { 1152 const int nDist; 1153 Mat_<Vec2d> point2D; 1154 Mat_<Vec3d> point3D; 1155 ProjectionModelKDRt(const MotionView& motionView0, const int nDist0) : nDist(nDist0) 1156 { 1157 point2D = motionView0.point2D; 1158 point3D = motionView0.point3D; 1159 } 1160 template <typename tp> bool operator()(const tp* const rot, const tp* const t, const tp* const K, const tp* const D, tp* errPoint2D) const 1161 { 1162 //1.Projection params 1163 tp fx = K[0]; 1164 tp fy = K[1]; 1165 tp cx = K[2]; 1166 tp cy = K[3]; 1167 1168 //2.Distortion params 1169 tp k1 = D[0]; 1170 tp k2 = D[1]; 1171 tp p1 = D[2]; 1172 tp p2 = D[3]; 1173 tp k3, k4, k5, k6; 1174 tp s1, s2, s3, s4; 1175 if (nDist > 4) k3 = D[4]; 1176 if (nDist > 5) { k4 = D[5]; k5 = D[6]; k6 = D[7]; } 1177 if (nDist > 8) { s1 = D[8]; s2 = D[9]; s3 = D[10]; s4 = D[11]; } 1178 1179 //3.Translation params 1180 tp tx = t[0]; 1181 tp ty = t[1]; 1182 tp tz = t[2]; 1183 1184 //4. 1185 tp R11, R12, R13, R21, R22, R23, R31, R32, R33; 1186 { 1187 R11 = rot[0]; R12 = rot[1]; R13 = rot[2]; 1188 R21 = rot[3]; R22 = rot[4]; R23 = rot[5]; 1189 R31 = rot[6]; R32 = rot[7]; R33 = rot[8]; 1190 } 1191 1192 //5.ReProjection 1193 const Vec2d* data2D = point2D.ptr<Vec2d>(); 1194 const Vec3d* data3D = point3D.ptr<Vec3d>(); 1195 Vec<tp, 2>* err2D = (Vec<tp, 2>*)errPoint2D; 1196 for (int k = 0; k < point3D.rows; ++k) 1197 { 1198 //5.1 WorldCoordinate 1199 double X = data3D[k][0]; 1200 double Y = data3D[k][1]; 1201 double Z = data3D[k][2]; 1202 1203 //5.2 CameraCoordinate 1204 tp x = R11 * X + R12 * Y + R13 * Z + tx; 1205 tp y = R21 * X + R22 * Y + R23 * Z + ty; 1206 tp z = R31 * X + R32 * Y + R33 * Z + tz; 1207 1208 //5.3 StandardPhysicsCoordinate 1209 tp iz = 1. / z; //if denominator==0 1210 tp xc = x * iz; 1211 tp yc = y * iz; 1212 1213 //5.4 DistortionPhysicsCoordinate 1214 tp xc2 = xc * xc; 1215 tp yc2 = yc * yc; 1216 tp d2 = xc2 + yc2; 1217 tp xcyc = 2. * xc * yc; 1218 tp d4 = d2 * d2; 1219 tp d6 = d2 * d4; 1220 tp d2xc2 = d2 + 2. * xc2; 1221 tp d2yc2 = d2 + 2. * yc2; 1222 tp nu, de, xd, yd; 1223 if (nDist < 5) 1224 { 1225 nu = 1. + k1 * d2 + k2 * d4; 1226 xd = xc * nu + p1 * xcyc + p2 * d2xc2; 1227 yd = yc * nu + p2 * xcyc + p1 * d2yc2; 1228 } 1229 else if (nDist < 8) 1230 { 1231 nu = 1. + k1 * d2 + k2 * d4 + k3 * d6; 1232 xd = xc * nu + p1 * xcyc + p2 * d2xc2; 1233 yd = yc * nu + p2 * xcyc + p1 * d2yc2; 1234 } 1235 else if (nDist < 12) 1236 { 1237 nu = 1. + k1 * d2 + k2 * d4 + k3 * d6; 1238 de = 1. / (1. + k4 * d2 + k5 * d4 + k6 * d6);//if denominator==0 1239 xd = xc * nu * de + p1 * xcyc + p2 * d2xc2; 1240 yd = yc * nu * de + p2 * xcyc + p1 * d2yc2; 1241 } 1242 else if (nDist < 14) 1243 { 1244 nu = 1. + k1 * d2 + k2 * d4 + k3 * d6; 1245 de = 1. / (1. + k4 * d2 + k5 * d4 + k6 * d6);//if denominator==0 1246 xd = xc * nu * de + p1 * xcyc + p2 * d2xc2 + s1 * d2 + s2 * d4; 1247 yd = yc * nu * de + p2 * xcyc + p1 * d2yc2 + s3 * d2 + s4 * d4; 1248 } 1249 err2D[k][0] = xd * fx + cx - data2D[k][0]; 1250 err2D[k][1] = yd * fy + cy - data2D[k][1]; 1251 } 1252 return true; 1253 } 1254 }; 1255 struct ProjectionCostKDRt : public ceres::CostFunction 1256 { 1257 const int nDist; 1258 Mat_<Vec2d> point2D; 1259 Mat_<Vec3d> point3D; 1260 const int nRot; 1261 const MotionView& motionViewForDBG;//Only used once 1262 ProjectionCostKDRt(const MotionView& motionView0, const int nDist0, const int nRot0) : nDist(nDist0), nRot(nRot0), motionViewForDBG(motionView0) 1263 { 1264 point2D = motionView0.point2D; 1265 point3D = motionView0.point3D; 1266 this->set_num_residuals(point2D.rows * 2); 1267 this->mutable_parameter_block_sizes()->push_back(nRot); 1268 this->mutable_parameter_block_sizes()->push_back(3); 1269 this->mutable_parameter_block_sizes()->push_back(4); 1270 this->mutable_parameter_block_sizes()->push_back(nDist); 1271 } 1272 bool Evaluate(double const* const* params, double* residuals, double** jacobians) const 1273 { 1274 //0.FormatInput 1275 const double* rot = params[0]; 1276 const double* t = params[1]; 1277 const double* K = params[2]; 1278 const double* D = params[3]; 1279 Vec2d* err2D = (Vec2d*)(residuals); 1280 Mat_<double> dpdR; if (jacobians && jacobians[0]) dpdR = Mat_<double>(point2D.rows * 2, nRot, jacobians[0]); 1281 Mat_<double> dpdt; if (jacobians && jacobians[1]) dpdt = Mat_<double>(point2D.rows * 2, 3, jacobians[1]); 1282 Mat_<double> dpdK; if (jacobians && jacobians[2]) dpdK = Mat_<double>(point2D.rows * 2, 4, jacobians[2]); 1283 Mat_<double> dpdD; if (jacobians && jacobians[3]) dpdD = Mat_<double>(point2D.rows * 2, nDist, jacobians[3]); 1284 1285 //1.Projection params 1286 double fx = K[0]; 1287 double fy = K[1]; 1288 double cx = K[2]; 1289 double cy = K[3]; 1290 1291 //2.Distortion params 1292 double k1 = D[0]; 1293 double k2 = D[1]; 1294 double p1 = D[2]; 1295 double p2 = D[3]; 1296 double k3 = 0, k4 = 0, k5 = 0, k6 = 0; 1297 double s1 = 0, s2 = 0, s3 = 0, s4 = 0; 1298 if (nDist > 4) k3 = D[4]; 1299 if (nDist > 5) { k4 = D[5]; k5 = D[6]; k6 = D[7]; } 1300 if (nDist > 8) { s1 = D[8]; s2 = D[9]; s3 = D[10]; s4 = D[11]; } 1301 1302 //3.Translation params 1303 double tx = t[0]; 1304 double ty = t[1]; 1305 double tz = t[2]; 1306 1307 //4.Rotation params 1308 double R11, R12, R13, R21, R22, R23, R31, R32, R33; 1309 if (nRot == 9) 1310 { 1311 R11 = rot[0]; R12 = rot[1]; R13 = rot[2]; 1312 R21 = rot[3]; R22 = rot[4]; R23 = rot[5]; 1313 R31 = rot[6]; R32 = rot[7]; R33 = rot[8]; 1314 } 1315 if (nRot == 3) 1316 { 1317 Matx33d R; cv::Rodrigues(Matx31d(rot), R); 1318 rot = R.val; 1319 R11 = rot[0]; R12 = rot[1]; R13 = rot[2]; 1320 R21 = rot[3]; R22 = rot[4]; R23 = rot[5]; 1321 R31 = rot[6]; R32 = rot[7]; R33 = rot[8]; 1322 rot = params[0]; 1323 } 1324 1325 //5.ReProjection 1326 const Vec2d* data2D = point2D.ptr<Vec2d>(); 1327 const Vec3d* data3D = point3D.ptr<Vec3d>(); 1328 for (int k = 0; k < point3D.rows; ++k) 1329 { 1330 //5.1 WorldCoordinate 1331 double X = data3D[k][0]; 1332 double Y = data3D[k][1]; 1333 double Z = data3D[k][2]; 1334 1335 //5.2 CameraCoordinate 1336 double x = R11 * X + R12 * Y + R13 * Z + tx; 1337 double y = R21 * X + R22 * Y + R23 * Z + ty; 1338 double z = R31 * X + R32 * Y + R33 * Z + tz; 1339 1340 //5.3 StandardPhysicsCoordinate 1341 double iz = 1. / z; //if denominator==0 1342 double xc = x * iz; 1343 double yc = y * iz; 1344 1345 //5.4 DistortionPhysicsCoordinate 1346 double xc2 = xc * xc; 1347 double yc2 = yc * yc; 1348 double d2 = xc2 + yc2; 1349 double xcyc = 2. * xc * yc; 1350 double d4 = d2 * d2; 1351 double d6 = d2 * d4; 1352 double d2xc2 = d2 + 2. * xc2; 1353 double d2yc2 = d2 + 2. * yc2; 1354 double nu = 0, de = 1., xd = 0, yd = 0; 1355 if (nDist < 5) 1356 { 1357 nu = 1. + k1 * d2 + k2 * d4; 1358 xd = xc * nu + p1 * xcyc + p2 * d2xc2; 1359 yd = yc * nu + p2 * xcyc + p1 * d2yc2; 1360 } 1361 else if (nDist < 8) 1362 { 1363 nu = 1. + k1 * d2 + k2 * d4 + k3 * d6; 1364 xd = xc * nu + p1 * xcyc + p2 * d2xc2; 1365 yd = yc * nu + p2 * xcyc + p1 * d2yc2; 1366 } 1367 else if (nDist < 12) 1368 { 1369 nu = 1. + k1 * d2 + k2 * d4 + k3 * d6; 1370 de = 1. / (1. + k4 * d2 + k5 * d4 + k6 * d6);//if denominator==0 1371 xd = xc * nu * de + p1 * xcyc + p2 * d2xc2; 1372 yd = yc * nu * de + p2 * xcyc + p1 * d2yc2; 1373 } 1374 else if (nDist < 14) 1375 { 1376 nu = 1. + k1 * d2 + k2 * d4 + k3 * d6; 1377 de = 1. / (1. + k4 * d2 + k5 * d4 + k6 * d6);//if denominator==0 1378 xd = xc * nu * de + p1 * xcyc + p2 * d2xc2 + s1 * d2 + s2 * d4; 1379 yd = yc * nu * de + p2 * xcyc + p1 * d2yc2 + s3 * d2 + s4 * d4; 1380 } 1381 err2D[k][0] = xd * fx + cx - data2D[k][0]; 1382 err2D[k][1] = yd * fy + cy - data2D[k][1]; 1383 if (!dpdR.empty() || !dpdt.empty()) 1384 { 1385 Mat_<double> dpDdpCD({ 2, 2 }, { fx, 0, 0, fy }); 1386 1387 Mat_<double> dpCDdh({ 2, 5 }, { 1388 xc * de, -xc * nu * de * de, p2 + s1 + 2 * s2 * d2, nu * de + 2 * p1 * yc + 4 * p2 * xc, 2 * p1 * xc, 1389 yc * de, -yc * nu * de * de, p1 + s3 + 2 * s4 * d2, 2 * p2 * yc, nu * de + 2 * p2 * xc + 4 * p1 * yc }); 1390 1391 Mat_<double> dhdpC({ 5, 2 }, { 1392 xc * (2 * k1 + 4 * k2 * d2 + 6 * k3 * d4), yc * (2 * k1 + 4 * k2 * d2 + 6 * k3 * d4), 1393 xc * (2 * k4 + 4 * k5 * d2 + 6 * k6 * d4), yc * (2 * k4 + 4 * k5 * d2 + 6 * k6 * d4), 1394 2 * xc, 2 * yc, 1395 1, 0, 1396 0, 1 }); 1397 1398 Mat_<double> dpCdPC({ 2, 3 }, { 1399 iz, 0, -xc * iz, 1400 0, iz, -yc * iz }); 1401 1402 if (!dpdt.empty()) dpdt.rowRange(2 * k, 2 * k + 2) = dpDdpCD * dpCDdh * dhdpC * dpCdPC; 1403 if (!dpdR.empty() && nRot == 9) 1404 { 1405 Mat_<double> dPCdR({ 3, 9 }, { 1406 X, Y, Z, 0, 0, 0, 0, 0, 0, 1407 0, 0, 0, X, Y, Z, 0, 0, 0, 1408 0, 0, 0, 0, 0, 0, X, Y, Z }); 1409 dpdR.rowRange(2 * k, 2 * k + 2) = dpDdpCD * dpCDdh * dhdpC * dpCdPC * dPCdR; 1410 } 1411 if (!dpdR.empty() && nRot == 3) 1412 { 1413 Mat_<double> _skewPC({ 3, 3 }, { 1414 0, (z - tz), -(y - ty), 1415 -(z - tz), 0, (x - tx), 1416 (y - ty), -(x - tx), 0 }); 1417 dpdR.rowRange(2 * k, 2 * k + 2) = dpDdpCD * dpCDdh * dhdpC * dpCdPC * _skewPC; 1418 } 1419 } 1420 if (!dpdD.empty()) 1421 { 1422 Mat_<double> dpDdpCD(2, 2); 1423 dpDdpCD << fx, 0, 0, fy; 1424 1425 Mat_<double> dpCDdD(2, nDist); 1426 if (nDist < 5) 1427 { 1428 dpCDdD << 1429 xc * d2 * de, xc* d4* de, xcyc, d2xc2, 1430 yc* d2* de, yc* d4* de, d2yc2, xcyc; 1431 } 1432 else if (nDist < 8) 1433 { 1434 dpCDdD << 1435 xc * d2 * de, xc* d4* de, xcyc, d2xc2, xc* d6* de, 1436 yc* d2* de, yc* d4* de, d2yc2, xcyc, yc* d6* de; 1437 } 1438 else if (nDist < 12) 1439 { 1440 dpCDdD << 1441 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, 1442 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; 1443 } 1444 else if (nDist < 14) 1445 { 1446 dpCDdD << 1447 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, 1448 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; 1449 } 1450 dpdD.rowRange(2 * k, 2 * k + 2) = dpDdpCD * dpCDdD; 1451 } 1452 if (!dpdK.empty()) Mat_<double>({ 2, 4 }, { xd, 0, 1, 0, 0, yd, 0, 1 }).copyTo(dpdK.rowRange(2 * k, 2 * k + 2)); 1453 } 1454 1455 return true; 1456 if (nRot == 9) 1457 { 1458 static const int nDist_ = 5; // set this value as nDist 1459 ceres::AutoDiffCostFunction<ProjectionModelKDRt, ceres::DYNAMIC, 9, 3, 4, nDist_> autoDeriv(new ProjectionModelKDRt(motionViewForDBG, nDist), motionViewForDBG.point3D.rows * 2); 1460 Mat_<double> dpdRDBG(point3D.rows * 2, 9); 1461 Mat_<double> dpdtDBG(point3D.rows * 2, 3); 1462 Mat_<double> dpdKDBG(point3D.rows * 2, 4); 1463 Mat_<double> dpdDDBG(point3D.rows * 2, nDist); 1464 Mat_<Vec2d> err2DDBG(point3D.rows, 1); 1465 const double* parametersDBG[4] = { rot, t, K, D }; 1466 double* jacobiansDBG[4] = { dpdRDBG.ptr<double>(), dpdtDBG.ptr<double>(), dpdKDBG.ptr<double>(), dpdDDBG.ptr<double>() }; 1467 double* residualsDBG = err2DDBG.ptr<double>(); 1468 autoDeriv.Evaluate(parametersDBG, residualsDBG, jacobiansDBG); 1469 1470 if (!dpdR.empty()) cout << endl << norm(dpdR, dpdRDBG, NORM_INF) << endl; 1471 if (!dpdt.empty()) cout << endl << norm(dpdt, dpdtDBG, NORM_INF) << endl; 1472 if (!dpdK.empty()) cout << endl << norm(dpdK, dpdKDBG, NORM_INF) << endl; 1473 if (!dpdD.empty()) cout << endl << norm(dpdD, dpdDDBG, NORM_INF) << endl; 1474 cout << endl << norm(Mat_<Vec2d>(point2D.rows, 1, err2D), err2DDBG, NORM_INF) << endl; 1475 std::getchar(); 1476 } 1477 return true; 1478 } 1479 }; 1480 }; 1481 1482 class OptimizeRtWithNormlizedPoint2D 1483 { 1484 public: 1485 using MotionView = MotionSim::MotionView; 1486 static void TestMe(int argc = 0, char** argv = 0) 1487 { 1488 int N = 99; 1489 for (int k = 0; k < N; ++k) 1490 { 1491 //0.GenerateData 1492 bool world2D = k % 2; 1493 int rotMode = k % 7 + 1; 1494 MotionSim motionSim(false); 1495 motionSim.camFovX = 90; 1496 motionSim.camFovY = 90; 1497 motionSim.camRand = 10; 1498 motionSim.nMinMotion = 16;//2 1499 motionSim.nMaxMotion = 32;//4 1500 motionSim.rndSeek = false; 1501 motionSim.nCamDist = 5; 1502 motionSim.runMotion(world2D, false, rotMode); 1503 //motionSim.visMotion(); 1504 int rndInd = int(motionSim.motionViews.size() * cv::randu<double>()); 1505 Mat_<double> r0 = -motionSim.motionViews[rndInd].r; 1506 Mat_<double> t0 = -motionSim.motionViews[rndInd].R.t() * motionSim.motionViews[rndInd].t; 1507 const MotionView& motionView = motionSim.motionViews[rndInd]; 1508 double errRatio = 0.9; 1509 1510 //1.CalcByCeresWithAutoDeriv 1511 Mat_<double> R1; cv::Rodrigues(r0 * errRatio, R1); 1512 Mat_<double> t1 = t0 * errRatio; 1513 ceres::Problem problem1; 1514 problem1.AddResidualBlock(new ceres::AutoDiffCostFunction<RotationModelRt, ceres::DYNAMIC, 9, 3>( 1515 new RotationModelRt(motionView), motionView.point3D.rows * 2), NULL, R1.ptr<double>(), t1.ptr<double>()); 1516 problem1.SetParameterization(R1.ptr<double>(), new OptimizeRt::LocalParamRWithGeneralJ); 1517 ceres::Solver::Options options1; 1518 ceres::Solver::Summary summary1; 1519 ceres::Solve(options1, &problem1, &summary1); 1520 Mat_<double> r1; cv::Rodrigues(R1, r1); 1521 int nIter1 = (int)summary1.iterations.size(); 1522 1523 //2.CalcByCeresWithGeneralJ 1524 Mat_<double> R2; cv::Rodrigues(r0 * errRatio, R2); 1525 Mat_<double> t2 = t0 * errRatio; 1526 ceres::Problem problem2; 1527 problem2.AddResidualBlock(new RotationCostRt(motionView, motionSim.nCamDist, 9), NULL, R2.ptr<double>(), t2.ptr<double>()); 1528 problem2.SetParameterization(R2.ptr<double>(), new OptimizeRt::LocalParamRWithGeneralJ); 1529 ceres::Solver::Options options2; 1530 ceres::Solver::Summary summary2; 1531 ceres::Solve(options2, &problem2, &summary2); 1532 Mat_<double> r2; cv::Rodrigues(R2, r2); 1533 int nIter2 = (int)summary2.iterations.size(); 1534 1535 //3.CalcByCeresWithGeneralJ 1536 Mat_<double> r3 = r0 * errRatio; 1537 Mat_<double> t3 = t0 * errRatio; 1538 ceres::Problem problem3; 1539 problem3.AddResidualBlock(new RotationCostRt(motionView, motionSim.nCamDist, 3), NULL, r3.ptr<double>(), t3.ptr<double>()); 1540 problem3.SetParameterization(r3.ptr<double>(), new OptimizeRt::LocalParamRWithIdentJ); 1541 ceres::Solver::Options options3; 1542 ceres::Solver::Summary summary3; 1543 ceres::Solve(options3, &problem3, &summary3); 1544 int nIter3 = (int)summary3.iterations.size(); 1545 1546 //4.AnalyzeError 1547 double infr0r0 = norm(r0, r0 * errRatio, NORM_INF); 1548 double infr0r1 = norm(r0, r1, NORM_INF); 1549 double infr0r2 = norm(r0, r2, NORM_INF); 1550 double infr0r3 = norm(r0, r3, NORM_INF); 1551 double inft0t0 = norm(t0, t0 * errRatio, NORM_INF); 1552 double inft0t1 = norm(t0, t1, NORM_INF); 1553 double inft0t2 = norm(t0, t2, NORM_INF); 1554 double inft0t3 = norm(t0, t3, NORM_INF); 1555 1556 //5.PrintError 1557 cout << fmt::format("LoopCount: {} AutoDeriv.iters: {} GeneralJ.iters: {} IdentJ.iters: {}\n", k, nIter1, nIter2, nIter3); 1558 if (infr0r1 > 1e-8 || infr0r2 > 1e-8 || infr0r3 > 1e-8 || inft0t1 > 1e-8 || inft0t2 > 1e-8 || inft0t3 > 1e-8) 1559 { 1560 cout << fmt::format("infr0r1: {:<15.9}\t\t{:<15.9}\n", infr0r1, infr0r0); 1561 cout << fmt::format("infr0r2: {:<15.9}\t\t{:<15.9}\n", infr0r2, infr0r0); 1562 cout << fmt::format("infr0r3: {:<15.9}\t\t{:<15.9}\n", infr0r3, infr0r0); 1563 cout << fmt::format("inft0t1: {:<15.9}\t\t{:<15.9}\n", inft0t1, inft0t0); 1564 cout << fmt::format("inft0t2: {:<15.9}\t\t{:<15.9}\n", inft0t2, inft0t0); 1565 cout << fmt::format("inft0t3: {:<15.9}\t\t{:<15.9}\n", inft0t3, inft0t0); 1566 cout << "Press any key to continue" << endl; std::getchar(); 1567 } 1568 } 1569 } 1570 1571 public: 1572 struct RotationModelRt 1573 { 1574 Mat_<Vec2d> point2D; 1575 Mat_<Vec3d> point3D; 1576 RotationModelRt(const MotionView& motionView0) 1577 { 1578 point3D = motionView0.point3D; 1579 cv::projectPoints(point3D, -motionView0.r, -motionView0.R.t() * motionView0.t, Mat_<double>::eye(3, 3), Mat_<double>::zeros(motionView0.D.rows, 1), point2D); 1580 //cv::undistortPoints(motionView0.point2D, point2D, motionView0.K, motionView0.D); 1581 } 1582 template <typename tp> bool operator()(const tp* const rot, const tp* const t, tp* errPoint2D) const 1583 { 1584 //1.Translation params 1585 tp tx = t[0]; 1586 tp ty = t[1]; 1587 tp tz = t[2]; 1588 1589 //2.Rotation params 1590 tp R11, R12, R13, R21, R22, R23, R31, R32, R33; 1591 { 1592 R11 = rot[0]; R12 = rot[1]; R13 = rot[2]; 1593 R21 = rot[3]; R22 = rot[4]; R23 = rot[5]; 1594 R31 = rot[6]; R32 = rot[7]; R33 = rot[8]; 1595 } 1596 1597 //3.ReProjection 1598 const Vec2d* data2D = point2D.ptr<Vec2d>(); 1599 const Vec3d* data3D = point3D.ptr<Vec3d>(); 1600 Vec<tp, 2>* err2D = (Vec<tp, 2>*)errPoint2D; 1601 for (int k = 0; k < point3D.rows; ++k) 1602 { 1603 //3.1 WorldCoordinate 1604 double X = data3D[k][0]; 1605 double Y = data3D[k][1]; 1606 double Z = data3D[k][2]; 1607 1608 //3.2 CameraCoordinate 1609 tp x = R11 * X + R12 * Y + R13 * Z + tx; 1610 tp y = R21 * X + R22 * Y + R23 * Z + ty; 1611 tp z = R31 * X + R32 * Y + R33 * Z + tz; 1612 1613 //3.3 StandardPhysicsCoordinate 1614 tp iz = 1. / z; //if denominator==0 1615 tp xc = x * iz; 1616 tp yc = y * iz; 1617 err2D[k][0] = xc - data2D[k][0]; 1618 err2D[k][1] = yc - data2D[k][1]; 1619 } 1620 return true; 1621 } 1622 }; 1623 struct RotationCostRt : public ceres::CostFunction 1624 { 1625 const int nDist; 1626 double K[4]; 1627 const double* D; 1628 Mat_<Vec2d> point2D; 1629 Mat_<Vec3d> point3D; 1630 const int nRot; 1631 const MotionView& motionViewForDBG;//only use once 1632 RotationCostRt(const MotionView& motionView0, const int nDist0, const int nRot0) : nDist(nDist0), nRot(nRot0), motionViewForDBG(motionView0) 1633 { 1634 K[0] = motionView0.K(0, 0); 1635 K[1] = motionView0.K(1, 1); 1636 K[2] = motionView0.K(0, 2); 1637 K[3] = motionView0.K(1, 2); 1638 D = motionView0.D.ptr<double>(); 1639 point2D = motionView0.point2D; 1640 point3D = motionView0.point3D; 1641 this->set_num_residuals(point2D.rows * 2); 1642 this->mutable_parameter_block_sizes()->push_back(nRot); 1643 this->mutable_parameter_block_sizes()->push_back(3); 1644 cv::projectPoints(point3D, -motionView0.r, -motionView0.R.t() * motionView0.t, Mat_<double>::eye(3, 3), Mat_<double>::zeros(motionView0.D.rows, 1), point2D); 1645 //cv::undistortPoints(motionView0.point2D, point2D, motionView0.K, motionView0.D); 1646 } 1647 bool Evaluate(double const* const* params, double* residuals, double** jacobians) const 1648 { 1649 //0.FormatInput 1650 const double* rot = params[0]; 1651 const double* t = params[1]; 1652 Vec2d* err2D = (Vec2d*)(residuals); 1653 Mat_<double> dpdR; if (jacobians && jacobians[0]) dpdR = Mat_<double>(point2D.rows * 2, nRot, jacobians[0]); 1654 Mat_<double> dpdt; if (jacobians && jacobians[1]) dpdt = Mat_<double>(point2D.rows * 2, 3, jacobians[1]); 1655 1656 //1.Translation params 1657 double tx = t[0]; 1658 double ty = t[1]; 1659 double tz = t[2]; 1660 1661 //2.Rotation params 1662 double R11, R12, R13, R21, R22, R23, R31, R32, R33; 1663 if (nRot == 9) 1664 { 1665 R11 = rot[0]; R12 = rot[1]; R13 = rot[2]; 1666 R21 = rot[3]; R22 = rot[4]; R23 = rot[5]; 1667 R31 = rot[6]; R32 = rot[7]; R33 = rot[8]; 1668 } 1669 if (nRot == 3) 1670 { 1671 Matx33d R; cv::Rodrigues(Matx31d(rot), R); 1672 rot = R.val; 1673 R11 = rot[0]; R12 = rot[1]; R13 = rot[2]; 1674 R21 = rot[3]; R22 = rot[4]; R23 = rot[5]; 1675 R31 = rot[6]; R32 = rot[7]; R33 = rot[8]; 1676 rot = params[0]; 1677 } 1678 1679 //3.ReProjection 1680 const Vec2d* data2D = point2D.ptr<Vec2d>(); 1681 const Vec3d* data3D = point3D.ptr<Vec3d>(); 1682 for (int k = 0; k < point3D.rows; ++k) 1683 { 1684 //5.1 WorldCoordinate 1685 double X = data3D[k][0]; 1686 double Y = data3D[k][1]; 1687 double Z = data3D[k][2]; 1688 1689 //5.2 CameraCoordinate 1690 double x = R11 * X + R12 * Y + R13 * Z + tx; 1691 double y = R21 * X + R22 * Y + R23 * Z + ty; 1692 double z = R31 * X + R32 * Y + R33 * Z + tz; 1693 1694 //5.3 StandardPhysicsCoordinate 1695 double iz = 1. / z; //if denominator==0 1696 double xc = x * iz; 1697 double yc = y * iz; 1698 err2D[k][0] = xc - data2D[k][0]; 1699 err2D[k][1] = yc - data2D[k][1]; 1700 if (!dpdR.empty() || !dpdt.empty()) 1701 { 1702 Mat_<double> dpCdPC({ 2, 3 }, { 1703 iz, 0, -xc * iz, 1704 0, iz, -yc * iz }); 1705 if (!dpdt.empty()) dpCdPC.copyTo(dpdt.rowRange(2 * k, 2 * k + 2)); 1706 if (!dpdR.empty() && nRot == 9) 1707 { 1708 Mat_<double> dPCdR({ 3, 9 }, { 1709 X, Y, Z, 0, 0, 0, 0, 0, 0, 1710 0, 0, 0, X, Y, Z, 0, 0, 0, 1711 0, 0, 0, 0, 0, 0, X, Y, Z }); 1712 dpdR.rowRange(2 * k, 2 * k + 2) = dpCdPC * dPCdR; 1713 } 1714 if (!dpdR.empty() && nRot == 3) 1715 { 1716 Mat_<double> _skewPC({ 3, 3 }, { 1717 0, (z - tz), -(y - ty), 1718 -(z - tz), 0, (x - tx), 1719 (y - ty), -(x - tx), 0 }); 1720 dpdR.rowRange(2 * k, 2 * k + 2) = dpCdPC * _skewPC; 1721 } 1722 } 1723 } 1724 1725 return true; 1726 if (nRot == 9) 1727 { 1728 ceres::AutoDiffCostFunction<RotationModelRt, ceres::DYNAMIC, 9, 3> autoDeriv(new RotationModelRt(motionViewForDBG), motionViewForDBG.point3D.rows * 2); 1729 Mat_<double> dpdRDBG(point3D.rows * 2, 9); 1730 Mat_<double> dpdtDBG(point3D.rows * 2, 3); 1731 Mat_<Vec2d> err2DDBG(point3D.rows, 1); 1732 const double* parametersDBG[2] = { rot, t }; 1733 double* jacobiansDBG[2] = { dpdRDBG.ptr<double>(), dpdtDBG.ptr<double>() }; 1734 double* residualsDBG = err2DDBG.ptr<double>(); 1735 autoDeriv.Evaluate(parametersDBG, residualsDBG, jacobiansDBG); 1736 1737 if (!dpdR.empty()) cout << endl << norm(dpdR, dpdRDBG, NORM_INF) << endl; 1738 if (!dpdt.empty()) cout << endl << norm(dpdt, dpdtDBG, NORM_INF) << endl; 1739 cout << endl << norm(Mat_<Vec2d>(point2D.rows, 1, err2D), err2DDBG, NORM_INF) << endl; 1740 std::getchar(); 1741 } 1742 return true; 1743 } 1744 }; 1745 }; 1746 1747 int main(int argc, char** argv) { OptimizeRtWithNormlizedPoint2D::TestMe(argc, argv); return 0; } 1748 int main1(int argc, char** argv) { OptimizeRt::TestMe(argc, argv); return 0; } 1749 int main2(int argc, char** argv) { OptimizeKDRt::TestMe(argc, argv); return 0; } 1750 int main3(int argc, char** argv) { OptimizeRtWithNormlizedPoint2D::TestMe(argc, argv); return 0; }