00001
00002
00003 #ifndef CAMERA_BARRETO_HPP
00004 #define CAMERA_BARRETO_HPP
00005
00006 #include "kernel/keyValueFile.hpp"
00007 #include "camera/cameraPinhole.hpp"
00008 #include "jmath/jblas.hpp"
00009
00010 namespace jafar {
00011 namespace camera {
00012
00017 class CameraBarreto : public jafar::kernel::KeyValueFileLoad {
00018
00019 public:
00020
00022 jafar::camera::CameraPinhole camera;
00023
00025 double xi;
00026
00028 double phi;
00029
00031 double mirrorDiameter;
00032
00034 double mirrorCenterU;
00036 double mirrorCenterV;
00037
00038 jblas::vec2 getMirrorCenter() const {
00039 jblas::vec2 mc;
00040 mc(0) = mirrorCenterU;
00041 mc(1) = mirrorCenterV;
00042 return mc;
00043 }
00044
00046 double imageRadius;
00047
00049 double maskRadius;
00050
00051 void applyScale(double scale);
00052
00053 template<class Vec, class Pix>
00054 bool project(Vec const& v, Pix& pix) const
00055 {
00056 JFR_PRECOND(v.size() == 3,
00057 "CameraBarreto::project: size of v does not match");
00058 JFR_PRECOND(pix.size() == 2,
00059 "CameraBarreto::project: size of pix does not match");
00060
00061 JFR_TRACE_BEGIN;
00062 jblas::vec3 vbar;
00063
00064 hbar(v, vbar);
00065
00066
00067 double d = xi - phi;
00068 vbar(0) *= d;
00069 vbar(1) *= d;
00070
00071 camera.project(vbar, pix);
00072
00073 double radius = ublas::norm_2(pix - getMirrorCenter());
00074 if ( radius <= imageRadius && radius > maskRadius )
00075 return true;
00076 else
00077 return false;
00078
00079 JFR_TRACE_END("CameraBarreto::project");
00080 }
00081
00082 template<class Vec, class Mat>
00083 void projectJac(Vec const& v, Mat& J) {
00084 JFR_PRECOND(v.size() == 3,
00085 "CameraBarreto::projectJac: size of v does not match");
00086 JFR_PRECOND(J.size1() == 2 && J.size2() == 3,
00087 "CameraBarreto::projectJac: size of J does not match");
00088
00089 double x = v(0);
00090 double y = v(1);
00091 double z = v(2);
00092
00093
00094 double t1 = xi - phi;
00095 double t2 = camera.alphaU * t1;
00096 double t3 = fabs(x);
00097 double t4 = t3 * t3;
00098 double t5 = fabs(y);
00099 double t6 = t5 * t5;
00100 double t7 = fabs(z);
00101 double t8 = t7 * t7;
00102 double t9 = t4 + t6 + t8;
00103 double t10 = sqrt(t9);
00104 double t11 = 0.1e1 / t10;
00105 double t13 = z * t11 - xi;
00106 double t14 = 0.1e1 / t13;
00107 double t15 = t11 * t14;
00108 double t17 = t2 * x;
00109 double t19 = 0.1e1 / t10 / t9;
00110 double t20 = t19 * t14;
00111 double t21 = fabs(x) / x;
00112 double t22 = t3 * t21;
00113 double t23 = t20 * t22;
00114 double t25 = t9 * t9;
00115 double t26 = 0.1e1 / t25;
00116 double t28 = t2 * x * t26;
00117 double t29 = t13 * t13;
00118 double t30 = 0.1e1 / t29;
00119 double t31 = t30 * z;
00120 double t32 = t31 * t22;
00121 double t35 = fabs(y) / y;
00122 double t36 = t5 * t35;
00123 double t37 = t20 * t36;
00124 double t39 = t31 * t36;
00125 double t42 = fabs(z) / z;
00126 double t43 = t7 * t42;
00127 double t44 = t20 * t43;
00128 double t50 = t11 * t30 * (t11 - z * t19 * t43);
00129 double t53 = camera.alphaV * t1;
00130 double t54 = t53 * y;
00131 double t57 = t53 * y * t26;
00132 J(0,0) = t2 * t15 - t17 * t23 + t28 * t32;
00133 J(0,1) = -t17 * t37 + t28 * t39;
00134 J(0,2) = -t17 * t44 - t17 * t50;
00135 J(1,0) = -t54 * t23 + t57 * t32;
00136 J(1,1) = t53 * t15 - t54 * t37 + t57 * t39;
00137 J(1,2) = -t54 * t44 - t54 * t50;
00138
00139
00140 }
00141
00143 template<class Pix, class Vec>
00144 void imageToCameraFrame(Pix const& pix, Vec& v) const
00145 {
00146 JFR_PRECOND(pix.size() == 2,
00147 "CameraBarreto::imageToCameraFrame: invalid pix size: " << pix.size());
00148 JFR_PRECOND(v.size() == 3,
00149 "CameraBarreto::imageToCameraFrame: invalid v size: " << v.size());
00150 JFR_TRACE_BEGIN;
00151
00152 jblas::vec3 vbar;
00153 camera.imageToCameraFrame(pix, vbar);
00154 double d = 1/(xi - phi);
00155 vbar(0) *= d;
00156 vbar(1) *= d;
00157
00158 hbarInv(vbar, v);
00159
00160 JFR_TRACE_END("CameraBarreto::imageToCameraFrame");
00161 }
00162
00166 template<class Pix, class Vec>
00167 void imageToCameraFrameDirection(Pix const& pix, Vec& v) const
00168 {
00169 JFR_TRACE_BEGIN;
00170 imageToCameraFrame(pix, v);
00171
00172 double n = ublas::norm_2(v);
00173 JFR_NUMERIC(n > camera.epsilon,
00174 "CameraBarreto::imageToCameraFrameDirection: vector too small");
00175 v /= n;
00176 JFR_TRACE_END("CameraBarreto::imageToCameraFrameDirection");
00177 }
00178
00179 template<class Pix, class Mat>
00180 void imageToCameraFrameDirectionJac(Pix const& pix, Mat& J) const
00181 {
00182 JFR_PRECOND(pix.size() == 2,
00183 "CameraBarreto::imageToCameraFrameDirectionJac: size of pix does not match");
00184 JFR_PRECOND(J.size1() == 3 && J.size2() == 2,
00185 "CameraBarreto::imageToCameraFrameDirectionJac: size of J does not match");
00186
00187 double u = pix(0);
00188 double v = pix(1);
00189
00190
00191 double t2 = xi * xi;
00192 double t3 = 0.1e1 - t2;
00193 double t4 = u - camera.u0;
00194 double t5 = t4 * t4;
00195 double t6 = camera.alphaU * camera.alphaU;
00196 double t7 = 0.1e1 / t6;
00197 double t9 = xi - phi;
00198 double t10 = t9 * t9;
00199 double t11 = 0.1e1 / t10;
00200 double t12 = t5 * t7 * t11;
00201 double t13 = v - camera.v0;
00202 double t14 = t13 * t13;
00203 double t15 = camera.alphaV * camera.alphaV;
00204 double t16 = 0.1e1 / t15;
00205 double t18 = t14 * t16 * t11;
00206 double t22 = sqrt(0.100e1 + t3 * (t12 + t18));
00207 double t23 = -0.10e1 * xi - t22;
00208 double t24 = t12 + t18 + 0.100e1;
00209 double t25 = 0.1e1 / t24;
00210 double t26 = t23 * t25;
00211 double t27 = 0.1e1 / camera.alphaU;
00212 double t28 = t4 * t27;
00213 double t29 = 0.1e1 / t9;
00214 double t31 = t26 * t28 * t29;
00215 double t32 = fabs(t31);
00216 double t33 = t32 * t32;
00217 double t34 = 0.1e1 / camera.alphaV;
00218 double t35 = t13 * t34;
00219 double t37 = t26 * t35 * t29;
00220 double t38 = fabs(t37);
00221 double t39 = t38 * t38;
00222 double t41 = 0.10e1 * t26 + xi;
00223 double t42 = fabs(t41);
00224 double t43 = t42 * t42;
00225 double t44 = t33 + t39 + t43;
00226 double t45 = sqrt(t44);
00227 double t47 = 0.1e1 / t45 / t44;
00228 double t49 = t47 * t23 * t25;
00229 double t50 = fabs(t31) / t31;
00230 double t51 = t32 * t50;
00231 double t52 = 0.1e1 / t22;
00232 double t53 = t52 * t3;
00233 double t56 = 0.1e1 / t6 / camera.alphaU;
00234 double t58 = 0.1e1 / t10 / t9;
00235 double t62 = t24 * t24;
00236 double t63 = 0.1e1 / t62;
00237 double t64 = t23 * t63;
00238 double t65 = t5 * t56;
00239 double t66 = t65 * t58;
00240 double t73 = fabs(t37) / t37;
00241 double t74 = t38 * t73;
00242 double t75 = t4 * t7;
00243 double t77 = t58 * t25;
00244 double t82 = t34 * t58 * t75;
00245 double t87 = fabs(t41) / t41;
00246 double t88 = t42 * t87;
00247 double t97 = -0.10e1 * t53 * t4 * t7 * t11 * t25 - 0.20e1 * t64 * t75 * t11;
00248 double t99 = t51 * (-t53 * t5 * t56 * t58 * t25 - 0.2e1 * t64 * t66 + t26 * t27 * t29) + t74 * (-t53 * t75 * t77 * t35 - 0.2e1 * t64 * t13 * t82) + t88 * t97;
00249 double t100 = 0.2e1 * t29 * t99;
00250 double t104 = 0.1e1 / t45;
00251 double t105 = t104 * t52;
00252 double t106 = t105 * t3;
00253 double t109 = t104 * t23;
00254 double t110 = t109 * t63;
00255 double t117 = t13 * t16;
00256 double t123 = t27 * t58 * t117;
00257 double t130 = 0.1e1 / t15 / camera.alphaV;
00258 double t134 = t14 * t130;
00259 double t135 = t134 * t58;
00260 double t150 = -0.10e1 * t53 * t13 * t16 * t11 * t25 - 0.20e1 * t64 * t117 * t11;
00261 double t152 = t51 * (-t53 * t117 * t77 * t28 - 0.2e1 * t64 * t4 * t123) + t74 * (-t53 * t14 * t130 * t58 * t25 - 0.2e1 * t64 * t135 + t26 * t34 * t29) + t88 * t150;
00262 double t153 = 0.2e1 * t29 * t152;
00263 double t195 = t47 * t41;
00264 J(0,0) = -t49 * t28 * t100 / 0.2e1 - t106 * t65 * t77 - 0.2e1 * t110 * t66 + t109 * t25 * t27 * t29;
00265 J(0,1) = -t49 * t28 * t153 / 0.2e1 - t105 * t3 * t13 * t16 * t58 * t25 * t4 * t27 - 0.2e1 * t109 * t63 * t4 * t123;
00266 J(1,0) = -t49 * t35 * t100 / 0.2e1 - t105 * t3 * t4 * t7 * t58 * t25 * t13 * t34 - 0.2e1 * t109 * t63 * t13 * t82;
00267 J(1,1) = -t49 * t35 * t153 / 0.2e1 - t106 * t134 * t77 - 0.2e1 * t110 * t135 + t109 * t25 * t34 * t29;
00268 J(2,0) = -t195 * t99 + t104 * t97;
00269 J(2,1) = -t195 * t152 + t104 * t150;
00270
00271
00272 }
00273
00274 protected:
00275
00276 CameraBarreto() : camera(), maskRadius(0.0) {}
00277
00278 void setParameters(int width_, int height_,
00279 double u0_, double v0_,
00280 double alphaU_, double alphaV_,
00281 double xi_, double phi_,
00282 double mirrorRadius_,
00283 double imageRadius_,
00284 double maskRadius_);
00285
00286 template<class Vec1, class Vec2>
00287 void hbar(Vec1 const& x, Vec2 & xbar) const
00288 {
00289 double nx = ublas::norm_2(x);
00290
00291 JFR_NUMERIC(nx > camera.epsilon,
00292 "CameraBarreto::hbar: 3D points norm is too small: " << x);
00293
00294 xbar.assign( x / nx );
00295 xbar(2) -= xi;
00296 }
00297
00298 template<class Vec1, class Vec2>
00299 void hbarInv(Vec1 const& xbar, Vec2 & x) const
00300 {
00301 double t1 = -xbar(2)*xi-sqrt(xbar(2)*xbar(2) + (1-xi*xi)*(xbar(0)*xbar(0) + xbar(1)*xbar(1)) );
00302 double t2 = xbar(0)*xbar(0) + xbar(1)*xbar(1) + xbar(2)*xbar(2);
00303 double t3 = t1/t2;
00304 x.assign(t3 * xbar);
00305 x(2) += xi;
00306 }
00307
00308 virtual void loadKeyValueFile(jafar::kernel::KeyValueFile const& keyValueFile) = 0;
00309
00310 };
00311
00312 std::ostream& operator <<(std::ostream& s, jafar::camera::CameraBarreto const& cb);
00313
00323 class CameraParabolicBarreto : public CameraBarreto {
00324
00325 public:
00326
00328 double p;
00329
00330 CameraParabolicBarreto() : CameraBarreto() {}
00331
00332 void setParameters(int width_, int height_,
00333 double u0_, double v0_,
00334 double alphaU_, double alphaV_,
00335 double p,
00336 double mirrorRadius_,
00337 double imageRadius_,
00338 double maskRadius_);
00339 protected:
00340
00341 void loadKeyValueFile(jafar::kernel::KeyValueFile const& keyValueFile);
00342
00343 };
00344
00345 }
00346 }
00347
00348 #endif // CAMERA_BARRETO_HPP
