TEST(SparseMatrix, CholeskyDecomp)
{
CSparseMatrix SM(10, 10);
const CMatrixDouble COV1 =
mrpt::random::getRandomGenerator().drawDefinitePositiveMatrix(6, 0.2);
const CMatrixDouble COV2 =
mrpt::random::getRandomGenerator().drawDefinitePositiveMatrix(4, 0.2);
SM.insert_submatrix(0, 0, COV1);
SM.insert_submatrix(6, 6, COV2);
SM.compressFromTriplet();
CSparseMatrix::CholeskyDecomp Chol(SM);
const CMatrixDouble L = Chol.get_L(); // lower triangle
// Compare with the dense matrix implementation:
CMatrixDouble D;
SM.get_dense(D);
CMatrixDouble Ud; // Upper triangle
D.chol(Ud);
const double err = ((Ud.transpose()) - L).array().abs().mean();
EXPECT_TRUE(err < 1e-8);
}
TEST(Matrices,multiply_skew3_A)
{
{
const double dat_A[] = {
1,2,
3,4,
5,6 };
const CMatrixDouble A(3,2,dat_A);
const std::vector<double> v = make_vector<3>(1.0,2.0,3.0);
const CMatrixDouble S = CMatrixDouble( mrpt::math::skew_symmetric3(v) );
CMatrixDouble R;
R.multiply_skew3_A(v,A);
EXPECT_TRUE(R == S*A );
}
{
const double dat_A[] = {
1,2,
3,4,
5,6 };
const double dat_v[] = { 1,2,3 };
const CMatrixFixedNumeric<double,3,2> A(dat_A);
const CArrayDouble<3> v(dat_v);
const CMatrixFixedNumeric<double,3,3> S = mrpt::math::skew_symmetric3(v);
CMatrixFixedNumeric<double,3,2> R;
R.multiply_skew3_A(v,A);
EXPECT_TRUE(R == S*A );
}
}
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);
}
}
TEST(Matrices,multiply_A_skew3)
{
{
const double dat_A[] = {
1,2,3,
4,5,6 };
const CMatrixDouble A(2,3,dat_A);
const std::vector<double> v = make_vector<3>(1.0,2.0,3.0);
const CMatrixDouble S = CMatrixDouble( mrpt::math::skew_symmetric3(v) );
CMatrixDouble R;
R.multiply_A_skew3(A,v);
EXPECT_EQ(R, (A*S).eval() );
}
{
const double dat_A[] = {
1,2,3,
4,5,6 };
const double dat_v[] = { 1,2,3 };
const CMatrixFixedNumeric<double,2,3> A(dat_A);
const CArrayDouble<3> v(dat_v);
const CMatrixFixedNumeric<double,3,3> S = mrpt::math::skew_symmetric3(v);
CMatrixFixedNumeric<double,2,3> R;
R.multiply_A_skew3(A,v);
EXPECT_TRUE(R== A*S );
}
}
TEST(Matrices,laplacian)
{
// The laplacian matrix of W is L = D - W. (D:diagonals with degrees of nodes)
const double W_vals[6*6]={
0, 1, 0, 0, 1, 0,
1, 0, 1, 0, 1, 0,
0, 1, 0, 1, 0, 0,
0, 0, 1, 0, 1, 1,
1, 1, 0, 1, 0, 0,
0, 0, 0, 1, 0, 0 };
const CMatrixDouble W(6,6, W_vals);
CMatrixDouble L;
W.laplacian(L);
const double real_laplacian_vals[6*6]={
2, -1, 0, 0, -1, 0,
-1, 3, -1, 0, -1, 0,
0, -1, 2, -1, 0, 0,
0, 0, -1, 3, -1, -1,
-1, -1, 0, -1, 3, 0,
0, 0, 0, -1, 0, 1 };
const CMatrixDouble GT_L(6,6, real_laplacian_vals);
EXPECT_NEAR( (GT_L-L).array().abs().sum(), 0, 1e-4);
}
void test_jacobs_P1DP2inv(
double x1,double y1,double z1, double yaw1,double pitch1,double roll1,
double xd,double yd,double zd, double yawd,double pitchd,double rolld,
double x2,double y2,double z2, double yaw2,double pitch2,double roll2)
{
const CPose3D P1_(x1,y1,z1,yaw1,pitch1,roll1);
const CPose3D Pd_(xd,yd,zd,yawd,pitchd,rolld);
const CPose3D P2_(x2,y2,z2,yaw2,pitch2,roll2);
const POSE_TYPE P1(P1_);
const POSE_TYPE Pd(Pd_);
const POSE_TYPE P2(P2_);
const CPose3D P1DP2inv_( CMatrixDouble44(P1.getHomogeneousMatrixVal() * Pd.getHomogeneousMatrixVal() * P2.getInverseHomogeneousMatrix()));
const POSE_TYPE P1DP2inv(P1DP2inv_); // Convert to 2D if needed
static const int DIMS = SE_TYPE::VECTOR_SIZE;
// Theoretical results:
CMatrixFixedNumeric<double,DIMS,DIMS> J1,J2;
SE_TYPE::jacobian_dP1DP2inv_depsilon(P1DP2inv, &J1, &J2);
// Numerical approx:
CMatrixFixedNumeric<double,DIMS,DIMS> num_J1,num_J2;
{
CArrayDouble<2*DIMS> x_mean;
for (int i=0;i<DIMS+DIMS;i++) x_mean[i]=0;
TParams params;
params.P1 = P1;
params.D = Pd;
params.P2 = P2;
CArrayDouble<DIMS+DIMS> x_incrs;
x_incrs.assign(1e-4);
CMatrixDouble numJacobs;
mrpt::math::jacobians::jacob_numeric_estimate(x_mean,func_numeric,x_incrs, params, numJacobs );
numJacobs.extractMatrix(0,0, num_J1);
numJacobs.extractMatrix(0,DIMS, num_J2);
}
const double max_eror = 1e-3;
EXPECT_NEAR(0, (num_J1-J1).array().abs().sum(), max_eror )
<< "p1: " << P1 << endl
<< "d: " << Pd << endl
<< "p2: " << P2 << endl
<< "Numeric J1:\n" << num_J1 << endl
<< "Implemented J1:\n" << J1 << endl
<< "Error:\n" << J1-num_J1 << endl;
EXPECT_NEAR(0, (num_J2-J2).array().abs().sum(), max_eror )
<< "p1: " << P1 << endl
<< "d: " << Pd << endl
<< "p2: " << P2 << endl
<< "Numeric J2:\n" << num_J2 << endl
<< "Implemented J2:\n" << J2 << endl
<< "Error:\n" << J2-num_J2 << endl;
}
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 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));
}
TEST(Matrices, A_times_B_dyn)
{
// Dyn. size, double.
CMatrixDouble A(3,2, dat_A);
CMatrixDouble B(2,2, dat_B);
CMatrixDouble C = A*B;
CMatrixDouble C_ok(3,2, dat_Cok);
CMatrixDouble err = C - C_ok;
EXPECT_NEAR(0,fabs(err.sum()), 1e-5) <<
"A: " << A <<
"B: " << B <<
"A*B: " << C << endl;
}
/** save as a dense matrix to a text file \return False on any error.
*/
bool CSparseMatrix::saveToTextFile_dense(const std::string& filName)
{
CMatrixDouble dense;
this->get_dense(dense);
try
{
dense.saveToTextFile(filName);
return true;
}
catch (...)
{
return false;
}
}
void testCompositionJacobian(
double x,double y, double z, double yaw, double pitch, double roll,
double x2,double y2, double z2, double yaw2, double pitch2, double roll2)
{
const CPose3D q1(x,y,z,yaw,pitch,roll);
const CPose3D q2(x2,y2,z2,yaw2,pitch2,roll2);
// Theoretical Jacobians:
CMatrixDouble66 df_dx(UNINITIALIZED_MATRIX), df_du(UNINITIALIZED_MATRIX);
CPose3DPDF::jacobiansPoseComposition(
q1, // x
q2, // u
df_dx,
df_du );
// Numerical approximation:
CMatrixDouble66 num_df_dx(UNINITIALIZED_MATRIX), num_df_du(UNINITIALIZED_MATRIX);
{
CArrayDouble<2*6> x_mean;
for (int i=0;i<6;i++) x_mean[i]=q1[i];
for (int i=0;i<6;i++) x_mean[6+i]=q2[i];
double DUMMY=0;
CArrayDouble<2*6> x_incrs;
x_incrs.assign(1e-7);
CMatrixDouble numJacobs;
mrpt::math::jacobians::jacob_numeric_estimate(x_mean,func_compose,x_incrs, DUMMY, numJacobs );
numJacobs.extractMatrix(0,0, num_df_dx);
numJacobs.extractMatrix(0,6, num_df_du);
}
// Compare:
EXPECT_NEAR(0, (df_dx-num_df_dx).Abs().sumAll(), 3e-3 )
<< "q1: " << q1 << endl
<< "q2: " << q2 << endl
<< "Numeric approximation of df_dx: " << endl << num_df_dx << endl
<< "Implemented method: " << endl << df_dx << endl
<< "Error: " << endl << df_dx-num_df_dx << endl;
EXPECT_NEAR(0, (df_du-num_df_du).Abs().sumAll(), 3e-3 )
<< "q1: " << q1 << endl
<< "q2: " << q2 << endl
<< "Numeric approximation of df_du: " << endl << num_df_du << endl
<< "Implemented method: " << endl << df_du << endl
<< "Error: " << endl << df_du-num_df_du << endl;
}
void test_composePointJacob(double x1,double y1,double z1, double yaw1,double pitch1,double roll1,
double x,double y,double z, bool use_aprox = false)
{
const CPose3D p1(x1,y1,z1,yaw1,pitch1,roll1);
const CPoint3D p(x,y,z);
CMatrixFixedNumeric<double,3,3> df_dpoint;
CMatrixFixedNumeric<double,3,6> df_dpose;
TPoint3D pp;
p1.composePoint(x,y,z, pp.x,pp.y,pp.z, &df_dpoint, &df_dpose, NULL, use_aprox );
// Numerical approx:
CMatrixFixedNumeric<double,3,3> num_df_dpoint(UNINITIALIZED_MATRIX);
CMatrixFixedNumeric<double,3,6> num_df_dpose(UNINITIALIZED_MATRIX);
{
CArrayDouble<6+3> x_mean;
for (int i=0;i<6;i++) x_mean[i]=p1[i];
x_mean[6+0]=x;
x_mean[6+1]=y;
x_mean[6+2]=z;
double DUMMY=0;
CArrayDouble<6+3> x_incrs;
x_incrs.assign(1e-7);
CMatrixDouble numJacobs;
mrpt::math::jacobians::jacob_numeric_estimate(x_mean,func_compose_point,x_incrs, DUMMY, numJacobs );
numJacobs.extractMatrix(0,0, num_df_dpose);
numJacobs.extractMatrix(0,6, num_df_dpoint);
}
const double max_eror = use_aprox ? 0.1 : 3e-3;
EXPECT_NEAR(0, (df_dpoint-num_df_dpoint).array().abs().sum(), max_eror )
<< "p1: " << p1 << endl
<< "p: " << p << endl
<< "Numeric approximation of df_dpoint: " << endl << num_df_dpoint << endl
<< "Implemented method: " << endl << df_dpoint << endl
<< "Error: " << endl << df_dpoint-num_df_dpoint << endl;
EXPECT_NEAR(0, (df_dpose-num_df_dpose).array().abs().sum(), max_eror )
<< "p1: " << p1 << endl
<< "p: " << p << endl
<< "Numeric approximation of df_dpose: " << endl << num_df_dpose << endl
<< "Implemented method: " << endl << df_dpose << endl
<< "Error: " << endl << df_dpose-num_df_dpose << endl;
}
TEST(Matrices,transpose)
{
const double dat_A[] = {
1,2,3,
4,5,6 };
const double dat_At[] = {
1,4,
2,5,
3,6};
const CMatrixDouble A(2,3,dat_A);
const CMatrixDouble At(3,2,dat_At);
EXPECT_EQ(A.t(), At);
EXPECT_EQ(~A, At);
EXPECT_EQ(A.t().t(), A);
}
/** Static method to convert a "cs" structure into a dense representation of the
* sparse matrix.
*/
void CSparseMatrix::cs2dense(const cs& SM, CMatrixDouble& d_M)
{
d_M.zeros(SM.m, SM.n);
if (SM.nz >= 0) // isTriplet ??
{ // It's in triplet form.
for (int idx = 0; idx < SM.nz; ++idx)
d_M(SM.i[idx], SM.p[idx]) +=
SM.x[idx]; // += since the convention is that duplicate (i,j)
// entries add to each other.
}
else
{ // Column compressed format:
ASSERT_(SM.x); // JL: Could it be nullptr and be OK???
for (int j = 0; j < SM.n; j++)
{
const int p0 = SM.p[j];
const int p1 = SM.p[j + 1];
for (int p = p0; p < p1; p++)
d_M(SM.i[p], j) += SM.x[p]; // += since the convention is that
// duplicate (i,j) entries add to
// each other.
}
}
}
void TPose3D::fromString(const std::string& s)
{
CMatrixDouble m;
if (!m.fromMatlabStringFormat(s))
THROW_EXCEPTION("Malformed expression in ::fromString");
ASSERTMSG_(
m.rows() == 1 && m.cols() == 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_(
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);
}
void TPoint3D::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);
z = m.get_unsafe(0, 2);
}
void Run_KF_SLAM(CConfigFile& cfgFile, const std::string& rawlogFileName)
{
// The EKF-SLAM class:
// Traits for this KF implementation (2D or 3D)
using traits_t = kfslam_traits<IMPL>;
using ekfslam_t = typename traits_t::ekfslam_t;
ekfslam_t mapping;
// The rawlog file:
// ----------------------------------------
const unsigned int rawlog_offset =
cfgFile.read_int("MappingApplication", "rawlog_offset", 0);
const unsigned int SAVE_LOG_FREQUENCY =
cfgFile.read_int("MappingApplication", "SAVE_LOG_FREQUENCY", 1);
const bool SAVE_DA_LOG =
cfgFile.read_bool("MappingApplication", "SAVE_DA_LOG", true);
const bool SAVE_3D_SCENES =
cfgFile.read_bool("MappingApplication", "SAVE_3D_SCENES", true);
const bool SAVE_MAP_REPRESENTATIONS = cfgFile.read_bool(
"MappingApplication", "SAVE_MAP_REPRESENTATIONS", true);
bool SHOW_3D_LIVE =
cfgFile.read_bool("MappingApplication", "SHOW_3D_LIVE", false);
const bool CAMERA_3DSCENE_FOLLOWS_ROBOT = cfgFile.read_bool(
"MappingApplication", "CAMERA_3DSCENE_FOLLOWS_ROBOT", false);
#if !MRPT_HAS_WXWIDGETS
SHOW_3D_LIVE = false;
#endif
string OUT_DIR = cfgFile.read_string(
"MappingApplication", "logOutput_dir", "OUT_KF-SLAM");
string ground_truth_file =
cfgFile.read_string("MappingApplication", "ground_truth_file", "");
string ground_truth_file_robot = cfgFile.read_string(
"MappingApplication", "ground_truth_file_robot", "");
string ground_truth_data_association = cfgFile.read_string(
"MappingApplication", "ground_truth_data_association", "");
cout << "RAWLOG FILE:" << endl << rawlogFileName << endl;
ASSERT_FILE_EXISTS_(rawlogFileName);
CFileGZInputStream rawlogFile(rawlogFileName);
cout << "---------------------------------------------------" << endl
<< endl;
deleteFilesInDirectory(OUT_DIR);
createDirectory(OUT_DIR);
// Load the config options for mapping:
// ----------------------------------------
mapping.loadOptions(cfgFile);
mapping.KF_options.dumpToConsole();
mapping.options.dumpToConsole();
// debug:
// mapping.KF_options.use_analytic_observation_jacobian = true;
// mapping.KF_options.use_analytic_transition_jacobian = true;
// mapping.KF_options.debug_verify_analytic_jacobians = true;
// Is there ground truth of the robot poses??
CMatrixDouble GT_PATH(0, 0);
if (ground_truth_file_robot.size() && fileExists(ground_truth_file_robot))
{
GT_PATH.loadFromTextFile(ground_truth_file_robot);
ASSERT_(GT_PATH.rows() > 0 && GT_PATH.cols() == 6);
}
// Is there a ground truth file of the data association?
std::map<double, std::vector<int>>
GT_DA; // Map: timestamp -> vector(index in observation -> real index)
mrpt::containers::bimap<int, int> DA2GTDA_indices; // Landmark indices
// bimapping: SLAM DA <--->
// GROUND TRUTH DA
if (!ground_truth_data_association.empty() &&
fileExists(ground_truth_data_association))
{
CMatrixDouble mGT_DA;
mGT_DA.loadFromTextFile(ground_truth_data_association);
ASSERT_ABOVEEQ_(mGT_DA.cols(), 3);
// Convert the loaded matrix into a std::map in GT_DA:
for (int i = 0; i < mGT_DA.rows(); i++)
{
std::vector<int>& v = GT_DA[mGT_DA(i, 0)];
if (v.size() <= mGT_DA(i, 1)) v.resize(mGT_DA(i, 1) + 1);
v[mGT_DA(i, 1)] = mGT_DA(i, 2);
}
cout << "Loaded " << GT_DA.size()
<< " entries from DA ground truth file\n";
}
// Create output file for DA perf:
std::ofstream out_da_performance_log;
{
const std::string f = std::string(
OUT_DIR + std::string("/data_association_performance.log"));
out_da_performance_log.open(f.c_str());
ASSERTMSG_(
out_da_performance_log.is_open(),
std::string("Error writing to: ") + f);
// Header:
out_da_performance_log
<< "% TIMESTAMP INDEX_IN_OBS TruePos "
"FalsePos TrueNeg FalseNeg NoGroundTruthSoIDontKnow \n"
<< "%--------------------------------------------------------------"
"--------------------------------------------------\n";
}
if (SHOW_3D_LIVE)
{
win3d = mrpt::make_aligned_shared<mrpt::gui::CDisplayWindow3D>(
"KF-SLAM live view", 800, 500);
win3d->addTextMessage(
0.01, 0.96, "Red: Estimated path", TColorf(0.8f, 0.8f, 0.8f), 100,
MRPT_GLUT_BITMAP_HELVETICA_10);
win3d->addTextMessage(
0.01, 0.93, "Black: Ground truth path", TColorf(0.8f, 0.8f, 0.8f),
101, MRPT_GLUT_BITMAP_HELVETICA_10);
}
// Create DA-log output file:
std::ofstream out_da_log;
if (SAVE_DA_LOG)
{
const std::string f =
std::string(OUT_DIR + std::string("/data_association.log"));
out_da_log.open(f.c_str());
ASSERTMSG_(out_da_log.is_open(), std::string("Error writing to: ") + f);
// Header:
out_da_log << "% TIMESTAMP INDEX_IN_OBS ID "
" RANGE(m) YAW(rad) PITCH(rad) \n"
<< "%-------------------------------------------------------"
"-------------------------------------\n";
}
// The main loop:
// ---------------------------------------
CActionCollection::Ptr action;
CSensoryFrame::Ptr observations;
size_t rawlogEntry = 0, step = 0;
vector<TPose3D> meanPath; // The estimated path
typename traits_t::posepdf_t robotPose;
const bool is_pose_3d = robotPose.state_length != 3;
std::vector<typename IMPL::landmark_point_t> LMs;
std::map<unsigned int, CLandmark::TLandmarkID> LM_IDs;
CMatrixDouble fullCov;
CVectorDouble fullState;
CTicTac kftictac;
auto rawlogArch = mrpt::serialization::archiveFrom(rawlogFile);
for (;;)
{
if (os::kbhit())
{
char pushKey = os::getch();
if (27 == pushKey) break;
}
// Load action/observation pair from the rawlog:
// --------------------------------------------------
if (!CRawlog::readActionObservationPair(
rawlogArch, action, observations, rawlogEntry))
break; // file EOF
if (rawlogEntry >= rawlog_offset)
{
// Process the action and observations:
// --------------------------------------------
kftictac.Tic();
mapping.processActionObservation(action, observations);
const double tim_kf_iter = kftictac.Tac();
// Get current state:
// -------------------------------
mapping.getCurrentState(robotPose, LMs, LM_IDs, fullState, fullCov);
cout << "Mean pose: " << endl << robotPose.mean << endl;
cout << "# of landmarks in the map: " << LMs.size() << endl;
// Get the mean robot pose as 3D:
const CPose3D robotPoseMean3D = CPose3D(robotPose.mean);
// Build the path:
meanPath.push_back(robotPoseMean3D.asTPose());
// Save mean pose:
if (!(step % SAVE_LOG_FREQUENCY))
{
const auto p = robotPose.mean.asVectorVal();
p.saveToTextFile(
OUT_DIR +
format("/robot_pose_%05u.txt", (unsigned int)step));
}
// Save full cov:
if (!(step % SAVE_LOG_FREQUENCY))
{
fullCov.saveToTextFile(
OUT_DIR + format("/full_cov_%05u.txt", (unsigned int)step));
}
// Generate Data Association log?
if (SAVE_DA_LOG)
{
const typename ekfslam_t::TDataAssocInfo& da =
mapping.getLastDataAssociation();
const CObservationBearingRange::Ptr obs =
observations
->getObservationByClass<CObservationBearingRange>();
if (obs)
{
const CObservationBearingRange* obsRB = obs.get();
const double tim =
mrpt::system::timestampToDouble(obsRB->timestamp);
for (size_t i = 0; i < obsRB->sensedData.size(); i++)
{
auto it = da.results.associations.find(i);
int assoc_ID_in_SLAM;
if (it != da.results.associations.end())
assoc_ID_in_SLAM = it->second;
else
{
// It should be a newly created LM:
auto itNewLM = da.newly_inserted_landmarks.find(i);
if (itNewLM != da.newly_inserted_landmarks.end())
assoc_ID_in_SLAM = itNewLM->second;
else
assoc_ID_in_SLAM = -1;
}
out_da_log << format(
"%35.22f %8i %10i %10f %12f %12f\n", tim, (int)i,
assoc_ID_in_SLAM,
(double)obsRB->sensedData[i].range,
(double)obsRB->sensedData[i].yaw,
(double)obsRB->sensedData[i].pitch);
}
}
}
// Save report on DA performance:
{
const typename ekfslam_t::TDataAssocInfo& da =
mapping.getLastDataAssociation();
const CObservationBearingRange::Ptr obs =
observations
->getObservationByClass<CObservationBearingRange>();
if (obs)
{
const CObservationBearingRange* obsRB = obs.get();
const double tim =
mrpt::system::timestampToDouble(obsRB->timestamp);
auto itDA = GT_DA.find(tim);
for (size_t i = 0; i < obsRB->sensedData.size(); i++)
{
bool is_FP = false, is_TP = false, is_FN = false,
is_TN = false;
if (itDA != GT_DA.end())
{
const std::vector<int>& vDA = itDA->second;
ASSERT_BELOW_(i, vDA.size());
const int GT_ASSOC = vDA[i];
auto it = da.results.associations.find(i);
if (it != da.results.associations.end())
{
// This observation was assigned the already
// existing LM in the map: "it->second"
// TruePos -> If that LM index corresponds to
// that in the GT (with index mapping):
// mrpt::containers::bimap<int,int>
// DA2GTDA_indices;
// // Landmark indices bimapping: SLAM DA <--->
// GROUND TRUTH DA
if (DA2GTDA_indices.hasKey(it->second))
{
const int slam_asigned_LM_idx =
DA2GTDA_indices.direct(it->second);
if (slam_asigned_LM_idx == GT_ASSOC)
is_TP = true;
else
is_FP = true;
}
else
{
// Is this case possible? Assigned to an
// index not ever seen for the first time
// with a GT....
// Just in case:
is_FP = true;
}
}
else
{
// No pairing, but should be a newly created LM:
auto itNewLM =
da.newly_inserted_landmarks.find(i);
if (itNewLM !=
da.newly_inserted_landmarks.end())
{
const int new_LM_in_SLAM = itNewLM->second;
// Was this really a NEW LM not observed
// before?
if (DA2GTDA_indices.hasValue(GT_ASSOC))
{
// GT says this LM was already observed,
// so it shouldn't appear here as new:
is_FN = true;
}
else
{
// Really observed for the first time:
is_TN = true;
DA2GTDA_indices.insert(
new_LM_in_SLAM, GT_ASSOC);
}
}
else
{
// Not associated neither inserted:
// Shouldn't really never arrive here.
}
}
}
// "% TIMESTAMP INDEX_IN_OBS
// TruePos FalsePos TrueNeg FalseNeg
// NoGroundTruthSoIDontKnow \n"
out_da_performance_log << format(
"%35.22f %13i %8i %8i %8i %8i %8i\n", tim, (int)i,
(int)(is_TP ? 1 : 0), (int)(is_FP ? 1 : 0),
(int)(is_TN ? 1 : 0), (int)(is_FN ? 1 : 0),
(int)(!is_FP && !is_TP && !is_FN && !is_TN ? 1 : 0));
}
}
}
// Save map to file representations?
if (SAVE_MAP_REPRESENTATIONS && !(step % SAVE_LOG_FREQUENCY))
{
mapping.saveMapAndPath2DRepresentationAsMATLABFile(
OUT_DIR + format("/slam_state_%05u.m", (unsigned int)step));
}
// Save 3D view of the filter state:
if (win3d || (SAVE_3D_SCENES && !(step % SAVE_LOG_FREQUENCY)))
{
COpenGLScene::Ptr scene3D =
mrpt::make_aligned_shared<COpenGLScene>();
{
opengl::CGridPlaneXY::Ptr grid =
mrpt::make_aligned_shared<opengl::CGridPlaneXY>(
-1000, 1000, -1000, 1000, 0, 5);
grid->setColor(0.4, 0.4, 0.4);
scene3D->insert(grid);
}
// Robot path:
{
opengl::CSetOfLines::Ptr linesPath =
mrpt::make_aligned_shared<opengl::CSetOfLines>();
linesPath->setColor(1, 0, 0);
TPose3D init_pose;
if (!meanPath.empty())
init_pose = CPose3D(meanPath[0]).asTPose();
int path_decim = 0;
for (auto& it : meanPath)
{
linesPath->appendLine(init_pose, it);
init_pose = it;
if (++path_decim > 10)
{
path_decim = 0;
mrpt::opengl::CSetOfObjects::Ptr xyz =
mrpt::opengl::stock_objects::CornerXYZSimple(
0.3f, 2.0f);
xyz->setPose(CPose3D(it));
scene3D->insert(xyz);
}
}
scene3D->insert(linesPath);
// finally a big corner for the latest robot pose:
{
mrpt::opengl::CSetOfObjects::Ptr xyz =
mrpt::opengl::stock_objects::CornerXYZSimple(
1.0, 2.5);
xyz->setPose(robotPoseMean3D);
scene3D->insert(xyz);
}
// The camera pointing to the current robot pose:
if (CAMERA_3DSCENE_FOLLOWS_ROBOT)
{
win3d->setCameraPointingToPoint(
robotPoseMean3D.x(), robotPoseMean3D.y(),
robotPoseMean3D.z());
}
}
// Do we have a ground truth?
if (GT_PATH.cols() == 6 || GT_PATH.cols() == 3)
{
opengl::CSetOfLines::Ptr GT_path =
mrpt::make_aligned_shared<opengl::CSetOfLines>();
GT_path->setColor(0, 0, 0);
size_t N =
std::min((int)GT_PATH.rows(), (int)meanPath.size());
if (GT_PATH.cols() == 6)
{
double gtx0 = 0, gty0 = 0, gtz0 = 0;
for (size_t i = 0; i < N; i++)
{
const CPose3D p(
GT_PATH(i, 0), GT_PATH(i, 1), GT_PATH(i, 2),
GT_PATH(i, 3), GT_PATH(i, 4), GT_PATH(i, 5));
GT_path->appendLine(
gtx0, gty0, gtz0, p.x(), p.y(), p.z());
gtx0 = p.x();
gty0 = p.y();
gtz0 = p.z();
}
}
else if (GT_PATH.cols() == 3)
{
double gtx0 = 0, gty0 = 0;
for (size_t i = 0; i < N; i++)
{
const CPose2D p(
GT_PATH(i, 0), GT_PATH(i, 1), GT_PATH(i, 2));
GT_path->appendLine(gtx0, gty0, 0, p.x(), p.y(), 0);
gtx0 = p.x();
gty0 = p.y();
}
}
scene3D->insert(GT_path);
}
// Draw latest data association:
{
const typename ekfslam_t::TDataAssocInfo& da =
mapping.getLastDataAssociation();
mrpt::opengl::CSetOfLines::Ptr lins =
mrpt::make_aligned_shared<mrpt::opengl::CSetOfLines>();
lins->setLineWidth(1.2f);
lins->setColor(1, 1, 1);
for (auto it = da.results.associations.begin();
it != da.results.associations.end(); ++it)
{
const prediction_index_t idxPred = it->second;
// This index must match the internal list of features
// in the map:
typename ekfslam_t::KFArray_FEAT featMean;
mapping.getLandmarkMean(idxPred, featMean);
TPoint3D featMean3D;
traits_t::landmark_to_3d(featMean, featMean3D);
// Line: robot -> landmark:
lins->appendLine(
robotPoseMean3D.x(), robotPoseMean3D.y(),
robotPoseMean3D.z(), featMean3D.x, featMean3D.y,
featMean3D.z);
}
scene3D->insert(lins);
}
// The current state of KF-SLAM:
{
opengl::CSetOfObjects::Ptr objs =
mrpt::make_aligned_shared<opengl::CSetOfObjects>();
mapping.getAs3DObject(objs);
scene3D->insert(objs);
}
if (win3d)
{
mrpt::opengl::COpenGLScene::Ptr& scn =
win3d->get3DSceneAndLock();
scn = scene3D;
// Update text messages:
win3d->addTextMessage(
0.02, 0.02,
format(
"Step %u - Landmarks in the map: %u",
(unsigned int)step, (unsigned int)LMs.size()),
TColorf(1, 1, 1), 0, MRPT_GLUT_BITMAP_HELVETICA_12);
win3d->addTextMessage(
0.02, 0.06,
format(
is_pose_3d
? "Estimated pose: (x y z qr qx qy qz) = %s"
: "Estimated pose: (x y yaw) = %s",
robotPose.mean.asString().c_str()),
TColorf(1, 1, 1), 1, MRPT_GLUT_BITMAP_HELVETICA_12);
static vector<double> estHz_vals;
const double curHz = 1.0 / std::max(1e-9, tim_kf_iter);
estHz_vals.push_back(curHz);
if (estHz_vals.size() > 50)
estHz_vals.erase(estHz_vals.begin());
const double meanHz = mrpt::math::mean(estHz_vals);
win3d->addTextMessage(
0.02, 0.10,
format(
"Iteration time: %7ss",
mrpt::system::unitsFormat(tim_kf_iter).c_str()),
TColorf(1, 1, 1), 2, MRPT_GLUT_BITMAP_HELVETICA_12);
win3d->addTextMessage(
0.02, 0.14,
format(
"Execution rate: %7sHz",
mrpt::system::unitsFormat(meanHz).c_str()),
TColorf(1, 1, 1), 3, MRPT_GLUT_BITMAP_HELVETICA_12);
win3d->unlockAccess3DScene();
win3d->repaint();
}
if (SAVE_3D_SCENES && !(step % SAVE_LOG_FREQUENCY))
{
// Save to file:
CFileGZOutputStream f(
OUT_DIR +
format("/kf_state_%05u.3Dscene", (unsigned int)step));
mrpt::serialization::archiveFrom(f) << *scene3D;
}
}
// Free rawlog items memory:
// --------------------------------------------
action.reset();
observations.reset();
} // (rawlogEntry>=rawlog_offset)
cout << format(
"\nStep %u - Rawlog entries processed: %i\n", (unsigned int)step,
(unsigned int)rawlogEntry);
step++;
}; // end "while(1)"
// Partitioning experiment: Only for 6D SLAM:
traits_t::doPartitioningExperiment(mapping, fullCov, OUT_DIR);
// Is there ground truth of landmarks positions??
if (ground_truth_file.size() && fileExists(ground_truth_file))
{
CMatrixFloat GT(0, 0);
try
{
GT.loadFromTextFile(ground_truth_file);
}
catch (const std::exception& e)
{
cerr << "Ignoring the following error loading ground truth file: "
<< mrpt::exception_to_str(e) << endl;
}
if (GT.rows() > 0 && !LMs.empty())
{
// Each row has:
// [0] [1] [2] [6]
// x y z ID
CVectorDouble ERRS(0);
for (size_t i = 0; i < LMs.size(); i++)
{
// Find the entry in the GT for this mapped LM:
bool found = false;
for (int r = 0; r < GT.rows(); r++)
{
if (LM_IDs[i] == GT(r, 6))
{
const CPoint3D gtPt(GT(r, 0), GT(r, 1), GT(r, 2));
ERRS.push_back(gtPt.distanceTo(
CPoint3D(TPoint3D(LMs[i])))); // All these
// conversions
// are to make it
// work with
// either
// CPoint3D &
// TPoint2D
found = true;
break;
}
}
if (!found)
{
cerr << "Ground truth entry not found for landmark ID:"
<< LM_IDs[i] << endl;
}
}
printf("ERRORS VS. GROUND TRUTH:\n");
printf("Mean Error: %f meters\n", math::mean(ERRS));
printf("Minimum error: %f meters\n", math::minimum(ERRS));
printf("Maximum error: %f meters\n", math::maximum(ERRS));
}
} // end if GT
cout << "********* KF-SLAM finished! **********" << endl;
if (win3d)
{
cout << "\n Close the 3D window to quit the application.\n";
win3d->waitForKey();
}
}
/*---------------------------------------------------------------
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; \
);
TEST(Matrices,loadFromTextFile)
{
{
const std::string s1 =
"1 2 3\n"
"4 5 6";
std::stringstream s(s1);
CMatrixDouble M;
bool retval = false;
try { M.loadFromTextFile(s); retval=true; } catch(std::exception &e) { std::cerr << e.what() << std::endl; }
EXPECT_TRUE(retval) << "string:\n" << s1 << endl;
EXPECT_EQ(M.rows(),2);
EXPECT_EQ(M.cols(),3);
}
{
const std::string s1 =
"1 \t 2\n"
" 4 \t\t 1 ";
std::stringstream s(s1);
CMatrixDouble M;
bool retval = false;
try { M.loadFromTextFile(s); retval=true; } catch(std::exception &e) { std::cerr << e.what() << std::endl; }
EXPECT_TRUE(retval) << "string:\n" << s1 << endl;
EXPECT_EQ(M.rows(),2);
EXPECT_EQ(M.cols(),2);
}
{
const std::string s1 =
"1 2";
std::stringstream s(s1);
CMatrixDouble M;
bool retval = false;
try { M.loadFromTextFile(s); retval=true; } catch(std::exception &e) { std::cerr << e.what() << std::endl; }
EXPECT_TRUE(retval) << "string:\n" << s1 << endl;
EXPECT_EQ(M.rows(),1);
EXPECT_EQ(M.cols(),2);
}
{
const std::string s1 =
"1 2 3\n"
"4 5 6\n";
std::stringstream s(s1);
CMatrixFixedNumeric<double,2,3> M;
bool retval = false;
try { M.loadFromTextFile(s); retval=true; } catch(std::exception &e) { std::cerr << e.what() << std::endl; }
EXPECT_TRUE(retval) << "string:\n" << s1 << endl;
EXPECT_EQ(M.rows(),2);
EXPECT_EQ(M.cols(),3);
}
{
const std::string s1 =
"1 2 3\n"
"4 5\n";
std::stringstream s(s1);
CMatrixFixedNumeric<double,2,3> M;
bool retval = false;
try { M.loadFromTextFile(s); retval=true; } catch(std::exception &) { }
EXPECT_FALSE(retval) << "string:\n" << s1 << endl;
}
{
const std::string s1 =
"1 2 3\n"
"4 5\n";
std::stringstream s(s1);
CMatrixDouble M;
bool retval = false;
try { M.loadFromTextFile(s); retval=true; } catch(std::exception &) { }
EXPECT_FALSE(retval) << "string:\n" << s1 << endl;
}
{
const std::string s1 =
" \n";
std::stringstream s(s1);
CMatrixFixedNumeric<double,2,3> M;
bool retval = false;
try { M.loadFromTextFile(s); retval=true; } catch(std::exception &) { }
EXPECT_FALSE(retval) << "string:\n" << s1 << endl;
}
{
const std::string s1 =
"1 2 3\n"
"1 2 3\n"
"1 2 3";
std::stringstream s(s1);
CMatrixFixedNumeric<double,2,3> M;
bool retval = false;
try { M.loadFromTextFile(s); retval=true; } catch(std::exception &) { }
EXPECT_FALSE(retval) << "string:\n" << s1 << endl;
}
}
void hmt_slam_guiFrame::updateLocalMapView()
{
WX_START_TRY
m_glLocalArea->m_openGLScene->clear();
// Get the hypothesis ID:
THypothesisID hypID = (THypothesisID )atoi( cbHypos->GetStringSelection().mb_str() );
if ( m_hmtslam->m_LMHs.find(hypID)==m_hmtslam->m_LMHs.end() )
{
wxMessageBox(_U( format("No LMH has hypothesis ID %i!", (int)hypID).c_str() ), _("Error with topological hypotesis"));
return;
}
// Get the selected area or LMH in the tree view:
wxArrayTreeItemIds lstSelect;
size_t nSel = treeView->GetSelections(lstSelect);
if (!nSel) return;
CItemData *data1 = static_cast<CItemData*>( treeView->GetItemData( lstSelect.Item(0) ) );
if (!data1) return;
if (!data1->m_ptr) return;
CSerializablePtr obj = data1->m_ptr;
if (obj->GetRuntimeClass()==CLASS_ID(CHMHMapNode))
{
// The 3D view:
opengl::CSetOfObjectsPtr objs = opengl::CSetOfObjects::Create();
// -------------------------------------------
// Draw a grid on the ground:
// -------------------------------------------
{
opengl::CGridPlaneXYPtr obj = opengl::CGridPlaneXY::Create(-100,100,-100,100,0,5);
obj->setColor(0.4,0.4,0.4);
objs->insert(obj); // it will free the memory
}
// Two passes: 1st draw the map on the ground, then the rest.
for (int nRound=0;nRound<2;nRound++)
{
CHMHMapNodePtr firstArea;
CPose3DPDFGaussian refPoseThisArea;
for (size_t nSelItem = 0; nSelItem<nSel;nSelItem++)
{
CItemData *data1 = static_cast<CItemData*>( treeView->GetItemData( lstSelect.Item(nSelItem) ) );
if (!data1) continue;
if (!data1->m_ptr) continue;
CHMHMapNodePtr area= CHMHMapNodePtr(data1->m_ptr);
if (!area) continue;
// Is this the first rendered area??
if ( !firstArea )
{
firstArea = area;
}
else
{
// Compute the translation btw. ref. and current area:
CPose3DPDFParticles pdf;
m_hmtslam->m_map.computeCoordinatesTransformationBetweenNodes(
firstArea->getID(),
area->getID(),
pdf,
hypID,
200 );
/*0.15f,
DEG2RAD(5.0f) );*/
refPoseThisArea.copyFrom( pdf );
cout << "Pose " << firstArea->getID() << " - " << area->getID() << refPoseThisArea << endl;
}
CMultiMetricMapPtr obj_mmap = area->m_annotations.getAs<CMultiMetricMap>( NODE_ANNOTATION_METRIC_MAPS, hypID, false );
CRobotPosesGraphPtr obj_robposes = area->m_annotations.getAs<CRobotPosesGraph>( NODE_ANNOTATION_POSES_GRAPH, hypID, false );
TPoseID refPoseID;
area->m_annotations.getElemental( NODE_ANNOTATION_REF_POSEID, refPoseID, hypID, true);
// ---------------------------------------------------------
// The metric map:
// ---------------------------------------------------------
if (nRound==0)
{
opengl::CSetOfObjectsPtr objMap= opengl::CSetOfObjects::Create();
obj_mmap->getAs3DObject(objMap);
objMap->setPose( refPoseThisArea.mean );
objs->insert(objMap);
}
if (nRound==1)
{
// ---------------------------------------------------------
// Bounding boxes for grid maps:
// ---------------------------------------------------------
if (obj_mmap->m_gridMaps.size())
{
float x_min = obj_mmap->m_gridMaps[0]->getXMin();
float x_max = obj_mmap->m_gridMaps[0]->getXMax();
float y_min = obj_mmap->m_gridMaps[0]->getYMin();
float y_max = obj_mmap->m_gridMaps[0]->getYMax();
opengl::CSetOfLinesPtr objBB = opengl::CSetOfLines::Create();
objBB->setColor(0,0,1);
objBB->setLineWidth( 4.0f );
objBB->appendLine(x_min,y_min,0, x_max,y_min,0);
objBB->appendLine(x_max,y_min,0, x_max,y_max,0);
objBB->appendLine(x_max,y_max,0, x_min,y_max,0);
objBB->appendLine(x_min,y_max,0, x_min,y_min,0);
objBB->setPose( refPoseThisArea.mean );
objs->insert(objBB);
}
// -----------------------------------------------
// Draw a 3D coordinates corner for the ref. pose
// -----------------------------------------------
{
CPose3D p;
(*obj_robposes)[refPoseID].pdf.getMean(p);
opengl::CSetOfObjectsPtr corner = stock_objects::CornerXYZ();
corner->setPose( refPoseThisArea.mean + p);
corner->setName(format("AREA %i",(int)area->getID() ));
corner->enableShowName();
objs->insert( corner );
}
// -----------------------------------------------
// Draw ellipsoid with uncertainty of pose transformation
// -----------------------------------------------
if (refPoseThisArea.cov(0,0)!=0 || refPoseThisArea.cov(1,1)!=0)
{
opengl::CEllipsoidPtr ellip = opengl::CEllipsoid::Create();
ellip->setPose( refPoseThisArea.mean );
ellip->enableDrawSolid3D(false);
CMatrixDouble C = CMatrixDouble(refPoseThisArea.cov);
if (C(2,2)<1e6)
C.setSize(2,2);
else C.setSize(3,3);
ellip->setCovMatrix(C);
ellip->setQuantiles(3);
ellip->setLocation( ellip->getPoseX(), ellip->getPoseY(), ellip->getPoseZ()+0.5);
ellip->setColor(1,0,0);
ellip->setLineWidth(3);
objs->insert( ellip );
}
// ---------------------------------------------------------
// Draw each of the robot poses as 2D/3D ellipsoids
// ---------------------------------------------------------
for (CRobotPosesGraph::iterator it=obj_robposes->begin();it!=obj_robposes->end();++it)
{
}
}
} // end for nSelItem
} // two pass
// Add to the scene:
m_glLocalArea->m_openGLScene->insert(objs);
}
else
if (obj->GetRuntimeClass()==CLASS_ID( CLocalMetricHypothesis ))
{
//CLocalMetricHypothesis *lmh = static_cast<CLocalMetricHypothesis*>( obj );
}
m_glLocalArea->Refresh();
WX_END_TRY
}
void test_schur_dense_vs_sparse(
const TGraphInitRandom *init_random,
const TGraphInitManual *init_manual,
const double lambda = 1e3 )
{
// A linear system object holds the sparse Jacobians for a set of observations.
my_rba_t::rba_problem_state_t::TLinearSystem lin_system;
size_t nUnknowns_k2k=0, nUnknowns_k2f=0;
if (init_random)
{
randomGenerator.randomize(init_random->random_seed);
nUnknowns_k2k=init_random->nUnknowns_k2k;
nUnknowns_k2f=init_random->nUnknowns_k2f;
}
else
{
nUnknowns_k2k=init_manual->nUnknowns_k2k;
nUnknowns_k2f=init_manual->nUnknowns_k2f;
}
// Fill example Jacobians for the test:
// * 2 keyframes -> 1 k2k edge (edges=unknowns)
// * 6 features with unknown positions.
// * 6*2 observations: each feature seen once from each keyframe
// Note: 6*2*2 = 24 is the minimum >= 1*6+3*6=24 unknowns so
// Hessians are invertible
// -----------------------------------------------------------------
// Create observations:
// Don't populate the symbolic structure, just the numeric part.
static char valid_true = 1; // Just to initialize valid bit pointers to this one.
{
lin_system.dh_dAp.setColCount(nUnknowns_k2k);
lin_system.dh_df.setColCount(nUnknowns_k2f);
size_t idx_obs = 0;
for (size_t nKF=0;nKF<=nUnknowns_k2k;nKF++)
{
my_rba_t::jacobian_traits_t::TSparseBlocksJacobians_dh_dAp::col_t * dh_dAp_i = (nKF==0) ? NULL : (&lin_system.dh_dAp.getCol(nKF-1));
for (size_t nLM=0;nLM<nUnknowns_k2f;nLM++)
{
// Do we have an observation of feature "nLM" from keyframe "nKF"??
bool add_this;
if (init_random)
add_this = (randomGenerator.drawUniform(0.0,1.0)<=init_random->PROB_OBS);
else add_this = init_manual->visible[nKF*nUnknowns_k2f + nLM];
if (add_this)
{
// Create observation: KF[nKF] -> LM[nLM]
my_rba_t::jacobian_traits_t::TSparseBlocksJacobians_dh_df::col_t & dh_df_j = lin_system.dh_df.getCol(nLM);
// Random is ok for this test:
if (dh_dAp_i)
{
randomGenerator.drawGaussian1DMatrix( (*dh_dAp_i)[idx_obs].num );
(*dh_dAp_i)[idx_obs].sym.is_valid = &valid_true;
}
randomGenerator.drawGaussian1DMatrix( dh_df_j[idx_obs].num.setRandom() );
dh_df_j[idx_obs].sym.is_valid = &valid_true;
idx_obs++;
}
}
}
// Debug point:
//if ( idx_obs != (1+nUnknowns_k2k)*nUnknowns_k2f ) cout << "#k2k: " << nUnknowns_k2k << " #k2f: " << nUnknowns_k2f << " #obs: " << idx_obs << " out of max.=" << (1+nUnknowns_k2k)*nUnknowns_k2f << endl;
}
// A default gradient:
Eigen::VectorXd minus_grad; // The negative of the gradient.
const size_t idx_start_f = 6*nUnknowns_k2k;
const size_t nLMs_scalars = 3*nUnknowns_k2f;
const size_t nUnknowns_scalars = idx_start_f + nLMs_scalars;
minus_grad.resize(nUnknowns_scalars);
minus_grad.setOnes();
#if 0
lin_system.dh_dAp.saveToTextFileAsDense("test_dh_dAp.txt");
lin_system.dh_df.saveToTextFileAsDense("test_dh_df.txt");
#endif
// ------------------------------------------------------------
// 1st) Evaluate Schur naively with dense matrices
// ------------------------------------------------------------
CMatrixDouble dense_dh_dAp, dense_dh_df;
lin_system.dh_dAp.getAsDense(dense_dh_dAp);
lin_system.dh_df.getAsDense (dense_dh_df);
const CMatrixDouble dense_HAp = dense_dh_dAp.transpose() * dense_dh_dAp;
const CMatrixDouble dense_Hf = dense_dh_df.transpose() * dense_dh_df;
const CMatrixDouble dense_HApf = dense_dh_dAp.transpose() * dense_dh_df;
CMatrixDouble dense_Hf_plus_lambda = dense_Hf;
for (size_t i=0;i<nLMs_scalars;i++)
dense_Hf_plus_lambda(i,i)+=lambda;
// Schur: naive dense computation:
const CMatrixDouble dense_HAp_schur = dense_HAp - dense_HApf*dense_Hf_plus_lambda.inv()*dense_HApf.transpose();
const Eigen::VectorXd dense_minus_grad_schur = minus_grad.head(idx_start_f) - dense_HApf*(dense_Hf_plus_lambda.inv())*minus_grad.tail(nLMs_scalars);
#if 0
dense_HAp.saveToTextFile("dense_HAp.txt");
dense_Hf.saveToTextFile("dense_Hf.txt");
dense_HApf.saveToTextFile("dense_HApf.txt");
#endif
// ------------------------------------------------------------
// 2nd) Evaluate using sparse Schur implementation
// ------------------------------------------------------------
// Build a list with ALL the unknowns:
vector<my_rba_t::jacobian_traits_t::TSparseBlocksJacobians_dh_dAp::col_t*> dh_dAp;
vector<my_rba_t::jacobian_traits_t::TSparseBlocksJacobians_dh_df::col_t*> dh_df;
for (size_t i=0;i<lin_system.dh_dAp.getColCount();i++)
dh_dAp.push_back( & lin_system.dh_dAp.getCol(i) );
for (size_t i=0;i<lin_system.dh_df.getColCount();i++)
dh_df.push_back( & lin_system.dh_df.getCol(i) );
#if 0
{
CMatrixDouble Jbin;
lin_system.dh_dAp.getBinaryBlocksRepresentation(Jbin);
Jbin.saveToTextFile("test_sparse_jacobs_blocks.txt", mrpt::math::MATRIX_FORMAT_INT );
lin_system.dh_dAp.saveToTextFileAsDense("test_sparse_jacobs.txt");
}
#endif
my_rba_t::hessian_traits_t::TSparseBlocksHessian_Ap HAp;
my_rba_t::hessian_traits_t::TSparseBlocksHessian_f Hf;
my_rba_t::hessian_traits_t::TSparseBlocksHessian_Apf HApf; // This one stores in row-compressed form (i.e. it's stored transposed!!!)
// This resizes and fills in the structs HAp,Hf,HApf from Jacobians:
my_rba_t::sparse_hessian_build_symbolic(
HAp,Hf,HApf,
dh_dAp,dh_df
);
my_rba_t rba;
rba.sparse_hessian_update_numeric(HAp);
rba.sparse_hessian_update_numeric(Hf);
rba.sparse_hessian_update_numeric(HApf);
#if 0
HAp.saveToTextFileAsDense("HAp.txt", true, true );
Hf.saveToTextFileAsDense("Hf.txt", true, true);
HApf.saveToTextFileAsDense("HApf.txt",false, false);
minus_grad.saveToTextFile("minus_grad_Ap.txt");
{ ofstream f("lambda.txt"); f << lambda << endl; }
#endif
// The constructor builds the symbolic Schur.
SchurComplement<
my_rba_t::hessian_traits_t::TSparseBlocksHessian_Ap,
my_rba_t::hessian_traits_t::TSparseBlocksHessian_f,
my_rba_t::hessian_traits_t::TSparseBlocksHessian_Apf
>
schur_compl(
HAp,Hf,HApf, // The different symbolic/numeric Hessian
&minus_grad[0], // minus gradient of the Ap part
&minus_grad[idx_start_f] // minus gradient of the features part
);
schur_compl.numeric_build_reduced_system(lambda);
//cout << "getNumFeaturesFullRank: " << schur_compl.getNumFeaturesFullRank() << endl;
// ------------------------------------------------------------
// 3rd) Both must match!
// ------------------------------------------------------------
// * HAp: Holds the updated Schur complement Hessian.
// * minus_grad: Holds the updated Schur complement -gradient.
CMatrixDouble final_HAp_schur;
HAp.getAsDense(final_HAp_schur, true /* force symmetry, i.e. replicate the half matrix not stored in sparse form */ );
#if 0
final_HAp_schur.saveToTextFile("schur_HAp.txt");
#endif
EXPECT_NEAR( (dense_HAp_schur-final_HAp_schur).array().abs().maxCoeff()/(dense_HAp_schur.array().abs().maxCoeff()),0, 1e-10)
<< "nUnknowns_k2k=" << nUnknowns_k2k << endl
<< "nUnknowns_k2f=" << nUnknowns_k2f << endl
<< "final_HAp_schur:\n" << final_HAp_schur << endl
<< "dense_HAp_schur:\n" << dense_HAp_schur << endl
;
const Eigen::VectorXd final_minus_grad_Ap = minus_grad.head(idx_start_f);
const double Ap_minus_grad_Ap_max_error = (dense_minus_grad_schur-final_minus_grad_Ap).array().abs().maxCoeff();
EXPECT_NEAR( Ap_minus_grad_Ap_max_error/dense_minus_grad_schur.array().abs().maxCoeff(),0, 1e-10)
<< "nUnknowns_k2k=" << nUnknowns_k2k << endl
<< "nUnknowns_k2f=" << nUnknowns_k2f << endl
//<< "dense_minus_grad_schur:\n" << dense_minus_grad_schur
//<< "final_minus_grad_Ap:\n" << final_minus_grad_Ap << endl
;
#if 0
HAp.saveToTextFileAsDense("test_HAp_sparse_schur.txt");
dense_HAp_schur.saveToTextFile("test_HAp_sparse_schur2.txt");
#endif
}
/*---------------------------------------------------------------
getAs3DScene
Returns a 3D representation of the current robot pose, all
the poses in the auxiliary graph, and each of the areas
they belong to.
---------------------------------------------------------------*/
void CLocalMetricHypothesis::getAs3DScene( opengl::CSetOfObjectsPtr &objs ) const
{
objs->clear();
// -------------------------------------------
// Draw a grid on the ground:
// -------------------------------------------
{
opengl::CGridPlaneXYPtr obj = opengl::CGridPlaneXY::Create(-100,100,-100,100,0,5);
obj->setColor(0.4,0.4,0.4);
objs->insert(obj); // it will free the memory
}
// ---------------------------------------------------------
// Draw each of the robot poses as 2D/3D ellipsoids
// For the current pose, draw the robot itself as well.
// ---------------------------------------------------------
std::map< TPoseID, CPose3DPDFParticles > lstPoses;
std::map< TPoseID, CPose3DPDFParticles >::iterator it;
getPathParticles( lstPoses );
// -----------------------------------------------
// Draw a 3D coordinates corner for each cluster
// -----------------------------------------------
{
CCriticalSectionLocker locker( &m_parent->m_map_cs );
for ( TNodeIDSet::const_iterator n=m_neighbors.begin();n!=m_neighbors.end();++n)
{
const CHMHMapNodePtr node = m_parent->m_map.getNodeByID( *n );
ASSERT_(node);
TPoseID poseID_origin;
CPose3D originPose;
if (node->m_annotations.getElemental( NODE_ANNOTATION_REF_POSEID,poseID_origin, m_ID))
{
lstPoses[ poseID_origin ].getMean(originPose);
opengl::CSetOfObjectsPtr corner = stock_objects::CornerXYZ();
corner->setPose(originPose);
objs->insert( corner );
}
}
} // end of lock on map_cs
// Add a camera following the robot:
// -----------------------------------------
const CPose3D meanCurPose = lstPoses[ m_currentRobotPose ].getMeanVal();
{
opengl::CCameraPtr cam = opengl::CCamera::Create();
cam->setZoomDistance(85);
cam->setAzimuthDegrees(45 + RAD2DEG(meanCurPose.yaw()));
cam->setElevationDegrees(45);
cam->setPointingAt(meanCurPose);
objs->insert(cam);
}
map<CHMHMapNode::TNodeID,CPoint3D> areas_mean; // Store the mean pose of each area
map<CHMHMapNode::TNodeID,int> areas_howmany;
for (it=lstPoses.begin(); it!=lstPoses.end();it++)
{
opengl::CEllipsoidPtr ellip = opengl::CEllipsoid::Create();
// Color depending on being into the current area:
if ( m_nodeIDmemberships.find(it->first)->second == m_nodeIDmemberships.find(m_currentRobotPose)->second )
ellip->setColor(0,0,1);
else
ellip->setColor(1,0,0);
const CPose3DPDFParticles *pdfParts = &it->second;
CPose3DPDFGaussian pdf;
pdf.copyFrom( *pdfParts );
// Mean:
ellip->setLocation(pdf.mean.x(), pdf.mean.y(), pdf.mean.z()+0.005); // To be above the ground gridmap...
// Cov:
CMatrixDouble C = CMatrixDouble(pdf.cov); // 6x6 cov.
C.setSize(3,3);
// Is a 2D pose??
if ( pdf.mean.pitch() == 0 && pdf.mean.roll() == 0 && pdf.cov(2,2) < 1e-4f )
C.setSize(2,2);
ellip->setCovMatrix(C);
ellip->enableDrawSolid3D(false);
// Name:
ellip->setName( format("P%u", (unsigned int) it->first ) );
ellip->enableShowName();
// Add it:
objs->insert( ellip );
// Add an arrow for the mean direction also:
{
mrpt::opengl::CArrowPtr obj = mrpt::opengl::CArrow::Create(
0,0,0,
0.20,0,0,
0.25f,0.005f,0.02f);
obj->setColor(1,0,0);
obj->setPose(pdf.mean);
objs->insert( obj );
}
// Is this the current robot pose?
if (it->first == m_currentRobotPose )
{
// Draw robot:
opengl::CSetOfObjectsPtr robot = stock_objects::RobotPioneer();
robot->setPose(pdf.mean);
// Add it:
objs->insert( robot );
}
else // The current robot pose does not count
{
// Compute the area means:
std::map<TPoseID,CHMHMapNode::TNodeID>::const_iterator itAreaId = m_nodeIDmemberships.find( it->first );
ASSERT_( itAreaId != m_nodeIDmemberships.end() );
CHMHMapNode::TNodeID areaId = itAreaId->second;
areas_howmany[ areaId ]++;
areas_mean[ areaId ].x_incr( pdf.mean.x() );
areas_mean[ areaId ].y_incr( pdf.mean.y() );
areas_mean[ areaId ].z_incr( pdf.mean.z() );
}
} // end for it
// Average area means:
map<CHMHMapNode::TNodeID,CPoint3D>::iterator itMeans;
map<CHMHMapNode::TNodeID,int>::iterator itHowMany;
ASSERT_( areas_howmany.size() == areas_mean.size() );
for ( itMeans = areas_mean.begin(), itHowMany = areas_howmany.begin(); itMeans!=areas_mean.end(); itMeans++, itHowMany++ )
{
ASSERT_( itHowMany->second >0);
float f = 1.0f / itHowMany->second;
itMeans->second *= f;
}
// -------------------------------------------------------------------
// Draw lines between robot poses & their corresponding area sphere
// -------------------------------------------------------------------
for (it=lstPoses.begin(); it!=lstPoses.end();it++)
{
if (it->first != m_currentRobotPose )
{
CPoint3D areaPnt( areas_mean[ m_nodeIDmemberships.find( it->first )->second ] );
areaPnt.z_incr( m_parent->m_options.VIEW3D_AREA_SPHERES_HEIGHT );
const CPose3DPDFParticles *pdfParts = &it->second;
CPose3DPDFGaussian pdf;
pdf.copyFrom( *pdfParts );
opengl::CSimpleLinePtr line = opengl::CSimpleLine::Create();
line->setColor(0.8,0.8,0.8, 0.3);
line->setLineWidth(2);
line->setLineCoords(
pdf.mean.x(), pdf.mean.y(), pdf.mean.z(),
areaPnt.x(), areaPnt.y(), areaPnt.z() );
objs->insert( line );
}
} // end for it
// -------------------------------------------------------------------
// Draw lines for links between robot poses
// -------------------------------------------------------------------
// for (it=m_robotPoses.begin(); it!=m_robotPoses.end();it++)
/* for (it=lstPoses.begin(); it!=lstPoses.end();it++)
{
const CPose3DPDFParticles *myPdfParts = &it->second;
CPose3DPDFGaussian myPdf;
myPdf.copyFrom( *myPdfParts );
std::map<TPoseID,TInterRobotPosesInfo>::const_iterator itLink;
for (itLink=it->second.m_links.begin();itLink!=it->second.m_links.end();itLink++)
{
if (itLink->second.SSO>0.7)
{
CRobotPosesAuxiliaryGraph::const_iterator hisIt = m_robotPoses.find( itLink->first );
ASSERT_( hisIt !=m_robotPoses.end() );
const CPose3DPDFGaussian *hisPdf = & hisIt->second.m_pose;
opengl::CSimpleLinePtr line = opengl::CSimpleLine::Create();
line->m_color_R = 0.2f;
line->m_color_G = 0.8f;
line->m_color_B = 0.2f;
line->m_color_A = 0.3f;
line->m_lineWidth = 3.0f;
line->m_x0 = myPdf->mean.x;
line->m_y0 = myPdf->mean.y;
line->m_z0 = myPdf->mean.z;
line->m_x1 = hisPdf->mean.x;
line->m_y1 = hisPdf->mean.y;
line->m_z1 = hisPdf->mean.z;
objs->insert( line );
}
}
} // end for it
*/
// ---------------------------------------------------------
// Draw each of the areas in the neighborhood:
// ---------------------------------------------------------
{
CCriticalSectionLocker locker( &m_parent->m_map_cs ); //To access nodes' labels.
for ( itMeans = areas_mean.begin(); itMeans!=areas_mean.end(); itMeans++ )
{
opengl::CSpherePtr sphere = opengl::CSphere::Create();
if (itMeans->first == m_nodeIDmemberships.find( m_currentRobotPose)->second )
{ // Color of current area
sphere->setColor(0.1,0.1,0.7);
}
else
{ // Color of other areas
sphere->setColor(0.7,0,0);
}
sphere->setLocation(itMeans->second.x(), itMeans->second.y(), itMeans->second.z() + m_parent->m_options.VIEW3D_AREA_SPHERES_HEIGHT);
sphere->setRadius( m_parent->m_options.VIEW3D_AREA_SPHERES_RADIUS );
// Add it:
objs->insert( sphere );
// And text label:
opengl::CTextPtr txt = opengl::CText::Create();
txt->setColor(1,1,1);
const CHMHMapNodePtr node = m_parent->m_map.getNodeByID( itMeans->first );
ASSERT_(node);
txt->setLocation( itMeans->second.x(), itMeans->second.y(), itMeans->second.z() + m_parent->m_options.VIEW3D_AREA_SPHERES_HEIGHT );
// txt->m_str = node->m_label;
txt->setString( format("%li",(long int)node->getID()) );
objs->insert( txt );
}
} // end of lock on map_cs
// ---------------------------------------------------------
// Draw links between areas:
// ---------------------------------------------------------
{
CCriticalSectionLocker locker( &m_parent->m_map_cs );
for ( itMeans = areas_mean.begin(); itMeans!=areas_mean.end(); itMeans++ )
{
CHMHMapNode::TNodeID srcAreaID = itMeans->first;
const CHMHMapNodePtr srcArea = m_parent->m_map.getNodeByID( srcAreaID );
ASSERT_(srcArea);
TArcList lstArcs;
srcArea->getArcs(lstArcs);
for (TArcList::const_iterator a=lstArcs.begin();a!=lstArcs.end();++a)
{
// target is in the neighborhood of LMH:
if ( (*a)->getNodeFrom() == srcAreaID )
{
map<CHMHMapNode::TNodeID,CPoint3D>::const_iterator trgAreaPoseIt = areas_mean.find( (*a)->getNodeTo() );
if ( trgAreaPoseIt != areas_mean.end() )
{
// Yes, target node of the arc is in the LMH: Draw it:
opengl::CSimpleLinePtr line = opengl::CSimpleLine::Create();
line->setColor(0.8,0.8,0);
line->setLineWidth(3);
line->setLineCoords(
areas_mean[srcAreaID].x(), areas_mean[srcAreaID].y(), areas_mean[srcAreaID].z() + m_parent->m_options.VIEW3D_AREA_SPHERES_HEIGHT,
trgAreaPoseIt->second.x(), trgAreaPoseIt->second.y(), trgAreaPoseIt->second.z() + m_parent->m_options.VIEW3D_AREA_SPHERES_HEIGHT );
objs->insert( line );
}
}
} // end for each arc
} // end for each area
} // end of lock on map_cs
}
static void doPartitioningExperiment(
ekfslam_t &mapping,
CMatrixDouble &fullCov,
const string &OUT_DIR)
{
// Compute the "information" between partitions:
if (mapping.options.doPartitioningExperiment)
{
// --------------------------------------------
// PART I:
// Comparison to fixed partitioning every K obs.
// --------------------------------------------
// Compute the information matrix:
for (size_t i=0;i<6;i++) fullCov(i,i) = max(fullCov(i,i), 1e-6);
CMatrix H( fullCov.inv() );
H.saveToTextFile(OUT_DIR+string("/information_matrix_final.txt"));
// Replace by absolute values:
H = H.array().abs().matrix();
CMatrix H2(H); H2.normalize(0,1);
CImage imgF(H2, true);
imgF.saveToFile(OUT_DIR+string("/information_matrix_final.png"));
// ----------------------------------------
// Compute the "approximation error factor" E:
// E = SUM() / SUM(ALL ELEMENTS IN MATRIX)
// ----------------------------------------
vector<vector_uint> landmarksMembership,partsInObsSpace;
CMatrix ERRS(50,3);
for (size_t i=0;i<ERRS.getRowCount();i++)
{
size_t K;
if (i==0)
{
K=0;
mapping.getLastPartitionLandmarks( landmarksMembership );
}
else
{
K=i+1;
mapping.getLastPartitionLandmarksAsIfFixedSubmaps(i+1,landmarksMembership);
}
mapping.getLastPartition(partsInObsSpace);
ERRS(i,0) = (float)K;
ERRS(i,1) = (float)partsInObsSpace.size();
ERRS(i,2) = mapping.computeOffDiagonalBlocksApproximationError(landmarksMembership);
}
ERRS.saveToTextFile( OUT_DIR+string("/ERRORS.txt" ));
//printf("Approximation error from partition:\n"); cout << ERRS << endl;
// --------------------------------------------
// PART II:
// Sweep partitioning threshold:
// --------------------------------------------
size_t STEPS = 50;
CVectorFloat ERRS_SWEEP(STEPS),ERRS_SWEEP_THRESHOLD(STEPS);
// Compute the error for each partitioning-threshold
for (size_t i=0;i<STEPS;i++)
{
float th = (1.0f*i)/(STEPS-1.0f);
ERRS_SWEEP_THRESHOLD[i] = th;
mapping.mapPartitionOptions()->partitionThreshold = th;
mapping.reconsiderPartitionsNow();
mapping.getLastPartitionLandmarks( landmarksMembership );
ERRS_SWEEP[i] = mapping.computeOffDiagonalBlocksApproximationError(landmarksMembership);
}
ERRS_SWEEP.saveToTextFile( OUT_DIR+string("/ERRORS_SWEEP.txt" ));
ERRS_SWEEP_THRESHOLD.saveToTextFile( OUT_DIR+string("/ERRORS_SWEEP_THRESHOLD.txt" ));
} // end if doPartitioningExperiment
}
/*---------------------------------------------------------------
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; \
);
