EC.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ALVAR, A Library for Virtual and Augmented Reality.
3  *
4  * Copyright 2007-2012 VTT Technical Research Centre of Finland
5  *
6  * Contact: VTT Augmented Reality Team <alvar.info@vtt.fi>
7  * <http://www.vtt.fi/multimedia/alvar.html>
8  *
9  * ALVAR is free software; you can redistribute it and/or modify it under the
10  * terms of the GNU Lesser General Public License as published by the Free
11  * Software Foundation; either version 2.1 of the License, or (at your option)
12  * any later version.
13  *
14  * This library is distributed in the hope that it will be useful, but WITHOUT
15  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
16  * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License
17  * for more details.
18  *
19  * You should have received a copy of the GNU Lesser General Public License
20  * along with ALVAR; if not, see
21  * <http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html>.
22  */
23 
24 #include "EC.h"
25 #include "Optimization.h"
26 
27 namespace alvar {
28 
29 struct ProjectParams
30 {
31  Camera *camera;
32  const CvMat *object_model;
33 };
34 
35 static void ProjectRot(CvMat* state, CvMat* projection, void* x)
36 {
37  ProjectParams *foo = (ProjectParams*)x;
38  Camera *camera = foo->camera;
39  const CvMat *object_model = foo->object_model;
40  int count = projection->rows;
41  CvMat rot_mat = cvMat(3, 1, CV_64F, &(state->data.db[0+0]));
42  double zeros[3] = {0};
43  CvMat zero_tra = cvMat(3, 1, CV_64F, zeros);
44  cvReshape(projection, projection, 2, 1);
45  cvProjectPoints2(object_model, &rot_mat, &zero_tra, &(camera->calib_K), &(camera->calib_D), projection);
46  cvReshape(projection, projection, 1, count);
47 }
48 
49 // TODO: How this differs from the Camera::ProjectPoints ???
50 static void Project(CvMat* state, CvMat* projection, void* x)
51 {
52  ProjectParams *foo = (ProjectParams*)x;
53  Camera *camera = foo->camera;
54  const CvMat *object_model = foo->object_model;
55  int count = projection->rows;
56  CvMat rot_mat = cvMat(3, 1, CV_64F, &(state->data.db[0+0]));
57  CvMat tra_mat = cvMat(3, 1, CV_64F, &(state->data.db[0+3]));
58  cvReshape(projection, projection, 2, 1);
59  cvProjectPoints2(object_model, &rot_mat, &tra_mat, &(camera->calib_K), &(camera->calib_D), projection);
60  cvReshape(projection, projection, 1, count);
61 }
62 
63 bool CameraEC::UpdatePose(const CvMat* object_points, CvMat* image_points, Pose *pose, CvMat *weights) {
64  double rot[3]; CvMat rotm = cvMat(3, 1, CV_64F, rot);
65  double tra[3]; CvMat tram = cvMat(3, 1, CV_64F, tra);
66  pose->GetRodriques(&rotm);
67  pose->GetTranslation(&tram);
68  bool ret = UpdatePose(object_points, image_points, &rotm, &tram, weights);
69  pose->SetRodriques(&rotm);
70  pose->SetTranslation(&tram);
71  return ret;
72 }
73 
74 bool CameraEC::UpdatePose(const CvMat* object_points, CvMat* image_points, CvMat *rot, CvMat *tra, CvMat *weights) {
75  if (object_points->height < 4) return false;
76  /* if (object_points->height < 6) {
77  return false;
78  // TODO: We need to change image_points into CV_32FC2
79  return Camera::CalcExteriorOrientation(object_points, image_points, rot, tra);
80  }*/
81  CvMat* par = cvCreateMat(6, 1, CV_64F);
82  memcpy(&(par->data.db[0+0]), rot->data.db, 3*sizeof(double));
83  memcpy(&(par->data.db[0+3]), tra->data.db, 3*sizeof(double));
84 
85  ProjectParams pparams;
86  pparams.camera = this;
87  pparams.object_model = object_points;
88 
89  alvar::Optimization *opt = new alvar::Optimization(6, image_points->height);
90  double tmp = opt->Optimize(par, image_points, 0.0005, 2, Project, &pparams, alvar::Optimization::TUKEY_LM, 0, 0, weights);
91 
92  memcpy(rot->data.db, &(par->data.db[0+0]), 3*sizeof(double));
93  memcpy(tra->data.db, &(par->data.db[0+3]), 3*sizeof(double));
94 
95  delete opt;
96 
97  cvReleaseMat(&par);
98  return true;
99 }
100 
101 bool CameraEC::UpdateRotation(const CvMat* object_points, CvMat* image_points, Pose *pose)
102 {
103  double rot[3]; CvMat rotm = cvMat(3, 1, CV_64F, rot);
104  double tra[3]; CvMat tram = cvMat(3, 1, CV_64F, tra);
105  pose->GetRodriques(&rotm);
106  pose->GetTranslation(&tram);
107  bool ret = UpdateRotation(object_points, image_points, &rotm, &tram);
108  pose->SetRodriques(&rotm);
109  pose->SetTranslation(&tram);
110  return ret;
111 }
112 
113 bool CameraEC::UpdateRotation(const CvMat* object_points, CvMat* image_points, CvMat *rot, CvMat *tra) {
114 
115  CvMat* par = cvCreateMat(3, 1, CV_64F);
116  memcpy(&(par->data.db[0+0]), rot->data.db, 3*sizeof(double));
117  ProjectParams pparams;
118  pparams.camera = this;
119  pparams.object_model = object_points;
120  alvar::Optimization *opt = new alvar::Optimization(3, image_points->height);
121  double tmp = opt->Optimize(par, image_points, 0.0005, 2, ProjectRot, &pparams, alvar::Optimization::TUKEY_LM);
122  memcpy(rot->data.db, &(par->data.db[0+0]), 3*sizeof(double));
123  delete opt;
124  cvReleaseMat(&par);
125  return true;
126 }
127 
128 // Ol etta mirror asia on kunnossa
129 void GetOrigo(Pose* pose, CvMat* O)
130 {
131  pose->GetTranslation(O);
132 }
133 
134 void GetPointOnLine(const Pose* pose, Camera *camera, const CvPoint2D32f *u, CvMat* P)
135 {
136  double kid[9], rotd[9], trad[3], ud[3] = {u->x, u->y, 1};
137  CvMat Ki = cvMat(3, 3, CV_64F, kid);
138  CvMat R = cvMat(3, 3, CV_64F, rotd);
139  CvMat T = cvMat(3, 1, CV_64F, trad);
140  CvMat U = cvMat(3, 1, CV_64F, ud);
141  pose->GetMatrix(&R);
142  pose->GetTranslation(&T);
143  cvInv(&(camera->calib_K), &Ki);
144  cvMatMul(&R, &Ki, &Ki);
145  cvGEMM(&Ki, &U, 1, &T, 1, P, 0);
146 }
147 
148 bool MidPointAlgorithm(CvMat* o1, CvMat* o2, CvMat* p1, CvMat* p2, CvPoint3D32f& X, double limit)
149 {
150  double ud[3], vd[3], wd[3];
151  CvMat u = cvMat(3, 1, CV_64F, ud);
152  CvMat v = cvMat(3, 1, CV_64F, vd);
153  CvMat w = cvMat(3, 1, CV_64F, wd);
154 
155  cvSub(p1, o1, &u);
156  cvSub(p2, o2, &v);
157  cvSub(o1, o2, &w);
158 
159  double a = cvDotProduct(&u, &u);
160  double b = cvDotProduct(&u, &v);
161  double c = cvDotProduct(&v, &v);
162  double d = cvDotProduct(&u, &w);
163  double e = cvDotProduct(&v, &w);
164  double D = a*c - b*b;
165  double sc, tc;
166 
167  // compute the line parameters of the two closest points
168  if (D < limit)
169  {
170  return false;
171  // the lines are almost parallel
172  sc = 0.0;
173  tc = (b>c ? d/b : e/c); // use the largest denominator
174  }
175  else
176  {
177  sc = (b*e - c*d) / D;
178  tc = (a*e - b*d) / D;
179  }
180 
181  double m1d[3], m2d[3];
182  CvMat m1 = cvMat(3, 1, CV_64F, m1d);
183  CvMat m2 = cvMat(3, 1, CV_64F, m2d);
184  cvAddWeighted(&u, sc, o1, 1.0, 0.0, &m1);
185  cvAddWeighted(&v, tc, o2, 1.0, 0.0, &m2);
186  cvAddWeighted(&m1, 0.5, &m2, 0.5, 0.0, &m1);
187 
188  X.x = (float)m1d[0];
189  X.y = (float)m1d[1];
190  X.z = (float)m1d[2];
191 
192  return true;
193 }
194 
195 // todo
196 bool CameraEC::ReconstructFeature(const Pose *pose1, const Pose *pose2, const CvPoint2D32f *u1, const CvPoint2D32f *u2, CvPoint3D32f *p3d, double limit) {
197  double o1d[3], o2d[3], p1d[3], p2d[3];
198  CvMat o1 = cvMat(3, 1, CV_64F, o1d);
199  CvMat o2 = cvMat(3, 1, CV_64F, o2d);
200  CvMat p1 = cvMat(3, 1, CV_64F, p1d);
201  CvMat p2 = cvMat(3, 1, CV_64F, p2d);
202 
203  Pose po1 = *pose1; // Make copy so that we don't destroy the pose content
204  Pose po2 = *pose2;
205  po1.Invert();
206  po2.Invert();
207  GetOrigo(&po1, &o1);
208  GetOrigo(&po2, &o2);
209  GetPointOnLine(&po1, this, u1, &p1);
210  GetPointOnLine(&po2, this, u2, &p2);
211 
212  return MidPointAlgorithm(&o1, &o2, &p1, &p2, *p3d, limit);
213 }
214 
215 void CameraEC::Get3dOnPlane(const Pose *pose, CvPoint2D32f p2d, CvPoint3D32f &p3d) {
216  double pd[16], md[9], kd[9];
217  CvMat P = cvMat(4, 4, CV_64F, pd);
218  CvMat H = cvMat(3, 3, CV_64F, md);
219  CvMat Ki = cvMat(3, 3, CV_64F, kd);
220 
221  pose->GetMatrix(&P);
222  cvInv(&(calib_K), &Ki);
223 
224  // Construct homography from pose
225  int ind_s = 0, ind_c = 0;
226  for(int i = 0; i < 3; ++i)
227  {
228  CvRect r; r.x = ind_s; r.y = 0; r.height = 3; r.width = 1;
229  CvMat sub = cvMat(3, 1, CV_64F);
230  cvGetSubRect(&P, &sub, r);
231  CvMat col = cvMat(3, 1, CV_64F);
232  cvGetCol(&H, &col, ind_c);
233  cvCopy(&sub, &col);
234  ind_c++;
235  ind_s++;
236  if(i == 1) ind_s++;
237  }
238 
239  // Apply H to get the 3D coordinates
240  Camera::Undistort(p2d);
241  double xd[3] = {p2d.x, p2d.y, 1};
242  CvMat X = cvMat(3, 1, CV_64F, xd);
243  cvMatMul(&Ki, &X, &X);
244  cvInv(&H, &H);
245  cvMatMul(&H, &X, &X);
246 
247  p3d.x = (float)(xd[0] / xd[2]);
248  p3d.y = (float)(xd[1] / xd[2]);
249  p3d.z = 0;
250 }
251 
252 void CameraEC::Get3dOnDepth(const Pose *pose, CvPoint2D32f p2d, float depth, CvPoint3D32f &p3d)
253 {
254  double wx, wy, wz;
255  Camera::Undistort(p2d);
256 
257  // Tassa asetetaan z = inf ja lasketaan x ja y jotenkin?!?
258  //double object_scale = _dist_to_target; // TODO Same as the pose?!?!?!?
259 
260  // inv(K)*[u v 1]'*scale
261  wx = depth*(p2d.x-calib_K_data[0][2])/calib_K_data[0][0];
262  wy = depth*(p2d.y-calib_K_data[1][2])/calib_K_data[1][1];
263  wz = depth;
264 
265  // Now the points are in camera coordinate frame.
266  alvar::Pose p = *pose;
267  p.Invert();
268 
269  double Xd[4] = {wx, wy, wz, 1};
270  CvMat Xdm = cvMat(4, 1, CV_64F, Xd);
271  double Pd[16];
272  CvMat Pdm = cvMat(4, 4, CV_64F, Pd);
273  p.GetMatrix(&Pdm);
274  cvMatMul(&Pdm, &Xdm, &Xdm);
275  p3d.x = float(Xd[0]/Xd[3]);
276  p3d.y = float(Xd[1]/Xd[3]);
277  p3d.z = float(Xd[2]/Xd[3]);
278 }
279 
280 } // namespace alvar
281 
alvar
Main ALVAR namespace.
Definition: Alvar.h:174
alvar::Rotation::GetRodriques
void GetRodriques(CvMat *mat) const
Returns the rotation in rotation vector form.
Definition: Rotation.cpp:341
alvar::GetOrigo
void GetOrigo(Pose *pose, CvMat *O)
Definition: EC.cpp:129
alvar::Camera::calib_D
CvMat calib_D
Definition: Camera.h:87
alvar::MidPointAlgorithm
bool MidPointAlgorithm(CvMat *o1, CvMat *o2, CvMat *p1, CvMat *p2, CvPoint3D32f &X, double limit)
Definition: EC.cpp:148
alvar::CameraEC::UpdatePose
bool UpdatePose(std::map< int, T > &container, Pose *pose, int type_id=-1, std::map< int, double > *weights=0)
Update the pose using the items with matching type_id.
Definition: EC.h:473
alvar::Pose::GetTranslation
void GetTranslation(CvMat *tra) const
Definition: Pose.cpp:168
alvar::GetPointOnLine
void GetPointOnLine(const Pose *pose, Camera *camera, const CvPoint2D32f *u, CvMat *P)
Definition: EC.cpp:134
alvar::CameraEC::Get3dOnPlane
void Get3dOnPlane(const Pose *pose, CvPoint2D32f p2d, CvPoint3D32f &p3d)
Get 3D-coordinate for 2D feature on the plane defined by the pose (z == 0)
Definition: EC.cpp:215
alvar::Optimization::Optimize
double Optimize(CvMat *parameters, CvMat *measurements, double stop, int max_iter, EstimateCallback Estimate, void *param=0, OptimizeMethod method=LEVENBERGMARQUARDT, CvMat *parameters_mask=0, CvMat *J_mat=0, CvMat *weights=0)
Runs the optimization loop with selected parameters.
Definition: Optimization.cpp:123
alvar::Optimization
Non-linear optimization routines. There are three methods implemented that include Gauss-Newton...
Definition: Optimization.h:44
alvar::Camera
Simple Camera class for calculating distortions, orientation or projections with pre-calibrated camer...
Definition: Camera.h:82
alvar::Project
static void Project(CvMat *state, CvMat *projection, void *x)
Definition: EC.cpp:50
alvar::Camera::calib_K
CvMat calib_K
Definition: Camera.h:86
Optimization.h
This file implements several optimization algorithms.
alvar::CameraEC::ReconstructFeature
bool ReconstructFeature(const Pose *pose1, const Pose *pose2, const CvPoint2D32f *u1, const CvPoint2D32f *u2, CvPoint3D32f *p3d, double limit)
Reconstruct 3D point using two camera poses and corresponding undistorted image feature positions...
Definition: EC.cpp:196
alvar::Pose
Pose representation derived from the Rotation class
Definition: Pose.h:50
ar_track_alvar
Definition: kinect_filtering.h:63
alvar::CameraEC::Get3dOnDepth
void Get3dOnDepth(const Pose *pose, CvPoint2D32f p2d, float depth, CvPoint3D32f &p3d)
Get 3D-coordinate for 2D feature assuming certain depth.
Definition: EC.cpp:252
d
Drawable d[32]
Definition: SampleMarkerDetector.cpp:11
alvar::ProjectParams::camera
Camera * camera
Definition: EC.cpp:31
alvar::Rotation::SetRodriques
void SetRodriques(const CvMat *mat)
Sets the rotation from given rotation vector.
Definition: Rotation.cpp:314
w
TFSIMD_FORCE_INLINE const tfScalar & w() const
alvar::Pose::Invert
void Invert()
Definition: Pose.cpp:132
alvar::Pose::GetMatrix
void GetMatrix(CvMat *mat) const
Definition: Pose.cpp:91
EC.h
This file implements a collection of External Container (EC) versions of many ALVAR classes...
alvar::Pose::SetTranslation
void SetTranslation(const CvMat *tra)
Definition: Pose.cpp:150
alvar::CameraEC::UpdateRotation
bool UpdateRotation(std::map< int, T > &container, Pose *pose, int type_id=-1)
Update the rotation in pose using the items with matching type_id.
Definition: EC.h:479
alvar::ProjectParams::object_model
const CvMat * object_model
Definition: EC.cpp:32
r
r
alvar::ProjectRot
static void ProjectRot(CvMat *state, CvMat *projection, void *x)
Definition: EC.cpp:35
alvar::Camera::Undistort
void Undistort(std::vector< PointDouble > &points)
Unapplys the lens distortion for points on image plane.

ar_track_alvar
Author(s): Scott Niekum
autogenerated on Mon Jun 10 2019 12:47:04