calcPose.cpp
Go to the documentation of this file.
1 
2 /*
3 
4 This library computes the pose of
5 the fiducial relative to a fiducial centered on the origin.
6 
7 */
8 
9 #include <stdio.h>
10 #include <math.h>
11 #include <malloc.h>
12 
13 #include <ros/ros.h>
14 #include <tf/transform_broadcaster.h>
15 #include <tf/LinearMath/Matrix3x3.h>
16 
17 #include <opencv2/opencv.hpp>
18 #include <opencv2/core/core.hpp>
19 #include <opencv2/calib3d/calib3d.hpp>
20 #include <opencv2/imgproc/imgproc.hpp>
21 
22 #include "RPP.h"
23 #include "fiducial_pose/rosrpp.h"
24 
25 // Radians to degrees
26 double r2d(double r)
27 {
28  return r / M_PI * 180.0;
29 }
30 
31 // Euclidean distance between two points
32 double dist(cv::Mat pts, int i1, int i2)
33 {
34  double x1 = pts.at<double>(0, i1);
35  double y1 = pts.at<double>(1, i1);
36  double x2 = pts.at<double>(0, i2);
37  double y2 = pts.at<double>(1, i2);
38 
39  double dx = x1 - x2;
40  double dy = y1 - y2;
41 
42  return sqrt(dx*dx + dy*dy);
43 }
44 
45 // Compute area in image of a fiducial, using Heron's formula
46 // to find the area of two triangles
47 double calcFiducialArea(cv::Mat pts)
48 {
49  double a1 = dist(pts, 0, 1);
50  double b1 = dist(pts, 0, 3);
51  double c1 = dist(pts, 1, 3);
52 
53  double a2 = dist(pts, 1, 2);
54  double b2 = dist(pts, 2, 3);
55  double c2 = c1;
56 
57  double s1 = (a1 + b1 + c1) / 2.0;
58  double s2 = (a2 + b2 + c2) / 2.0;
59 
60  a1 = sqrt(s1*(s1-a1)*(s1-b1)*(s1-c1));
61  a2 = sqrt(s2*(s2-a2)*(s2-b2)*(s2-c2));
62  return a1+a2;
63 }
64 
65 
66 void undistortPoints(cv::Mat pts, cv::Mat K, cv::Mat dist, bool doUndistort)
67 {
68  if (!doUndistort) {
69  printf("fx %lf fy %lf cx %lf cy %lf", K.at<double>(0, 0), K.at<double>(1, 1), K.at<double>(0, 2), K.at<double>(1, 2));
70  for (int i=0; i<pts.cols; i++) {
71  pts.at<double>(0, i) = (pts.at<double>(0, i) - K.at<double>(0, 2)) / K.at<double>(0, 0);
72  pts.at<double>(1, i) = (pts.at<double>(1, i) - K.at<double>(1, 2)) / K.at<double>(1, 1);
73  }
74  }
75  else {
76  cv::Mat src(1, pts.cols, CV_64FC2);
77  cv::Mat dst(1, pts.cols, CV_64FC2);
78 
79  for (int i=0; i<pts.cols; i++) {
80  src.at<cv::Vec2d>(0, i)[0] = pts.at<double>(0, i);
81  src.at<cv::Vec2d>(0, i)[1] = pts.at<double>(1, i);
82  }
83  std::vector<cv::Point2f> dest;
84  cv::undistortPoints(src, dst, K, dist);
85 
86  for (int i=0; i<pts.cols; i++) {
87  pts.at<double>(0, i) = dst.at<cv::Vec2d>(0, i)[0];
88  pts.at<double>(1, i) = dst.at<cv::Vec2d>(0, i)[1];
89  }
90  }
91 }
92 
93 
94 bool RosRpp::fiducialCallback(fiducial_msgs::Fiducial* msg,
95  fiducial_msgs::FiducialTransform* ft)
96 {
97  ROS_INFO("id %d direction %d", msg->fiducial_id, msg->direction);
98 
99  if (!haveCamInfo) {
100  ROS_ERROR("No camera info");
101  return false;
102  }
103 
104  /* The verices are ordered anti-clockwise, starting with the top-left
105  we want to end up with this:
106  2 3
107  1 0
108  */
109 
110  printf("fid %d dir %d vertices %lf %lf %lf %lf %lf %lf %lf %lf\n",
111  msg->fiducial_id, msg->direction,
112  msg->x0, msg->y0, msg->x1, msg->y1,
113  msg->x2, msg->y2, msg->x3, msg->y3);
114 
115  switch(msg->direction) {
116  case 0:
117  // 0 1 2 3
118  ipts.at<double>(0,0) = msg->x0;
119  ipts.at<double>(1,0) = msg->y0;
120  ipts.at<double>(0,1) = msg->x1;
121  ipts.at<double>(1,1) = msg->y1;
122  ipts.at<double>(0,2) = msg->x2;
123  ipts.at<double>(1,2) = msg->y2;
124  ipts.at<double>(0,3) = msg->x3;
125  ipts.at<double>(1,3) = msg->y3;
126  break;
127 
128  case 1:
129  // 3 0 1 2
130  ipts.at<double>(0,0) = msg->x3;
131  ipts.at<double>(1,0) = msg->y3;
132  ipts.at<double>(0,1) = msg->x0;
133  ipts.at<double>(1,1) = msg->y0;
134  ipts.at<double>(0,2) = msg->x1;
135  ipts.at<double>(1,2) = msg->y1;
136  ipts.at<double>(0,3) = msg->x2;
137  ipts.at<double>(1,3) = msg->y2;
138  break;
139 
140  case 2:
141  // 2 3 0 1
142  ipts.at<double>(0,0) = msg->x2;
143  ipts.at<double>(1,0) = msg->y2;
144  ipts.at<double>(0,1) = msg->x3;
145  ipts.at<double>(1,1) = msg->y3;
146  ipts.at<double>(0,2) = msg->x0;
147  ipts.at<double>(1,2) = msg->y0;
148  ipts.at<double>(0,3) = msg->x1;
149  ipts.at<double>(1,3) = msg->y1;
150  break;
151 
152  case 3:
153  // 1 2 3 0
154  ipts.at<double>(0,0) = msg->x1;
155  ipts.at<double>(1,0) = msg->y1;
156  ipts.at<double>(0,1) = msg->x2;
157  ipts.at<double>(1,1) = msg->y2;
158  ipts.at<double>(0,2) = msg->x3;
159  ipts.at<double>(1,2) = msg->y3;
160  ipts.at<double>(0,3) = msg->x0;
161  ipts.at<double>(1,3) = msg->y0;
162  break;
163  }
164 
165 #if 0
166  // This was an attempt to replace RPP with OpenCV's pose estimation.
167  // It seemed to be more noisy, probably because it is not designed to work with coplanar points.
168 
169  std::vector<cv::Point2f> imagePoints;
170  std::vector<cv::Point3f> model2;
171  for (int i=0; i<4; i++) {
172  imagePoints.push_back(cv::Point2f(ipts.at<double>(0, i), ipts.at<double>(1, i)));
173  model2.push_back(cv::Point3f(model.at<double>(0, i), model.at<double>(1, i), model.at<double>(2, i)));
174  ROS_INFO("model %f %f %f", model.at<double>(0, i), model.at<double>(1, i), model.at<double>(2, i));
175  }
176 
177  cv::Mat rvec(3,1,cv::DataType<double>::type);
178  cv::Mat tvec(3,1,cv::DataType<double>::type);
179 
180  cv::solvePnP(model2, imagePoints, K, dist, rvec, tvec, false, CV_EPNP);
181 
182  ROS_INFO("tvec %lf %lf %lf", tvec.at<double>(0, 0), tvec.at<double>(1, 0), tvec.at<double>(2, 0));
183  ROS_INFO("rvec %lf %lf %lf", r2d(rvec.at<double>(0, 0)), r2d(rvec.at<double>(1, 0)), r2d(rvec.at<double>(2, 0)));
184 
185  tf::Vector3 translation(tvec.at<double>(0, 0), tvec.at<double>(1, 0), tvec.at<double>(2, 0));
186  tf::Quaternion rotation;
187  rotation.setRPY(rvec.at<double>(0, 0), rvec.at<double>(1, 0), rvec.at<double>(2, 0));
188  tf::Transform transform(rotation, translation);
189 
190  ft->transform.translation.x = translation.x();
191  ft->transform.translation.y = translation.y();
192  ft->transform.translation.z = translation.z();
193 
194  ft->transform.rotation.w = rotation.w();
195  ft->transform.rotation.x = rotation.x();
196  ft->transform.rotation.y = rotation.y();
197  ft->transform.rotation.z = rotation.z();
198  ft->fiducial_id = msg->fiducial_id;
199 #else
200 
201  undistortPoints(ipts, K, dist, doUndistort);
202 
203  cv::Mat rotation;
204  cv::Mat translation;
205  int iterations;
206  double obj_err;
207  double img_err;
208 
209  std::vector<RPP::Solution> sol;
210 
211  std::map<int, cv::Mat>::iterator it;
212 
213  it = prevRots.find(msg->fiducial_id);
214  if (it == prevRots.end())
215  rotation = cv::Mat();
216  else
217  rotation = it->second;
218 
219  if(!RPP::Rpp(model, ipts, rotation, translation, iterations, obj_err, img_err, sol)) {
220  ROS_ERROR("Cannot find transform for fiducial %d", msg->fiducial_id);
221  return false;
222  }
223 
224  prevRots[msg->fiducial_id] = rotation;
225 
226  //ROS_INFO("fid %d iterations %d object error %f image error %f",
227  // msg->fiducial_id, iterations, obj_err, img_err);
228 
229  /*
230  ROS_INFO("fid %d solutions %lu", msg->fiducial_id, sol.size());
231  for (unsigned int i=0; i<sol.size(); i++) {
232  double r, p, y;
233  tf::Matrix3x3 m(sol[i].R.at<double>(0,0), sol[i].R.at<double>(0,1), sol[i].R.at<double>(0,2),
234  sol[i].R.at<double>(1,0), sol[i].R.at<double>(1,1), sol[i].R.at<double>(1,2),
235  sol[i].R.at<double>(2,0), sol[i].R.at<double>(2,1), sol[i].R.at<double>(2,2));
236 
237  tf::Vector3 t(translation.at<double>(0), translation.at<double>(1), translation.at<double>(2));
238 
239  tf::Transform trans(m, t);
240 
241  for (int j=0; j<4; j++) {
242  tf::Vector3 pi(model.at<double>(0,j), model.at<double>(1,j), 0);
243  tf::Vector3 pw = trans * pi;
244  ROS_INFO("vertex %d %f %f %f", j, pw.x(), pw.y(), pw.z());
245  }
246  m.getRPY(r, p, y);
247  ROS_INFO("fid %d sol %d R: %.2f %.2f %.2f", msg->fiducial_id, i, r2d(r), r2d(p), r2d(y));
248  }
249  */
250 
251  tf::Matrix3x3 m1(rotation.at<double>(0,0), rotation.at<double>(0,1), rotation.at<double>(0,2),
252  rotation.at<double>(1,0), rotation.at<double>(1,1), rotation.at<double>(1,2),
253  rotation.at<double>(2,0), rotation.at<double>(2,1), rotation.at<double>(2,2));
254  tf::Vector3 t1(translation.at<double>(0), translation.at<double>(1), translation.at<double>(2));
255 
256  tf::Transform trans1(m1, t1);
257 
258  frameTransforms[msg->fiducial_id] = trans1.inverse();
259  t1 = trans1.getOrigin();
260  m1 = trans1.getBasis();
261 
262  double r, p, y;
263  m1.getRPY(r, p, y);
264  ROS_INFO("fid %d T: %.3lf %.3lf %.3lf R: %.2f %.2f %.2f",
265  msg->fiducial_id,
266  t1.x(), t1.y(), t1.z(),
267  r2d(r), r2d(p), r2d(y));
268 
269  /*
270  ft->header.stamp = frameTime;
271  char t1name[32];
272  sprintf(t1name, "fiducial_%d", msg->fiducial_id);
273  ft->header.frame_id = t1name;
274  */
275 
276  ft->transform.translation.x = t1.x();
277  ft->transform.translation.y = t1.y();
278  ft->transform.translation.z = t1.z();
279  tf::Quaternion q = trans1.getRotation();
280  ft->transform.rotation.w = q.w();
281  ft->transform.rotation.x = q.x();
282  ft->transform.rotation.y = q.y();
283  ft->transform.rotation.z = q.z();
284 
285  ft->fiducial_id = msg->fiducial_id;
286  // ft->image_seq = msg->image_seq;
287  ft->image_error = img_err;
288  ft->object_error = obj_err;
289  ft->fiducial_area = calcFiducialArea(ipts);
290 #endif
291  return true;
292 }
293 
294 
295 void RosRpp::camInfoCallback(const sensor_msgs::CameraInfo::ConstPtr& msg)
296 {
297  if (haveCamInfo) {
298  return;
299  }
300 
301  for (int i=0; i<3; i++) {
302  for (int j=0; j<3; j++) {
303  K.at<double>(i, j) = msg->K[i*3+j];
304  }
305  }
306 
307  printf("camera intrinsics:\n");
308  RPP::Print(K);
309 
310  for (int i=0; i<5; i++) {
311  dist.at<double>(0,i) = msg->D[i];
312  }
313 
314  printf("distortion coefficients:\n");
315  RPP::Print(dist);
316 
317  haveCamInfo = true;
318 }
319 
320 
321 RosRpp::RosRpp(double fiducialLen, bool doUndistort)
322 {
323  this->fiducialLen = fiducialLen;
324  this->doUndistort = doUndistort;
325 
326  // Camera intrinsics
327  K = cv::Mat::zeros(3, 3, CV_64F);
328 
329  // distortion coefficients
330  dist = cv::Mat::zeros(1, 5, CV_64F);
331 
332  // homogeneous 2D points
333  ipts = cv::Mat::ones(3, 4, CV_64F);
334 
335  // 3D points of a fiducial at the origin z = 0
336  model = cv::Mat::zeros(3, 4, CV_64F);
337 
338  haveCamInfo = false;
339 
340  /*
341  Vertex ordering:
342 
343  2 3
344  1 0
345 
346  World
347  y
348  ^
349  |
350  `-->x
351 
352  Fiducial with origin at center:
353  */
354 
355  model.at<double>(0,0) = fiducialLen / 2.0;
356  model.at<double>(1,0) = -fiducialLen / 2.0;
357 
358  model.at<double>(0,1) = -fiducialLen / 2.0;
359  model.at<double>(1,1) = -fiducialLen / 2.0;
360 
361  model.at<double>(0,2) = -fiducialLen / 2.0;
362  model.at<double>(1,2) = fiducialLen / 2.0;
363 
364  model.at<double>(0,3) = fiducialLen / 2.0;
365  model.at<double>(1,3) = fiducialLen / 2.0;
366 
367  currentFrame = 0;
368 }
RosRpp::prevRots
std::map< int, cv::Mat > prevRots
Definition: rosrpp.h:23
RosRpp::dist
cv::Mat dist
Definition: rosrpp.h:10
r2d
double r2d(double r)
Definition: calcPose.cpp:26
tf::Transform::getBasis
TFSIMD_FORCE_INLINE Matrix3x3 & getBasis()
undistortPoints
void undistortPoints(cv::Mat pts, cv::Mat K, cv::Mat dist, bool doUndistort)
Definition: calcPose.cpp:66
RPP::Rpp
bool Rpp(const cv::Mat &model_3D, const cv::Mat &iprts, cv::Mat &Rlu, cv::Mat &tlu, int &it1, double &obj_err1, double &img_err1, std::vector< Solution > &sol)
RosRpp::RosRpp
RosRpp(double fiducialLen, bool doUndistort)
Definition: calcPose.cpp:321
transform_broadcaster.h
RosRpp::haveCamInfo
bool haveCamInfo
Definition: rosrpp.h:12
RosRpp::currentFrame
int currentFrame
Definition: rosrpp.h:13
RosRpp::fiducialCallback
bool fiducialCallback(fiducial_msgs::Fiducial *fiducial, fiducial_msgs::FiducialTransform *transform)
Definition: calcPose.cpp:94
y
TFSIMD_FORCE_INLINE const tfScalar & y() const
RPP::Print
void Print(const cv::Mat &m)
dist
double dist(cv::Mat pts, int i1, int i2)
Definition: calcPose.cpp:32
tf::Vector3::x
TFSIMD_FORCE_INLINE const tfScalar & x() const
tf::Vector3::z
TFSIMD_FORCE_INLINE const tfScalar & z() const
RosRpp::fiducialLen
double fiducialLen
Definition: rosrpp.h:21
tf::Quaternion::setRPY
void setRPY(const tfScalar &roll, const tfScalar &pitch, const tfScalar &yaw)
tf::Matrix3x3::getRPY
void getRPY(tfScalar &roll, tfScalar &pitch, tfScalar &yaw, unsigned int solution_number=1) const
ROS_INFO
#define ROS_INFO(...)
tf::Vector3::y
TFSIMD_FORCE_INLINE const tfScalar & y() const
RosRpp::doUndistort
bool doUndistort
Definition: rosrpp.h:20
tf::Transform::inverse
Transform inverse() const
RosRpp::frameTransforms
std::map< int, tf::Transform > frameTransforms
Definition: rosrpp.h:15
tf::Transform::getOrigin
TFSIMD_FORCE_INLINE Vector3 & getOrigin()
RosRpp::ipts
cv::Mat ipts
Definition: rosrpp.h:8
tf::Transform::getRotation
Quaternion getRotation() const
RosRpp::model
cv::Mat model
Definition: rosrpp.h:7
RosRpp::K
cv::Mat K
Definition: rosrpp.h:9
RosRpp::camInfoCallback
void camInfoCallback(const sensor_msgs::CameraInfo::ConstPtr &msg)
Definition: calcPose.cpp:295
ROS_ERROR
#define ROS_ERROR(...)
calcFiducialArea
double calcFiducialArea(cv::Mat pts)
Definition: calcPose.cpp:47

fiducial_pose
Author(s): Jim Vaughan
autogenerated on Thu Dec 28 2017 04:06:58