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 ); } }
// ------------------------------------------------------ // TestHCH // ------------------------------------------------------ void TestHCH() { CMatrixFloat H,C,RES; cout << "reading H.txt..."; H.loadFromTextFile(myDataDir+string("H.txt")); cout << "ok"<<endl; cout << "reading C.txt..."; C.loadFromTextFile(myDataDir+string("C.txt")); cout << "ok"<<endl; // RES = H * C * ~H H.multiply_HCHt(C, RES); cout << "Saving RES.txt ..."; RES.saveToTextFile("RES.txt"); cout << "ok"<<endl; // The same for a column vector: H.loadFromTextFile(myDataDir+string("H_col.txt")); cout << "H*C*(~H) = " << H.multiply_HCHt_scalar(C) << endl; cout << "Should be= 31.434 "<< endl; // The same for a row vector: H.loadFromTextFile(myDataDir+string("H_row.txt")); cout << "Loaded H: " << endl << H; cout << "H*C*(~H) = " << H.multiply_HCHt_scalar(C) << endl; cout << "Should be= 31.434"<< endl; CMatrixFixedNumeric<double,1,5> Hfix; Hfix.loadFromTextFile(myDataDir+string("H_row.txt")); cout << "Again, loaded as a fixed matrix: " << endl << Hfix; }
/*--------------------------------------------------------------- inverse ---------------------------------------------------------------*/ void CPose3DQuatPDFGaussian::inverse(CPose3DQuatPDF &o) const { ASSERT_(o.GetRuntimeClass() == CLASS_ID(CPose3DQuatPDFGaussian)); CPose3DQuatPDFGaussian &out = static_cast<CPose3DQuatPDFGaussian&>(o); // COV: CMatrixFixedNumeric<double,3,7> df_dpose(UNINITIALIZED_MATRIX); double lx,ly,lz; mean.inverseComposePoint(0,0,0,lx,ly,lz, NULL, &df_dpose); CMatrixFixedNumeric<double,7,7> jacob; jacob.insertMatrix(0,0, df_dpose ); jacob.set_unsafe(3,3, 1); jacob.set_unsafe(4,4, -1); jacob.set_unsafe(5,5, -1); jacob.set_unsafe(6,6, -1); // C(0:2,0:2): H C H^t jacob.multiply_HCHt( this->cov, out.cov ); // Mean: out.mean.x(lx); out.mean.y(ly); out.mean.z(lz); this->mean.quat().conj( out.mean.quat() ); }
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 ); } }
TEST(Matrices, loadFromArray) { alignas(16) const double nums[3 * 4] = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12}; CMatrixFixedNumeric<double, 3, 4> mat; mat.loadFromArray(nums); for (int r = 0; r < 3; r++) for (int c = 0; c < 4; c++) EXPECT_EQ(nums[4 * r + c], mat(r, c)); }
double matrix_test_chol_fix(int a1, int a2) { CMatrixFixedNumeric<T,DIM1,DIM1> A = randomGenerator.drawDefinitePositiveMatrix(DIM1, 0.2); CMatrixFixedNumeric<T,DIM1,DIM1> chol_U; const long N = 100; CTicTac tictac; for (long i=0;i<N;i++) { A.chol(chol_U); } return tictac.Tac()/N; }
double matrix_test_unit_fix(int a1, int a2) { CMatrixFixedNumeric<T,DIM,DIM> C; const long N = 1000000; CTicTac tictac; for (long i=0;i<N;i++) { C.resize(DIM,DIM); C.setIdentity(); } return tictac.Tac()/N; }
double matrix_test_det_fix(int a1, int a2) { CMatrixFixedNumeric<T,DIM1,DIM1> A; randomGenerator.drawGaussian1DMatrix(A,T(0),T(1)); const long N = 10000; CTicTac tictac; for (long i=0;i<N;i++) { A.det(); } return tictac.Tac()/N; }
double matrix_test_loadFromArray(int N, int a2) { EIGEN_ALIGN16 double nums[4*4] = { 0,1,2,3, 4,5,6,7, 8,9,10,11, 12,13,14,15 }; CMatrixFixedNumeric<double,4,4> M; CTicTac tictac; M.loadFromArray(nums); return tictac.Tac(); }
double matrix_test_mult_fix(int a1, int a2) { CMatrixFixedNumeric<T,DIM1,DIM2> A; CMatrixFixedNumeric<T,DIM2,DIM3> B; CMatrixFixedNumeric<T,DIM1,DIM3> C; randomGenerator.drawGaussian1DMatrix(A,T(0),T(1)); randomGenerator.drawGaussian1DMatrix(B,T(0),T(1)); const long N = 10000; CTicTac tictac; for (long i=0;i<N;i++) { C.multiply(A,B); } return tictac.Tac()/N; }
double random_test_8(int a1, int a2) { CRandomGenerator rg; CMatrixFixedNumeric<double, DIM, DIM> R; rg.drawGaussian1DMatrix(R, 0.0, 1.0); CMatrixFixedNumeric<double, DIM, DIM> COV; COV.multiply_AAt(R); const size_t NSAMPS = 1000; // test 8: // ---------------------------------------- const long N = 1000; CTicTac tictac; std::vector<CVectorDouble> res; for (long i = 0; i < N; i++) { rg.drawGaussianMultivariateMany(res, NSAMPS, COV); } return tictac.Tac() / (N * NSAMPS); }
TEST(Matrices, setSize) { { CMatrixFixedNumeric<double,6,6> M; EXPECT_TRUE( (M.array() == 0).all() ); } { CMatrixDouble M(5,5); EXPECT_TRUE( (M.array() == 0).all() ); } { CMatrixDouble M(5,5); M.setSize(6,5); EXPECT_TRUE( (M.array() == 0).all() ); } { CMatrixDouble M(5,5); M.setSize(10,5); EXPECT_TRUE( (M.array() == 0).all() ); } { CMatrixDouble M(5,5); M.setSize(5,6); EXPECT_TRUE( (M.array() == 0).all() ); } { CMatrixDouble M(5,5); M.setSize(6,6); EXPECT_TRUE( (M.array() == 0).all() ); } { CMatrixDouble M(5,5); M.setSize(10,10); EXPECT_TRUE( (M.array() == 0).all() ); } }
/*---------------------------------------------------------------------------- ICP_Method_LM ----------------------------------------------------------------------------*/ CPosePDFPtr CICP::ICP_Method_LM( const mrpt::maps::CMetricMap *mm1, const mrpt::maps::CMetricMap *m2, const CPosePDFGaussian &initialEstimationPDF, TReturnInfo &outInfo ) { MRPT_START // The result can be either a Gaussian or a SOG: size_t nCorrespondences=0; bool keepIteratingICP; CPose2D grossEst = initialEstimationPDF.mean; TMatchingPairList correspondences,old_correspondences; CPose2D lastMeanPose; std::vector<float> other_xs_trans,other_ys_trans; // temporary container of "other" map (map2) transformed by "q" CMatrixFloat dJ_dq; // The jacobian CPose2D q; // The updated 2D transformation estimate CPose2D q_new; const bool onlyUniqueRobust = options.onlyUniqueRobust; const bool onlyKeepTheClosest = options.onlyClosestCorrespondences; // Assure the class of the maps: ASSERT_(mm1->GetRuntimeClass()->derivedFrom(CLASS_ID(CPointsMap))); const CPointsMap *m1 = static_cast<const CPointsMap*>(mm1); // Asserts: // ----------------- ASSERT_( options.ALFA>0 && options.ALFA<1 ); // The algorithm output auxiliar info: // ------------------------------------------------- outInfo.nIterations = 0; outInfo.goodness = 1.0f; TMatchingParams matchParams; TMatchingExtraResults matchExtraResults; matchParams.maxDistForCorrespondence = options.thresholdDist; // Distance threshold matchParams.maxAngularDistForCorrespondence = options.thresholdAng; // Angular threshold matchParams.angularDistPivotPoint = TPoint3D(q.x(),q.y(),0); // Pivot point for angular measurements matchParams.onlyKeepTheClosest = onlyKeepTheClosest; matchParams.onlyUniqueRobust = onlyUniqueRobust; matchParams.decimation_other_map_points = options.corresponding_points_decimation; // The gaussian PDF to estimate: // ------------------------------------------------------ // First gross approximation: q = grossEst; // For LM inverse CMatrixFixedNumeric<float,3,3> C; CMatrixFixedNumeric<float,3,3> C_inv; // This will keep the cov. matrix at the end // Asure maps are not empty! // ------------------------------------------------------ if ( !m2->isEmpty() ) { matchParams.offset_other_map_points = 0; // ------------------------------------------------------ // The ICP loop // ------------------------------------------------------ do { // ------------------------------------------------------ // Find the matching (for a points map) // ------------------------------------------------------ m1->determineMatching2D( m2, // The other map q, // The other map pose correspondences, matchParams, matchExtraResults); nCorrespondences = correspondences.size(); if ( !nCorrespondences ) { // Nothing we can do !! keepIteratingICP = false; } else { // Compute the estimated pose through iterative least-squares: Levenberg-Marquardt // ---------------------------------------------------------------------- dJ_dq.setSize(3,nCorrespondences); // The jacobian of the error function wrt the transformation q double lambda = options.LM_initial_lambda; // The LM parameter double ccos = cos(q.phi()); double csin = sin(q.phi()); double w1,w2,w3; double q1,q2,q3; double A,B; const double Axy = options.Axy_aprox_derivatives; // For approximating the derivatives // Compute at once the square errors for each point with the current "q" and the transformed points: std::vector<float> sq_errors; correspondences.squareErrorVector( q, sq_errors, other_xs_trans,other_ys_trans); double OSE_initial = math::sum( sq_errors ); // Compute "dJ_dq" // ------------------------------------ double rho2 = square( options.kernel_rho ); mrpt::utils::TMatchingPairList::iterator it; std::vector<float>::const_iterator other_x_trans,other_y_trans; size_t i; for (i=0, it=correspondences.begin(), other_x_trans = other_xs_trans.begin(), other_y_trans = other_ys_trans.begin(); i<nCorrespondences; ++i, ++it,++other_x_trans,++other_y_trans ) { // Jacobian: dJ_dx // -------------------------------------- //#define ICP_DISTANCES_TO_LINE #ifndef ICP_DISTANCES_TO_LINE w1= *other_x_trans-Axy; q1 = m1->squareDistanceToClosestCorrespondence( w1, *other_y_trans ); q1= kernel( q1, rho2 ); w2= *other_x_trans; q2 = m1->squareDistanceToClosestCorrespondence( w2, *other_y_trans ); q2= kernel( q2, rho2 ); w3= *other_x_trans+Axy; q3 = m1->squareDistanceToClosestCorrespondence( w3, *other_y_trans ); q3= kernel( q3, rho2 ); #else // The distance to the line that interpolates the TWO closest points: float x1,y1, x2,y2, d1,d2; m1->kdTreeTwoClosestPoint2D( *other_x_trans, *other_y_trans, // The query x1, y1, // Closest point #1 x2, y2, // Closest point #2 d1,d2); w1= *other_x_trans-Axy; q1 = math::closestSquareDistanceFromPointToLine( w1, *other_y_trans, x1,y1, x2,y2 ); q1= kernel( q1, rho2 ); w2= *other_x_trans; q2 = math::closestSquareDistanceFromPointToLine( w2, *other_y_trans, x1,y1, x2,y2 ); q2= kernel( q2, rho2 ); w3= *other_x_trans+Axy; q3 = math::closestSquareDistanceFromPointToLine( w3, *other_y_trans, x1,y1, x2,y2 ); q3= kernel( q3, rho2 ); #endif //interpolate A=( (q3-q2)/((w3-w2)*(w3-w1)) ) - ( (q1-q2)/((w1-w2)*(w3-w1)) ); B=( (q1-q2)+(A*((w2*w2)-(w1*w1))) )/(w1-w2); dJ_dq.get_unsafe(0,i) = (2*A* *other_x_trans)+B; // Jacobian: dJ_dy // -------------------------------------- w1= *other_y_trans-Axy; #ifdef ICP_DISTANCES_TO_LINE q1 = math::closestSquareDistanceFromPointToLine( *other_x_trans, w1, x1,y1, x2,y2 ); q1= kernel( q1, rho2 ); #else q1= kernel( square(it->this_x - *other_x_trans)+ square( it->this_y - w1 ), rho2 ); #endif w2= *other_y_trans; // q2 is alreay computed from above! //q2 = m1->squareDistanceToClosestCorrespondence( *other_x_trans, w2 ); //q2= kernel( square(it->this_x - *other_x_trans)+ square( it->this_y - w2 ), rho2 ); w3= *other_y_trans+Axy; #ifdef ICP_DISTANCES_TO_LINE q3 = math::closestSquareDistanceFromPointToLine( *other_x_trans, w3, x1,y1, x2,y2 ); q3= kernel( q3, rho2 ); #else q3= kernel( square(it->this_x - *other_x_trans)+ square( it->this_y - w3 ), rho2 ); #endif //interpolate A=( (q3-q2)/((w3-w2)*(w3-w1)) ) - ( (q1-q2)/((w1-w2)*(w3-w1)) ); B=( (q1-q2)+(A*((w2*w2)-(w1*w1))) )/(w1-w2); dJ_dq.get_unsafe(1,i) = (2*A* *other_y_trans)+B; // Jacobian: dR_dphi // -------------------------------------- dJ_dq.get_unsafe(2,i) = dJ_dq.get_unsafe(0,i) * ( -csin * it->other_x - ccos * it->other_y ) + dJ_dq.get_unsafe(1,i) * ( ccos * it->other_x - csin * it->other_y ); } // end for each corresp. // Now we have the Jacobian in dJ_dq. // Compute the Hessian matrix H = dJ_dq * dJ_dq^T CMatrixFloat H_(3,3); H_.multiply_AAt(dJ_dq); CMatrixFixedNumeric<float,3,3> H = CMatrixFixedNumeric<float,3,3>(H_); bool keepIteratingLM = true; // --------------------------------------------------- // Iterate the inner LM loop until convergence: // --------------------------------------------------- q_new = q; std::vector<float> new_sq_errors, new_other_xs_trans, new_other_ys_trans; size_t nLMiters = 0; const size_t maxLMiters = 100; while ( keepIteratingLM && ++nLMiters<maxLMiters) { // The LM heuristic is: // x_{k+1} = x_k - ( H + \lambda diag(H) )^-1 * grad(J) // grad(J) = dJ_dq * e (vector of errors) C = H; for (i=0;i<3;i++) C(i,i) *= (1+lambda); // Levenberg-Maquardt heuristic C_inv = C.inv(); // LM_delta = C_inv * dJ_dq * sq_errors Eigen::VectorXf dJsq, LM_delta; dJ_dq.multiply_Ab( Eigen::Map<Eigen::VectorXf>(&sq_errors[0],sq_errors.size()), dJsq ); C_inv.multiply_Ab(dJsq,LM_delta); q_new.x( q.x() - LM_delta[0] ); q_new.y( q.y() - LM_delta[1] ); q_new.phi( q.phi() - LM_delta[2] ); // Compute the new square errors: // --------------------------------------- correspondences.squareErrorVector( q_new, new_sq_errors, new_other_xs_trans, new_other_ys_trans); float OSE_new = math::sum( new_sq_errors ); bool improved = OSE_new < OSE_initial; #if 0 // Debuggin' cout << "_____________" << endl; cout << "q -> q_new : " << q << " -> " << q_new << endl; printf("err: %f -> %f lambda: %e\n", OSE_initial ,OSE_new, lambda ); cout << "\\/J = "; utils::operator <<(cout,dJsq); cout << endl; mrpt::system::pause(); #endif keepIteratingLM = fabs(LM_delta[0])>options.minAbsStep_trans || fabs(LM_delta[1])>options.minAbsStep_trans || fabs(LM_delta[2])>options.minAbsStep_rot; if(improved) { //If resids have gone down, keep change and make lambda smaller by factor of 10 lambda/=10; q=q_new; OSE_initial = OSE_new; } else { // Discard movement and try with larger lambda: lambda*=10; } } // end iterative LM #if 0 // Debuggin' cout << "ICP loop: " << lastMeanPose << " -> " << q << " LM iters: " << nLMiters << " threshold: " << matchParams.maxDistForCorrespondence << endl; #endif // -------------------------------------------------------- // now the conditions for the outer ICP loop // -------------------------------------------------------- keepIteratingICP = true; if (fabs(lastMeanPose.x()-q.x())<options.minAbsStep_trans && fabs(lastMeanPose.y()-q.y())<options.minAbsStep_trans && fabs( math::wrapToPi( lastMeanPose.phi()-q.phi()) )<options.minAbsStep_rot) { matchParams.maxDistForCorrespondence *= options.ALFA; matchParams.maxAngularDistForCorrespondence *= options.ALFA; if (matchParams.maxDistForCorrespondence < options.smallestThresholdDist ) keepIteratingICP = false; if (++matchParams.offset_other_map_points>=options.corresponding_points_decimation) matchParams.offset_other_map_points=0; } lastMeanPose = q; } // end of "else, there are correspondences" // Next iteration: outInfo.nIterations++; if (outInfo.nIterations >= options.maxIterations && matchParams.maxDistForCorrespondence>options.smallestThresholdDist) { matchParams.maxDistForCorrespondence *= options.ALFA; } } while ( (keepIteratingICP && outInfo.nIterations<options.maxIterations) || (outInfo.nIterations >= options.maxIterations && matchParams.maxDistForCorrespondence>options.smallestThresholdDist) ); outInfo.goodness = matchExtraResults.correspondencesRatio; //if (!options.skip_quality_calculation) { outInfo.quality= matchExtraResults.correspondencesRatio; } } // end of "if m2 is not empty" return CPosePDFGaussianPtr( new CPosePDFGaussian(q, C_inv.cast<double>() ) ); MRPT_END }
/*--------------------------------------------------------------- AlignPDF_robustMatch ---------------------------------------------------------------*/ CPosePDF::Ptr CGridMapAligner::AlignPDF_robustMatch( const mrpt::maps::CMetricMap* mm1, const mrpt::maps::CMetricMap* mm2, const CPosePDFGaussian& initialEstimationPDF, float* runningTime, void* info) { MRPT_START ASSERT_( options.methodSelection == CGridMapAligner::amRobustMatch || options.methodSelection == CGridMapAligner::amModifiedRANSAC); TReturnInfo outInfo; mrpt::tfest::TMatchingPairList& correspondences = outInfo.correspondences; // Use directly this placeholder to save 1 // variable & 1 copy. mrpt::tfest::TMatchingPairList largestConsensusCorrs; CTicTac* tictac = nullptr; CPose2D grossEst = initialEstimationPDF.mean; CLandmarksMap::Ptr lm1(new CLandmarksMap()); CLandmarksMap::Ptr lm2(new CLandmarksMap()); std::vector<size_t> idxs1, idxs2; std::vector<size_t>::iterator it1, it2; // Asserts: // ----------------- const CMultiMetricMap* multimap1 = nullptr; const CMultiMetricMap* multimap2 = nullptr; const COccupancyGridMap2D* m1 = nullptr; const COccupancyGridMap2D* m2 = nullptr; if (IS_CLASS(mm1, CMultiMetricMap) && IS_CLASS(mm2, CMultiMetricMap)) { multimap1 = static_cast<const CMultiMetricMap*>(mm1); multimap2 = static_cast<const CMultiMetricMap*>(mm2); ASSERT_(multimap1->m_gridMaps.size() && multimap1->m_gridMaps[0]); ASSERT_(multimap2->m_gridMaps.size() && multimap2->m_gridMaps[0]); m1 = multimap1->m_gridMaps[0].get(); m2 = multimap2->m_gridMaps[0].get(); } else if ( IS_CLASS(mm1, COccupancyGridMap2D) && IS_CLASS(mm2, COccupancyGridMap2D)) { m1 = static_cast<const COccupancyGridMap2D*>(mm1); m2 = static_cast<const COccupancyGridMap2D*>(mm2); } else THROW_EXCEPTION( "Metric maps must be of classes COccupancyGridMap2D or " "CMultiMetricMap"); ASSERTMSG_( m1->getResolution() == m2->getResolution(), mrpt::format( "Different resolutions for m1, m2:\n" "\tres(m1) = %f\n\tres(m2) = %f\n", m1->getResolution(), m2->getResolution())); // The algorithm output auxiliar info: // ------------------------------------------------- outInfo.goodness = 1.0f; outInfo.landmarks_map1 = lm1; outInfo.landmarks_map2 = lm2; // The PDF to estimate: // ------------------------------------------------------ CPosePDFSOG::Ptr pdf_SOG = mrpt::make_aligned_shared<CPosePDFSOG>(); // Extract features from grid-maps: // ------------------------------------------------------ const size_t N1 = std::max(40, mrpt::round(m1->getArea() * options.featsPerSquareMeter)); const size_t N2 = std::max(40, mrpt::round(m2->getArea() * options.featsPerSquareMeter)); m_grid_feat_extr.extractFeatures( *m1, *lm1, N1, options.feature_descriptor, options.feature_detector_options); m_grid_feat_extr.extractFeatures( *m2, *lm2, N2, options.feature_descriptor, options.feature_detector_options); if (runningTime) { tictac = new CTicTac(); tictac->Tic(); } const size_t nLM1 = lm1->size(); const size_t nLM2 = lm2->size(); // At least two landmarks at each map! // ------------------------------------------------------ if (nLM1 < 2 || nLM2 < 2) { outInfo.goodness = 0; } else { //#define METHOD_FFT //#define DEBUG_SHOW_CORRELATIONS // Compute correlation between landmarks: // --------------------------------------------- CMatrixFloat CORR(lm1->size(), lm2->size()), auxCorr; CImage im1, im2; // Grayscale CVectorFloat corr; unsigned int corrsCount = 0; std::vector<bool> hasCorr(nLM1, false); const double thres_max = options.threshold_max; const double thres_delta = options.threshold_delta; // CDisplayWindowPlots::Ptr auxWin; if (options.debug_show_corrs) { // auxWin = CDisplayWindowPlots::Ptr( new // CDisplayWindowPlots("Individual corr.") ); std::cerr << "Warning: options.debug_show_corrs has no effect " "since MRPT 0.9.1\n"; } for (size_t idx1 = 0; idx1 < nLM1; idx1++) { // CVectorFloat corrs_indiv; vector<pair<size_t, float>> corrs_indiv; // (index, distance); // Index is used to // recover the original // index after sorting. vector<float> corrs_indiv_only; corrs_indiv.reserve(nLM2); corrs_indiv_only.reserve(nLM2); for (size_t idx2 = 0; idx2 < nLM2; idx2++) { float minDist; minDist = lm1->landmarks.get(idx1)->features[0]->descriptorDistanceTo( *lm2->landmarks.get(idx2)->features[0]); corrs_indiv.emplace_back(idx2, minDist); corrs_indiv_only.push_back(minDist); } // end for idx2 // double corr_mean,corr_std; // mrpt::math::meanAndStd(corrs_indiv_only,corr_mean,corr_std); const double corr_best = mrpt::math::minimum(corrs_indiv_only); // cout << "M: " << corr_mean << " std: " << corr_std << " best: " // << corr_best << endl; // Sort the list and keep the N best features: std::sort(corrs_indiv.begin(), corrs_indiv.end(), myVectorOrder); // const size_t nBestToKeep = std::min( (size_t)30, // corrs_indiv.size() ); const size_t nBestToKeep = corrs_indiv.size(); for (size_t w = 0; w < nBestToKeep; w++) { if (corrs_indiv[w].second <= thres_max && corrs_indiv[w].second <= (corr_best + thres_delta)) { idxs1.push_back(idx1); idxs2.push_back(corrs_indiv[w].first); outInfo.correspondences_dists_maha.emplace_back( idx1, corrs_indiv[w].first, corrs_indiv[w].second); } } } // end for idx1 // Save an image with each feature and its matches: if (options.save_feat_coors) { mrpt::system::deleteFilesInDirectory("grid_feats"); mrpt::system::createDirectory("grid_feats"); std::map<size_t, std::set<size_t>> corrs_by_idx; for (size_t l = 0; l < idxs1.size(); l++) corrs_by_idx[idxs1[l]].insert(idxs2[l]); for (auto& it : corrs_by_idx) { CMatrixFloat descriptor1; lm1->landmarks.get(it.first) ->features[0] ->getFirstDescriptorAsMatrix(descriptor1); im1 = CImage(descriptor1, true); const size_t FEAT_W = im1.getWidth(); const size_t FEAT_H = im1.getHeight(); const size_t nF = it.second.size(); CImage img_compose(FEAT_W * 2 + 15, 10 + (5 + FEAT_H) * nF); img_compose.filledRectangle( 0, 0, img_compose.getWidth() - 1, img_compose.getHeight() - 1, TColor::black()); img_compose.drawImage(5, 5, im1); size_t j; std::set<size_t>::iterator it_j; string fil = format("grid_feats/_FEAT_MATCH_%03i", (int)it.first); for (j = 0, it_j = it.second.begin(); j < nF; ++j, ++it_j) { fil += format("_%u", static_cast<unsigned int>(*it_j)); CMatrixFloat descriptor2; lm2->landmarks.get(*it_j) ->features[0] ->getFirstDescriptorAsMatrix(descriptor2); im2 = CImage(descriptor2, true); img_compose.drawImage( 10 + FEAT_W, 5 + j * (FEAT_H + 5), im2); } fil += ".png"; img_compose.saveToFile(fil); } // end for map } // ------------------------------------------------------------------ // Create the list of correspondences from the lists: idxs1 & idxs2 // ------------------------------------------------------------------ correspondences.clear(); for (it1 = idxs1.begin(), it2 = idxs2.begin(); it1 != idxs1.end(); ++it1, ++it2) { mrpt::tfest::TMatchingPair mp; mp.this_idx = *it1; mp.this_x = lm1->landmarks.get(*it1)->pose_mean.x; mp.this_y = lm1->landmarks.get(*it1)->pose_mean.y; mp.this_z = lm1->landmarks.get(*it1)->pose_mean.z; mp.other_idx = *it2; mp.other_x = lm2->landmarks.get(*it2)->pose_mean.x; mp.other_y = lm2->landmarks.get(*it2)->pose_mean.y; mp.other_z = lm2->landmarks.get(*it2)->pose_mean.z; correspondences.push_back(mp); if (!hasCorr[*it1]) { hasCorr[*it1] = true; corrsCount++; } } // end for corres. outInfo.goodness = (2.0f * corrsCount) / (nLM1 + nLM2); // Compute the estimation using ALL the correspondences (NO ROBUST): // ---------------------------------------------------------------------- mrpt::math::TPose2D noRobustEst; if (!mrpt::tfest::se2_l2(correspondences, noRobustEst)) { // There's no way to match the maps! e.g. no correspondences outInfo.goodness = 0; pdf_SOG->clear(); outInfo.sog1 = pdf_SOG; outInfo.sog2 = pdf_SOG; outInfo.sog3 = pdf_SOG; } else { outInfo.noRobustEstimation = mrpt::poses::CPose2D(noRobustEst); MRPT_LOG_INFO(mrpt::format( "[CGridMapAligner] Overall estimation(%u corrs, total: " "%u): (%.03f,%.03f,%.03fdeg)\n", corrsCount, (unsigned)correspondences.size(), outInfo.noRobustEstimation.x(), outInfo.noRobustEstimation.y(), RAD2DEG(outInfo.noRobustEstimation.phi()))); // The list of SOG modes & their corresponding sub-sets of // matchings: using TMapMatchingsToPoseMode = mrpt::aligned_std_map< mrpt::tfest::TMatchingPairList, CPosePDFSOG::TGaussianMode>; TMapMatchingsToPoseMode sog_modes; // --------------------------------------------------------------- // Now, we have to choose between the methods: // - CGridMapAligner::amRobustMatch ("normal" RANSAC) // - CGridMapAligner::amModifiedRANSAC // --------------------------------------------------------------- if (options.methodSelection == CGridMapAligner::amRobustMatch) { // ==================================================== // METHOD: "Normal" RANSAC // ==================================================== // RANSAC over the found correspondences: // ------------------------------------------------- const unsigned int min_inliers = options.ransac_minSetSizeRatio * (nLM1 + nLM2) / 2; mrpt::tfest::TSE2RobustParams tfest_params; tfest_params.ransac_minSetSize = min_inliers; tfest_params.ransac_maxSetSize = nLM1 + nLM2; tfest_params.ransac_mahalanobisDistanceThreshold = options.ransac_mahalanobisDistanceThreshold; tfest_params.ransac_nSimulations = 0; // 0=auto tfest_params.ransac_fuseByCorrsMatch = true; tfest_params.ransac_fuseMaxDiffXY = 0.01; tfest_params.ransac_fuseMaxDiffPhi = DEG2RAD(0.1); tfest_params.ransac_algorithmForLandmarks = true; tfest_params.probability_find_good_model = options.ransac_prob_good_inliers; tfest_params.verbose = false; mrpt::tfest::TSE2RobustResult tfest_result; mrpt::tfest::se2_l2_robust( correspondences, options.ransac_SOG_sigma_m, tfest_params, tfest_result); ASSERT_(pdf_SOG); *pdf_SOG = tfest_result.transformation; largestConsensusCorrs = tfest_result.largestSubSet; // Simplify the SOG by merging close modes: // ------------------------------------------------- size_t nB = pdf_SOG->size(); outInfo.sog1 = pdf_SOG; // Keep only the most-likely Gaussian mode: CPosePDFSOG::TGaussianMode best_mode; best_mode.log_w = -std::numeric_limits<double>::max(); for (size_t n = 0; n < pdf_SOG->size(); n++) { const CPosePDFSOG::TGaussianMode& m = (*pdf_SOG)[n]; if (m.log_w > best_mode.log_w) best_mode = m; } pdf_SOG->clear(); pdf_SOG->push_back(best_mode); outInfo.sog2 = pdf_SOG; MRPT_LOG_INFO_STREAM( "[CGridMapAligner] amRobustMatch: " << nB << " SOG modes reduced to " << pdf_SOG->size() << " (most-likely) (min.inliers=" << min_inliers << ")\n"); } // end of amRobustMatch else { // ==================================================== // METHOD: "Modified" RANSAC // ==================================================== mrpt::tfest::TMatchingPairList all_corrs = correspondences; const size_t nCorrs = all_corrs.size(); ASSERT_(nCorrs >= 2); pdf_SOG->clear(); // Start with 0 Gaussian modes // Build a points-map for exploiting its KD-tree: // ---------------------------------------------------- CSimplePointsMap lm1_pnts, lm2_pnts; lm1_pnts.reserve(nLM1); for (size_t i = 0; i < nLM1; i++) lm1_pnts.insertPoint(lm1->landmarks.get(i)->pose_mean); lm2_pnts.reserve(nLM2); for (size_t i = 0; i < nLM2; i++) lm2_pnts.insertPoint(lm2->landmarks.get(i)->pose_mean); // RANSAC loop // --------------------- const size_t minInliersTOaccept = round(options.ransac_minSetSizeRatio * 0.5 * (nLM1 + nLM2)); // Set an initial # of iterations: const unsigned int ransac_min_nSimulations = 2 * (nLM1 + nLM2); // 1000; unsigned int ransac_nSimulations = 10; // It doesn't matter actually, since will be changed in // the first loop const double probability_find_good_model = 0.9999; const double chi2_thres_dim1 = mrpt::math::chi2inv(options.ransac_chi2_quantile, 1); const double chi2_thres_dim2 = mrpt::math::chi2inv(options.ransac_chi2_quantile, 2); // Generic 2x2 covariance matrix for all features in their local // coords: CMatrixDouble22 COV_pnt; COV_pnt.get_unsafe(0, 0) = COV_pnt.get_unsafe(1, 1) = square(options.ransac_SOG_sigma_m); // The absolute number of trials. // in practice it's only important for a reduced number of // correspondences, to avoid a dead-lock: // It's the binomial coefficient: // / n \ n! n * (n-1) // | | = ----------- = ----------- // \ 2 / 2! (n-2)! 2 // const unsigned int max_trials = (nCorrs * (nCorrs - 1) / 2) * 5; // "*5" is just for safety... unsigned int iter = 0; // Valid iterations (those passing the // first mahalanobis test) unsigned int trials = 0; // counter of all iterations, // including "iter" + failing ones. while (iter < ransac_nSimulations && trials < max_trials) // ransac_nSimulations can be dynamic { trials++; mrpt::tfest::TMatchingPairList tentativeSubSet; // Pick 2 random correspondences: uint32_t idx1, idx2; idx1 = getRandomGenerator().drawUniform32bit() % nCorrs; do { idx2 = getRandomGenerator().drawUniform32bit() % nCorrs; } while (idx1 == idx2); // Avoid a degenerated case! // Uniqueness of features: if (all_corrs[idx1].this_idx == all_corrs[idx2].this_idx || all_corrs[idx1].this_idx == all_corrs[idx2].other_idx) continue; if (all_corrs[idx1].other_idx == all_corrs[idx2].this_idx || all_corrs[idx1].other_idx == all_corrs[idx2].other_idx) continue; // Check the feasibility of this pair "idx1"-"idx2": // The distance between the pair of points in MAP1 must be // very close // to that of their correspondences in MAP2: const double corrs_dist1 = mrpt::math::distanceBetweenPoints( all_corrs[idx1].this_x, all_corrs[idx1].this_y, all_corrs[idx1].this_z, all_corrs[idx2].this_x, all_corrs[idx2].this_y, all_corrs[idx2].this_z); const double corrs_dist2 = mrpt::math::distanceBetweenPoints( all_corrs[idx1].other_x, all_corrs[idx1].other_y, all_corrs[idx1].other_z, all_corrs[idx2].other_x, all_corrs[idx2].other_y, all_corrs[idx2].other_z); // Is is a consistent possibility? // We use a chi2 test (see paper for the derivation) const double corrs_dist_chi2 = square(square(corrs_dist1) - square(corrs_dist2)) / (8.0 * square(options.ransac_SOG_sigma_m) * (square(corrs_dist1) + square(corrs_dist2))); if (corrs_dist_chi2 > chi2_thres_dim1) continue; // Nope iter++; // Do not count iterations if they fail the test // above. // before proceeding with this hypothesis, is it an old one? bool is_new_hyp = true; for (auto& sog_mode : sog_modes) { if (sog_mode.first.contains(all_corrs[idx1]) && sog_mode.first.contains(all_corrs[idx2])) { // Increment weight: sog_mode.second.log_w = std::log(std::exp(sog_mode.second.log_w) + 1.0); is_new_hyp = false; break; } } if (!is_new_hyp) continue; // Ok, it's a new hypothesis: tentativeSubSet.push_back(all_corrs[idx1]); tentativeSubSet.push_back(all_corrs[idx2]); // Maintain a list of already used landmarks IDs in both // maps to avoid repetitions: std::vector<bool> used_landmarks1(nLM1, false); std::vector<bool> used_landmarks2(nLM2, false); used_landmarks1[all_corrs[idx1].this_idx] = true; used_landmarks1[all_corrs[idx2].this_idx] = true; used_landmarks2[all_corrs[idx1].other_idx] = true; used_landmarks2[all_corrs[idx2].other_idx] = true; // Build the transformation for these temptative // correspondences: bool keep_incorporating = true; CPosePDFGaussian temptPose; do // Incremently incorporate inliers: { if (!mrpt::tfest::se2_l2(tentativeSubSet, temptPose)) continue; // Invalid matching... // The computed cov is "normalized", i.e. must be // multiplied by std^2_xy temptPose.cov *= square(options.ransac_SOG_sigma_m); // cout << "q: " << temptPose << endl; // Find the landmark in MAP2 with the best (maximum) // product-integral: // (i^* , j^*) = arg max_(i,j) \int p_i()p_j() //---------------------------------------------------------------------- const double ccos = cos(temptPose.mean.phi()); const double ssin = sin(temptPose.mean.phi()); CMatrixDouble22 Hc; // Jacobian wrt point_j Hc.get_unsafe(1, 1) = ccos; Hc.get_unsafe(0, 0) = ccos; Hc.get_unsafe(1, 0) = ssin; Hc.get_unsafe(0, 1) = -ssin; CMatrixFixedNumeric<double, 2, 3> Hq; // Jacobian wrt transformation q Hq(0, 0) = 1; Hq(1, 1) = 1; TPoint2D p2_j_local; vector<float> matches_dist; std::vector<size_t> matches_idx; CPoint2DPDFGaussian pdf_M2_j; CPoint2DPDFGaussian pdf_M1_i; // Use integral-of-product vs. mahalanobis distances to match: #define GRIDMAP_USE_PROD_INTEGRAL #ifdef GRIDMAP_USE_PROD_INTEGRAL double best_pair_value = 0; #else double best_pair_value = std::numeric_limits<double>::max(); #endif double best_pair_d2 = std::numeric_limits<double>::max(); pair<size_t, size_t> best_pair_ij; //#define SHOW_CORRS #ifdef SHOW_CORRS CDisplayWindowPlots win("Matches"); #endif for (size_t j = 0; j < nLM2; j++) { if (used_landmarks2[j]) continue; lm2_pnts.getPoint( j, p2_j_local); // In local coords. pdf_M2_j.mean = mrpt::poses::CPoint2D( temptPose.mean + p2_j_local); // In (temptative) global coords: pdf_M2_j.cov.get_unsafe(0, 0) = pdf_M2_j.cov.get_unsafe(1, 1) = square(options.ransac_SOG_sigma_m); #ifdef SHOW_CORRS win.plotEllipse( pdf_M2_j.mean.x(), pdf_M2_j.mean.y(), pdf_M2_j.cov, 2, "b-", format("M2_%u", (unsigned)j), true); #endif static const unsigned int N_KDTREE_SEARCHED = 3; // Look for a few close features which may be // potential matches: lm1_pnts.kdTreeNClosestPoint2DIdx( pdf_M2_j.mean.x(), pdf_M2_j.mean.y(), N_KDTREE_SEARCHED, matches_idx, matches_dist); // And for each one, compute the product-integral: for (unsigned long u : matches_idx) { if (used_landmarks1[u]) continue; // Jacobian wrt transformation q Hq.get_unsafe(0, 2) = -p2_j_local.x * ssin - p2_j_local.y * ccos; Hq.get_unsafe(1, 2) = p2_j_local.x * ccos - p2_j_local.y * ssin; // COV_j = Hq \Sigma_q Hq^t + Hc Cj Hc^t Hc.multiply_HCHt(COV_pnt, pdf_M1_i.cov); Hq.multiply_HCHt( temptPose.cov, pdf_M1_i.cov, true); float px, py; lm1_pnts.getPoint(u, px, py); pdf_M1_i.mean.x(px); pdf_M1_i.mean.y(py); #ifdef SHOW_CORRS win.plotEllipse( pdf_M1_i.mean.x(), pdf_M1_i.mean.y(), pdf_M1_i.cov, 2, "r-", format("M1_%u", (unsigned)matches_idx[u]), true); #endif // And now compute the product integral: #ifdef GRIDMAP_USE_PROD_INTEGRAL const double prod_ij = pdf_M1_i.productIntegralWith(pdf_M2_j); // const double prod_ij_d2 = square( // pdf_M1_i.mahalanobisDistanceTo(pdf_M2_j) ); if (prod_ij > best_pair_value) #else const double prod_ij = pdf_M1_i.mean.distanceTo(pdf_M2_j.mean); if (prod_ij < best_pair_value) #endif { // const double prodint_ij = // pdf_M1_i.productIntegralWith2D(pdf_M2_j); best_pair_value = prod_ij; best_pair_ij.first = u; best_pair_ij.second = j; best_pair_d2 = square(pdf_M1_i.mahalanobisDistanceTo( pdf_M2_j)); // cout << "P1: " << pdf_M1_i.mean << " C= " // << pdf_M1_i.cov.inMatlabFormat() << endl; // cout << "P2: " << pdf_M2_j.mean << " C= " // << pdf_M2_j.cov.inMatlabFormat() << endl; // cout << " -> " << format("%e",prod_ij) // << " d2: " << best_pair_d2 << endl << // endl; } } // end for u (closest matches of LM2 in MAP 1) #ifdef SHOW_CORRS win.axis_fit(true); win.waitForKey(); win.clear(); #endif } // end for each LM2 // Stop when the best choice has a bad mahal. dist. keep_incorporating = false; // For the best (i,j), gate by the mahalanobis distance: if (best_pair_d2 < chi2_thres_dim2) { // AND, also, check if the descriptors have some // resemblance!! // ---------------------------------------------------------------- // double feat_dist = // lm1->landmarks.get(best_pair_ij.first)->features[0]->descriptorDistanceTo(*lm1->landmarks.get(best_pair_ij.second)->features[0]); // if (feat_dist< options.threshold_max) { float p1_i_localx, p1_i_localy; float p2_j_localx, p2_j_localy; lm1_pnts.getPoint( best_pair_ij.first, p1_i_localx, p1_i_localy); lm2_pnts.getPoint( best_pair_ij.second, p2_j_localx, p2_j_localy); // In local coords. used_landmarks1[best_pair_ij.first] = true; used_landmarks2[best_pair_ij.second] = true; tentativeSubSet.push_back( mrpt::tfest::TMatchingPair( best_pair_ij.first, best_pair_ij.second, p1_i_localx, p1_i_localy, 0, // MAP1 p2_j_localx, p2_j_localy, 0 // MAP2 )); keep_incorporating = true; } } } while (keep_incorporating); // Consider this pairing? const size_t ninliers = tentativeSubSet.size(); if (ninliers > minInliersTOaccept) { CPosePDFSOG::TGaussianMode newGauss; newGauss.log_w = 0; // log(1); // // std::log(static_cast<double>(nCoincidences)); newGauss.mean = temptPose.mean; newGauss.cov = temptPose.cov; sog_modes[tentativeSubSet] = newGauss; // cout << "ITER: " << iter << " #inliers: " << ninliers // << " q: " << temptPose.mean << endl; } // Keep the largest consensus & dynamic # of steps: if (largestConsensusCorrs.size() < ninliers) { largestConsensusCorrs = tentativeSubSet; // Update estimate of N, the number of trials to ensure // we pick, // with probability p, a data set with no outliers. const double fracinliers = ninliers / static_cast<double>(std::min(nLM1, nLM2)); double pNoOutliers = 1 - pow(fracinliers, static_cast<double>( 2.0)); // minimumSizeSamplesToFit pNoOutliers = std::max( std::numeric_limits<double>::epsilon(), pNoOutliers); // Avoid division by -Inf pNoOutliers = std::min( 1.0 - std::numeric_limits<double>::epsilon(), pNoOutliers); // Avoid division by 0. // Number of ransac_nSimulations = log(1 - probability_find_good_model) / log(pNoOutliers); if (ransac_nSimulations < ransac_min_nSimulations) ransac_nSimulations = ransac_min_nSimulations; // if (verbose) // cout << "[Align] Iter #" << iter << " Est. #iters: " //<< ransac_nSimulations << " pNoOutliers=" << // pNoOutliers << " #inliers: " << ninliers << endl; } } // end of RANSAC loop // Move SOG modes into pdf_SOG: pdf_SOG->clear(); for (auto s = sog_modes.begin(); s != sog_modes.end(); ++s) { cout << "SOG mode: " << s->second.mean << " inliers: " << s->first.size() << endl; pdf_SOG->push_back(s->second); } // Normalize: if (pdf_SOG->size() > 0) pdf_SOG->normalizeWeights(); // Simplify the SOG by merging close modes: // ------------------------------------------------- size_t nB = pdf_SOG->size(); outInfo.sog1 = pdf_SOG; CTicTac merge_clock; pdf_SOG->mergeModes(options.maxKLd_for_merge, false); const double merge_clock_tim = merge_clock.Tac(); outInfo.sog2 = pdf_SOG; size_t nA = pdf_SOG->size(); MRPT_LOG_INFO(mrpt::format( "[CGridMapAligner] amModifiedRANSAC: %u SOG modes " "merged to %u in %.03fsec\n", (unsigned int)nB, (unsigned int)nA, merge_clock_tim)); } // end of amModifiedRANSAC // Save best corrs: if (options.debug_save_map_pairs) { static unsigned int NN = 0; static const COccupancyGridMap2D* lastM1 = nullptr; if (lastM1 != m1) { lastM1 = m1; NN = 0; } printf( " Largest consensus: %u\n", static_cast<unsigned>(largestConsensusCorrs.size())); CEnhancedMetaFile::LINUX_IMG_WIDTH( m1->getSizeX() + m2->getSizeX() + 50); CEnhancedMetaFile::LINUX_IMG_HEIGHT( max(m1->getSizeY(), m2->getSizeY()) + 50); for (auto s = sog_modes.begin(); s != sog_modes.end(); ++s) { COccupancyGridMap2D::saveAsEMFTwoMapsWithCorrespondences( format("__debug_corrsGrid_%05u.emf", NN), m1, m2, s->first); COccupancyGridMap2D::saveAsBitmapTwoMapsWithCorrespondences( format("__debug_corrsGrid_%05u.png", NN), m1, m2, s->first); ++NN; } } // -------------------------------------------------------------------- // At this point: // - "pdf_SOG": has the resulting PDF with the SOG (from whatever // method) // - "largestConsensusCorrs": The 'best' set of correspondences // // Now: If we had a multi-metric map, use the points map to improve // the estimation with ICP. // -------------------------------------------------------------------- if (multimap1 && multimap2 && !multimap1->m_pointsMaps.empty() && !multimap2->m_pointsMaps.empty() && multimap1->m_pointsMaps[0] && multimap2->m_pointsMaps[0]) { CSimplePointsMap::Ptr pnts1 = multimap1->m_pointsMaps[0]; CSimplePointsMap::Ptr pnts2 = multimap2->m_pointsMaps[0]; CICP icp; CICP::TReturnInfo icpInfo; icp.options.maxIterations = 20; icp.options.smallestThresholdDist = 0.05f; icp.options.thresholdDist = 0.75f; // cout << "Points: " << pnts1->size() << " " << pnts2->size() // << endl; // Invoke ICP once for each mode in the SOG: size_t cnt = 0; outInfo.icp_goodness_all_sog_modes.clear(); for (auto i = pdf_SOG->begin(); i != pdf_SOG->end(); ++cnt) { CPosePDF::Ptr icp_est = icp.Align( pnts1.get(), pnts2.get(), (i)->mean, nullptr, &icpInfo); //(i)->cov(0,0) += square( 0.05 ); //(i)->cov(1,1) += square( 0.05 ); //(i)->cov(2,2) += square( DEG2RAD(0.05) ); CPosePDFGaussian i_gauss(i->mean, i->cov); CPosePDFGaussian icp_gauss(icp_est->getMeanVal(), i->cov); const double icp_maha_dist = i_gauss.mahalanobisDistanceTo(icp_gauss); cout << "ICP " << cnt << " log-w: " << i->log_w << " Goodness: " << 100 * icpInfo.goodness << " D_M= " << icp_maha_dist << endl; cout << " final pos: " << icp_est->getMeanVal() << endl; cout << " org pos: " << i->mean << endl; outInfo.icp_goodness_all_sog_modes.push_back( icpInfo.goodness); // Discard? if (icpInfo.goodness > options.min_ICP_goodness && icp_maha_dist < options.max_ICP_mahadist) { icp_est->getMean((i)->mean); ++i; } else { // Delete: i = pdf_SOG->erase(i); } } // end for i // Merge: outInfo.sog3 = pdf_SOG; pdf_SOG->mergeModes(options.maxKLd_for_merge, false); MRPT_LOG_DEBUG_STREAM( "[CGridMapAligner] " << pdf_SOG->size() << " SOG modes merged after ICP."); } // end multimapX } // end of, yes, we have enough correspondences } // end of: yes, there are landmarks in the grid maps! // Copy the output info if requested: // ------------------------------------------------- MRPT_TODO( "Refactor `info` so it is polymorphic and can use dynamic_cast<> here"); if (info) { auto* info_ = static_cast<TReturnInfo*>(info); *info_ = outInfo; } if (runningTime) { *runningTime = tictac->Tac(); delete tictac; } return pdf_SOG; MRPT_END }
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; } }