/*---------------------------------------------------------------
drawSingleSample
---------------------------------------------------------------*/
void CPoint2DPDFGaussian::drawSingleSample(CPoint2D &outSample) const
{
MRPT_START
// Eigen3 emits an out-of-array warning here, but it seems to be a false warning? (WTF)
CVectorDouble vec;
randomGenerator.drawGaussianMultivariate(vec,cov);
ASSERT_(vec.size()==2);
outSample.x( mean.x() + vec[0] );
outSample.y( mean.y() + vec[1] );
MRPT_END
}
/*---------------------------------------------------------------
getAsVector
---------------------------------------------------------------*/
void CPose2D::getAsVector(CVectorDouble &v) const
{
v.resize(3);
v[0]=m_coords[0];
v[1]=m_coords[1];
v[2]=m_phi;
}
void ffff(const CVectorDouble &x,const CQuaternionDouble &Q, CVectorDouble &OUT)
{
OUT.resize(3);
CQuaternionDouble q(x[0],x[1],x[2],x[3]);
q.normalize();
q.rpy(OUT[2],OUT[1],OUT[0]);
}
// The error function F(x):
void myFunction(
const CVectorDouble& x, const CVectorDouble& y, CVectorDouble& out_f)
{
out_f.resize(1);
// 1-cos(x+1) *cos(x*y+1)
out_f[0] = 1 - cos(x[0] + 1) * cos(x[0] * x[1] + 1);
}
/** Returns a 1x7 vector with [x y z qr qx qy qz] */
void CPose3DQuat::getAsVector(CVectorDouble &v) const
{
v.resize(7);
v[0] = m_coords[0];
v[1] = m_coords[1];
v[2] = m_coords[2];
v[3] = m_quat[0];
v[4] = m_quat[1];
v[5] = m_quat[2];
v[6] = m_quat[3];
}
/*---------------------------------------------------------------
getCurrentState
---------------------------------------------------------------*/
void CRangeBearingKFSLAM::getCurrentState(
CPose3DQuatPDFGaussian& out_robotPose,
std::vector<TPoint3D>& out_landmarksPositions,
std::map<unsigned int, CLandmark::TLandmarkID>& out_landmarkIDs,
CVectorDouble& out_fullState, CMatrixDouble& out_fullCovariance) const
{
MRPT_START
ASSERT_(size_t(m_xkk.size()) >= get_vehicle_size());
// Copy xyz+quat: (explicitly unroll the loop)
out_robotPose.mean.m_coords[0] = m_xkk[0];
out_robotPose.mean.m_coords[1] = m_xkk[1];
out_robotPose.mean.m_coords[2] = m_xkk[2];
out_robotPose.mean.m_quat[0] = m_xkk[3];
out_robotPose.mean.m_quat[1] = m_xkk[4];
out_robotPose.mean.m_quat[2] = m_xkk[5];
out_robotPose.mean.m_quat[3] = m_xkk[6];
// and cov:
m_pkk.extractMatrix(0, 0, out_robotPose.cov);
// Landmarks:
ASSERT_(((m_xkk.size() - get_vehicle_size()) % get_feature_size()) == 0);
size_t i, nLMs = (m_xkk.size() - get_vehicle_size()) / get_feature_size();
out_landmarksPositions.resize(nLMs);
for (i = 0; i < nLMs; i++)
{
out_landmarksPositions[i].x =
m_xkk[get_vehicle_size() + i * get_feature_size() + 0];
out_landmarksPositions[i].y =
m_xkk[get_vehicle_size() + i * get_feature_size() + 1];
out_landmarksPositions[i].z =
m_xkk[get_vehicle_size() + i * get_feature_size() + 2];
} // end for i
// IDs:
out_landmarkIDs = m_IDs.getInverseMap(); // m_IDs_inverse;
// Full state:
out_fullState.resize(m_xkk.size());
for (KFVector::Index i = 0; i < m_xkk.size(); i++)
out_fullState[i] = m_xkk[i];
// Full cov:
out_fullCovariance = m_pkk;
MRPT_END
}
// A feedback functor, which is called on each iteration by the optimizer to let
// us know on the progress:
void my_BundleAdjustmentFeedbackFunctor(
const size_t cur_iter, const double cur_total_sq_error,
const size_t max_iters,
const TSequenceFeatureObservations& input_observations,
const TFramePosesVec& current_frame_estimate,
const TLandmarkLocationsVec& current_landmark_estimate)
{
const double avr_err =
std::sqrt(cur_total_sq_error / input_observations.size());
history_avr_err.push_back(std::log(1e-100 + avr_err));
if ((cur_iter % 10) == 0)
{
cout << "[PROGRESS] Iter: " << cur_iter
<< " avrg err in px: " << avr_err << endl;
cout.flush();
}
}
/*---------------------------------------------------------------
getCurrentState
---------------------------------------------------------------*/
void CRangeBearingKFSLAM2D::getCurrentState(
CPosePDFGaussian &out_robotPose,
std::vector<TPoint2D> &out_landmarksPositions,
std::map<unsigned int,CLandmark::TLandmarkID> &out_landmarkIDs,
CVectorDouble &out_fullState,
CMatrixDouble &out_fullCovariance ) const
{
MRPT_START
ASSERT_(m_xkk.size()>=3);
// Set 6D pose mean:
out_robotPose.mean = CPose2D(m_xkk[0],m_xkk[1],m_xkk[2]);
// and cov:
CMatrixTemplateNumeric<kftype> COV(3,3);
m_pkk.extractMatrix(0,0,COV);
out_robotPose.cov = COV;
// Landmarks:
ASSERT_( ((m_xkk.size() - 3) % 2)==0 );
size_t i,nLMs = (m_xkk.size() - 3) / 2;
out_landmarksPositions.resize(nLMs);
for (i=0;i<nLMs;i++)
{
out_landmarksPositions[i].x = m_xkk[3+i*2+0];
out_landmarksPositions[i].y = m_xkk[3+i*2+1];
} // end for i
// IDs:
out_landmarkIDs = m_IDs.getInverseMap(); //m_IDs_inverse;
// Full state:
out_fullState.resize( m_xkk.size() );
for (KFVector::Index i=0;i<m_xkk.size();i++)
out_fullState[i] = m_xkk[i];
// Full cov:
out_fullCovariance = m_pkk;
MRPT_END
}
/*---------------------------------------------------------------
interpolate
---------------------------------------------------------------*/
CPose3D & CPose3DInterpolator::interpolate( mrpt::system::TTimeStamp t, CPose3D &out_interp, bool &out_valid_interp ) const
{
TTimePosePair p1, p2, p3, p4;
// Invalid?
if (t==INVALID_TIMESTAMP)
{
out_valid_interp = false;
return out_interp;
}
// Out of range?
const_iterator it_ge1 = m_path.lower_bound( t );
// Exact match?
if( it_ge1 != m_path.end() && it_ge1->first == t )
{
out_interp = it_ge1->second;
out_valid_interp = true;
return out_interp;
}
// Are we in the beginning or the end of the path?
if( it_ge1 == m_path.end() || it_ge1 == m_path.begin() )
{
out_valid_interp = false;
out_interp.setFromValues(0,0,0);
return out_interp;
} // end
p3 = *it_ge1; // Third pair
++it_ge1;
if( it_ge1 == m_path.end() )
{
out_valid_interp = false;
out_interp.setFromValues(0,0,0);
return out_interp;
}
p4 = *(it_ge1); // Fourth pair
--it_ge1;
p2 = *(--it_ge1); // Second pair
if( it_ge1 == m_path.begin() )
{
out_valid_interp = false;
out_interp.setFromValues(0,0,0);
return out_interp;
}
p1 = *(--it_ge1); // First pair
// Test if the difference between the desired timestamp and the next timestamp is lower than a certain (configurable) value
if( maxTimeInterpolation > 0 &&
( (p4.first - p3.first)/1e7 > maxTimeInterpolation ||
(p3.first - p2.first)/1e7 > maxTimeInterpolation ||
(p2.first - p1.first)/1e7 > maxTimeInterpolation ))
{
out_valid_interp = false;
out_interp.setFromValues(0,0,0);
return out_interp;
}
// Do interpolation:
// ------------------------------------------
// First Previous point: p1
// Second Previous point: p2
// First Next point: p3
// Second Next point: p4
// Time where to interpolate: t
double td = mrpt::system::timestampTotime_t(t);
CVectorDouble ts;
ts.resize(4);
ts[0] = mrpt::system::timestampTotime_t(p1.first);
ts[1] = mrpt::system::timestampTotime_t(p2.first);
ts[2] = mrpt::system::timestampTotime_t(p3.first);
ts[3] = mrpt::system::timestampTotime_t(p4.first);
CVectorDouble X,Y,Z,yaw,pitch,roll;
X.resize(4); Y.resize(4); Z.resize(4);
X[0] = p1.second.x(); Y[0] = p1.second.y(); Z[0] = p1.second.z();
X[1] = p2.second.x(); Y[1] = p2.second.y(); Z[1] = p2.second.z();
X[2] = p3.second.x(); Y[2] = p3.second.y(); Z[2] = p3.second.z();
X[3] = p4.second.x(); Y[3] = p4.second.y(); Z[3] = p4.second.z();
yaw.resize(4); pitch.resize(4); roll.resize(4);
yaw[0] = p1.second.yaw(); pitch[0] = p1.second.pitch(); roll[0] = p1.second.roll();
yaw[1] = p2.second.yaw(); pitch[1] = p2.second.pitch(); roll[1] = p2.second.roll();
yaw[2] = p3.second.yaw(); pitch[2] = p3.second.pitch(); roll[2] = p3.second.roll();
yaw[3] = p4.second.yaw(); pitch[3] = p4.second.pitch(); roll[3] = p4.second.roll();
unwrap2PiSequence(yaw);
unwrap2PiSequence(pitch);
unwrap2PiSequence(roll);
// Target interpolated values:
double int_x,int_y,int_z,int_yaw,int_pitch,int_roll;
switch (m_method)
{
case imSpline:
{
// ---------------------------------------
// SPLINE INTERPOLATION
// ---------------------------------------
int_x = math::spline(td, ts, X);
int_y = math::spline(td, ts, Y);
int_z = math::spline(td, ts, Z);
int_yaw = math::spline(td, ts, yaw, true ); // Wrap 2pi
int_pitch = math::spline(td, ts, pitch, true );
int_roll = math::spline(td, ts, roll, true );
}
break;
case imLinear2Neig:
{
int_x = math::interpolate2points(td, ts[1],X[1],ts[2],X[2]);
int_y = math::interpolate2points(td, ts[1],Y[1],ts[2],Y[2]);
int_z = math::interpolate2points(td, ts[1],Z[1],ts[2],Z[2]);
int_yaw = math::interpolate2points(td, ts[1],yaw[1],ts[2],yaw[2], true ); // Wrap 2pi
int_pitch = math::interpolate2points(td, ts[1],pitch[1],ts[2],pitch[2], true );
int_roll = math::interpolate2points(td, ts[1],roll[1],ts[2],roll[2], true );
}
break;
case imLinear4Neig:
{
int_x = math::leastSquareLinearFit(td, ts, X);
int_y = math::leastSquareLinearFit(td, ts, Y);
int_z = math::leastSquareLinearFit(td, ts, Z);
int_yaw = math::leastSquareLinearFit(td, ts, yaw, true ); // Wrap 2pi
int_pitch = math::leastSquareLinearFit(td, ts, pitch, true );
int_roll = math::leastSquareLinearFit(td, ts, roll, true );
}
break;
case imSSLLLL:
{
int_x = math::spline(td, ts, X);
int_y = math::spline(td, ts, Y);
int_z = math::leastSquareLinearFit(td, ts, Z);
int_yaw = math::leastSquareLinearFit(td, ts, yaw, true ); // Wrap 2pi
int_pitch = math::leastSquareLinearFit(td, ts, pitch, true );
int_roll = math::leastSquareLinearFit(td, ts, roll, true );
}
break;
case imSSLSLL:
{
int_x = math::spline(td, ts, X);
int_y = math::spline(td, ts, Y);
int_z = math::leastSquareLinearFit(td, ts, Z);
int_yaw = math::spline(td, ts, yaw, true ); // Wrap 2pi
int_pitch = math::leastSquareLinearFit(td, ts, pitch, true );
int_roll = math::leastSquareLinearFit(td, ts, roll, true );
}
break;
case imLinearSlerp:
{
int_x = math::interpolate2points(td, ts[1],X[1],ts[2],X[2]);
int_y = math::interpolate2points(td, ts[1],Y[1],ts[2],Y[2]);
int_z = math::interpolate2points(td, ts[1],Z[1],ts[2],Z[2]);
const CPose3D aux1(0,0,0,yaw[1],pitch[1],roll[1]);
const CPose3D aux2(0,0,0,yaw[2],pitch[2],roll[2]);
CPose3D q_interp;
const double ratio = (td-ts[1])/(ts[2]-ts[1]);
mrpt::math::slerp(aux1,aux2, ratio, q_interp);
q_interp.getYawPitchRoll(int_yaw,int_pitch,int_roll);
}
break;
case imSplineSlerp:
{
int_x = math::spline(td, ts, X);
int_y = math::spline(td, ts, Y);
int_z = math::spline(td, ts, Z);
const CPose3D aux1(0,0,0,yaw[1],pitch[1],roll[1]);
const CPose3D aux2(0,0,0,yaw[2],pitch[2],roll[2]);
CPose3D q_interp;
const double ratio = (td-ts[1])/(ts[2]-ts[1]);
mrpt::math::slerp(aux1,aux2, ratio, q_interp);
q_interp.getYawPitchRoll(int_yaw,int_pitch,int_roll);
}
break;
default: THROW_EXCEPTION("Unknown value for interpolation method!");
}; // end switch
out_interp.setFromValues(int_x, int_y, int_z, int_yaw, int_pitch, int_roll);
out_valid_interp = true;
return out_interp;
} // end interpolate
/*---------------------------------------------------------------
getCovarianceAndMean
---------------------------------------------------------------*/
void CPose3DPDFParticles::getCovarianceAndMean(
CMatrixDouble66& cov, CPose3D& mean) const
{
MRPT_START
getMean(mean); // First! the mean value:
// Now the covariance:
cov.zeros();
CVectorDouble vars;
vars.assign(6, 0.0); // The diagonal of the final covariance matrix
// Elements off the diagonal of the covariance matrix:
double std_xy = 0, std_xz = 0, std_xya = 0, std_xp = 0, std_xr = 0;
double std_yz = 0, std_yya = 0, std_yp = 0, std_yr = 0;
double std_zya = 0, std_zp = 0, std_zr = 0;
double std_yap = 0, std_yar = 0;
double std_pr = 0;
// Mean values in [0, 2pi] range:
double mean_yaw = mean.yaw();
double mean_pitch = mean.pitch();
double mean_roll = mean.roll();
if (mean_yaw < 0) mean_yaw += M_2PI;
if (mean_pitch < 0) mean_pitch += M_2PI;
if (mean_roll < 0) mean_roll += M_2PI;
// Enought information to estimate the covariance?
if (m_particles.size() < 2) return;
// Sum all weight values:
double W = 0;
for (CPose3DPDFParticles::CParticleList::const_iterator it =
m_particles.begin();
it != m_particles.end(); ++it)
W += exp(it->log_w);
ASSERT_(W > 0);
// Compute covariance:
for (CPose3DPDFParticles::CParticleList::const_iterator it =
m_particles.begin();
it != m_particles.end(); ++it)
{
double w = exp(it->log_w) / W;
// Manage 1 PI range:
double err_yaw = wrapToPi(fabs(it->d->yaw() - mean_yaw));
double err_pitch = wrapToPi(fabs(it->d->pitch() - mean_pitch));
double err_roll = wrapToPi(fabs(it->d->roll() - mean_roll));
double err_x = it->d->x() - mean.x();
double err_y = it->d->y() - mean.y();
double err_z = it->d->z() - mean.z();
vars[0] += square(err_x) * w;
vars[1] += square(err_y) * w;
vars[2] += square(err_z) * w;
vars[3] += square(err_yaw) * w;
vars[4] += square(err_pitch) * w;
vars[5] += square(err_roll) * w;
std_xy += err_x * err_y * w;
std_xz += err_x * err_z * w;
std_xya += err_x * err_yaw * w;
std_xp += err_x * err_pitch * w;
std_xr += err_x * err_roll * w;
std_yz += err_y * err_z * w;
std_yya += err_y * err_yaw * w;
std_yp += err_y * err_pitch * w;
std_yr += err_y * err_roll * w;
std_zya += err_z * err_yaw * w;
std_zp += err_z * err_pitch * w;
std_zr += err_z * err_roll * w;
std_yap += err_yaw * err_pitch * w;
std_yar += err_yaw * err_roll * w;
std_pr += err_pitch * err_roll * w;
} // end for it
// Unbiased estimation of variance:
cov(0, 0) = vars[0];
cov(1, 1) = vars[1];
cov(2, 2) = vars[2];
cov(3, 3) = vars[3];
cov(4, 4) = vars[4];
cov(5, 5) = vars[5];
cov(1, 0) = cov(0, 1) = std_xy;
cov(2, 0) = cov(0, 2) = std_xz;
cov(3, 0) = cov(0, 3) = std_xya;
cov(4, 0) = cov(0, 4) = std_xp;
cov(5, 0) = cov(0, 5) = std_xr;
cov(2, 1) = cov(1, 2) = std_yz;
cov(3, 1) = cov(1, 3) = std_yya;
cov(4, 1) = cov(1, 4) = std_yp;
cov(5, 1) = cov(1, 5) = std_yr;
cov(3, 2) = cov(2, 3) = std_zya;
cov(4, 2) = cov(2, 4) = std_zp;
cov(5, 2) = cov(2, 5) = std_zr;
cov(4, 3) = cov(3, 4) = std_yap;
cov(5, 3) = cov(3, 5) = std_yar;
cov(5, 4) = cov(4, 5) = std_pr;
MRPT_END
}
TEST(Matrices,fromMatlabStringFormat)
{
const char* mat1 = "[1 2 3;-3 -6 -5]";
const double vals1[] = {1,2,3,-3,-6,-5};
const char* mat2 = " [ -8.2 9.232 ; -2e+2 +6 ; 1.000 7 ] "; // With tabs and spaces...
const double vals2[] = {-8.2, 9.232, -2e+2, +6, 1.000 ,7};
const char* mat3 = "[9]";
const char* mat4 = "[1 2 3 4 5 6 7 9 10 ; 1 2 3 4 5 6 7 8 9 10 11]"; // An invalid matrix
const char* mat5 = "[ ]"; // Empty
const char* mat6 = "[ -405.200 42.232 ; 1219.600 -98.696 ]"; // M1 * M2
const char* mat13 = "[9 8 7]";
const char* mat31 = "[9; 8; 7]";
CMatrixDouble M1,M2,M3, M4, M5, M6;
if (! M1.fromMatlabStringFormat(mat1) ||
(CMatrixFixedNumeric<double,2,3>(vals1)-M1).array().abs().sum() > 1e-4 )
GTEST_FAIL() << mat1;
{
CMatrixFixedNumeric<double,2,3> M1b;
if (! M1b.fromMatlabStringFormat(mat1) ||
(CMatrixFixedNumeric<double,2,3>(vals1)-M1b).array().abs().sum() > 1e-4 )
GTEST_FAIL() << mat1;
}
if (! M2.fromMatlabStringFormat(mat2) ||
M2.cols()!=2 || M2.rows()!=3 ||
(CMatrixFixedNumeric<double,3,2>(vals2)-M2).array().abs().sum() > 1e-4 )
GTEST_FAIL() << mat2;
{
CMatrixFixedNumeric<double,3,2> M2b;
if (! M2b.fromMatlabStringFormat(mat2) ||
(CMatrixFixedNumeric<double,3,2>(vals2)-M2b).array().abs().sum() > 1e-4 )
GTEST_FAIL() << mat2;
}
if (! M3.fromMatlabStringFormat(mat3) )
GTEST_FAIL() << mat3;
{
CVectorDouble m;
if (! m.fromMatlabStringFormat(mat3) || m.size()!=1 ) GTEST_FAIL() << "CVectorDouble:" << mat3;
}
{
CArrayDouble<1> m;
if (! m.fromMatlabStringFormat(mat3) ) GTEST_FAIL() << "CArrayDouble<1>:" << mat3;
}
{
CVectorDouble m;
if (! m.fromMatlabStringFormat(mat31) || m.size()!=3 ) GTEST_FAIL() << "CVectorDouble:" << mat31;
}
{
CArrayDouble<3> m;
if (! m.fromMatlabStringFormat(mat31) ) GTEST_FAIL() << "CArrayDouble<3>:" << mat31;
}
{
Eigen::Matrix<double,1,3> m;
if (! m.fromMatlabStringFormat(mat13) ) GTEST_FAIL() << "Matrix<double,1,3>:" << mat13;
}
{
Eigen::Matrix<double,1,Eigen::Dynamic> m;
if (! m.fromMatlabStringFormat(mat13) || m.size()!=3 ) GTEST_FAIL() << "Matrix<double,1,Dynamic>:" << mat13;
}
// This one MUST BE detected as WRONG:
if ( M4.fromMatlabStringFormat(mat4, NULL /*dont dump errors to cerr*/) )
GTEST_FAIL() << mat4;
if (! M5.fromMatlabStringFormat(mat5) || size(M5,1)!=0 || size(M5,2)!=0 )
GTEST_FAIL() << mat5;
if (! M6.fromMatlabStringFormat(mat6) )
GTEST_FAIL() << mat6;
// Check correct values loaded:
CMatrixDouble RES = M1*M2;
EXPECT_NEAR(0,(M6 - M1*M2).array().square().sum(), 1e-3);
}
// ------------------------------------------------------
// TestCamera3DFaceDetection
// ------------------------------------------------------
void TestCamera3DFaceDetection(CCameraSensor::Ptr cam)
{
CDisplayWindow win("Live video");
CDisplayWindow win2("FaceDetected");
cout << "Close the window to exit." << endl;
mrpt::gui::CDisplayWindow3D win3D("3D camera view", 800, 600);
mrpt::gui::CDisplayWindow3D win3D2;
win3D.setCameraAzimuthDeg(140);
win3D.setCameraElevationDeg(20);
win3D.setCameraZoom(6.0);
win3D.setCameraPointingToPoint(2.5, 0, 0);
mrpt::opengl::COpenGLScene::Ptr& scene = win3D.get3DSceneAndLock();
mrpt::opengl::COpenGLScene::Ptr scene2;
mrpt::opengl::CPointCloudColoured::Ptr gl_points =
mrpt::make_aligned_shared<mrpt::opengl::CPointCloudColoured>();
gl_points->setPointSize(4.5);
mrpt::opengl::CPointCloudColoured::Ptr gl_points2 =
mrpt::make_aligned_shared<mrpt::opengl::CPointCloudColoured>();
gl_points2->setPointSize(4.5);
// Create the Opengl object for the point cloud:
scene->insert(gl_points);
scene->insert(mrpt::make_aligned_shared<mrpt::opengl::CGridPlaneXY>());
scene->insert(mrpt::opengl::stock_objects::CornerXYZ());
win3D.unlockAccess3DScene();
if (showEachDetectedFace)
{
win3D2.setWindowTitle("3D Face detected");
win3D2.resize(400, 300);
win3D2.setCameraAzimuthDeg(140);
win3D2.setCameraElevationDeg(20);
win3D2.setCameraZoom(6.0);
win3D2.setCameraPointingToPoint(2.5, 0, 0);
scene2 = win3D2.get3DSceneAndLock();
scene2->insert(gl_points2);
scene2->insert(mrpt::make_aligned_shared<mrpt::opengl::CGridPlaneXY>());
win3D2.unlockAccess3DScene();
}
double counter = 0;
mrpt::utils::CTicTac tictac;
CVectorDouble fps;
while (win.isOpen())
{
if (!counter) tictac.Tic();
CObservation3DRangeScan::Ptr o;
try
{
o = std::dynamic_pointer_cast<CObservation3DRangeScan>(
cam->getNextFrame());
}
catch (CExceptionEOF&)
{
break;
}
ASSERT_(o);
vector_detectable_object detected;
// CObservation3DRangeScan::Ptr o =
// std::dynamic_pointer_cast<CObservation3DRangeScan>(obs);
faceDetector.detectObjects(o, detected);
// static int x = 0;
if (detected.size() > 0)
{
for (unsigned int i = 0; i < detected.size(); i++)
{
ASSERT_(IS_CLASS(detected[i], CDetectable3D))
CDetectable3D::Ptr obj =
std::dynamic_pointer_cast<CDetectable3D>(detected[i]);
if (showEachDetectedFace)
{
CObservation3DRangeScan face;
o->getZoneAsObs(
face, obj->m_y, obj->m_y + obj->m_height, obj->m_x,
obj->m_x + obj->m_width);
win2.showImage(face.intensityImage);
if (o->hasPoints3D)
{
win3D2.get3DSceneAndLock();
CColouredPointsMap pntsMap;
if (!o->hasConfidenceImage)
{
pntsMap.colorScheme.scheme =
CColouredPointsMap::cmFromIntensityImage;
pntsMap.loadFromRangeScan(face);
}
else
{
vector<float> xs, ys, zs;
unsigned int i = 0;
for (unsigned int j = 0;
j < face.confidenceImage.getHeight(); j++)
for (unsigned int k = 0;
k < face.confidenceImage.getWidth();
k++, i++)
{
unsigned char c =
*(face.confidenceImage.get_unsafe(
k, j, 0));
if (c > faceDetector.m_options
.confidenceThreshold)
{
xs.push_back(face.points3D_x[i]);
ys.push_back(face.points3D_y[i]);
zs.push_back(face.points3D_z[i]);
}
}
pntsMap.setAllPoints(xs, ys, zs);
}
gl_points2->loadFromPointsMap(&pntsMap);
win3D2.unlockAccess3DScene();
win3D2.repaint();
}
}
o->intensityImage.rectangle(
obj->m_x, obj->m_y, obj->m_x + obj->m_width,
obj->m_y + obj->m_height, TColor(255, 0, 0));
// x++;
// if (( showEachDetectedFace ) && ( x > 430 ) )
// system::pause();
}
}
win.showImage(o->intensityImage);
/*if (( showEachDetectedFace ) && ( detected.size() ))
system::pause();*/
win3D.get3DSceneAndLock();
CColouredPointsMap pntsMap;
pntsMap.colorScheme.scheme = CColouredPointsMap::cmFromIntensityImage;
pntsMap.loadFromRangeScan(*(o.get()));
gl_points->loadFromPointsMap(&pntsMap);
win3D.unlockAccess3DScene();
win3D.repaint();
if (++counter == 10)
{
double t = tictac.Tac();
cout << "Frame Rate: " << counter / t << " fps" << endl;
fps.push_back(counter / t);
counter = 0;
}
std::this_thread::sleep_for(2ms);
}
cout << "Fps mean: " << fps.sumAll() / fps.size() << endl;
faceDetector.experimental_showMeasurements();
cout << "Closing..." << endl;
}