void exec_setPoseByLabel( mrpt::slam::CActionCollection *acts, mrpt::slam::CSensoryFrame *SF, int &changesCount ) { if (SF) { for (CSensoryFrame::iterator it= SF->begin();it!=SF->end();++it) { CObservationPtr obs = *it; if ( obs->sensorLabel == labelToProcess) { if (changeOnlyXYZ) { CPose3D tmpPose; obs->getSensorPose(tmpPose); tmpPose.setFromValues( sensorPoseToSet.x, sensorPoseToSet.y, sensorPoseToSet.z, tmpPose.yaw(), tmpPose.pitch(), tmpPose.roll() ); obs->setSensorPose( tmpPose ); } else { obs->setSensorPose( sensorPoseToSet ); } changesCount++; } } // end for each obs. } }
// ------------------------------------------------------------ // Set the sensor pose // ------------------------------------------------------------ void exec_setPoseByIdx( mrpt::slam::CActionCollection *acts, mrpt::slam::CSensoryFrame *SF, int &changesCount ) { if (SF) if (SF->size()>idxToProcess) { CObservationPtr obs = SF->getObservationByIndex(idxToProcess); if (changeOnlyXYZ) { CPose3D tmpPose; obs->getSensorPose(tmpPose); tmpPose.setFromValues( sensorPoseToSet.x, sensorPoseToSet.y, sensorPoseToSet.z, tmpPose.yaw(), tmpPose.pitch(), tmpPose.roll() ); obs->setSensorPose( tmpPose ); } else { obs->setSensorPose( sensorPoseToSet ); } changesCount++; } }
void test_default_values(const CPose3D &p, const std::string & label) { EXPECT_EQ(p.x(),0); EXPECT_EQ(p.y(),0); EXPECT_EQ(p.z(),0); EXPECT_EQ(p.yaw(),0); EXPECT_EQ(p.pitch(),0); EXPECT_EQ(p.roll(),0); for (size_t i=0;i<4;i++) for (size_t j=0;j<4;j++) EXPECT_NEAR(p.getHomogeneousMatrixVal()(i,j), i==j ? 1.0 : 0.0, 1e-8 ) << "Failed for (i,j)=" << i << "," << j << endl << "Matrix is: " << endl << p.getHomogeneousMatrixVal() << endl << "case was: " << label << endl; }
/*--------------------------------------------------------------- saveInterpolatedToTextFile ---------------------------------------------------------------*/ bool CPose3DInterpolator::saveInterpolatedToTextFile(const std::string &s, double period) const { try { CFileOutputStream f(s); if (m_path.empty()) return true; const TTimeStamp t_ini = m_path.begin()->first; const TTimeStamp t_end = m_path.rbegin()->first; TTimeStamp At = mrpt::system::secondsToTimestamp(period); //cout << "Interp: " << t_ini << " " << t_end << " " << At << endl; CPose3D p; bool valid; for (TTimeStamp t=t_ini;t<=t_end;t+=At) { this->interpolate( t, p, valid ); if (!valid) continue; int r = f.printf("%.06f %.06f %.06f %.06f %.06f %.06f %.06f\n", mrpt::system::timestampTotime_t(t), p.x(),p.y(),p.z(), p.yaw(),p.pitch(),p.roll()); ASSERT_(r>0); } return true; } catch(...) { return false; } }
CPose3DPDFPtr CICP::ICP3D_Method_Classic( const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *mm2, const CPose3DPDFGaussian &initialEstimationPDF, TReturnInfo &outInfo ) { MRPT_START // The result can be either a Gaussian or a SOG: CPose3DPDFPtr resultPDF; CPose3DPDFGaussianPtr gaussPdf; size_t nCorrespondences=0; bool keepApproaching; CPose3D grossEst = initialEstimationPDF.mean; mrpt::utils::TMatchingPairList correspondences,old_correspondences; CPose3D lastMeanPose; // Assure the class of the maps: ASSERT_(mm2->GetRuntimeClass()->derivedFrom(CLASS_ID(CPointsMap))); const CPointsMap *m2 = (CPointsMap*)mm2; // Asserts: // ----------------- ASSERT_( options.ALFA>0 && options.ALFA<1 ); // The algorithm output auxiliar info: // ------------------------------------------------- outInfo.nIterations = 0; outInfo.goodness = 1; outInfo.quality = 0; // The gaussian PDF to estimate: // ------------------------------------------------------ gaussPdf = CPose3DPDFGaussian::Create(); // First gross approximation: gaussPdf->mean = grossEst; // Initial thresholds: TMatchingParams matchParams; TMatchingExtraResults matchExtraResults; matchParams.maxDistForCorrespondence = options.thresholdDist; // Distance threshold matchParams.maxAngularDistForCorrespondence = options.thresholdAng; // Angular threshold matchParams.onlyKeepTheClosest = options.onlyClosestCorrespondences; matchParams.onlyUniqueRobust = options.onlyUniqueRobust; matchParams.decimation_other_map_points = options.corresponding_points_decimation; // Asure maps are not empty! // ------------------------------------------------------ if ( !m2->isEmpty() ) { matchParams.offset_other_map_points = 0; // ------------------------------------------------------ // The ICP loop // ------------------------------------------------------ do { matchParams.angularDistPivotPoint = TPoint3D(gaussPdf->mean.x(),gaussPdf->mean.y(),gaussPdf->mean.z()); // ------------------------------------------------------ // Find the matching (for a points map) // ------------------------------------------------------ m1->determineMatching3D( m2, // The other map gaussPdf->mean, // The other map pose correspondences, matchParams, matchExtraResults); nCorrespondences = correspondences.size(); if ( !nCorrespondences ) { // Nothing we can do !! keepApproaching = false; } else { // Compute the estimated pose, using Horn's method. // ---------------------------------------------------------------------- mrpt::poses::CPose3DQuat estPoseQuat; double transf_scale; mrpt::tfest::se3_l2(correspondences, estPoseQuat, transf_scale, false /* dont force unit scale */ ); gaussPdf->mean = estPoseQuat; // If matching has not changed, decrease the thresholds: // -------------------------------------------------------- keepApproaching = true; if (!(fabs(lastMeanPose.x()-gaussPdf->mean.x())>options.minAbsStep_trans || fabs(lastMeanPose.y()-gaussPdf->mean.y())>options.minAbsStep_trans || fabs(lastMeanPose.z()-gaussPdf->mean.z())>options.minAbsStep_trans || fabs(math::wrapToPi(lastMeanPose.yaw()-gaussPdf->mean.yaw()))>options.minAbsStep_rot || fabs(math::wrapToPi(lastMeanPose.pitch()-gaussPdf->mean.pitch()))>options.minAbsStep_rot || fabs(math::wrapToPi(lastMeanPose.roll()-gaussPdf->mean.roll()))>options.minAbsStep_rot )) { matchParams.maxDistForCorrespondence *= options.ALFA; matchParams.maxAngularDistForCorrespondence *= options.ALFA; if (matchParams.maxDistForCorrespondence < options.smallestThresholdDist ) keepApproaching = false; if (++matchParams.offset_other_map_points>=options.corresponding_points_decimation) matchParams.offset_other_map_points=0; } lastMeanPose = gaussPdf->mean; } // end of "else, there are correspondences" // Next iteration: outInfo.nIterations++; if (outInfo.nIterations >= options.maxIterations && matchParams.maxDistForCorrespondence>options.smallestThresholdDist) { matchParams.maxDistForCorrespondence *= options.ALFA; } } while ( (keepApproaching && outInfo.nIterations<options.maxIterations) || (outInfo.nIterations >= options.maxIterations && matchParams.maxDistForCorrespondence>options.smallestThresholdDist) ); // ------------------------------------------------- // Obtain the covariance matrix of the estimation // ------------------------------------------------- if (!options.skip_cov_calculation && nCorrespondences) { // ... } // outInfo.goodness = matchExtraResults.correspondencesRatio; } // end of "if m2 is not empty" // Return the gaussian distribution: resultPDF = gaussPdf; return resultPDF; MRPT_END }
int main(int argc, char ** argv) { try { bool showHelp = argc>1 && !os::_strcmp(argv[1],"--help"); bool showVersion = argc>1 && !os::_strcmp(argv[1],"--version"); printf(" simul-landmarks - Part of the MRPT\n"); printf(" MRPT C++ Library: %s - BUILD DATE %s\n", MRPT_getVersion().c_str(), MRPT_getCompilationDate().c_str()); if (showVersion) return 0; // Program end // Process arguments: if (argc<2 || showHelp ) { printf("Usage: %s <config_file.ini>\n\n",argv[0]); if (!showHelp) { mrpt::system::pause(); return -1; } else return 0; } string INI_FILENAME = std::string( argv[1] ); ASSERT_FILE_EXISTS_(INI_FILENAME) CConfigFile ini( INI_FILENAME ); const int random_seed = ini.read_int("Params","random_seed",0); if (random_seed!=0) randomGenerator.randomize(random_seed); else randomGenerator.randomize(); // Set default values: unsigned int nLandmarks = 3; unsigned int nSteps = 100; std::string outFile("out.rawlog"); std::string outDir("OUT"); float min_x =-5; float max_x =5; float min_y =-5; float max_y =5; float min_z =0; float max_z =0; float odometryNoiseXY_std = 0.001f; float odometryNoisePhi_std_deg = 0.01f; float odometryNoisePitch_std_deg = 0.01f; float odometryNoiseRoll_std_deg = 0.01f; float minSensorDistance = 0; float maxSensorDistance = 10; float fieldOfView_deg= 180.0f; float sensorPose_x = 0; float sensorPose_y = 0; float sensorPose_z = 0; float sensorPose_yaw_deg = 0; float sensorPose_pitch_deg = 0; float sensorPose_roll_deg = 0; float stdRange = 0.01f; float stdYaw_deg = 0.1f; float stdPitch_deg = 0.1f; bool sensorDetectsIDs=true; bool circularPath = true; bool random6DPath = false; size_t squarePathLength=40; // Load params from INI: MRPT_LOAD_CONFIG_VAR(outFile,string, ini,"Params"); MRPT_LOAD_CONFIG_VAR(outDir,string, ini,"Params"); MRPT_LOAD_CONFIG_VAR(nSteps,int, ini,"Params"); MRPT_LOAD_CONFIG_VAR(circularPath,bool, ini,"Params"); MRPT_LOAD_CONFIG_VAR(random6DPath,bool, ini,"Params"); MRPT_LOAD_CONFIG_VAR(squarePathLength,int,ini,"Params"); MRPT_LOAD_CONFIG_VAR(sensorDetectsIDs,bool, ini,"Params"); MRPT_LOAD_CONFIG_VAR(odometryNoiseXY_std,float, ini,"Params"); MRPT_LOAD_CONFIG_VAR(odometryNoisePhi_std_deg,float, ini,"Params"); MRPT_LOAD_CONFIG_VAR(odometryNoisePitch_std_deg,float, ini,"Params"); MRPT_LOAD_CONFIG_VAR(odometryNoiseRoll_std_deg,float, ini,"Params"); MRPT_LOAD_CONFIG_VAR(sensorPose_x,float, ini,"Params"); MRPT_LOAD_CONFIG_VAR(sensorPose_y,float, ini,"Params"); MRPT_LOAD_CONFIG_VAR(sensorPose_z,float, ini,"Params"); MRPT_LOAD_CONFIG_VAR(sensorPose_yaw_deg,float, ini,"Params"); MRPT_LOAD_CONFIG_VAR(sensorPose_pitch_deg,float, ini,"Params"); MRPT_LOAD_CONFIG_VAR(sensorPose_roll_deg,float, ini,"Params"); MRPT_LOAD_CONFIG_VAR(minSensorDistance,float, ini,"Params"); MRPT_LOAD_CONFIG_VAR(maxSensorDistance,float, ini,"Params"); MRPT_LOAD_CONFIG_VAR(fieldOfView_deg,float, ini,"Params"); bool show_in_3d = false; MRPT_LOAD_CONFIG_VAR(show_in_3d,bool, ini,"Params"); MRPT_LOAD_CONFIG_VAR(stdRange,float, ini,"Params"); MRPT_LOAD_CONFIG_VAR(stdYaw_deg,float, ini,"Params"); MRPT_LOAD_CONFIG_VAR(stdPitch_deg,float, ini,"Params"); float stdYaw = DEG2RAD(stdYaw_deg); float stdPitch = DEG2RAD(stdPitch_deg); float odometryNoisePhi_std = DEG2RAD(odometryNoisePhi_std_deg); float odometryNoisePitch_std = DEG2RAD(odometryNoisePitch_std_deg); float odometryNoiseRoll_std = DEG2RAD(odometryNoiseRoll_std_deg); float fieldOfView = DEG2RAD(fieldOfView_deg); CPose3D sensorPoseOnRobot( sensorPose_x, sensorPose_y, sensorPose_z, DEG2RAD( sensorPose_yaw_deg ), DEG2RAD( sensorPose_pitch_deg ), DEG2RAD( sensorPose_roll_deg ) ); // Create out dir: mrpt::system::createDirectory(outDir); // --------------------------------------------- // Create the point-beacons: // --------------------------------------------- printf("Creating landmark map..."); mrpt::maps::CLandmarksMap landmarkMap; int randomSetCount = 0; int uniqueIds = 1; // Process each of the "RANDOMSET_%i" found: do { string sectName = format("RANDOMSET_%i",++randomSetCount); nLandmarks = 0; MRPT_LOAD_CONFIG_VAR(nLandmarks,uint64_t, ini,sectName); MRPT_LOAD_CONFIG_VAR(min_x,float, ini,sectName); MRPT_LOAD_CONFIG_VAR(max_x,float, ini,sectName); MRPT_LOAD_CONFIG_VAR(min_y,float, ini,sectName); MRPT_LOAD_CONFIG_VAR(max_y,float, ini,sectName); MRPT_LOAD_CONFIG_VAR(min_z,float, ini,sectName); MRPT_LOAD_CONFIG_VAR(max_z,float, ini,sectName); for (size_t i=0; i<nLandmarks; i++) { CLandmark LM; CPointPDFGaussian pt3D; // Random coordinates: pt3D.mean = CPoint3D( randomGenerator.drawUniform(min_x,max_x), randomGenerator.drawUniform(min_y,max_y), randomGenerator.drawUniform(min_z,max_z) ); // Add: LM.createOneFeature(); LM.features[0]->type = featBeacon; LM.ID = uniqueIds++; LM.setPose( pt3D ); landmarkMap.landmarks.push_back(LM); } if (nLandmarks) cout << nLandmarks << " generated for the 'randomset' " << randomSetCount << endl; } while (nLandmarks); landmarkMap.saveToTextFile( format("%s/%s_ground_truth.txt",outDir.c_str(),outFile.c_str()) ); printf("Done!\n"); // --------------------------------------------- // Simulate: // --------------------------------------------- size_t nWarningsNoSight=0; CActionRobotMovement2D::TMotionModelOptions opts; opts.modelSelection = CActionRobotMovement2D::mmGaussian; opts.gausianModel.a1=0; opts.gausianModel.a2=0; opts.gausianModel.a3=0; opts.gausianModel.a4=0; opts.gausianModel.minStdXY = odometryNoiseXY_std; opts.gausianModel.minStdPHI = odometryNoisePhi_std; // Output rawlog, gz-compressed. CFileGZOutputStream fil( format("%s/%s",outDir.c_str(),outFile.c_str())); CPose3D realPose; const size_t N_STEPS_STOP_AT_THE_BEGINNING = 4; CMatrixDouble GT_path; for (size_t i=0; i<nSteps; i++) { cout << "Generating step " << i << "...\n"; CSensoryFrame SF; CActionCollection acts; CPose3D incPose3D; bool incPose_is_3D = random6DPath; if (i>=N_STEPS_STOP_AT_THE_BEGINNING) { if (random6DPath) { // 3D path const double Ar = DEG2RAD(3); TPose3D Ap = TPose3D(0.20*cos(Ar),0.20*sin(Ar),0,Ar,0,0); //Ap.z += randomGenerator.drawGaussian1D(0,0.05); Ap.yaw += randomGenerator.drawGaussian1D(0,DEG2RAD(0.2)); Ap.pitch += randomGenerator.drawGaussian1D(0,DEG2RAD(2)); Ap.roll += randomGenerator.drawGaussian1D(0,DEG2RAD(4)); incPose3D = CPose3D(Ap); } else { // 2D path: if (circularPath) { // Circular path: float Ar = DEG2RAD(5); incPose3D = CPose3D(CPose2D(0.20f*cos(Ar),0.20f*sin(Ar),Ar)); } else { // Square path: if ( (i % squarePathLength) > (squarePathLength-5) ) incPose3D = CPose3D(CPose2D(0,0,DEG2RAD(90.0f/4))); else incPose3D = CPose3D(CPose2D(0.20f,0,0)); } } } else { // Robot is still at the beginning: incPose3D = CPose3D(0,0,0,0,0,0); } // Simulate observations: CObservationBearingRangePtr obs=CObservationBearingRange::Create(); obs->minSensorDistance=minSensorDistance; obs->maxSensorDistance=maxSensorDistance; obs->fieldOfView_yaw = fieldOfView; obs->fieldOfView_pitch = fieldOfView; obs->sensorLocationOnRobot = sensorPoseOnRobot; landmarkMap.simulateRangeBearingReadings( realPose, sensorPoseOnRobot, *obs, sensorDetectsIDs, // wheter to identy landmarks stdRange, stdYaw, stdPitch ); // Keep the GT of the robot pose: GT_path.setSize(i+1,6); for (size_t k=0; k<6; k++) GT_path(i,k)=realPose[k]; cout << obs->sensedData.size() << " landmarks in sight"; if (obs->sensedData.empty()) nWarningsNoSight++; SF.push_back( obs ); // Simulate odometry, from "incPose3D" with noise: if (!incPose_is_3D) { // 2D odometry: CActionRobotMovement2D act; CPose2D incOdo( incPose3D ); if (incPose3D.x()!=0 || incPose3D.y()!=0 || incPose3D.yaw()!=0) { incOdo.x_incr( randomGenerator.drawGaussian1D_normalized() * odometryNoiseXY_std ); incOdo.y_incr( randomGenerator.drawGaussian1D_normalized() * odometryNoiseXY_std ); incOdo.phi_incr( randomGenerator.drawGaussian1D_normalized() * odometryNoisePhi_std ); } act.computeFromOdometry(incOdo, opts); acts.insert( act ); } else { // 3D odometry: CActionRobotMovement3D act; act.estimationMethod = CActionRobotMovement3D::emOdometry; CPose3D noisyIncPose = incPose3D; if (incPose3D.x()!=0 || incPose3D.y()!=0 || incPose3D.yaw()!=0) { noisyIncPose.x_incr( randomGenerator.drawGaussian1D_normalized() * odometryNoiseXY_std ); noisyIncPose.y_incr( randomGenerator.drawGaussian1D_normalized() * odometryNoiseXY_std ); noisyIncPose.z_incr( randomGenerator.drawGaussian1D_normalized() * odometryNoiseXY_std ); noisyIncPose.setYawPitchRoll( noisyIncPose.yaw()+ randomGenerator.drawGaussian1D_normalized() * odometryNoisePhi_std, noisyIncPose.pitch()+ randomGenerator.drawGaussian1D_normalized() * odometryNoisePitch_std, noisyIncPose.roll()+ randomGenerator.drawGaussian1D_normalized() * odometryNoiseRoll_std ); } act.poseChange.mean = noisyIncPose; act.poseChange.cov.eye(); act.poseChange.cov(0,0) = act.poseChange.cov(1,1) = act.poseChange.cov(2,2) = square(odometryNoiseXY_std); act.poseChange.cov(3,3) = square(odometryNoisePhi_std); act.poseChange.cov(4,4) = square(odometryNoisePitch_std); act.poseChange.cov(5,5) = square(odometryNoiseRoll_std); acts.insert( act ); } // Save: fil << SF << acts; // Next pose: realPose = realPose + incPose3D; cout << endl; } // Save the ground truth for the robot poses as well: GT_path.saveToTextFile( format("%s/%s_ground_truth_robot_path.txt",outDir.c_str(),outFile.c_str()), MATRIX_FORMAT_FIXED, true, "% Columns are: x(m) y(m) z(m) yaw(rad) pitch(rad) roll(rad)\n"); cout << "Data saved to directory: " << outDir << endl; if (nWarningsNoSight) cout << "WARNING: " << nWarningsNoSight << " observations contained zero landmarks in the sensor range." << endl; // Optionally, display in 3D: if (show_in_3d && size(GT_path,1)>1) { #if MRPT_HAS_OPENGL_GLUT && MRPT_HAS_WXWIDGETS mrpt::gui::CDisplayWindow3D win("Final simulation",400,300); mrpt::opengl::COpenGLScenePtr &scene = win.get3DSceneAndLock(); scene->insert( mrpt::opengl::CGridPlaneXY::Create( min_x-10,max_x+10,min_y-10,max_y+10,0 )); scene->insert( mrpt::opengl::stock_objects::CornerXYZ() ); // Insert all landmarks: for (CLandmarksMap::TCustomSequenceLandmarks::const_iterator it=landmarkMap.landmarks.begin(); it!=landmarkMap.landmarks.end(); ++it) { mrpt::opengl::CSpherePtr lm = mrpt::opengl::CSphere::Create(); lm->setColor(1,0,0); lm->setRadius(0.1); lm->setLocation( it->pose_mean ); lm->setName( format("LM#%u",(unsigned) it->ID ) ); //lm->enableShowName(true); scene->insert(lm); } // Insert all robot poses: const size_t N = size(GT_path,1); mrpt::opengl::CSetOfLinesPtr pathLines = mrpt::opengl::CSetOfLines::Create(); pathLines->setColor(0,0,1,0.5); pathLines->setLineWidth(3.0); pathLines->resize(N-1); for (size_t i=0; i<N-1; i++) pathLines->setLineByIndex(i, GT_path(i,0),GT_path(i,1),GT_path(i,2), GT_path(i+1,0),GT_path(i+1,1),GT_path(i+1,2) ); scene->insert(pathLines); for (size_t i=0; i<N; i++) { mrpt::opengl::CSetOfObjectsPtr corner = mrpt::opengl::stock_objects::CornerXYZ(); corner->setScale(0.2); corner->setPose(TPose3D(GT_path(i,0),GT_path(i,1),GT_path(i,2),GT_path(i,3),GT_path(i,4),GT_path(i,5))); scene->insert(corner); } win.unlockAccess3DScene(); win.forceRepaint(); cout << "Press any key or close the 3D window to exit." << endl; win.waitForKey(); #endif // MRPT_HAS_OPENGL_GLUT && MRPT_HAS_WXWIDGETS } } catch (std::exception &e) { std::cout << e.what(); } catch (...) { std::cout << "Untyped exception!"; } }
/*--------------------------------------------------------------- getCovarianceAndMean ---------------------------------------------------------------*/ void CPose3DPDFParticles::getCovarianceAndMean( CMatrixDouble66& cov, CPose3D& mean) const { MRPT_START getMean(mean); // First! the mean value: // Now the covariance: cov.zeros(); CVectorDouble vars; vars.assign(6, 0.0); // The diagonal of the final covariance matrix // Elements off the diagonal of the covariance matrix: double std_xy = 0, std_xz = 0, std_xya = 0, std_xp = 0, std_xr = 0; double std_yz = 0, std_yya = 0, std_yp = 0, std_yr = 0; double std_zya = 0, std_zp = 0, std_zr = 0; double std_yap = 0, std_yar = 0; double std_pr = 0; // Mean values in [0, 2pi] range: double mean_yaw = mean.yaw(); double mean_pitch = mean.pitch(); double mean_roll = mean.roll(); if (mean_yaw < 0) mean_yaw += M_2PI; if (mean_pitch < 0) mean_pitch += M_2PI; if (mean_roll < 0) mean_roll += M_2PI; // Enought information to estimate the covariance? if (m_particles.size() < 2) return; // Sum all weight values: double W = 0; for (CPose3DPDFParticles::CParticleList::const_iterator it = m_particles.begin(); it != m_particles.end(); ++it) W += exp(it->log_w); ASSERT_(W > 0); // Compute covariance: for (CPose3DPDFParticles::CParticleList::const_iterator it = m_particles.begin(); it != m_particles.end(); ++it) { double w = exp(it->log_w) / W; // Manage 1 PI range: double err_yaw = wrapToPi(fabs(it->d->yaw() - mean_yaw)); double err_pitch = wrapToPi(fabs(it->d->pitch() - mean_pitch)); double err_roll = wrapToPi(fabs(it->d->roll() - mean_roll)); double err_x = it->d->x() - mean.x(); double err_y = it->d->y() - mean.y(); double err_z = it->d->z() - mean.z(); vars[0] += square(err_x) * w; vars[1] += square(err_y) * w; vars[2] += square(err_z) * w; vars[3] += square(err_yaw) * w; vars[4] += square(err_pitch) * w; vars[5] += square(err_roll) * w; std_xy += err_x * err_y * w; std_xz += err_x * err_z * w; std_xya += err_x * err_yaw * w; std_xp += err_x * err_pitch * w; std_xr += err_x * err_roll * w; std_yz += err_y * err_z * w; std_yya += err_y * err_yaw * w; std_yp += err_y * err_pitch * w; std_yr += err_y * err_roll * w; std_zya += err_z * err_yaw * w; std_zp += err_z * err_pitch * w; std_zr += err_z * err_roll * w; std_yap += err_yaw * err_pitch * w; std_yar += err_yaw * err_roll * w; std_pr += err_pitch * err_roll * w; } // end for it // Unbiased estimation of variance: cov(0, 0) = vars[0]; cov(1, 1) = vars[1]; cov(2, 2) = vars[2]; cov(3, 3) = vars[3]; cov(4, 4) = vars[4]; cov(5, 5) = vars[5]; cov(1, 0) = cov(0, 1) = std_xy; cov(2, 0) = cov(0, 2) = std_xz; cov(3, 0) = cov(0, 3) = std_xya; cov(4, 0) = cov(0, 4) = std_xp; cov(5, 0) = cov(0, 5) = std_xr; cov(2, 1) = cov(1, 2) = std_yz; cov(3, 1) = cov(1, 3) = std_yya; cov(4, 1) = cov(1, 4) = std_yp; cov(5, 1) = cov(1, 5) = std_yr; cov(3, 2) = cov(2, 3) = std_zya; cov(4, 2) = cov(2, 4) = std_zp; cov(5, 2) = cov(2, 5) = std_zr; cov(4, 3) = cov(3, 4) = std_yap; cov(5, 3) = cov(3, 5) = std_yar; cov(5, 4) = cov(4, 5) = std_pr; MRPT_END }
/*--------------------------------------------------------------- pose3D = point3D + pose3D ---------------------------------------------------------------*/ CPose3D CPoint3D::operator+(const CPose3D& b) const { return CPose3D( m_coords[0] + b.x(), m_coords[1] + b.y(), m_coords[2] + b.z(), b.yaw(), b.pitch(), b.roll()); }
// ------------------------------------------------------ // Test_Kinect // ------------------------------------------------------ void Test_Kinect() { // Launch grabbing thread: // -------------------------------------------------------- TThreadParam thrPar; std::thread thHandle = std::thread(thread_grabbing, std::ref(thrPar)); // Wait until data stream starts so we can say for sure the sensor has been // initialized OK: cout << "Waiting for sensor initialization...\n"; do { CObservation3DRangeScan::Ptr possiblyNewObs = std::atomic_load(&thrPar.new_obs); if (possiblyNewObs && possiblyNewObs->timestamp != INVALID_TIMESTAMP) break; else std::this_thread::sleep_for(10ms); } while (!thrPar.quit); // Check error condition: if (thrPar.quit) return; // Feature tracking variables: CFeatureList trackedFeats; unsigned int step_num = 0; bool SHOW_FEAT_IDS = true; bool SHOW_RESPONSES = true; CGenericFeatureTrackerAutoPtr tracker; // "CFeatureTracker_KL" is by far the most robust implementation for now: tracker = CGenericFeatureTrackerAutoPtr(new CFeatureTracker_KL); tracker->enableTimeLogger(true); // Do time profiling. // Set of parameters common to any tracker implementation: // To see all the existing params and documentation, see // mrpt::vision::CGenericFeatureTracker // http://reference.mrpt.org/devel/structmrpt_1_1vision_1_1_c_generic_feature_tracker.html tracker->extra_params["add_new_features"] = 1; // track, AND ALSO, add new features tracker->extra_params["add_new_feat_min_separation"] = 25; tracker->extra_params["add_new_feat_max_features"] = 150; tracker->extra_params["add_new_feat_patch_size"] = 21; tracker->extra_params["minimum_KLT_response_to_add"] = 40; tracker->extra_params["check_KLT_response_every"] = 5; // Re-check the KLT-response to assure features are in good points. tracker->extra_params["minimum_KLT_response"] = 25; // Re-check the KLT-response to assure features are in good points. tracker->extra_params["update_patches_every"] = 0; // Update patches // Specific params for "CFeatureTracker_KL" tracker->extra_params["window_width"] = 25; tracker->extra_params["window_height"] = 25; // Global points map: CColouredPointsMap globalPtsMap; globalPtsMap.colorScheme.scheme = CColouredPointsMap::cmFromIntensityImage; // Take points color from // RGB+D observations // globalPtsMap.colorScheme.scheme = // CColouredPointsMap::cmFromHeightRelativeToSensorGray; // Create window and prepare OpenGL object in the scene: // -------------------------------------------------------- mrpt::gui::CDisplayWindow3D win3D("kinect-3d-slam 3D view", 800, 600); win3D.setCameraAzimuthDeg(140); win3D.setCameraElevationDeg(20); win3D.setCameraZoom(8.0); win3D.setFOV(90); win3D.setCameraPointingToPoint(2.5, 0, 0); mrpt::opengl::CPointCloudColoured::Ptr gl_points = mrpt::make_aligned_shared<mrpt::opengl::CPointCloudColoured>(); gl_points->setPointSize(2.5); mrpt::opengl::CSetOfObjects::Ptr gl_curFeats = mrpt::make_aligned_shared<mrpt::opengl::CSetOfObjects>(); mrpt::opengl::CSetOfObjects::Ptr gl_keyframes = mrpt::make_aligned_shared<mrpt::opengl::CSetOfObjects>(); mrpt::opengl::CPointCloudColoured::Ptr gl_points_map = mrpt::make_aligned_shared<mrpt::opengl::CPointCloudColoured>(); gl_points_map->setPointSize(2.0); const double aspect_ratio = 480.0 / 640.0; // kinect.rows() / double( kinect.cols() ); mrpt::opengl::CSetOfObjects::Ptr gl_cur_cam_corner = mrpt::opengl::stock_objects::CornerXYZSimple(0.4f, 4); opengl::COpenGLViewport::Ptr viewInt; { mrpt::opengl::COpenGLScene::Ptr& scene = win3D.get3DSceneAndLock(); // Create the Opengl object for the point cloud: scene->insert(gl_points_map); scene->insert(gl_points); scene->insert(gl_curFeats); scene->insert(gl_keyframes); scene->insert(mrpt::make_aligned_shared<mrpt::opengl::CGridPlaneXY>()); scene->insert(gl_cur_cam_corner); const int VW_WIDTH = 350; // Size of the viewport into the window, in pixel units. const int VW_HEIGHT = aspect_ratio * VW_WIDTH; // Create the Opengl objects for the planar images each in a separate // viewport: viewInt = scene->createViewport("view2d_int"); viewInt->setViewportPosition(2, 2, VW_WIDTH, VW_HEIGHT); viewInt->setTransparent(true); win3D.unlockAccess3DScene(); win3D.repaint(); } CImage previous_image; map<TFeatureID, TPoint3D> lastVisibleFeats; std::vector<TPose3D> camera_key_frames_path; // The 6D path of the Kinect camera. CPose3D currentCamPose_wrt_last; // wrt last pose in "camera_key_frames_path" bool gl_keyframes_must_refresh = true; // Need to update gl_keyframes from camera_key_frames_path?? CObservation3DRangeScan::Ptr last_obs; string str_status, str_status2; while (win3D.isOpen() && !thrPar.quit) { CObservation3DRangeScan::Ptr possiblyNewObs = std::atomic_load(&thrPar.new_obs); if (possiblyNewObs && possiblyNewObs->timestamp != INVALID_TIMESTAMP && (!last_obs || possiblyNewObs->timestamp != last_obs->timestamp)) { // It IS a new observation: last_obs = possiblyNewObs; // Feature tracking ------------------------------------------- ASSERT_(last_obs->hasIntensityImage); CImage theImg; // The grabbed image: theImg = last_obs->intensityImage; // Do tracking: if (step_num > 1) // we need "previous_image" to be valid. { tracker->trackFeatures(previous_image, theImg, trackedFeats); // Remove those now out of the image plane: CFeatureList::iterator itFeat = trackedFeats.begin(); while (itFeat != trackedFeats.end()) { const TFeatureTrackStatus status = (*itFeat)->track_status; bool eras = (status_TRACKED != status && status_IDLE != status); if (!eras) { // Also, check if it's too close to the image border: const float x = (*itFeat)->x; const float y = (*itFeat)->y; static const float MIN_DIST_MARGIN_TO_STOP_TRACKING = 10; if (x < MIN_DIST_MARGIN_TO_STOP_TRACKING || y < MIN_DIST_MARGIN_TO_STOP_TRACKING || x > (last_obs->cameraParamsIntensity.ncols - MIN_DIST_MARGIN_TO_STOP_TRACKING) || y > (last_obs->cameraParamsIntensity.nrows - MIN_DIST_MARGIN_TO_STOP_TRACKING)) { eras = true; } } if (eras) // Erase or keep? itFeat = trackedFeats.erase(itFeat); else ++itFeat; } } // Create list of 3D features in space, wrt current camera pose: // -------------------------------------------------------------------- map<TFeatureID, TPoint3D> curVisibleFeats; for (CFeatureList::iterator itFeat = trackedFeats.begin(); itFeat != trackedFeats.end(); ++itFeat) { // Pixel coordinates in the intensity image: const int int_x = (*itFeat)->x; const int int_y = (*itFeat)->y; // Convert to pixel coords in the range image: // APPROXIMATION: Assume coordinates are equal (that's not // exact!!) const int x = int_x; const int y = int_y; // Does this (x,y) have valid range data? const float d = last_obs->rangeImage(y, x); if (d > 0.05 && d < 10.0) { ASSERT_( size_t( last_obs->rangeImage.cols() * last_obs->rangeImage.rows()) == last_obs->points3D_x.size()); const size_t nPt = last_obs->rangeImage.cols() * y + x; curVisibleFeats[(*itFeat)->ID] = TPoint3D( last_obs->points3D_x[nPt], last_obs->points3D_y[nPt], last_obs->points3D_z[nPt]); } } // Load local points map from 3D points + color: CColouredPointsMap localPntsMap; localPntsMap.colorScheme.scheme = CColouredPointsMap::cmFromIntensityImage; localPntsMap.loadFromRangeScan(*last_obs); // Estimate our current camera pose from feature2feature matching: // -------------------------------------------------------------------- if (!lastVisibleFeats.empty()) { TMatchingPairList corrs; // pairs of correspondences for (map<TFeatureID, TPoint3D>::const_iterator itCur = curVisibleFeats.begin(); itCur != curVisibleFeats.end(); ++itCur) { map<TFeatureID, TPoint3D>::const_iterator itFound = lastVisibleFeats.find(itCur->first); if (itFound != lastVisibleFeats.end()) { corrs.push_back( TMatchingPair( itFound->first, itCur->first, itFound->second.x, itFound->second.y, itFound->second.z, itCur->second.x, itCur->second.y, itCur->second.z)); } } if (corrs.size() >= 3) { // Find matchings: mrpt::tfest::TSE3RobustParams params; params.ransac_minSetSize = 3; params.ransac_maxSetSizePct = 6.0 / corrs.size(); mrpt::tfest::TSE3RobustResult results; bool register_ok = false; try { mrpt::tfest::se3_l2_robust(corrs, params, results); register_ok = true; } catch (std::exception&) { /* Cannot find a minimum number of matches, inconsistent * parameters due to very reduced numberof matches,etc. */ } const CPose3D relativePose = CPose3D(results.transformation); str_status = mrpt::format( "%d corrs | inliers: %d | rel.pose: %s ", int(corrs.size()), int(results.inliers_idx.size()), relativePose.asString().c_str()); str_status2 = string( results.inliers_idx.size() == 0 ? "LOST! Please, press 'r' to restart" : ""); if (register_ok && std::abs(results.scale - 1.0) < 0.1) { // Seems a good match: if ((relativePose.norm() > KEYFRAMES_MIN_DISTANCE || std::abs(relativePose.yaw()) > KEYFRAMES_MIN_ANG || std::abs(relativePose.pitch()) > KEYFRAMES_MIN_ANG || std::abs(relativePose.roll()) > KEYFRAMES_MIN_ANG)) { // Accept this as a new key-frame pose ------------ // Append new global pose of this key-frame: const CPose3D new_keyframe_global = CPose3D(*camera_key_frames_path.rbegin()) + relativePose; camera_key_frames_path.push_back( new_keyframe_global.asTPose()); gl_keyframes_must_refresh = true; currentCamPose_wrt_last = CPose3D(); // It's (0,0,0) since the last // key-frame is the current pose! lastVisibleFeats = curVisibleFeats; cout << "Adding new key-frame: pose=" << new_keyframe_global << endl; // Update global map: append another map at a given // position: globalPtsMap.insertObservation( last_obs.get(), &new_keyframe_global); win3D.get3DSceneAndLock(); gl_points_map->loadFromPointsMap(&globalPtsMap); win3D.unlockAccess3DScene(); } else { currentCamPose_wrt_last = relativePose; // cout << "cur pose: " << currentCamPose_wrt_last // << endl; } } } } if (camera_key_frames_path.empty() || lastVisibleFeats.empty()) { // First iteration: camera_key_frames_path.clear(); camera_key_frames_path.push_back(TPose3D(0, 0, 0, 0, 0, 0)); gl_keyframes_must_refresh = true; lastVisibleFeats = curVisibleFeats; // Update global map: globalPtsMap.clear(); globalPtsMap.insertObservation(last_obs.get()); win3D.get3DSceneAndLock(); gl_points_map->loadFromPointsMap(&globalPtsMap); win3D.unlockAccess3DScene(); } // Save the image for the next step: previous_image = theImg; // Draw marks on the RGB image: theImg.selectTextFont("10x20"); { // Tracked feats: theImg.drawFeatures( trackedFeats, TColor(0, 0, 255), SHOW_FEAT_IDS, SHOW_RESPONSES); theImg.textOut( 3, 22, format("# feats: %u", (unsigned int)trackedFeats.size()), TColor(200, 20, 20)); } // Update visualization --------------------------------------- // Show intensity image win3D.get3DSceneAndLock(); viewInt->setImageView(theImg); win3D.unlockAccess3DScene(); // Show 3D points & current visible feats, at the current camera 3D // pose "currentCamPose_wrt_last" // --------------------------------------------------------------------- if (last_obs->hasPoints3D) { const CPose3D curGlobalPose = CPose3D(*camera_key_frames_path.rbegin()) + currentCamPose_wrt_last; win3D.get3DSceneAndLock(); // All 3D points: gl_points->loadFromPointsMap(&localPntsMap); gl_points->setPose(curGlobalPose); gl_cur_cam_corner->setPose(curGlobalPose); // Current visual landmarks: gl_curFeats->clear(); for (map<TFeatureID, TPoint3D>::const_iterator it = curVisibleFeats.begin(); it != curVisibleFeats.end(); ++it) { static double D = 0.02; mrpt::opengl::CBox::Ptr box = mrpt::make_aligned_shared<mrpt::opengl::CBox>( TPoint3D(-D, -D, -D), TPoint3D(D, D, D)); box->setWireframe(true); box->setName(format("%d", int(it->first))); box->enableShowName(true); box->setLocation(it->second); gl_curFeats->insert(box); } gl_curFeats->setPose(curGlobalPose); win3D.unlockAccess3DScene(); win3D.repaint(); } win3D.get3DSceneAndLock(); win3D.addTextMessage( -100, -20, format("%.02f Hz", thrPar.Hz), TColorf(0, 1, 1), 100, MRPT_GLUT_BITMAP_HELVETICA_18); win3D.unlockAccess3DScene(); win3D.repaint(); step_num++; } // end update visualization: if (gl_keyframes_must_refresh) { gl_keyframes_must_refresh = false; // cout << "Updating gl_keyframes with " << // camera_key_frames_path.size() << " frames.\n"; win3D.get3DSceneAndLock(); gl_keyframes->clear(); for (size_t i = 0; i < camera_key_frames_path.size(); i++) { CSetOfObjects::Ptr obj = mrpt::opengl::stock_objects::CornerXYZSimple(0.3f, 3); obj->setPose(camera_key_frames_path[i]); gl_keyframes->insert(obj); } win3D.unlockAccess3DScene(); } // Process possible keyboard commands: // -------------------------------------- if (win3D.keyHit() && thrPar.pushed_key == 0) { const int key = tolower(win3D.getPushedKey()); switch (key) { // Some of the keys are processed in this thread: case 'r': lastVisibleFeats.clear(); camera_key_frames_path.clear(); gl_keyframes_must_refresh = true; globalPtsMap.clear(); win3D.get3DSceneAndLock(); gl_points_map->loadFromPointsMap(&globalPtsMap); win3D.unlockAccess3DScene(); break; case 's': { const std::string s = "point_cloud.txt"; cout << "Dumping 3D point-cloud to: " << s << endl; globalPtsMap.save3D_to_text_file(s); break; } case 'o': win3D.setCameraZoom(win3D.getCameraZoom() * 1.2); win3D.repaint(); break; case 'i': win3D.setCameraZoom(win3D.getCameraZoom() / 1.2); win3D.repaint(); break; // ...and the rest in the kinect thread: default: thrPar.pushed_key = key; break; }; } win3D.get3DSceneAndLock(); win3D.addTextMessage( 2, -30, format( "'s':save point cloud, 'r': reset, 'o'/'i': zoom " "out/in, mouse: orbit 3D, ESC: quit"), TColorf(1, 1, 1), 110, MRPT_GLUT_BITMAP_HELVETICA_12); win3D.addTextMessage( 2, -50, str_status, TColorf(1, 1, 1), 111, MRPT_GLUT_BITMAP_HELVETICA_12); win3D.addTextMessage( 2, -70, str_status2, TColorf(1, 1, 1), 112, MRPT_GLUT_BITMAP_HELVETICA_18); win3D.unlockAccess3DScene(); std::this_thread::sleep_for(1ms); } cout << "Waiting for grabbing thread to exit...\n"; thrPar.quit = true; thHandle.join(); cout << "Bye!\n"; }
/*--------------------------------------------------------------- path_from_rtk_gps ---------------------------------------------------------------*/ void mrpt::topography::path_from_rtk_gps( mrpt::poses::CPose3DInterpolator &robot_path, const mrpt::slam::CRawlog &rawlog, size_t first, size_t last, bool isGUI, bool disableGPSInterp, int PATH_SMOOTH_FILTER , TPathFromRTKInfo *outInfo ) { MRPT_START #if MRPT_HAS_WXWIDGETS // Use a smart pointer so we are safe against exceptions: stlplus::smart_ptr<wxBusyCursor> waitCursorPtr; if (isGUI) waitCursorPtr = stlplus::smart_ptr<wxBusyCursor>( new wxBusyCursor() ); #endif // Go: generate the map: size_t i; ASSERT_(first<=last); ASSERT_(last<=rawlog.size()-1); set<string> lstGPSLabels; size_t count = 0; robot_path.clear(); robot_path.setMaxTimeInterpolation(3.0); // Max. seconds of GPS blackout not to interpolate. robot_path.setInterpolationMethod( CPose3DInterpolator::imSSLSLL ); TPathFromRTKInfo outInfoTemp; if (outInfo) *outInfo = outInfoTemp; map<string, map<TTimeStamp,TPoint3D> > gps_paths; // label -> (time -> 3D local coords) bool abort = false; bool ref_valid = false; // Load configuration block: CConfigFileMemory memFil; rawlog.getCommentTextAsConfigFile(memFil); TGeodeticCoords ref( memFil.read_double("GPS_ORIGIN","lat_deg",0), memFil.read_double("GPS_ORIGIN","lon_deg",0), memFil.read_double("GPS_ORIGIN","height",0) ); ref_valid = !ref.isClear(); // Do we have info for the consistency test? const double std_0 = memFil.read_double("CONSISTENCY_TEST","std_0",0); bool doConsistencyCheck = std_0>0; // Do we have the "reference uncertainty" matrix W^\star ?? memFil.read_matrix("UNCERTAINTY","W_star",outInfoTemp.W_star); const bool doUncertaintyCovs = size(outInfoTemp.W_star,1)!=0; if (doUncertaintyCovs && (size(outInfoTemp.W_star,1)!=6 || size(outInfoTemp.W_star,2)!=6)) THROW_EXCEPTION("ERROR: W_star matrix for uncertainty estimation is provided but it's not a 6x6 matrix."); // ------------------------------------------ // Look for the 2 observations: // ------------------------------------------ #if MRPT_HAS_WXWIDGETS wxProgressDialog *progDia=NULL; if (isGUI) { progDia = new wxProgressDialog( wxT("Building map"), wxT("Getting GPS observations..."), (int)(last-first+1), // range NULL, // parent wxPD_CAN_ABORT | wxPD_APP_MODAL | wxPD_SMOOTH | wxPD_AUTO_HIDE | wxPD_ELAPSED_TIME | wxPD_ESTIMATED_TIME | wxPD_REMAINING_TIME); } #endif // The list with all time ordered gps's in valid RTK mode typedef std::map< mrpt::system::TTimeStamp, std::map<std::string,CObservationGPSPtr> > TListGPSs; TListGPSs list_gps_obs; map<string,size_t> GPS_RTK_reads; // label-># of RTK readings map<string,TPoint3D> GPS_local_coords_on_vehicle; // label -> local pose on the vehicle for (i=first;!abort && i<=last;i++) { switch ( rawlog.getType(i) ) { default: break; case CRawlog::etObservation: { CObservationPtr o = rawlog.getAsObservation(i); if (o->GetRuntimeClass()==CLASS_ID(CObservationGPS)) { CObservationGPSPtr obs = CObservationGPSPtr(o); if (obs->has_GGA_datum && obs->GGA_datum.fix_quality==4) { // Add to the list: list_gps_obs[obs->timestamp][obs->sensorLabel] = obs; lstGPSLabels.insert(obs->sensorLabel); } // Save to GPS paths: if (obs->has_GGA_datum && (obs->GGA_datum.fix_quality==4 || obs->GGA_datum.fix_quality==5)) { GPS_RTK_reads[obs->sensorLabel]++; // map<string,TPoint3D> GPS_local_coords_on_vehicle; // label -> local pose on the vehicle if (GPS_local_coords_on_vehicle.find(obs->sensorLabel)==GPS_local_coords_on_vehicle.end()) GPS_local_coords_on_vehicle[obs->sensorLabel] = TPoint3D( obs->sensorPose ); //map<string, map<TTimeStamp,TPoint3D> > gps_paths; // label -> (time -> 3D local coords) gps_paths[obs->sensorLabel][obs->timestamp] = TPoint3D( obs->GGA_datum.longitude_degrees, obs->GGA_datum.latitude_degrees, obs->GGA_datum.altitude_meters ); } } } break; } // end switch type // Show progress: if ((count++ % 100)==0) { #if MRPT_HAS_WXWIDGETS if (progDia) { if (!progDia->Update((int)(i-first))) abort = true; wxTheApp->Yield(); } #endif } } // end for i #if MRPT_HAS_WXWIDGETS if (progDia) { delete progDia; progDia=NULL; } #endif // ----------------------------------------------------------- // At this point we already have the sensor positions, thus // we can estimate the covariance matrix D: // // TODO: Generalize equations for # of GPS > 3 // ----------------------------------------------------------- map< set<string>, double > Ad_ij; // InterGPS distances in 2D map< set<string>, double > phi_ij; // Directions on XY of the lines between i-j map< string, size_t> D_cov_indexes; // Sensor label-> index in the matrix (first=0, ...) map< size_t, string> D_cov_rev_indexes; // Reverse of D_cov_indexes CMatrixDouble D_cov; // square distances cov CMatrixDouble D_cov_1; // square distances cov (inverse) vector_double D_mean; // square distances mean if (doConsistencyCheck && GPS_local_coords_on_vehicle.size()==3) { unsigned int cnt = 0; for(map<string,TPoint3D>::iterator i=GPS_local_coords_on_vehicle.begin();i!=GPS_local_coords_on_vehicle.end();++i) { // Index tables: D_cov_indexes[i->first] = cnt; D_cov_rev_indexes[cnt] = i->first; cnt++; for(map<string,TPoint3D>::iterator j=i;j!=GPS_local_coords_on_vehicle.end();++j) { if (i!=j) { const TPoint3D &pi = i->second; const TPoint3D &pj = j->second; Ad_ij[ make_set(i->first,j->first) ] = pi.distanceTo( pj ); phi_ij[ make_set(i->first,j->first) ] = atan2( pj.y-pi.y, pj.x-pi.x ); } } } ASSERT_( D_cov_indexes.size()==3 && D_cov_rev_indexes.size()==3 ); D_cov.setSize( D_cov_indexes.size(), D_cov_indexes.size() ); D_mean.resize( D_cov_indexes.size() ); // See paper for the formulas! // TODO: generalize for N>3 D_cov(0,0) = 2*square( Ad_ij[ make_set(D_cov_rev_indexes[0],D_cov_rev_indexes[1])] ); D_cov(1,1) = 2*square( Ad_ij[ make_set(D_cov_rev_indexes[0],D_cov_rev_indexes[2])] ); D_cov(2,2) = 2*square( Ad_ij[ make_set(D_cov_rev_indexes[1],D_cov_rev_indexes[2])] ); D_cov(1,0) = Ad_ij[ make_set(D_cov_rev_indexes[0],D_cov_rev_indexes[1])] * Ad_ij[ make_set(D_cov_rev_indexes[0],D_cov_rev_indexes[2])] * cos( phi_ij[ make_set(D_cov_rev_indexes[0],D_cov_rev_indexes[1])] - phi_ij[ make_set(D_cov_rev_indexes[0],D_cov_rev_indexes[2])] ); D_cov(0,1) = D_cov(1,0); D_cov(2,0) =-Ad_ij[ make_set(D_cov_rev_indexes[0],D_cov_rev_indexes[1])] * Ad_ij[ make_set(D_cov_rev_indexes[1],D_cov_rev_indexes[2])] * cos( phi_ij[ make_set(D_cov_rev_indexes[0],D_cov_rev_indexes[1])] - phi_ij[ make_set(D_cov_rev_indexes[1],D_cov_rev_indexes[2])] ); D_cov(0,2) = D_cov(2,0); D_cov(2,1) = Ad_ij[ make_set(D_cov_rev_indexes[0],D_cov_rev_indexes[2])] * Ad_ij[ make_set(D_cov_rev_indexes[1],D_cov_rev_indexes[2])] * cos( phi_ij[ make_set(D_cov_rev_indexes[0],D_cov_rev_indexes[2])] - phi_ij[ make_set(D_cov_rev_indexes[1],D_cov_rev_indexes[2])] ); D_cov(1,2) = D_cov(2,1); D_cov *= 4*square(std_0); D_cov_1 = D_cov.inv(); //cout << D_cov.inMatlabFormat() << endl; D_mean[0] = square( Ad_ij[ make_set(D_cov_rev_indexes[0],D_cov_rev_indexes[1])] ); D_mean[1] = square( Ad_ij[ make_set(D_cov_rev_indexes[0],D_cov_rev_indexes[2])] ); D_mean[2] = square( Ad_ij[ make_set(D_cov_rev_indexes[1],D_cov_rev_indexes[2])] ); } else doConsistencyCheck =false; // ------------------------------------------ // Look for the 2 observations: // ------------------------------------------ int N_GPSs = list_gps_obs.size(); if (N_GPSs) { // loop interpolate 1-out-of-5: this solves the issue with JAVAD GPSs // that skip some readings at some times .0 .2 .4 .6 .8 if (list_gps_obs.size()>4) { TListGPSs::iterator F = list_gps_obs.begin(); ++F; ++F; TListGPSs::iterator E = list_gps_obs.end(); --E; --E; for (TListGPSs::iterator i=F;i!=E;++i) { // Now check if we have 3 gps with the same time stamp: //const size_t N = i->second.size(); std::map<std::string, CObservationGPSPtr> & GPS = i->second; // Check if any in "lstGPSLabels" is missing here: for (set<string>::iterator l=lstGPSLabels.begin();l!=lstGPSLabels.end();++l) { // For each GPS in the current timestamp: bool fnd = ( GPS.find(*l)!=GPS.end() ); if (fnd) continue; // this one is present. // Ok, we have "*l" missing in the set "*i". // Try to interpolate from neighbors: TListGPSs::iterator i_b1 = i; --i_b1; TListGPSs::iterator i_a1 = i; ++i_a1; CObservationGPSPtr GPS_b1, GPS_a1; if (i_b1->second.find( *l )!=i_b1->second.end()) GPS_b1 = i_b1->second.find( *l )->second; if (i_a1->second.find( *l )!=i_a1->second.end()) GPS_a1 = i_a1->second.find( *l )->second; if (!disableGPSInterp && GPS_a1 && GPS_b1) { if ( mrpt::system::timeDifference( GPS_b1->timestamp, GPS_a1->timestamp ) < 0.5 ) { CObservationGPSPtr new_gps = CObservationGPSPtr( new CObservationGPS(*GPS_a1) ); new_gps->sensorLabel = *l; //cout << mrpt::system::timeLocalToString(GPS_b1->timestamp) << " " << mrpt::system::timeLocalToString(GPS_a1->timestamp) << " " << *l; //cout << endl; new_gps->GGA_datum.longitude_degrees = 0.5 * ( GPS_a1->GGA_datum.longitude_degrees + GPS_b1->GGA_datum.longitude_degrees ); new_gps->GGA_datum.latitude_degrees = 0.5 * ( GPS_a1->GGA_datum.latitude_degrees + GPS_b1->GGA_datum.latitude_degrees ); new_gps->GGA_datum.altitude_meters = 0.5 * ( GPS_a1->GGA_datum.altitude_meters + GPS_b1->GGA_datum.altitude_meters ); new_gps->timestamp = (GPS_a1->timestamp + GPS_b1->timestamp) / 2; i->second[new_gps->sensorLabel] = new_gps; } } } } // end loop interpolate 1-out-of-5 } #if MRPT_HAS_WXWIDGETS wxProgressDialog *progDia3=NULL; if (isGUI) { progDia3 = new wxProgressDialog( wxT("Building map"), wxT("Estimating 6D path..."), N_GPSs, // range NULL, // parent wxPD_CAN_ABORT | wxPD_APP_MODAL | wxPD_SMOOTH | wxPD_AUTO_HIDE | wxPD_ELAPSED_TIME | wxPD_ESTIMATED_TIME | wxPD_REMAINING_TIME); } #endif int idx_in_GPSs = 0; for (TListGPSs::iterator i=list_gps_obs.begin();i!=list_gps_obs.end();++i, idx_in_GPSs++) { // Now check if we have 3 gps with the same time stamp: if (i->second.size()>=3) { const size_t N = i->second.size(); std::map<std::string, CObservationGPSPtr> & GPS = i->second; vector_double X(N),Y(N),Z(N); // Global XYZ coordinates std::map<string,size_t> XYZidxs; // Sensor label -> indices in X Y Z if (!ref_valid) // get the reference lat/lon, if it's not set from rawlog configuration block. { ref_valid = true; ref = GPS.begin()->second->GGA_datum.getAsStruct<TGeodeticCoords>(); } // Compute the XYZ coordinates of all sensors: TMatchingPairList corrs; unsigned int k; std::map<std::string, CObservationGPSPtr>::iterator g_it; for (k=0,g_it=GPS.begin();g_it!=GPS.end();g_it++,k++) { TPoint3D P; mrpt::topography::geodeticToENU_WGS84( g_it->second->GGA_datum.getAsStruct<TGeodeticCoords>(), P, ref ); // Correction of offsets: const string sect = string("OFFSET_")+g_it->second->sensorLabel; P.x += memFil.read_double( sect, "x", 0 ); P.y += memFil.read_double( sect, "y", 0 ); P.z += memFil.read_double( sect, "z", 0 ); XYZidxs[g_it->second->sensorLabel] = k; // Save index correspondence // Create the correspondence: corrs.push_back( TMatchingPair( k,k, // Indices P.x,P.y,P.z, // "This"/Global coords g_it->second->sensorPose.x(),g_it->second->sensorPose.y(),g_it->second->sensorPose.z() // "other"/local coordinates )); X[k] = P.x; Y[k] = P.y; Z[k] = P.z; } if (doConsistencyCheck && GPS.size()==3) { // XYZ[k] have the k'd final coordinates of each GPS // GPS[k] are the CObservations: // Compute the inter-GPS square distances: vector_double iGPSdist2(3); // [0]: sq dist between: D_cov_rev_indexes[0],D_cov_rev_indexes[1] TPoint3D P0( X[XYZidxs[D_cov_rev_indexes[0]]], Y[XYZidxs[D_cov_rev_indexes[0]]], Z[XYZidxs[D_cov_rev_indexes[0]]] ); TPoint3D P1( X[XYZidxs[D_cov_rev_indexes[1]]], Y[XYZidxs[D_cov_rev_indexes[1]]], Z[XYZidxs[D_cov_rev_indexes[1]]] ); TPoint3D P2( X[XYZidxs[D_cov_rev_indexes[2]]], Y[XYZidxs[D_cov_rev_indexes[2]]], Z[XYZidxs[D_cov_rev_indexes[2]]] ); iGPSdist2[0] = P0.sqrDistanceTo(P1); iGPSdist2[1] = P0.sqrDistanceTo(P2); iGPSdist2[2] = P1.sqrDistanceTo(P2); double mahaD = mrpt::math::mahalanobisDistance( iGPSdist2, D_mean, D_cov_1 ); outInfoTemp.mahalabis_quality_measure[i->first] = mahaD; //cout << "x: " << iGPSdist2 << " MU: " << D_mean << " -> " << mahaD << endl; } // end consistency // Use a 6D matching method to estimate the location of the vehicle: CPose3D optimal_pose; double optimal_scale; // "this" (reference map) -> GPS global coordinates // "other" -> GPS local coordinates on the vehicle mrpt::scanmatching::leastSquareErrorRigidTransformation6D( corrs,optimal_pose,optimal_scale, true ); // Force scale=1 //cout << "optimal pose: " << optimal_pose << " " << optimal_scale << endl; // Final vehicle pose: CPose3D &veh_pose= optimal_pose; // Add to the interpolator: MRPT_CHECK_NORMAL_NUMBER( veh_pose.x() ); MRPT_CHECK_NORMAL_NUMBER( veh_pose.y() ); MRPT_CHECK_NORMAL_NUMBER( veh_pose.z() ); MRPT_CHECK_NORMAL_NUMBER( veh_pose.yaw() ); MRPT_CHECK_NORMAL_NUMBER( veh_pose.pitch() ); MRPT_CHECK_NORMAL_NUMBER( veh_pose.roll() ); robot_path.insert( i->first, veh_pose ); // If we have W_star, compute the pose uncertainty: if (doUncertaintyCovs) { CPose3DPDFGaussian final_veh_uncert; final_veh_uncert.mean.setFromValues(0,0,0,0,0,0); final_veh_uncert.cov = outInfoTemp.W_star; // Rotate the covariance according to the real vehicle pose: final_veh_uncert.changeCoordinatesReference(veh_pose); outInfoTemp.vehicle_uncertainty[ i->first ] = final_veh_uncert.cov; } } // Show progress: if ((count++ % 100)==0) { #if MRPT_HAS_WXWIDGETS if (progDia3) { if (!progDia3->Update(idx_in_GPSs)) abort = true; wxTheApp->Yield(); } #endif } } // end for i #if MRPT_HAS_WXWIDGETS if (progDia3) { delete progDia3; progDia3=NULL; } #endif if (PATH_SMOOTH_FILTER>0 && robot_path.size()>1) { CPose3DInterpolator filtered_robot_path = robot_path; // Do Angles smoother filter of the path: // --------------------------------------------- const double MAX_DIST_TO_FILTER = 4.0; for (CPose3DInterpolator::iterator i=robot_path.begin();i!=robot_path.end();++i) { CPose3D p = i->second; vector_double pitchs, rolls; // The elements to average pitchs.push_back(p.pitch()); rolls.push_back(p.roll()); CPose3DInterpolator::iterator q=i; for (int k=0;k<PATH_SMOOTH_FILTER && q!=robot_path.begin();k++) { --q; if (abs( mrpt::system::timeDifference(q->first,i->first))<MAX_DIST_TO_FILTER ) { pitchs.push_back( q->second.pitch() ); rolls.push_back( q->second.roll() ); } } q=i; for (int k=0;k<PATH_SMOOTH_FILTER && q!=(--robot_path.end()) ;k++) { ++q; if (abs( mrpt::system::timeDifference(q->first,i->first))<MAX_DIST_TO_FILTER ) { pitchs.push_back( q->second.pitch() ); rolls.push_back( q->second.roll() ); } } p.setYawPitchRoll(p.yaw(), mrpt::math::averageWrap2Pi(pitchs), mrpt::math::averageWrap2Pi(rolls) ); // save in filtered path: filtered_robot_path.insert( i->first, p ); } // Replace: robot_path = filtered_robot_path; } // end PATH_SMOOTH_FILTER } // end step generate 6D path // Here we can set best_gps_path (that with the max. number of RTK fixed/foat readings): if (outInfo) { string bestLabel; size_t bestNum = 0; for (map<string,size_t>::iterator i=GPS_RTK_reads.begin();i!=GPS_RTK_reads.end();++i) { if (i->second>bestNum) { bestNum = i->second; bestLabel = i->first; } } outInfoTemp.best_gps_path = gps_paths[bestLabel]; // and transform to XYZ: // Correction of offsets: const string sect = string("OFFSET_")+bestLabel; const double off_X = memFil.read_double( sect, "x", 0 ); const double off_Y = memFil.read_double( sect, "y", 0 ); const double off_Z = memFil.read_double( sect, "z", 0 ); // map<TTimeStamp,TPoint3D> best_gps_path; // time -> 3D local coords for (map<TTimeStamp,TPoint3D>::iterator i=outInfoTemp.best_gps_path.begin();i!=outInfoTemp.best_gps_path.end();++i) { TPoint3D P; mrpt::topography::geodeticToENU_WGS84( TGeodeticCoords(i->second.x,i->second.y,i->second.z), // i->second.x,i->second.y,i->second.z, // lat, lon, heigh P, // X Y Z ref ); i->second.x = P.x + off_X; i->second.y = P.y + off_Y; i->second.z = P.z + off_Z; } } // end best_gps_path if (outInfo) *outInfo = outInfoTemp; MRPT_END }