void ransac3Dplane_distance( const CMatrixDouble &allData, const vector< CMatrixDouble > & testModels, const double distanceThreshold, unsigned int & out_bestModelIndex, vector_size_t & out_inlierIndices ) { ASSERT_( testModels.size()==1 ) out_bestModelIndex = 0; const CMatrixDouble &M = testModels[0]; ASSERT_( size(M,1)==1 && size(M,2)==4 ) TPlane plane; plane.coefs[0] = M(0,0); plane.coefs[1] = M(0,1); plane.coefs[2] = M(0,2); plane.coefs[3] = M(0,3); const size_t N = size(allData,2); out_inlierIndices.clear(); out_inlierIndices.reserve(100); for (size_t i=0;i<N;i++) { const double d = plane.distance( TPoint3D( allData.get_unsafe(0,i),allData.get_unsafe(1,i),allData.get_unsafe(2,i) ) ); if (d<distanceThreshold) out_inlierIndices.push_back(i); } }
void TTwist3D::fromString(const std::string &s) { CMatrixDouble m; if (!m.fromMatlabStringFormat(s)) THROW_EXCEPTION("Malformed expression in ::fromString"); ASSERTMSG_(mrpt::math::size(m,1)==1 && mrpt::math::size(m,2)==6, "Wrong size of vector in ::fromString"); for (int i=0;i<3;i++) (*this)[i] = m.get_unsafe(0,i); for (int i=0;i<3;i++) (*this)[3+i] = DEG2RAD(m.get_unsafe(0,3+i)); }
void TTwist2D::fromString(const std::string &s) { CMatrixDouble m; if (!m.fromMatlabStringFormat(s)) THROW_EXCEPTION("Malformed expression in ::fromString"); ASSERTMSG_(mrpt::math::size(m,1)==1 && mrpt::math::size(m,2)==3, "Wrong size of vector in ::fromString"); vx = m.get_unsafe(0,0); vy = m.get_unsafe(0,1); omega = DEG2RAD(m.get_unsafe(0,2)); }
void TPose2D::fromString(const std::string& s) { CMatrixDouble m; if (!m.fromMatlabStringFormat(s)) THROW_EXCEPTION("Malformed expression in ::fromString"); ASSERTMSG_( m.rows() == 1 && m.cols() == 3, "Wrong size of vector in ::fromString"); x = m.get_unsafe(0, 0); y = m.get_unsafe(0, 1); phi = DEG2RAD(m.get_unsafe(0, 2)); }
void TPose3D::fromString(const std::string &s) { CMatrixDouble m; if (!m.fromMatlabStringFormat(s)) THROW_EXCEPTION("Malformed expression in ::fromString"); ASSERTMSG_(mrpt::math::size(m,1)==1 && mrpt::math::size(m,2)==6, "Wrong size of vector in ::fromString"); x=m.get_unsafe(0,0); y=m.get_unsafe(0,1); z=m.get_unsafe(0,2); yaw=DEG2RAD(m.get_unsafe(0,3)); pitch=DEG2RAD(m.get_unsafe(0,4)); roll=DEG2RAD(m.get_unsafe(0,5)); }
void TPose3DQuat::fromString(const std::string &s) { CMatrixDouble m; if (!m.fromMatlabStringFormat(s)) THROW_EXCEPTION("Malformed expression in ::fromString"); ASSERTMSG_(mrpt::math::size(m,1)==1 && mrpt::math::size(m,2)==7, "Wrong size of vector in ::fromString"); for (size_t i=0;i<m.getColCount();i++) (*this)[i] = m.get_unsafe(0,i); }
void TPose3DQuat::fromString(const std::string& s) { CMatrixDouble m; if (!m.fromMatlabStringFormat(s)) THROW_EXCEPTION("Malformed expression in ::fromString"); ASSERTMSG_( m.rows() == 1 && m.cols() == 7, "Wrong size of vector in ::fromString"); for (int i = 0; i < m.cols(); i++) (*this)[i] = m.get_unsafe(0, i); }
/*--------------------------------------------------------------- render ---------------------------------------------------------------*/ void CEllipsoid::render_dl() const { #if MRPT_HAS_OPENGL_GLUT MRPT_START const size_t dim = m_cov.getColCount(); if(m_eigVal(0,0) != 0.0 && m_eigVal(1,1) != 0.0 && (dim==2 || m_eigVal(2,2) != 0.0) && m_quantiles!=0.0) { glEnable(GL_BLEND); checkOpenGLError(); glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); checkOpenGLError(); glLineWidth(m_lineWidth); checkOpenGLError(); if (dim==2) { glDisable(GL_LIGHTING); // Disable lights when drawing lines // --------------------- // 2D ellipse // --------------------- /* Equivalent MATLAB code: * * q=1; * [vec val]=eig(C); * M=(q*val*vec)'; * R=M*[x;y]; * xx=R(1,:);yy=R(2,:); * plot(xx,yy), axis equal; */ double ang; unsigned int i; // Compute the new vectors for the ellipsoid: CMatrixDouble M; M.noalias() = double(m_quantiles) * m_eigVal * m_eigVec.adjoint(); glBegin( GL_LINE_LOOP ); // Compute the points of the 2D ellipse: for (i=0,ang=0;i<m_2D_segments;i++,ang+= (M_2PI/m_2D_segments)) { double ccos = cos(ang); double ssin = sin(ang); const float x = ccos * M.get_unsafe(0,0) + ssin * M.get_unsafe(1,0); const float y = ccos * M.get_unsafe(0,1) + ssin * M.get_unsafe(1,1); glVertex2f( x,y ); } // end for points on ellipse glEnd(); // 2D: Save bounding box: const double max_radius = m_quantiles * std::max( m_eigVal(0,0), m_eigVal(1,1) ); m_bb_min = mrpt::math::TPoint3D(-max_radius,-max_radius, 0); m_bb_max = mrpt::math::TPoint3D(max_radius,max_radius, 0); // Convert to coordinates of my parent: m_pose.composePoint(m_bb_min, m_bb_min); m_pose.composePoint(m_bb_max, m_bb_max); glEnable(GL_LIGHTING); } else { // --------------------- // 3D ellipsoid // --------------------- GLfloat mat[16]; // A homogeneous transformation matrix, in this order: // // 0 4 8 12 // 1 5 9 13 // 2 6 10 14 // 3 7 11 15 // mat[3] = mat[7] = mat[11] = 0; mat[15] = 1; mat[12] = mat[13] = mat[14] = 0; mat[0] = m_eigVec(0,0); mat[1] = m_eigVec(1,0); mat[2] = m_eigVec(2,0); // New X-axis mat[4] = m_eigVec(0,1); mat[5] = m_eigVec(1,1); mat[6] = m_eigVec(2,1); // New X-axis mat[8] = m_eigVec(0,2); mat[9] = m_eigVec(1,2); mat[10] = m_eigVec(2,2); // New X-axis GLUquadricObj *obj = gluNewQuadric(); checkOpenGLError(); if (!m_drawSolid3D) glDisable(GL_LIGHTING); // Disable lights when drawing lines gluQuadricDrawStyle( obj, m_drawSolid3D ? GLU_FILL : GLU_LINE); glPushMatrix(); glMultMatrixf( mat ); glScalef(m_eigVal(0,0)*m_quantiles,m_eigVal(1,1)*m_quantiles,m_eigVal(2,2)*m_quantiles); gluSphere( obj, 1,m_3D_segments,m_3D_segments); checkOpenGLError(); glPopMatrix(); gluDeleteQuadric(obj); checkOpenGLError(); // 3D: Save bounding box: const double max_radius = m_quantiles * std::max( m_eigVal(0,0), std::max(m_eigVal(1,1), m_eigVal(2,2) ) ); m_bb_min = mrpt::math::TPoint3D(-max_radius,-max_radius, 0); m_bb_max = mrpt::math::TPoint3D(max_radius,max_radius, 0); // Convert to coordinates of my parent: m_pose.composePoint(m_bb_min, m_bb_min); m_pose.composePoint(m_bb_max, m_bb_max); } glDisable(GL_BLEND); glEnable(GL_LIGHTING); } MRPT_END_WITH_CLEAN_UP( \ cout << "Covariance matrix leading to error is:" << endl << m_cov << endl; \ );
/*--------------------------------------------------------------- render ---------------------------------------------------------------*/ void CEllipsoid::render_dl() const { #if MRPT_HAS_OPENGL_GLUT MRPT_START const size_t dim = m_cov.getColCount(); if(m_eigVal(0,0) != 0.0 && m_eigVal(1,1) != 0.0 && (dim==2 || m_eigVal(2,2) != 0.0) && m_quantiles!=0.0) { glEnable(GL_BLEND); checkOpenGLError(); glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); checkOpenGLError(); glLineWidth(m_lineWidth); checkOpenGLError(); if (dim==2) { // --------------------- // 2D ellipse // --------------------- float x1=0,y1=0,x2=0,y2=0; double ang; unsigned int i; // Compute the new vectors for the ellipsoid: CMatrixDouble M; M.noalias() = double(m_quantiles) * m_eigVal * m_eigVec.adjoint(); glBegin( GL_LINES ); // Compute the points of the 2D ellipse: for (i=0,ang=0; i<m_2D_segments; i++,ang+= (M_2PI/(m_2D_segments-1))) { double ccos = cos(ang); double ssin = sin(ang); x2 = ccos * M.get_unsafe(0,0) + ssin * M.get_unsafe(1,0); y2 = ccos * M.get_unsafe(0,1) + ssin * M.get_unsafe(1,1); if (i>0) { glVertex2f( x1,y1 ); glVertex2f( x2,y2 ); } x1 = x2; y1 = y2; } // end for points on ellipse glEnd(); } else { // --------------------- // 3D ellipsoid // --------------------- GLfloat mat[16]; // A homogeneous transformation matrix, in this order: // // 0 4 8 12 // 1 5 9 13 // 2 6 10 14 // 3 7 11 15 // mat[3] = mat[7] = mat[11] = 0; mat[15] = 1; mat[12] = mat[13] = mat[14] = 0; mat[0] = m_eigVec(0,0); mat[1] = m_eigVec(1,0); mat[2] = m_eigVec(2,0); // New X-axis mat[4] = m_eigVec(0,1); mat[5] = m_eigVec(1,1); mat[6] = m_eigVec(2,1); // New X-axis mat[8] = m_eigVec(0,2); mat[9] = m_eigVec(1,2); mat[10] = m_eigVec(2,2); // New X-axis glEnable(GL_LIGHTING); glEnable(GL_LIGHT0); glEnable(GL_COLOR_MATERIAL); glShadeModel(GL_SMOOTH); GLUquadricObj *obj = gluNewQuadric(); checkOpenGLError(); gluQuadricDrawStyle( obj, m_drawSolid3D ? GLU_FILL : GLU_LINE); glPushMatrix(); glMultMatrixf( mat ); glScalef(m_eigVal(0,0)*m_quantiles,m_eigVal(1,1)*m_quantiles,m_eigVal(2,2)*m_quantiles); gluSphere( obj, 1,m_3D_segments,m_3D_segments); checkOpenGLError(); glPopMatrix(); gluDeleteQuadric(obj); checkOpenGLError(); glDisable(GL_LIGHTING); glDisable(GL_LIGHT0); } glLineWidth(1.0f); glDisable(GL_BLEND); } MRPT_END_WITH_CLEAN_UP( \ cout << "Covariance matrix leading to error is:" << endl << m_cov << endl; \ );