/** 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 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 }
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(); } }