pcdwriter.cpp
Go to the documentation of this file.
1 #include <opencv2/calib3d/calib3d.hpp>
2 #include <opencv2/imgproc/imgproc.hpp>
3 #include "markermap.h"
4 #include <fstream>
5 #include <map>
6 using namespace std;
7 
8 
9 
10 
11 void getRTfromMatrix44 ( const cv::Mat &M, cv::Mat &R,cv::Mat &T ) {
12 
13  assert ( M.cols==M.rows && M.cols==4 );
14  assert ( M.type() ==CV_32F || M.type() ==CV_64F );
15 //extract the rotation part
16  cv::Mat r33=cv::Mat ( M,cv::Rect ( 0,0,3,3 ) );
17  cv::SVD svd ( r33 );
18  cv::Mat Rpure=svd.u*svd.vt;
19  cv::Rodrigues ( Rpure,R );
20  T.create ( 1,3,M.type() );
21  if ( M.type() ==CV_32F )
22  for ( int i=0; i<3; i++ )
23  T.ptr<float> ( 0 ) [i]=M.at<float> ( i,3 );
24  else
25  for ( int i=0; i<3; i++ )
26  T.ptr<double> ( 0 ) [i]=M.at<double> ( i,3 );
27 }
28 
29 /**********************
30  *
31  *
32  **********************/
33 struct Quaternion
34 {
35  Quaternion ( float q0,float q1,float q2,float q3 ) {
36  q[0]=q0;
37  q[1]=q1;
38  q[2]=q2;
39  q[3]=q3;
40  }
41  cv::Mat getRotation() const
42  {
43  cv::Mat R ( 3,3,CV_32F );
44  R.at<float> ( 0,0 ) =q[0]*q[0] +q[1]*q[1] -q[2]*q[2] -q[3]*q[3];
45  R.at<float> ( 0,1 ) =2.* ( q[1]*q[2] - q[0]*q[3] );
46  R.at<float> ( 0,2 ) =2.* ( q[1]*q[3] + q[0]*q[2] );
47 
48  R.at<float> ( 1,0 ) =2.* ( q[1]*q[2] + q[0]*q[3] );
49  R.at<float> ( 1,1 ) =q[0]*q[0] +q[2]*q[2] -q[1]*q[1] -q[3]*q[3];
50  R.at<float> ( 1,2 ) =2.* ( q[2]*q[3] - q[0]*q[1] );
51 
52  R.at<float> ( 2,0 ) =2.* ( q[1]*q[3] - q[0]*q[2] );
53  R.at<float> ( 2,1 ) =2.* ( q[2]*q[3] + q[0]*q[1] );
54  R.at<float> ( 2,2 ) =q[0]*q[0] +q[3]*q[3] -q[1]*q[1] -q[2]*q[2];
55  return R;
56  }
57  float q[4];
58 };
59 
60 /**********************
61  *
62  *
63  **********************/
64 float rigidBodyTransformation_Horn1987 ( cv::Mat& S, cv::Mat& M,cv::Mat &RT_4x4 ) {
65 
66  assert ( S.total() ==M.total() );
67  assert ( S.type() ==M.type() );
68  assert ( S.rows>S.cols && M.rows>M.cols );
69 
70  cv::Mat _s,_m;
71  S.convertTo ( _s,CV_32F );
72  M.convertTo ( _m,CV_32F );
73  _s=_s.reshape ( 1 );
74  _m=_m.reshape ( 1 );
75  cv::Mat Mu_s=cv::Mat::zeros ( 1,3,CV_32F );
76  cv::Mat Mu_m=cv::Mat::zeros ( 1,3,CV_32F );
77 // cout<<_s<<endl<<_m<<endl;
78 //calculate means
79  for ( int i=0; i<_s.rows; i++ ) {
80  Mu_s+=_s ( cv::Range ( i,i+1 ),cv::Range ( 0,3 ) );
81  Mu_m+=_m ( cv::Range ( i,i+1 ),cv::Range ( 0,3 ) );
82  }
83 //now, divide
84  for ( int i=0; i<3; i++ ) {
85  Mu_s.ptr<float> ( 0 ) [i]/=float ( _s.rows );
86  Mu_m.ptr<float> ( 0 ) [i]/=float ( _m.rows );
87  }
88 
89 // cout<<"Mu_s="<<Mu_s<<endl;
90 // cout<<"Mu_m="<<Mu_m<<endl;
91 
92  cv::Mat Mu_st=Mu_s.t() *Mu_m;
93 // cout<<"Mu_st="<<Mu_st<<endl;
94  cv::Mat Var_sm=cv::Mat::zeros ( 3,3,CV_32F );
95  for ( int i=0; i<_s.rows; i++ )
96  Var_sm+= ( _s ( cv::Range ( i,i+1 ),cv::Range ( 0,3 ) ).t() *_m ( cv::Range ( i,i+1 ),cv::Range ( 0,3 ) ) ) - Mu_st;
97 // cout<<"Var_sm="<<Var_sm<<endl;
98  for ( int i=0; i<3; i++ )
99  for ( int j=0; j<3; j++ )
100  Var_sm.at<float> ( i,j ) /=float ( _s.rows );
101 // cout<<"Var_sm="<<Var_sm<<endl;
102 
103  cv::Mat AA=Var_sm-Var_sm.t();
104 // cout<<"AA="<<AA<<endl;
105  cv::Mat A ( 3,1,CV_32F );
106  A.at<float> ( 0,0 ) =AA.at<float> ( 1,2 );
107  A.at<float> ( 1,0 ) =AA.at<float> ( 2,0 );
108  A.at<float> ( 2,0 ) =AA.at<float> ( 0,1 );
109 // cout<<"A ="<<A <<endl;
110  cv::Mat Q_Var_sm ( 4,4,CV_32F );
111  Q_Var_sm.at<float> ( 0,0 ) =trace ( Var_sm ) [0];
112  for ( int i=1; i<4; i++ ) {
113  Q_Var_sm.at<float> ( 0,i ) =A.ptr<float> ( 0 ) [i-1];
114  Q_Var_sm.at<float> ( i,0 ) =A.ptr<float> ( 0 ) [i-1];
115  }
116  cv::Mat q33=Var_sm+Var_sm.t()- ( trace ( Var_sm ) [0]*cv::Mat::eye ( 3,3,CV_32F ) );
117 
118  cv::Mat Q33=Q_Var_sm ( cv::Range ( 1,4 ),cv::Range ( 1,4 ) );
119  q33.copyTo ( Q33 );
120 // cout<<"Q_Var_sm"<<endl<< Q_Var_sm<<endl;
121  cv::Mat eigenvalues,eigenvectors;
122  eigen ( Q_Var_sm,eigenvalues,eigenvectors );
123 // cout<<"EEI="<<eigenvalues<<endl;
124 // cout<<"V="<<(eigenvectors.type()==CV_32F)<<" "<<eigenvectors<<endl;
125 
126  Quaternion rot ( eigenvectors.at<float> ( 0,0 ),eigenvectors.at<float> ( 0,1 ),eigenvectors.at<float> ( 0,2 ),eigenvectors.at<float> ( 0,3 ) );
127  cv::Mat RR=rot.getRotation();
128 // cout<<"RESULT="<<endl<<RR<<endl;
129  cv::Mat T= Mu_m.t()- RR*Mu_s.t();
130 // cout<<"T="<<T<<endl;
131 
132  RT_4x4=cv::Mat::eye ( 4,4,CV_32F );
133  cv::Mat r33=RT_4x4 ( cv::Range ( 0,3 ),cv::Range ( 0,3 ) );
134  RR.copyTo ( r33 );
135  for ( int i=0; i<3; i++ ) RT_4x4.at<float> ( i,3 ) =T.ptr<float> ( 0 ) [i];
136 // cout<<"RESS="<<RT<<endl;
137 
138  //compute the average transform error
139 
140  float err=0;
141  float *matrix=RT_4x4.ptr<float> ( 0 );
142  for ( int i=0; i<S.rows; i++ ) {
143  cv::Point3f org= S.ptr<cv::Point3f> ( 0 ) [i];
144  cv::Point3f dest_est;
145  dest_est.x= matrix[0]*org.x+ matrix[1]*org.y +matrix[2]*org.z+matrix[3];
146  dest_est.y= matrix[4]*org.x+ matrix[5]*org.y +matrix[6]*org.z+matrix[7];
147  dest_est.z= matrix[8]*org.x+ matrix[9]*org.y +matrix[10]*org.z+matrix[11];
148  cv::Point3f dest_real=M.ptr<cv::Point3f> ( 0 ) [ i ];
149  err+=cv::norm ( dest_est-dest_real );
150  }
151  return err/float ( S.rows );;
152 }
153 /**********************
154  *
155  *
156  **********************/
157 float rigidBodyTransformation_Horn1987 ( cv::Mat& _s, cv::Mat& _m,cv::Mat &Rvec,cv::Mat &Tvec ) {
158  cv::Mat RT;
159  float err= rigidBodyTransformation_Horn1987 ( _s,_m,RT );
160  getRTfromMatrix44 ( RT,Rvec,Tvec );
161  return err;
162 }
163 
164 
165 float rigidBodyTransformation_Horn1987 (const vector<cv::Point3f> & orgPoints_32FC3,const vector<cv::Point3f> &dstPoints_32FC3,cv::Mat &Rvec,cv::Mat &Tvec ) {
166  cv::Mat Morg,Mdest;
167  Morg.create ( orgPoints_32FC3.size(),1,CV_32FC3 );
168  Mdest.create ( dstPoints_32FC3.size(),1,CV_32FC3 );
169  for ( size_t i=0; i<dstPoints_32FC3.size(); i++ ) {
170  Morg.ptr< cv::Point3f> ( 0 ) [i]=orgPoints_32FC3[i];
171  Mdest.ptr< cv::Point3f> ( 0 ) [i]=dstPoints_32FC3[i];
172  }
173  return rigidBodyTransformation_Horn1987 ( Morg,Mdest,Rvec,Tvec );
174 
175 }
176 
177 cv::Mat getRTMatrix ( const cv::Mat &R_,const cv::Mat &T_ ,int forceType=-1 ) {
178  cv::Mat M;
179  cv::Mat R,T;
180  R_.copyTo ( R );
181  T_.copyTo ( T );
182  if ( R.type() ==CV_64F ) {
183  assert ( T.type() ==CV_64F );
184  cv::Mat Matrix=cv::Mat::eye ( 4,4,CV_64FC1 );
185 
186  cv::Mat R33=cv::Mat ( Matrix,cv::Rect ( 0,0,3,3 ) );
187  if ( R.total() ==3 ) {
188  cv::Rodrigues ( R,R33 );
189  } else if ( R.total() ==9 ) {
190  cv::Mat R64;
191  R.convertTo ( R64,CV_64F );
192  R.copyTo ( R33 );
193  }
194  for ( int i=0; i<3; i++ )
195  Matrix.at<double> ( i,3 ) =T.ptr<double> ( 0 ) [i];
196  M=Matrix;
197  } else if ( R.depth() ==CV_32F ) {
198  cv::Mat Matrix=cv::Mat::eye ( 4,4,CV_32FC1 );
199  cv::Mat R33=cv::Mat ( Matrix,cv::Rect ( 0,0,3,3 ) );
200  if ( R.total() ==3 ) {
201  cv::Rodrigues ( R,R33 );
202  } else if ( R.total() ==9 ) {
203  cv::Mat R32;
204  R.convertTo ( R32,CV_32F );
205  R.copyTo ( R33 );
206  }
207 
208  for ( int i=0; i<3; i++ )
209  Matrix.at<float> ( i,3 ) =T.ptr<float> ( 0 ) [i];
210  M=Matrix;
211  }
212 
213  if ( forceType==-1 ) return M;
214  else {
215  cv::Mat MTyped;
216  M.convertTo ( MTyped,forceType );
217  return MTyped;
218  }
219 }
220 
221 cv::Mat rigidBodyTransformation_Horn1987 (const std::vector<cv::Point3f> &org, const std::vector<cv::Point3f> &dst,double *err=0){
222  double e=0;
223  cv::Mat r,t;
224  e=rigidBodyTransformation_Horn1987(org,dst,r,t);
225  if (err) *err=e;
226  return getRTMatrix(r,t);
227 }
228 
229 
230  cv::Point3f mult ( const cv::Mat &m, cv::Point3f p ) {
231  assert ( m.isContinuous() );
232  if ( m.type() ==CV_32F ) {
233  const float *ptr=m.ptr<float> ( 0 );
234  cv::Point3f res;
235  res.x= ptr[0]*p.x +ptr[1]*p.y +ptr[2]*p.z+ptr[3];
236  res.y= ptr[4]*p.x +ptr[5]*p.y +ptr[6]*p.z+ptr[7];
237  res.z= ptr[8]*p.x +ptr[9]*p.y +ptr[10]*p.z+ptr[11];
238  return res;
239  } else if ( m.type() ==CV_64F ) {
240  const double *ptr=m.ptr<double> ( 0 );
241  cv::Point3f res;
242  res.x= ptr[0]*p.x +ptr[1]*p.y +ptr[2]*p.z+ptr[3];
243  res.y= ptr[4]*p.x +ptr[5]*p.y +ptr[6]*p.z+ptr[7];
244  res.z= ptr[8]*p.x +ptr[9]*p.y +ptr[10]*p.z+ptr[11];
245  return res;
246 
247  }
248 }
249 
250 std::vector<cv::Vec4f> getPcdPoints(const vector<cv::Point3f> &mpoints,cv::Scalar color,int npoints=100){
251  vector<cv::Vec4f> points;
252  double msize=cv::norm(mpoints[0]-mpoints[1]);
253  float fcolor;uchar *c=(uchar*)&fcolor;
254  for(int i=0;i<3;i++)c[i]=color[i];
255 
256  //lines joining points
257  for(size_t i=0;i<mpoints.size();i++){
258  cv::Point3f v=mpoints[(i+1)%mpoints.size()]-mpoints[i];
259  float ax=1./float(npoints);//npoints
260  for(float x=0;x<=1;x+=ax){
261  cv::Point3f p3=mpoints[i]+v*x;
262  points.push_back(cv::Vec4f(p3.x,p3.y,p3.z, fcolor));
263  }
264  }
265 
266  //line indicating direction
267  //take first and second, first and last , and get the cross vector indicating the direction
268  cv::Point3f v1=mpoints[1]-mpoints[0];
269  cv::Point3f v2=mpoints[3]-mpoints[0];
270  v1*=1./cv::norm(v1);
271  v2*=1./cv::norm(v2);
272  cv::Point3f vz=v2.cross(v1);
273  vz*=1./cv::norm(vz);//now, unit
274 
275  //set the center
276  cv::Point3f center=(mpoints[0]+mpoints[1]+mpoints[2]+mpoints[3])*0.25;
277  float ax=(msize/3)/100;
278  for(float x=0;x<=msize/3;x+=ax){
279  cv::Point3f p3=center+vz*x;
280  points.push_back(cv::Vec4f(p3.x,p3.y,p3.z, fcolor));
281 
282  }
283 
284 
285  return points;
286 
287 }
288 
289 
290 
291 vector<cv::Vec4f> getMarkerIdPcd( aruco::Marker3DInfo &minfo,cv::Scalar color ){
292 
293 int id=minfo.id;
294  float markerSize=minfo.getMarkerSize();
295  cv::Mat rt_g2m= rigidBodyTransformation_Horn1987(aruco::Marker::get3DPoints(markerSize),minfo);
296  //marker id as a set of points
297  string text = std::to_string(id);
298  int fontFace = cv::FONT_HERSHEY_SCRIPT_SIMPLEX;
299  double fontScale = 2;
300  int thickness = 3;
301  int baseline=0;
302  float markerSize_2=markerSize/2;
303  cv::Size textSize = cv::getTextSize(text, fontFace,
304  fontScale, thickness, &baseline);
305  cv::Mat img(textSize +cv::Size(0,baseline/2), CV_8UC1,cv::Scalar::all(0));
306  // center the text
307  // then put the text itself
308  cv::putText(img, text, cv::Point(0,textSize.height+baseline/4), fontFace, fontScale,cv::Scalar::all(255), thickness, 8);
309  //raster 2d points as 3d points
310  vector<cv::Point3f> points_id;
311  for(int y=0;y<img.rows;y++)
312  for(int x=0;x<img.cols;x++)
313  if (img.at<uchar>(y,x)!=0) points_id.push_back( cv::Point3f((float(x)/float(img.cols))-0.5,(float(img.rows-y)/float(img.rows))-0.5,0));
314 
315 
316  //now,scale
317  for(auto &p:points_id)p*=markerSize_2;
318  //finally, translate
319  for(auto &p:points_id)p= mult( rt_g2m,p);
320  //now, add to ouput
321 
322  float fcolor;uchar *c=(uchar*)&fcolor;
323  for(int i=0;i<3;i++)c[i]=color[i];
324 
325  vector<cv::Vec4f> res;
326  for(auto &p:points_id)
327  res.push_back(cv::Vec4f(p.x,p.y,p.z, fcolor));
328 
329 
330 
331  return res;
332 }
333 
334 
335 void savePCDFile(string fpath,const aruco::MarkerMap &ms,const std::map<int,cv::Mat> frame_pose_map)throw(std::exception) {
336 
337  std::vector<cv::Vec4f> points2write;
338  for(auto m:ms)
339  {
340  cv::Scalar color;
341  color=cv::Scalar(255,0,0);
342  auto points4=getPcdPoints(m,color);
343  points2write.insert(points2write.end(),points4.begin(),points4.end());
344  auto points_id=getMarkerIdPcd(m,color);
345  points2write.insert(points2write.end(),points_id.begin(),points_id.end());
346  //max_msize=std::max(max_msize,m.second.markerSize);
347  }
348  for(auto frame_pose:frame_pose_map){
349  if (!frame_pose.second.empty()){
350  cv::Mat g2c=frame_pose.second.inv();
351  auto mpoints=aruco::Marker::get3DPoints(ms[0].getMarkerSize()/2);
352  for(auto &p:mpoints) p=mult(g2c,p);
353  auto pcam=getPcdPoints( mpoints,cv::Scalar(0,255,0),25);
354  points2write.insert(points2write.end(),pcam.begin(),pcam.end());
355 
356  }
357  }
358  //now, the 3d points
359 
360 
361 
362  std::ofstream filePCD ( fpath, std::ios::binary );
363 
364  filePCD<<"# .PCD v.7 - Point Cloud Data file format\nVERSION .7\nFIELDS x y z rgb\nSIZE 4 4 4 4\nTYPE F F F F\nCOUNT 1 1 1 1\nVIEWPOINT 0 0 0 1 0 0 0\nWIDTH "<<points2write.size()<<"\nHEIGHT 1\nPOINTS "<<points2write.size()<<"\nDATA binary\n";
365 
366 
367  filePCD.write((char*)&points2write[0],points2write.size()*sizeof(points2write[0]));
368 
369 }
frame_pose_map
std::map< int, cv::Mat > frame_pose_map
Definition: aruco_test_markermap.cpp:51
savePCDFile
void savePCDFile(string fpath, const aruco::MarkerMap &ms, const std::map< int, cv::Mat > frame_pose_map)
Definition: pcdwriter.cpp:335
aruco::Marker::get3DPoints
static vector< cv::Point3f > get3DPoints(float msize)
Definition: marker.cpp:285
getRTfromMatrix44
void getRTfromMatrix44(const cv::Mat &M, cv::Mat &R, cv::Mat &T)
Definition: pcdwriter.cpp:11
getMarkerIdPcd
vector< cv::Vec4f > getMarkerIdPcd(aruco::Marker3DInfo &minfo, cv::Scalar color)
Definition: pcdwriter.cpp:291
Quaternion::Quaternion
Quaternion(float q0, float q1, float q2, float q3)
Definition: pcdwriter.cpp:35
Quaternion::getRotation
cv::Mat getRotation() const
Definition: pcdwriter.cpp:41
mult
cv::Point3f mult(const cv::Mat &m, cv::Point3f p)
Definition: pcdwriter.cpp:230
rigidBodyTransformation_Horn1987
float rigidBodyTransformation_Horn1987(cv::Mat &S, cv::Mat &M, cv::Mat &RT_4x4)
Definition: pcdwriter.cpp:64
getRTMatrix
cv::Mat getRTMatrix(const cv::Mat &R_, const cv::Mat &T_, int forceType=-1)
Definition: pcdwriter.cpp:177
x
TFSIMD_FORCE_INLINE const tfScalar & x() const
aruco::Marker3DInfo::getMarkerSize
float getMarkerSize() const
Definition: markermap.h:47
getPcdPoints
std::vector< cv::Vec4f > getPcdPoints(const vector< cv::Point3f > &mpoints, cv::Scalar color, int npoints=100)
Definition: pcdwriter.cpp:250
aruco::MarkerMap
This class defines a set of markers whose locations are attached to a common reference system...
Definition: markermap.h:71

tuw_aruco
Author(s): Lukas Pfeifhofer
autogenerated on Mon Jun 10 2019 15:40:55