6 : ndi(ci), kp(q), stereo(stereo),
7 isValid(true), useCovar(false), pointPlane(false) {}
10 : ndi(ci), kp(q(0), q(1), 0),
11 stereo(false), isValid(true), useCovar(false), pointPlane(false) {}
14 : ndi(0), kp(0, 0, 0),
15 stereo(false), isValid(false), useCovar(false), pointPlane(false) {}
38 return err.head<2>().norm();
44 return err.squaredNorm();
46 return err.head<2>().squaredNorm();
63 Eigen::Matrix<double,3,1> pc = nd.
w2n * pt;
66 Eigen::Matrix<double,2,6> jacc;
69 Eigen::Matrix<double,2,3> jacp;
76 double ipz2 = 1.0/(pz*pz);
77 if (std::isnan(ipz2) ) { printf(
"[SetJac] infinite jac\n"); *(
int *)0x0 = 0; }
79 double ipz2fx = ipz2*nd.
Kcam(0,0);
80 double ipz2fy = ipz2*nd.
Kcam(1,1);
82 double ipz2fxq =
qScale*ipz2fx;
83 double ipz2fyq =
qScale*ipz2fy;
84 Eigen::Matrix<double,3,1> pwt;
87 pwt = (pt-nd.
trans).head<3>();
90 Eigen::Matrix<double,3,1> dp = nd.
dRdx * pwt;
91 jacc(0,3) = (pz*dp(0) - px*dp(2))*ipz2fxq;
92 jacc(1,3) = (pz*dp(1) - py*dp(2))*ipz2fyq;
95 jacc(0,4) = (pz*dp(0) - px*dp(2))*ipz2fxq;
96 jacc(1,4) = (pz*dp(1) - py*dp(2))*ipz2fyq;
99 jacc(0,5) = (pz*dp(0) - px*dp(2))*ipz2fxq;
100 jacc(1,5) = (pz*dp(1) - py*dp(2))*ipz2fyq;
104 jacc(0,0) = (pz*dp(0) - px*dp(2))*ipz2fx;
105 jacc(1,0) = (pz*dp(1) - py*dp(2))*ipz2fy;
107 jacc(0,1) = (pz*dp(0) - px*dp(2))*ipz2fx;
108 jacc(1,1) = (pz*dp(1) - py*dp(2))*ipz2fy;
110 jacc(0,2) = (pz*dp(0) - px*dp(2))*ipz2fx;
111 jacc(1,2) = (pz*dp(1) - py*dp(2))*ipz2fy;
116 jacp(0,0) = (pz*dp(0) - px*dp(2))*ipz2fx;
117 jacp(1,0) = (pz*dp(1) - py*dp(2))*ipz2fy;
119 jacp(0,1) = (pz*dp(0) - px*dp(2))*ipz2fx;
120 jacp(1,1) = (pz*dp(1) - py*dp(2))*ipz2fy;
122 jacp(0,2) = (pz*dp(0) - px*dp(2))*ipz2fx;
123 jacp(1,2) = (pz*dp(1) - py*dp(2))*ipz2fy;
126 for (
int i=0; i<2; i++)
127 for (
int j=0; j<6; j++)
128 if (std::isnan(jacc(i,j)) ) { printf(
"[SetJac] NaN in jacc(%d,%d)\n", i, j); *(
int *)0x0 = 0; }
132 jpp->
Hpp = jacp.transpose() * jacp;
133 jpp->
Hcc = jacc.transpose() * jacc;
134 jpp->
Hpc = jacp.transpose() * jacc;
135 jpp->
JcTE = jacc.transpose() *
err.head<2>();
136 jpp->
Bp = jacp.transpose() *
err.head<2>();
145 Eigen::Vector3d p1 = nd.
w2i * pt;
150 printf(
"[CalcErr] negative Z! Node %d \n",
ndi);
151 if (std::isnan(
err[0]) || std::isnan(
err[1]) ) printf(
"[CalcErr] NaN!\n");
153 err = Eigen::Vector3d(0.0,0.0,0.0);
157 err.head<2>() = p1.head<2>()/p1(2);
167 double b2 = huber*huber;
168 double e2 =
err.head<2>().squaredNorm();
171 double c = 2.0*huber*sqrt(e2) - b2;
172 double w = sqrt(c/e2);
186 return err.head<2>().squaredNorm();
193 Eigen::Matrix<double,3,1> pc = nd.
w2n * pt;
196 Eigen::Matrix<double,3,3> jacp;
199 Eigen::Matrix<double,3,6> jacc;
206 double ipz2 = 1.0/(pz*pz);
207 if (std::isnan(ipz2) ) { printf(
"[SetJac] infinite jac\n"); *(
int *)0x0 = 0; }
209 double ipz2fx = ipz2*nd.
Kcam(0,0);
210 double ipz2fy = ipz2*nd.
Kcam(1,1);
213 double ipz2fxq =
qScale*ipz2fx;
214 double ipz2fyq =
qScale*ipz2fy;
215 Eigen::Matrix<double,3,1> pwt;
218 pwt = (pt-nd.
trans).head(3);
221 Eigen::Matrix<double,3,1> dp = nd.
dRdx * pwt;
222 jacc(0,3) = (pz*dp(0) - px*dp(2))*ipz2fxq;
223 jacc(1,3) = (pz*dp(1) - py*dp(2))*ipz2fyq;
224 jacc(2,3) = (pz*dp(0) - (px-b)*dp(2))*ipz2fxq;
227 jacc(0,4) = (pz*dp(0) - px*dp(2))*ipz2fxq;
228 jacc(1,4) = (pz*dp(1) - py*dp(2))*ipz2fyq;
229 jacc(2,4) = (pz*dp(0) - (px-b)*dp(2))*ipz2fxq;
232 jacc(0,5) = (pz*dp(0) - px*dp(2))*ipz2fxq;
233 jacc(1,5) = (pz*dp(1) - py*dp(2))*ipz2fyq;
234 jacc(2,5) = (pz*dp(0) - (px-b)*dp(2))*ipz2fxq;
238 jacc(0,0) = (pz*dp(0) - px*dp(2))*ipz2fxq;
239 jacc(1,0) = (pz*dp(1) - py*dp(2))*ipz2fy;
240 jacc(2,0) = (pz*dp(0) - (px-b)*dp(2))*ipz2fxq;
242 jacc(0,1) = (pz*dp(0) - px*dp(2))*ipz2fxq;
243 jacc(1,1) = (pz*dp(1) - py*dp(2))*ipz2fy;
244 jacc(2,1) = (pz*dp(0) - (px-b)*dp(2))*ipz2fxq;
246 jacc(0,2) = (pz*dp(0) - px*dp(2))*ipz2fxq;
247 jacc(1,2) = (pz*dp(1) - py*dp(2))*ipz2fy;
248 jacc(2,2) = (pz*dp(0) - (px-b)*dp(2))*ipz2fxq;
253 jacp(0,0) = (pz*dp(0) - px*dp(2))*ipz2fxq;
254 jacp(1,0) = (pz*dp(1) - py*dp(2))*ipz2fy;
255 jacp(2,0) = (pz*dp(0) - (px-b)*dp(2))*ipz2fxq;
257 jacp(0,1) = (pz*dp(0) - px*dp(2))*ipz2fxq;
258 jacp(1,1) = (pz*dp(1) - py*dp(2))*ipz2fy;
259 jacp(2,1) = (pz*dp(0) - (px-b)*dp(2))*ipz2fxq;
261 jacp(0,2) = (pz*dp(0) - px*dp(2))*ipz2fxq;
262 jacp(1,2) = (pz*dp(1) - py*dp(2))*ipz2fy;
263 jacp(2,2) = (pz*dp(0) - (px-b)*dp(2))*ipz2fxq;
266 for (
int i=0; i<2; i++)
267 for (
int j=0; j<6; j++)
268 if (std::isnan(jacc(i,j)) ) { printf(
"[SetJac] NaN in jacc(%d,%d)\n", i, j); *(
int *)0x0 = 0; }
277 jpp->
Hpp = jacp.transpose() * jacp;
278 jpp->
Hcc = jacc.transpose() * jacc;
279 jpp->
Hpc = jacp.transpose() * jacc;
280 jpp->
JcTE = jacc.transpose() *
err;
281 jpp->
Bp = jacp.transpose() *
err;
290 Eigen::Vector3d p1 = nd.
w2i * pt;
291 Eigen::Vector3d p2 = nd.
w2n * pt;
292 Eigen::Vector3d pb(nd.
baseline,0,0);
305 p1 = nd.
w2i*Eigen::Vector4d(projpt.x(), projpt.y(), projpt.z(), 1.0);
306 p2 = nd.
w2n*Eigen::Vector4d(projpt.x(), projpt.y(), projpt.z(), 1.0);
309 double invp1 = 1.0/p1(2);
311 err.head<2>() = p1.head<2>()*invp1;
313 p2 = nd.
Kcam*(p2-pb);
315 err(2) = p2(0)/p2(2);
319 printf(
"[CalcErr] negative Z! Node %d\n",
ndi);
320 if (std::isnan(
err[0]) || std::isnan(
err[1]) ) printf(
"[CalcErr] NaN!\n");
328 if (abs(
err(0)) > 1e6 || abs(
err(1)) > 1e6 || abs(
err(2)) > 1e6)
330 printf(
"\n\n[CalcErr] Excessive error.\n");
341 double b2 = huber*huber;
342 double e2 =
err.squaredNorm();
345 double c = 2.0*huber*sqrt(e2) - b2;
346 double w = sqrt(c/e2);
352 return err.squaredNorm();