@@ -46,7 +46,7 @@ Matrix3d uStraightLineSegment::CC()
4646uPoint uStraightLineSegment::ux () const
4747{
4848 Matrix<double , 3 ,6 > JJ;
49- Matrix<double , 6 ,6 > Cov = Cov_tt_ .topLeftCorner (6 ,6 );
49+ Matrix<double , 6 ,6 > Cov = m_Cov_tt .topLeftCorner (6 ,6 );
5050 Matrix3d Sl = skew ( hl () );
5151
5252 JJ.leftCols (3 ) = -skew ( hm () );
@@ -73,7 +73,7 @@ uPoint uStraightLineSegment::uy() const
7373// ! Construction of an uncertain straight line segment via two uncorrelated endpoints
7474uStraightLineSegment::uStraightLineSegment ( const uPoint & ux,
7575 const uPoint & uy)
76- : bounding_box_ ( ux.bbox().united( uy.bbox()) )
76+ : m_bounding_box ( ux.bbox().united( uy.bbox()) )
7777{
7878 // (1) 9-parameter vector t = [l; m; n];
7979 const Matrix3d Sx = skew ( ux.v () );
@@ -88,9 +88,9 @@ uStraightLineSegment::uStraightLineSegment( const uPoint & ux,
8888 double xh = ux.v ()(2 );
8989 double yh = uy.v ()(2 );
9090 // Vector9d t;
91- t_ .segment ( 0 , 3 ) = l;
92- t_ .segment ( 3 , 3 ) = +sign (yh)*UUx*uy.v ();
93- t_ .segment ( 6 , 3 ) = -sign (xh)*UUy*ux.v ();
91+ m_t .segment ( 0 , 3 ) = l;
92+ m_t .segment ( 3 , 3 ) = +sign (yh)*UUx*uy.v ();
93+ m_t .segment ( 6 , 3 ) = -sign (xh)*UUy*ux.v ();
9494
9595 // (2) 9x9 covariance matrix
9696 // Matrix3d JJlx = -Sy;
@@ -120,7 +120,7 @@ uStraightLineSegment::uStraightLineSegment( const uPoint & ux,
120120 Cov_pp.block (0 ,0 ,3 ,3 ) = ux.Cov ();
121121 Cov_pp.block (3 ,3 ,3 ,3 ) = uy.Cov ();
122122
123- Cov_tt_ = JJ*Cov_pp*JJ.transpose ();
123+ m_Cov_tt = JJ*Cov_pp*JJ.transpose ();
124124 // return { t, JJ*Cov_pp*JJ.transpose()};
125125}
126126
@@ -186,41 +186,41 @@ bool uStraightLineSegment::touchedBy( const uPoint & ux,
186186// ! Get the endpoints connecting uncertain straight line l = S(x)*y
187187uStraightLine uStraightLineSegment::ul () const
188188{
189- Matrix3d Cov_ll = Cov_tt_ .topLeftCorner (3 ,3 );
190- return { t_ .head (3 ), Cov_ll };
189+ Matrix3d Cov_ll = m_Cov_tt .topLeftCorner (3 ,3 );
190+ return { m_t .head (3 ), Cov_ll };
191191}
192192
193193
194194// ! Get the delimiting uncertain straight line m in homogeneous coordinates.
195195uStraightLine uStraightLineSegment::um () const
196196{
197- Matrix3d Cov_mm = Cov_tt_ .block (3 ,3 ,3 ,3 );
198- return { t_ .segment (3 ,3 ), Cov_mm};
197+ Matrix3d Cov_mm = m_Cov_tt .block (3 ,3 ,3 ,3 );
198+ return { m_t .segment (3 ,3 ), Cov_mm};
199199}
200200
201201
202202// ! Get the delimiting uncertain straight line n in homogeneous coordinates.
203203uStraightLine uStraightLineSegment::un () const
204204{
205- Matrix3d Cov_nn = Cov_tt_ .bottomRightCorner (3 ,3 );
206- return { t_ .tail (3 ), Cov_nn};
205+ Matrix3d Cov_nn = m_Cov_tt .bottomRightCorner (3 ,3 );
206+ return { m_t .tail (3 ), Cov_nn};
207207}
208208
209209
210210// ! Get the endpoint x in homogeneous coordinates
211211Vector3d uStraightLineSegment::hx () const
212212{
213- Vector3d l = t_ .head (3 );
214- Vector3d m = t_ .segment (3 ,3 );
213+ Vector3d l = m_t .head (3 );
214+ Vector3d m = m_t .segment (3 ,3 );
215215 return skew ( l )*m;
216216}
217217
218218
219219// ! Get the endpoint y in homogeneous coordinates
220220Vector3d uStraightLineSegment::hy () const
221221{
222- Vector3d l = t_ .head (3 );
223- Vector3d n = t_ .tail (3 );
222+ Vector3d l = m_t .head (3 );
223+ Vector3d n = m_t .tail (3 );
224224 return skew ( l )*n;
225225}
226226
@@ -294,8 +294,8 @@ bool uStraightLineSegment::isCopunctualWith( const uStraightLineSegment & us,
294294// ! Transform uncertain straight line segment via 9x9 transformation matrix for t = [l',m',n']'.
295295void uStraightLineSegment::transform ( const Matrix9d & TT)
296296{
297- t_ = TT*t_ ;
298- Cov_tt_ = TT*Cov_tt_ *TT.adjoint ();
297+ m_t = TT*m_t ;
298+ m_Cov_tt = TT*m_Cov_tt *TT.adjoint ();
299299}
300300
301301/* Matlab: [~,idx] = sort(x);
@@ -318,6 +318,26 @@ VectorXi uStraightLineSegment::indices_of_sorting( const VectorXd &v)
318318}*/
319319
320320
321+ QDataStream & operator << (QDataStream & out, const Aabb & bbox)
322+ {
323+ out << bbox.x_min () << bbox.x_max ()
324+ << bbox.y_min () << bbox.y_max ();
325+ return out;
326+ }
327+
328+ QDataStream & operator >> (QDataStream & in, Aabb & bbox)
329+ {
330+ double x_min;
331+ double x_max;
332+ double y_min;
333+ double y_max;
334+ in >> x_min >> x_max >> y_min >> y_max;
335+ bbox = Aabb ( x_min, x_max, y_min, y_max);
336+
337+ return in;
338+ }
339+
340+
321341QDataStream & operator >> ( QDataStream & in, Eigen::Matrix<double ,9 ,1 > & v);
322342
323343// ! Overloaded >>operator for 9-vectors
@@ -374,38 +394,29 @@ QDataStream & operator<< ( QDataStream & out, const Eigen::Matrix<double,9,9> &M
374394 return out;
375395}
376396
377- QDataStream & operator << ( QDataStream & out, const aabb & box);
378-
379- // ! Overloaded <<operator for axis-aligned bounding box
380- QDataStream & operator << ( QDataStream & out, const aabb & bbox)
381- {
382- qDebug () << Q_FUNC_INFO;
383- out << bbox.x_min () << bbox.x_max () << bbox.y_min () << bbox.y_max ();
384- return out;
385- }
386-
387397
388398// ! Serialization of the uncertain straight line segment t=[l',m',n']' and its bounding box
389399void uStraightLineSegment::serialize ( QDataStream & out ) const
390400{
391401 qDebug () << Q_FUNC_INFO;
392- out << t_ ;
393- out << Cov_tt_ ;
394- bounding_box_. serialize ( out) ;
402+ out << m_t ;
403+ out << m_Cov_tt ;
404+ out << m_bounding_box ;
395405}
396406
397407// ! Deserialization of uncertain straight line segment and its bounding box
398408bool uStraightLineSegment::deserialize ( QDataStream & in )
399409{
400410 qDebug () << Q_FUNC_INFO;
401- in >> t_;
402- in >> Cov_tt_;
403- if ( in.status ()!=0 ) {
404- return false ;
405- }
406- if ( !bounding_box_.deserialize ( in) ) {
411+ in >> m_t ;
412+ in >> m_Cov_tt;
413+ // if ( in.status()!=0 ) {
414+ // return false;
415+ // }
416+ in >> m_bounding_box;
417+ /* if ( !bounding_box_.deserialize( in) ) {
407418 return false;
408- }
419+ } */
409420 return in.status ()==0 ;
410421}
411422
0 commit comments