// ------------------------------------------------------ // Benchmark Point Maps // ------------------------------------------------------ double pointmap_test_0(int a1, int a2) { // test 0: insert scan // ---------------------------------------- // prepare the laser scan: CObservation2DRangeScan scan1; scan1.aperture = M_PIf; scan1.rightToLeft = true; scan1.validRange.resize( sizeof(SCAN_RANGES_1)/sizeof(SCAN_RANGES_1[0]) ); scan1.scan.resize( sizeof(SCAN_RANGES_1)/sizeof(SCAN_RANGES_1[0]) ); memcpy( &scan1.scan[0], SCAN_RANGES_1, sizeof(SCAN_RANGES_1) ); memcpy( &scan1.validRange[0], SCAN_VALID_1, sizeof(SCAN_VALID_1) ); CSimplePointsMap pt_map; pt_map.insertionOptions.minDistBetweenLaserPoints = 0.03; CPose3D pose; CTicTac tictac; for (int n=0;n<a2;n++) { pt_map.clear(); for (long i=0;i<a1;i++) { pose.setFromValues( pose.x()+0.04, pose.y()+0.08,0, pose.yaw()+0.02); pt_map.insertObservation(&scan1, &pose); } } return tictac.Tac()/a2; }
CPose3D monoslamMRPT3DScene::getRobotState() { // Put the camera in the right place in the image display scene->get_motion_model()->func_xp(scene->get_xv()); ThreeD_Motion_Model *threed_motion_model = (ThreeD_Motion_Model *) scene->get_motion_model(); VW::Vector3D r = threed_motion_model->get_rRES(); VW::Quaternion qWR = threed_motion_model->get_qRES(); // q is qWR between world frame and Scene robot frame // What we need to plot though uses GL object frame O // Know qRO: pi rotation about y axis // qWO = qWR * qRO VW::Quaternion qRO(0.0, 1.0, 0.0, 0.0); VW::Quaternion qWO = qWR * qRO; CPose3D position; double yaw,pitch,roll; qWO.GetZYXEuler(yaw,pitch,roll); position.setFromValues(r._x,r._y,r._z,yaw,pitch,roll); return position; // if (display_trajectory_button->GetState()) { // trajectory_store.push_back(r.GetVNL3()); // if (trajectory_store.size() > 800) { // trajectory_store.erase(trajectory_store.begin()); // } // } // // // // image_threedtool->SetCameraPositionOrientation(r, qWO); }
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++; } }
double pointmap_test_1(int a1, int a2) { // test 1: insert scan // ---------------------------------------- // prepare the laser scan: CObservation2DRangeScan scan1; scan1.aperture = M_PIf; scan1.rightToLeft = true; scan1.validRange.resize( sizeof(SCAN_RANGES_1)/sizeof(SCAN_RANGES_1[0]) ); scan1.scan.resize( sizeof(SCAN_RANGES_1)/sizeof(SCAN_RANGES_1[0]) ); memcpy( &scan1.scan[0], SCAN_RANGES_1, sizeof(SCAN_RANGES_1) ); memcpy( &scan1.validRange[0], SCAN_VALID_1, sizeof(SCAN_VALID_1) ); CSimplePointsMap pt_map; pt_map.insertionOptions.minDistBetweenLaserPoints = 0.03; CPose3D pose; CTicTac tictac; for (long i=0;i<a1;i++) { pose.setFromValues( pose.x()+0.04, pose.y()+0.08,0, pose.yaw()+0.02); pt_map.insertObservation(&scan1, &pose); } const unsigned N_REPS = 25; if (a2==0) return tictac.Tac(); else if (a2==1) { // 2d kd-tree float x,y, dist2; tictac.Tic(); for (unsigned k=N_REPS;k!=0;--k) /*size_t idx = */ pt_map.kdTreeClosestPoint2D(5.0, 6.0, x,y, dist2); return tictac.Tac()/N_REPS; } else { // 3d kd-tree float x,y,z, dist2; tictac.Tic(); for (unsigned k=N_REPS;k!=0;--k) /*size_t idx =*/ pt_map.kdTreeClosestPoint3D(5.0, 6.0, 1.0, x,y,z, dist2); return tictac.Tac()/N_REPS; } }
/*--------------------------------------------------------------- drawSingleSample ---------------------------------------------------------------*/ void CPose3DPDFGaussianInf::drawSingleSample(CPose3D& outPart) const { MRPT_UNUSED_PARAM(outPart); MRPT_START CMatrixDouble66 cov(UNINITIALIZED_MATRIX); this->cov_inv.inv(cov); CVectorDouble v; getRandomGenerator().drawGaussianMultivariate(v, cov); outPart.setFromValues( mean.x() + v[0], mean.y() + v[1], mean.z() + v[2], mean.yaw() + v[3], mean.pitch() + v[4], mean.roll() + v[5]); MRPT_END_WITH_CLEAN_UP( cov_inv.saveToTextFile( "__DEBUG_EXC_DUMP_drawSingleSample_COV_INV.txt"););
double pointmap_test_3(int a1, int a2) { // test 3: query2D // ---------------------------------------- // prepare the laser scan: CObservation2DRangeScan scan1; scan1.aperture = M_PIf; scan1.rightToLeft = true; scan1.validRange.resize( sizeof(SCAN_RANGES_1)/sizeof(SCAN_RANGES_1[0]) ); scan1.scan.resize( sizeof(SCAN_RANGES_1)/sizeof(SCAN_RANGES_1[0]) ); memcpy( &scan1.scan[0], SCAN_RANGES_1, sizeof(SCAN_RANGES_1) ); memcpy( &scan1.validRange[0], SCAN_VALID_1, sizeof(SCAN_VALID_1) ); CSimplePointsMap pt_map; pt_map.insertionOptions.minDistBetweenLaserPoints = 0.03; CPose3D pose; for (long i=0;i<a1;i++) { pose.setFromValues( pose.x()+0.04, pose.y()+0.08,0, pose.yaw()+0.02); pt_map.insertObservation(&scan1, &pose); } CTicTac tictac; float x0=-5; float y0=-4; float sq; float x,y; for (long i=0;i<a2;i++) { pt_map.kdTreeClosestPoint2D(x0,y0,x,y,sq); x0+=0.05; y0+=0.05; if (x0>20) x0=-10; if (y0>20) y0=-10; } return tictac.Tac()/a2; }
double pointmap_test_5(int a1, int a2) { // test 5: boundingBox // ---------------------------------------- // prepare the laser scan: CObservation2DRangeScan scan1; scan1.aperture = M_PIf; scan1.rightToLeft = true; scan1.validRange.resize( sizeof(SCAN_RANGES_1)/sizeof(SCAN_RANGES_1[0]) ); scan1.scan.resize( sizeof(SCAN_RANGES_1)/sizeof(SCAN_RANGES_1[0]) ); memcpy( &scan1.scan[0], SCAN_RANGES_1, sizeof(SCAN_RANGES_1) ); memcpy( &scan1.validRange[0], SCAN_VALID_1, sizeof(SCAN_VALID_1) ); CSimplePointsMap pt_map; pt_map.insertionOptions.minDistBetweenLaserPoints = 0.03; CPose3D pose; for (long i=0;i<a1;i++) { pose.setFromValues( pose.x()+0.04, pose.y()+0.08,0, pose.yaw()+0.02); pt_map.insertObservation(&scan1, &pose); } CTicTac tictac; float x0,x1, y0,y1, z0,z1; for (long i=0;i<a2;i++) { // Modify the map so the bounding box cache is invalidated: pt_map.setPoint(0, 0,0,0); pt_map.boundingBox(x0,x1, y0,y1, z0,z1); } return tictac.Tac()/a2; }
/*--------------------------------------------------------------- getMean Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF), computed as a weighted average over all m_particles. ---------------------------------------------------------------*/ void CPose3DPDFParticles::getMean(CPose3D &p) const { MRPT_START double X=0,Y=0,Z=0,YAW=0,PITCH=0,ROLL=0; double ang,w,W=0; double W_yaw_R=0,W_yaw_L=0; double yaw_R=0,yaw_L=0; double W_roll_R=0,W_roll_L=0; double roll_R=0,roll_L=0; // First: XYZ // ----------------------------------- for (CPose3DPDFParticles::CParticleList::const_iterator it=m_particles.begin();it!=m_particles.end();++it) { w = exp(it->log_w); W += w; X += w * it->d->x(); Y += w * it->d->y(); Z += w * it->d->z(); PITCH += w * it->d->pitch(); // Angles Yaw and Roll are especial!: ang = it->d->yaw(); if (fabs( ang )>0.5*M_PI) { // LEFT HALF: 0,2pi if (ang<0) ang = (M_2PI + ang); yaw_L += ang * w; W_yaw_L += w; } else { // RIGHT HALF: -pi,pi yaw_R += ang * w; W_yaw_R += w; } // Angles Yaw and Roll are especial!: ang = it->d->roll(); if (fabs( ang )>0.5*M_PI) { // LEFT HALF: 0,2pi if (ang<0) ang = (M_2PI + ang); roll_L += ang * w; W_roll_L += w; } else { // RIGHT HALF: -pi,pi roll_R += ang * w; W_roll_R += w; } } if (W==0) { p.setFromValues(0,0,0,0,0,0); return; } X /= W; Y /= W; Z /= W; PITCH /= W; // Next: Yaw and Roll: // ----------------------------------- // The mean value from each side: if (W_yaw_L>0) yaw_L /= W_yaw_L; // [0,2pi] if (W_yaw_R>0) yaw_R /= W_yaw_R; // [-pi,pi] // Left side to [-pi,pi] again: if (yaw_L>M_PI) yaw_L = yaw_L - M_2PI; // The mean value from each side: if (W_roll_L>0) roll_L /= W_roll_L; // [0,2pi] if (W_roll_R>0) roll_R /= W_roll_R; // [-pi,pi] // Left side to [-pi,pi] again: if (roll_L>M_PI) roll_L = roll_L - M_2PI; // The total means: YAW = ((yaw_L * W_yaw_L + yaw_R * W_yaw_R )/(W_yaw_L+W_yaw_R)); ROLL = ((roll_L * W_roll_L + roll_R * W_roll_R )/(W_roll_L+W_roll_R)); p.setFromValues(X,Y,Z,YAW,PITCH,ROLL); MRPT_END }