tuple CICP_AlignPDF2(CICP &self, CSimplePointsMap &m1, CSimplePointsMap &m2, CPosePDFGaussian &initialEstimationPDF) { CPosePDFGaussian posePDF; float runningTime; CICP::TReturnInfo info; CPosePDFPtr posePDFPtr = self.AlignPDF(&m1, &m2, initialEstimationPDF, &runningTime, &info); posePDF.copyFrom(*posePDFPtr); boost::python::list ret_val; ret_val.append(posePDF); ret_val.append(runningTime); ret_val.append(info); return tuple(ret_val); }
/** The PF algorithm implementation for "optimal sampling" approximated with scan matching (Stachniss method) */ void CLSLAM_RBPF_2DLASER::prediction_and_update_pfOptimalProposal( CLocalMetricHypothesis *LMH, const mrpt::slam::CActionCollection * actions, const mrpt::slam::CSensoryFrame * sf, const bayes::CParticleFilter::TParticleFilterOptions &PF_options ) { MRPT_START CTicTac tictac; // Get the current robot pose estimation: TPoseID currentPoseID = LMH->m_currentRobotPose; // ---------------------------------------------------------------------- // We can execute optimal PF only when we have both, an action, and // a valid observation from which to compute the likelihood: // Accumulate odometry/actions until we have a valid observation, then // process them simultaneously. // ---------------------------------------------------------------------- bool SFhasValidObservations = false; // A valid action? if (actions!=NULL) { CActionRobotMovement2DPtr act = actions->getBestMovementEstimation(); // Find a robot movement estimation: if (!act) THROW_EXCEPTION("Action list does not contain any CActionRobotMovement2D derived object!"); if (!LMH->m_accumRobotMovementIsValid) // Reset accum. { act->poseChange->getMean( LMH->m_accumRobotMovement.rawOdometryIncrementReading ); LMH->m_accumRobotMovement.motionModelConfiguration = act->motionModelConfiguration; } else LMH->m_accumRobotMovement.rawOdometryIncrementReading = LMH->m_accumRobotMovement.rawOdometryIncrementReading + act->poseChange->getMeanVal(); LMH->m_accumRobotMovementIsValid = true; } if (sf!=NULL) { ASSERT_(LMH->m_particles.size()>0); SFhasValidObservations = (*LMH->m_particles.begin()).d->metricMaps.canComputeObservationsLikelihood( *sf ); } // All the needed things? if (!LMH->m_accumRobotMovementIsValid || !SFhasValidObservations) return; // Nothing we can do here... ASSERT_(sf!=NULL); ASSERT_(!PF_options.adaptiveSampleSize); // OK, we have all we need, let's start! // The odometry-based increment since last step is // in: LMH->m_accumRobotMovement.rawOdometryIncrementReading const CPose2D initialPoseEstimation = LMH->m_accumRobotMovement.rawOdometryIncrementReading; LMH->m_accumRobotMovementIsValid = false; // To reset odometry at next iteration! // ---------------------------------------------------------------------- // 1) FIXED SAMPLE SIZE VERSION // ---------------------------------------------------------------------- // ICP used if "pfOptimalProposal_mapSelection" = 0 or 1 CICP icp; CICP::TReturnInfo icpInfo; // ICP options // ------------------------------ icp.options.maxIterations = 80; icp.options.thresholdDist = 0.50f; icp.options.thresholdAng = DEG2RAD( 20 ); icp.options.smallestThresholdDist = 0.05f; icp.options.ALFA = 0.5f; icp.options.onlyClosestCorrespondences = true; icp.options.doRANSAC = false; // Build the map of points to align: CSimplePointsMap localMapPoints; ASSERT_( LMH->m_particles[0].d->metricMaps.m_gridMaps.size() > 0); //float minDistBetweenPointsInLocalMaps = 0.02f; //3.0f * m_particles[0].d->metricMaps.m_gridMaps[0]->getResolution(); // Build local map: localMapPoints.clear(); localMapPoints.insertionOptions.minDistBetweenLaserPoints = 0.02; sf->insertObservationsInto( &localMapPoints ); // Process the particles const size_t M = LMH->m_particles.size(); LMH->m_log_w_metric_history.resize(M); for (size_t i=0;i<M;i++) { CLocalMetricHypothesis::CParticleData &part = LMH->m_particles[i]; CPose3D *part_pose = LMH->getCurrentPose(i); if ( LMH->m_SFs.empty() ) { // The first robot pose in the SLAM execution: All m_particles start // at the same point (this is the lowest bound of subsequent uncertainty): // New pose = old pose. // part_pose: Unmodified } else { // ICP and add noise: CPosePDFGaussian icpEstimation; // Select map to use with ICP: CMetricMap *mapalign; if (!part.d->metricMaps.m_pointsMaps.empty()) mapalign = part.d->metricMaps.m_pointsMaps[0].pointer(); else if (!part.d->metricMaps.m_gridMaps.empty()) mapalign = part.d->metricMaps.m_gridMaps[0].pointer(); else THROW_EXCEPTION("There is no point or grid map. At least one needed for ICP."); // Use ICP to align to each particle's map: CPosePDFPtr alignEst = icp.Align( mapalign, &localMapPoints, initialPoseEstimation, NULL, &icpInfo); icpEstimation.copyFrom( *alignEst ); #if 0 // HACK: CPose3D Ap = finalEstimatedPoseGauss.mean - ith_last_pose; double Ap_dist = Ap.norm(); finalEstimatedPoseGauss.cov.zeros(); finalEstimatedPoseGauss.cov(0,0) = square( fabs(Ap_dist)*0.01 ); finalEstimatedPoseGauss.cov(1,1) = square( fabs(Ap_dist)*0.01 ); finalEstimatedPoseGauss.cov(2,2) = square( fabs(Ap.yaw())*0.02 ); #endif // Generate gaussian-distributed 2D-pose increments according to "finalEstimatedPoseGauss": // ------------------------------------------------------------------------------------------- // Set the gaussian pose: CPose3DPDFGaussian finalEstimatedPoseGauss( icpEstimation ); CPose3D noisy_increment; finalEstimatedPoseGauss.drawSingleSample(noisy_increment); CPose3D new_pose; new_pose.composeFrom(*part_pose,noisy_increment); CPose2D new_pose2d = CPose2D(new_pose); // Add the pose to the path: part.d->robotPoses[ LMH->m_currentRobotPose ] = new_pose; // Update the weight: // --------------------------------------------------------------------------- const double log_lik = PF_options.powFactor * auxiliarComputeObservationLikelihood( PF_options, LMH, i, sf, &new_pose2d); part.log_w += log_lik; // Add to historic record of log_w weights: LMH->m_log_w_metric_history[i][currentPoseID] += log_lik; } // end else we can do ICP } // end for each particle i // Accumulate the log likelihood of this LMH as a whole: double out_max_log_w; LMH->normalizeWeights( &out_max_log_w ); // Normalize weights: LMH->m_log_w += out_max_log_w; printf("[CLSLAM_RBPF_2DLASER] Overall likelihood = %.2e\n",out_max_log_w); printf("[CLSLAM_RBPF_2DLASER] Done in %.03fms\n",1e3*tictac.Tac()); MRPT_END }
// ------------------------------------------------------ // TestICP // ------------------------------------------------------ void TestICP() { CSimplePointsMap m1,m2; float runningTime; CICP::TReturnInfo info; CICP ICP; // Load scans: CObservation2DRangeScan scan1; scan1.aperture = M_PIf; scan1.rightToLeft = true; scan1.validRange.resize( SCANS_SIZE ); scan1.scan.resize(SCANS_SIZE); ASSERT_( sizeof(SCAN_RANGES_1) == sizeof(float)*SCANS_SIZE ); memcpy( &scan1.scan[0], SCAN_RANGES_1, sizeof(SCAN_RANGES_1) ); memcpy( &scan1.validRange[0], SCAN_VALID_1, sizeof(SCAN_VALID_1) ); CObservation2DRangeScan scan2 = scan1; memcpy( &scan2.scan[0], SCAN_RANGES_2, sizeof(SCAN_RANGES_2) ); memcpy( &scan2.validRange[0], SCAN_VALID_2, sizeof(SCAN_VALID_2) ); // Build the points maps from the scans: m1.insertObservation( &scan1 ); m2.insertObservation( &scan2 ); #if MRPT_HAS_PCL cout << "Saving map1.pcd and map2.pcd in PCL format...\n"; m1.savePCDFile("map1.pcd", false); m2.savePCDFile("map2.pcd", false); #endif // ----------------------------------------------------- // ICP.options.ICP_algorithm = icpLevenbergMarquardt; // ICP.options.ICP_algorithm = icpClassic; ICP.options.ICP_algorithm = (TICPAlgorithm)ICP_method; ICP.options.maxIterations = 100; ICP.options.thresholdAng = DEG2RAD(10.0f); ICP.options.thresholdDist = 0.75f; ICP.options.ALFA = 0.5f; ICP.options.smallestThresholdDist = 0.05f; ICP.options.doRANSAC = false; ICP.options.dumpToConsole(); // ----------------------------------------------------- CPose2D initialPose(0.8f,0.0f,(float)DEG2RAD(0.0f)); CPosePDFPtr pdf = ICP.Align( &m1, &m2, initialPose, &runningTime, (void*)&info); printf("ICP run in %.02fms, %d iterations (%.02fms/iter), %.01f%% goodness\n -> ", runningTime*1000, info.nIterations, runningTime*1000.0f/info.nIterations, info.goodness*100 ); cout << "Mean of estimation: " << pdf->getMeanVal() << endl<< endl; CPosePDFGaussian gPdf; gPdf.copyFrom(*pdf); cout << "Covariance of estimation: " << endl << gPdf.cov << endl; cout << " std(x): " << sqrt( gPdf.cov(0,0) ) << endl; cout << " std(y): " << sqrt( gPdf.cov(1,1) ) << endl; cout << " std(phi): " << RAD2DEG(sqrt( gPdf.cov(2,2) )) << " (deg)" << endl; //cout << "Covariance of estimation (MATLAB format): " << endl << gPdf.cov.inMatlabFormat() << endl; cout << "-> Saving reference map as scan1.txt" << endl; m1.save2D_to_text_file("scan1.txt"); cout << "-> Saving map to align as scan2.txt" << endl; m2.save2D_to_text_file("scan2.txt"); cout << "-> Saving transformed map to align as scan2_trans.txt" << endl; CSimplePointsMap m2_trans = m2; m2_trans.changeCoordinatesReference( gPdf.mean ); m2_trans.save2D_to_text_file("scan2_trans.txt"); cout << "-> Saving MATLAB script for drawing 2D ellipsoid as view_ellip.m" << endl; CMatrixFloat COV22 = CMatrixFloat( CMatrixDouble( gPdf.cov )); COV22.setSize(2,2); CVectorFloat MEAN2D(2); MEAN2D[0] = gPdf.mean.x(); MEAN2D[1] = gPdf.mean.y(); { ofstream f("view_ellip.m"); f << math::MATLAB_plotCovariance2D( COV22, MEAN2D, 3.0f); } // If we have 2D windows, use'em: #if MRPT_HAS_WXWIDGETS if (!skip_window) { gui::CDisplayWindowPlots win("ICP results"); // Reference map: vector<float> map1_xs, map1_ys, map1_zs; m1.getAllPoints(map1_xs,map1_ys,map1_zs); win.plot( map1_xs, map1_ys, "b.3", "map1"); // Translated map: vector<float> map2_xs, map2_ys, map2_zs; m2_trans.getAllPoints(map2_xs,map2_ys,map2_zs); win.plot( map2_xs, map2_ys, "r.3", "map2"); // Uncertainty win.plotEllipse(MEAN2D[0],MEAN2D[1],COV22,3.0,"b2", "cov"); win.axis(-1,10,-6,6); win.axis_equal(); cout << "Close the window to exit" << endl; win.waitForKey(); } #endif }