137 v.
K=1+v.
R*(cam.
k1()+v.
R*(cam.
k2()+v.
R*cam.
k3()));
138 T xy=v.
x_*v.
y_,p1=cam.
p1(),p2=cam.
p2();
143 template<
typename T,
typename POINT,
typename PIXEL>
inline void getFullProjection(
const POINT &pIn,PIXEL &pOut)
const {
145 getTemporaryVariablesForTransform(pIn,tmp);
146 getFullProjectionT(tmp,pOut);
150 pOut[0]=cam.
fx()*tmp.
x__+cam.
cx();
151 pOut[1]=cam.
fy()*tmp.
y__+cam.
cy();
154 template<
typename T,
typename POINT,
typename MATRIX>
inline void getFullJacobian(
const POINT &pIn,MATRIX &mOut)
const {
156 getTemporaryVariablesForTransform(pIn,tmp);
157 getFullJacobianT(pIn,tmp,mOut);
165 T tmpK=2*(cam.
k1()+tmp.
R*(2*cam.
k2()+3*tmp.
R*cam.
k3()));
170 J21.set_unsafe(0,0,yx2);
171 J21.set_unsafe(0,1,x_);
172 J21.set_unsafe(0,2,0);
173 J21.set_unsafe(1,0,zx2);
174 J21.set_unsafe(1,1,0);
175 J21.set_unsafe(1,2,x_);
176 J21.set_unsafe(2,0,tmpKx*yx2+tmpKy*zx2);
177 J21.set_unsafe(2,1,tmpKx*x_);
178 J21.set_unsafe(2,2,tmpKy*x_);
180 T pxpy=2*(cam.
p1()*tmp.
x_+cam.
p2()*tmp.
y_);
181 T p1y=cam.
p1()*tmp.
y_;
182 T p2x=cam.
p2()*tmp.
x_;
184 T fx=cam.
fx(),fy=cam.
fy();
185 J43.set_unsafe(0,0,fx*(tmp.
K+2*p1y+6*p2x));
186 J43.set_unsafe(0,1,fx*pxpy);
187 J43.set_unsafe(0,2,fx*tmp.
x_);
188 J43.set_unsafe(1,0,fy*pxpy);
189 J43.set_unsafe(1,1,fy*(tmp.
K+6*p1y+2*p2x));
190 J43.set_unsafe(1,2,fy*tmp.
y_);
191 mOut.multiply(J43,J21);
201 res.set_unsafe(0,1,0);
202 res.set_unsafe(1,0,0);
207 res.set_unsafe(0,0,1);
208 res.set_unsafe(0,1,0);
209 res.set_unsafe(1,0,0);
210 res.set_unsafe(1,1,1);
215 res.set_unsafe(0,1,0);
216 res.set_unsafe(0,2,0);
217 res.set_unsafe(1,0,0);
218 res.set_unsafe(1,2,0);
219 res.set_unsafe(2,0,0);
220 res.set_unsafe(2,1,0);
221 res.set_unsafe(2,2,1);
222 res.set_unsafe(2,3,0);
239 double cx=cam.
cx(),cy=cam.
cy(),ifx=1/cam.
fx(),ify=1/cam.
fy();
240 double K1=cam.
k1(),K2=cam.
k2(),p1=cam.
p1(),p2=cam.
p2(),K3=cam.
k3();
242 tmp1[0]=(pIn[0]-cx)*ifx;
243 tmp1[1]=(pIn[1]-cy)*ify;
244 J1.set_unsafe(0,0,ifx);
245 J1.set_unsafe(1,1,ify);
250 double K123=-K1*sK1+2*K1*K2-K3;
252 tmp1[3]=1+tmp1[2]*(-K1+tmp1[2]*(K12+tmp1[2]*K123));
253 J2.set_unsafe(2,0,2*tmp1[0]);
254 J2.set_unsafe(2,1,2*tmp1[1]);
255 double jTemp=-2*K1+4*tmp1[2]*K12+6*
square(tmp1[2])*K123;
256 J2.set_unsafe(3,0,tmp1[0]*jTemp);
257 J2.set_unsafe(3,1,tmp1[1]*jTemp);
259 tmp2[0]=tmp1[0]*tmp1[3];
260 tmp2[1]=tmp1[1]*tmp1[3];
261 J3.set_unsafe(0,0,tmp1[3]);
262 J3.set_unsafe(0,3,tmp1[0]);
263 J3.set_unsafe(1,1,tmp1[3]);
264 J3.set_unsafe(1,3,tmp1[1]);
266 double prod=tmp2[0]*tmp2[1];
268 pOut[0]=tmp2[0]-p1*prod-p2*(tmp1[2]+2*
square(tmp2[0]));
269 pOut[1]=tmp2[1]-p1*(tmp1[2]+2*
square(tmp2[1]))-p2*prod;
270 J4.set_unsafe(0,0,1-p1*tmp2[1]-4*p2*tmp2[0]);
271 J4.set_unsafe(0,1,-p1*tmp2[0]);
272 J4.set_unsafe(0,2,-p2);
273 J4.set_unsafe(1,0,-p2*tmp2[1]);
274 J4.set_unsafe(1,1,1-4*p1*tmp2[1]-p2*tmp2[0]);
275 J4.set_unsafe(1,2,-p1);
277 jOut.multiply_ABC(J4,J3,J2);
278 jOut.multiply(jOut,J1);
285 #endif //__CCamModel_H