00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023 #include "projective_cam.h"
00024
00025 #include <geometry/hom_point.h>
00026 #include <geometry/vector.h>
00027 #include <core/exceptions/software.h>
00028
00029 #include <cmath>
00030 #include <iostream>
00031
00032 using namespace fawkes;
00033 using std::cout;
00034 using std::endl;
00035
00036 namespace firevision {
00037 #if 0
00038 }
00039 #endif
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049 AboveHorizonException::AboveHorizonException(const char *msg, const center_in_roi_t img_pt) throw()
00050 : fawkes::Exception("AboveHorizonException: %s (%0.1f, %0.1f)", msg, img_pt.x, img_pt.y)
00051 {
00052 __img_pt = img_pt;
00053 }
00054
00055
00056
00057
00058 const center_in_roi_t &
00059 AboveHorizonException::get_img_pt() const
00060 {
00061 return __img_pt;
00062 }
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077 ProjectiveCam::ProjectiveCam(const Calibration &cal, const HomTransform *loc) :
00078 __cal(cal)
00079 {
00080 __p = NULL;
00081 __gpa_inv = NULL;
00082 __gpa_inv_data = new float[9];
00083
00084 if (loc) set_location(*loc);
00085 }
00086
00087
00088
00089
00090
00091
00092
00093
00094
00095
00096 ProjectiveCam::ProjectiveCam(const Calibration &cal,
00097 float roll, float pitch, float yaw,
00098 float height, float x, float y):
00099 __cal(cal)
00100 {
00101 __p = NULL;
00102 __gpa_inv = NULL;
00103 __gpa_inv_data = new float[9];
00104
00105 set_location(roll, pitch, yaw, height, x, y);
00106 }
00107
00108
00109
00110
00111 ProjectiveCam::ProjectiveCam(const ProjectiveCam &pc):
00112 __cal(pc.__cal)
00113 {
00114 __p = (pc.__p != NULL ? new Matrix(*pc.__p) : NULL);
00115 __gpa_inv_data = new float[9];
00116
00117 if (pc.__gpa_inv) {
00118 for (unsigned int i = 0; i < 9; ++i) {
00119 __gpa_inv_data[i] = pc.__gpa_inv_data[i];
00120 }
00121
00122 __gpa_inv = new Matrix(3, 3, __gpa_inv_data, false);
00123 }
00124 else __gpa_inv = NULL;
00125 }
00126
00127
00128
00129 ProjectiveCam::~ProjectiveCam()
00130 {
00131 delete __p;
00132 delete __gpa_inv;
00133 delete[] __gpa_inv_data;
00134 }
00135
00136
00137
00138
00139
00140
00141
00142
00143
00144
00145
00146 ProjectiveCam&
00147 ProjectiveCam::set_location(float roll, float pitch, float yaw, float height, float x, float y)
00148 {
00149 HomTransform t;
00150
00151
00152
00153
00154 t.rotate_x(-M_PI_2);
00155 t.rotate_z(-M_PI_2);
00156
00157 t.rotate_y(pitch);
00158 t.rotate_x(-roll);
00159
00160 t.trans(-x, y, height);
00161 t.rotate_z(yaw);
00162
00163 return set_location(t);
00164 }
00165
00166
00167
00168
00169
00170
00171
00172 ProjectiveCam&
00173 ProjectiveCam::set_location(const HomTransform& loc)
00174 {
00175 if (__p) {
00176 delete __gpa_inv;
00177 delete __p;
00178 __p = NULL;
00179 }
00180
00181 __p = new Matrix (__cal * loc.get_matrix().get_submatrix(0, 0, 3, 4));
00182
00183 __gpa_inv = new Matrix(3, 3, __gpa_inv_data, false);
00184 __gpa_inv->overlay(0, 0, __p->get_submatrix(0, 0, 3, 2));
00185 __gpa_inv->overlay(0, 2, __p->get_submatrix(0, 3, 3, 1));
00186 __gpa_inv->invert();
00187
00188 return *this;
00189 }
00190
00191
00192
00193
00194
00195 fawkes::cart_coord_2d_t
00196 ProjectiveCam::get_GPA_world_coord(const center_in_roi_t &img_p) const
00197 {
00198 Vector img_v(3);
00199 img_v.x(img_p.x);
00200 img_v.y(img_p.y);
00201 img_v.z(1);
00202
00203 Vector wld_v = *__gpa_inv * img_v;
00204
00205 wld_v /= wld_v.z();
00206
00207 if (wld_v.x() < 0) {
00208 throw AboveHorizonException("The given point is above the horizon!\n", img_p);
00209 }
00210
00211 return (fawkes::cart_coord_2d_t){ wld_v.x(), -wld_v.y() };
00212 }
00213
00214
00215
00216
00217
00218 center_in_roi_t
00219 ProjectiveCam::get_GPA_image_coord(const fawkes::cart_coord_2d_t &wld_p) const
00220 {
00221 Vector wld_v(4);
00222 wld_v.x(wld_p.x);
00223 wld_v.y(wld_p.y);
00224 wld_v.z(0);
00225 wld_v.set(3, 1);
00226
00227 Vector img_v = *__p * wld_v;
00228 img_v /= img_v.z();
00229
00230 center_in_roi_t res;
00231 res.x = img_v.x();
00232 res.y = img_v.y();
00233
00234 return res;
00235 }
00236
00237
00238
00239
00240
00241 Calibration
00242 ProjectiveCam::get_cal() const
00243 {
00244 return Calibration(__cal);
00245 }
00246
00247
00248
00249
00250
00251 Matrix
00252 ProjectiveCam::get_p() const
00253 {
00254 return Matrix(*__p);
00255 }
00256
00257
00258
00259
00260
00261 Matrix
00262 ProjectiveCam::get_GPA_p() const
00263 {
00264 Matrix res(3, 3);
00265 res.overlay(0, 0, __p->get_submatrix(0, 0, 3, 2));
00266 res.overlay(0, 2, __p->get_submatrix(0, 3, 3, 1));
00267
00268 return res;
00269 }
00270
00271
00272
00273
00274
00275
00276 void
00277 ProjectiveCam::print_info (const char *name, const char *col_sep, const char *row_sep) const
00278 {
00279 __p->print_info(name ? name : "Projective Camera", col_sep, row_sep);
00280 __cal.print_info("Calibration Matrix", col_sep, row_sep);
00281 }
00282
00283 }