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; \
);
