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