00001
00002
00003 #ifndef _CAMERA_PINHOLE_HPP
00004 #define _CAMERA_PINHOLE_HPP
00005
00006 #include <iostream>
00007
00008 #include "kernel/jafarException.hpp"
00009 #include "kernel/keyValueFile.hpp"
00010 #include "jmath/jblas.hpp"
00011
00012 namespace jafar {
00013 namespace camera {
00014
00015 class StereoBench;
00016
00026 class CameraPinhole : public jafar::kernel::KeyValueFileLoad {
00027
00028 protected:
00029
00030 void loadKeyValueFile(jafar::kernel::KeyValueFile const& keyValueFile);
00031
00032 public :
00033
00034 static const double epsilon;
00035
00036 int width;
00037 int height;
00038 double u0;
00039 double v0;
00040 double alphaU;
00041 double alphaV;
00042
00044 jblas::mat Ph;
00045
00047 jblas::mat_range P;
00048
00050 CameraPinhole();
00051
00053 CameraPinhole(CameraPinhole const& c_);
00054
00055 ~CameraPinhole();
00056
00057 double getApertureU() const;
00058 double getApertureV() const;
00059
00062 void setParameters(int width_, int height_,
00063 double u0_, double v0_,
00064 double alphaU_, double alphaV_);
00065
00068 void applyScale(double scale);
00069
00076 template<class Vec, class Pix>
00077 bool project(Vec const& v, Pix& pix) const
00078 {
00079 JFR_PRECOND(v.size() == 3,
00080 "CameraPinhole::project: size of v does not match");
00081 JFR_PRECOND(pix.size() == 2,
00082 "CameraPinhole::project: size of pix does not match");
00083 JFR_NUMERIC(fabs(v(2)) > epsilon,
00084 "CameraPinhole::project: 3D point z is too small: " << v);
00085
00086
00087 jblas::vec3 v_n;
00088 v_n(0) = v(0)/v(2);
00089 v_n(1) = v(1)/v(2);
00090 v_n(2) = 1.0;
00091
00092 pix.assign(ublas::prod(P,v_n));
00093
00094 if ( (v(2)<0.0) )
00095 return false;
00096 else if ((pix(0) < 0) || (pix(0) > width) || (pix(1) < 0) || (pix(1) > height))
00097 return false;
00098 else
00099 return true;
00100 }
00101
00106 template<class Vec, class Pix>
00107 bool projectH(Vec const& v, Pix& pix) const
00108 {
00109 JFR_PRECOND(v.size() == 4,
00110 "CameraPinhole::project: size of v does not match");
00111 JFR_PRECOND(pix.size() == 3,
00112 "CameraPinhole::project: size of pix does not match");
00113
00114 pix.assign(ublas::prod(Ph,v));
00115
00116 if ( (v(2)<0.0) )
00117 return false;
00118 else if ((pix(0) < 0) || (pix(0) > width) || (pix(1) < 0) || (pix(1) > height))
00119 return false;
00120 else
00121 return true;
00122 }
00123
00125 template<class Vec, class Mat>
00126 void projectJac(Vec const& v, Mat& J) const
00127 {
00128 JFR_PRECOND(v.size() == 3,
00129 "CameraPinhole::projectJac: size of v does not match");
00130 JFR_PRECOND(J.size1() == 2 && J.size2() == 3,
00131 "CameraPinhole::projectJac: size of J does not match");
00132 JFR_NUMERIC(fabs(v(2)) > epsilon,
00133 "CameraPinhole::projectJac: 3D points z is too small: " << v);
00134
00135 double t1 = 0.1e1 / v(2);
00136 double t4 = v(2) * v(2);
00137 double t5 = 0.1e1 / t4;
00138 J(0,0) = alphaU * t1;
00139 J(0,1) = 0.0e0;
00140 J(0,2) = -alphaU * v(0) * t5;
00141 J(1,0) = 0.0e0;
00142 J(1,1) = alphaV * t1;
00143 J(1,2) = -alphaV * v(1) * t5;
00144 }
00145
00147 template<class Vec, class Mat, class Pix>
00148 void projectJac(Vec const& v, Pix& pix, Mat& J) const
00149 {
00150 project(v,pix);
00151 projectJac(v,J);
00152 }
00153
00155 template<class Pix, class Vec>
00156 void imageToCameraFrame(Pix const& pix, Vec& v) const
00157 {
00158 JFR_PRECOND(pix.size() == 2,
00159 "CameraPinhole::imageToCameraFrame: size of pix does not match");
00160 JFR_PRECOND(v.size() == 3,
00161 "CameraPinhole::imageToCameraFrame: size of v does not match");
00162
00163 v(0) = (pix(0) - u0) / alphaU;
00164 v(1) = (pix(1) - v0) / alphaV;
00165 v(2) = 1.0;
00166 }
00167
00169 template<class Pix, class Mat>
00170 void imageToCameraFrameJac(Pix const& pix, Mat& J) const
00171 {
00172 JFR_PRECOND(pix.size() == 2,
00173 "CameraPinhole::imageToCameraFrameJac: size of pix does not match");
00174 JFR_PRECOND(J.size1() == 3 && J.size1() == 2,
00175 "CameraPinhole::imageToCameraFrameJac: size of j does not match");
00176
00177 J.clear();
00178 J(0,0) = 1/alphaU;
00179 J(1,1) = 1/alphaV;
00180 }
00181
00183 template<class Pix, class Vec>
00184 void imageToCameraFrameDirection(Pix const& pix_, Vec& v) const
00185 {
00186 JFR_PRECOND(pix_.size() == 2,
00187 "CameraPinhole::imageToCameraFrameDirection: size of pix does not match");
00188 JFR_PRECOND(v.size() == 3,
00189 "CameraPinhole::imageToCameraFrameDirection: size of v does not match");
00190
00191 imageToCameraFrame(pix_, v);
00192 v /= ublas::norm_2(v);
00193 }
00194
00196 template<class Pix, class Mat>
00197 void imageToCameraFrameDirectionJac(Pix const& pix, Mat& J) const
00198 {
00199 JFR_PRECOND(pix.size() == 2,
00200 "CameraPinhole::imageToCameraFrameDirectionJac: size of pix does not match");
00201 JFR_PRECOND(J.size1() == 3 && J.size2() == 2,
00202 "CameraPinhole::imageToCameraFrameDirectionJac: size of J does not match");
00203
00204 double u = pix(0);
00205 double v = pix(1);
00206
00207
00208 double t1 = u - u0;
00209 double t2 = 0.1e1 / alphaU;
00210 double t3 = t1 * t2;
00211 double t4 = fabs(t3);
00212 double t5 = t4 * t4;
00213 double t6 = v - v0;
00214 double t7 = 0.1e1 / alphaV;
00215 double t8 = t6 * t7;
00216 double t9 = fabs(t8);
00217 double t10 = t9 * t9;
00218 double t11 = 0.100e1 + t5 + t10;
00219 double t12 = sqrt(t11);
00220 double t14 = 0.1e1 / t12 / t11;
00221 double t15 = t14 * t1;
00222 double t16 = alphaU * alphaU;
00223 double t19 = fabs(t3) / t3;
00224 double t22 = 0.1e1 / t12;
00225 double t26 = fabs(t8) / t8;
00226 double t30 = t14 * t6;
00227 double t35 = alphaV * alphaV;
00228 J(0,0) = -t15 / t16 * t4 * t19 + t22 * t2;
00229 J(0,1) = -t15 * t2 * t9 * t26 * t7;
00230 J(1,0) = -t30 * t7 * t4 * t19 * t2;
00231 J(1,1) = -t30 / t35 * t9 * t26 + t22 * t7;
00232 J(2,0) = -0.10e1 * t14 * t4 * t19 * t2;
00233 J(2,1) = -0.10e1 * t14 * t9 * t26 * t7;
00234
00235
00236 }
00237
00238
00241 jblas::vec4 getIntrinsicParams()
00242 {
00243 jblas::vec4 k;
00244 k(0) = u0;
00245 k(1) = v0;
00246 k(2) = alphaU;
00247 k(3) = alphaV;
00248
00249 return k;
00250 };
00251
00252 friend class StereoBench;
00253
00254 };
00255
00256 std::ostream& operator <<(std::ostream& s, const jafar::camera::CameraPinhole& camera_);
00257
00258
00266 class StereoBench : public jafar::kernel::KeyValueFileLoad {
00267
00268 protected:
00269
00270 void loadKeyValueFile(jafar::kernel::KeyValueFile const& keyValueFile);
00271
00272 public:
00273
00274 static const double epsilon;
00275
00276 CameraPinhole leftCamera;
00277
00279 double alpha;
00280
00282 double baseline;
00283
00284 StereoBench();
00285
00286 StereoBench(StereoBench const& s);
00287
00288 ~StereoBench() {};
00289
00292 void setParameters(int width_, int height_,
00293 double u0_, double v0_,
00294 double alphaU_, double alphaV_,
00295 double alpha_);
00296
00299 void applyScale(double scale);
00300
00302 template<class Pix, class Vec>
00303 void compute3dPoint(Pix const& pix, double disp, Vec& point) const {
00304 JFR_PRECOND(pix.size() == 2,
00305 "StereoBench::compute3dPoint: pix is a 2-vector (u,v)");
00306 JFR_PRECOND(point.size() == 3,
00307 "StereoBench::compute3dPoint: point is a 3-vector (x,y,z)");
00308
00309 JFR_NUMERIC(disp > epsilon,
00310 "StereoBench::compute3dPoint: disparity value is too small (disp=" << disp <<")");
00311
00312 point(2) = alpha / disp;
00313
00314 double beta = (pix(0) - leftCamera.u0) / leftCamera.alphaU;
00315 double gamma = (pix(1) - leftCamera.v0) / leftCamera.alphaV;
00316
00317 point(0) = point(2) * beta;
00318 point(1) = point(2) * gamma;
00319
00320 }
00321
00322 template<class Pix, class Mat>
00323 void compute3dPointJac(Pix const& pix, double disp, Mat& J) const {
00324 JFR_PRECOND(pix.size() == 2,
00325 "StereoBench::compute3dPointJac: pix is a 2-vector (u,v)");
00326 JFR_PRECOND(J.size1() == 3 && J.size2() == 3,
00327 "StereoBench::compute3dPointJac: invalid size of J (" << J.size1() << "x" << J.size2() << ")");
00328
00329 JFR_NUMERIC(disp > epsilon,
00330 "StereoBench::compute3dPointJac: disparity value is too small (disp=" << disp << ")");
00331
00332 double u = pix(0);
00333 double v = pix(1);
00334
00335
00336 double t2 = alpha / disp;
00337 double t3 = 0.1e1 / leftCamera.alphaU;
00338 double t5 = disp * disp;
00339 double t7 = alpha / t5;
00340 double t11 = 0.1e1 / leftCamera.alphaV;
00341 J(0,0) = -t2 * t3;
00342 J(0,1) = 0.0e0;
00343 J(0,2) = t7 * (u - leftCamera.u0) * t3;
00344 J(1,0) = 0.0e0;
00345 J(1,1) = -t2 * t11;
00346 J(1,2) = t7 * (v - leftCamera.v0) * t11;
00347 J(2,0) = 0.0e0;
00348 J(2,1) = 0.0e0;
00349 J(2,2) = -t7;
00350
00351 }
00352
00353 template<class Vec, class Pix>
00354 void computePixDisp(Vec const& point, Pix& pix, double& disp) const {
00355 JFR_PRECOND(point.size() == 3,
00356 "StereoBench::computePixDisp: v is a 3-vector (x,y,z)");
00357 JFR_PRECOND(pix.size() == 2,
00358 "StereoBench::computePixDisp: pixDisp is a 2-vector (u,v)");
00359 JFR_NUMERIC(point(2) > epsilon,
00360 "StereoBench::computePixDisp: z value is too small (z=" << point(2) << " > "<< epsilon << ")");
00361
00362 disp = alpha / point(2);
00363 pix(0) = leftCamera.u0 + leftCamera.alphaU * point(0)/point(2);
00364 pix(1) = leftCamera.v0 + leftCamera.alphaV * point(1)/point(2);
00365 }
00366
00367 template<class Vec, class Mat>
00368 void computePixDispJac(Vec const& point, Mat& J) const {
00369 JFR_PRECOND(point.size() == 3,
00370 "StereoBench::computePixDispJac: v is a 3-vector (x,y,z)");
00371 JFR_PRECOND(J.size1() == 3 && J.size2() == 3,
00372 "StereoBench::computePixDispJac: invalid size of J (" << J.size1() << "x" << J.size2() << ")");
00373
00374 JFR_NUMERIC(point(2) > epsilon,
00375 "StereoBench::computePixDispJac: z value is too small (z=" << point(2) << ")");
00376 double x = point(0);
00377 double y = point(1);
00378 double z = point(2);
00379
00380 double t1 = 0.1e1 / z;
00381 double t4 = z * z;
00382 double t5 = 0.1e1 / t4;
00383 J(0,0) = leftCamera.alphaU * t1;
00384 J(0,1) = 0.0e0;
00385 J(0,2) = -leftCamera.alphaU * x * t5;
00386 J(1,0) = 0.0e0;
00387 J(1,1) = leftCamera.alphaV * t1;
00388 J(1,2) = -leftCamera.alphaV * y * t5;
00389 J(2,0) = 0.0e0;
00390 J(2,1) = 0.0e0;
00391 J(2,2) = -alpha * t5;
00392 }
00393
00394 };
00395
00396 std::ostream& operator <<(std::ostream& s, jafar::camera::StereoBench const& stereoBench_);
00397
00398 }
00399 }
00400
00401 #endif // IMAGE_CAMERA_PINHOLE_HPP
