/*---------------------------------------------------------------
computeObservationLikelihood_CellsDifference
---------------------------------------------------------------*/
double COccupancyGridMap2D::computeObservationLikelihood_CellsDifference(
const CObservation *obs,
const CPose2D &takenFrom )
{
double ret = 0.5;
// This function depends on the observation type:
// -----------------------------------------------------
if ( obs->GetRuntimeClass() == CLASS_ID(CObservation2DRangeScan) )
{
// Observation is a laser range scan:
// -------------------------------------------
const CObservation2DRangeScan *o = static_cast<const CObservation2DRangeScan*>( obs );
// Insert only HORIZONTAL scans, since the grid is supposed to
// be a horizontal representation of space.
if (!o->isPlanarScan(insertionOptions.horizontalTolerance)) return 0.5; // NO WAY TO ESTIMATE NON HORIZONTAL SCANS!!
// Build a copy of this occupancy grid:
COccupancyGridMap2D compareGrid(takenFrom.x()-10,takenFrom.x()+10,takenFrom.y()-10,takenFrom.y()+10,resolution);
CPose3D robotPose(takenFrom);
int Ax, Ay;
// Insert in this temporary grid:
compareGrid.insertionOptions.maxDistanceInsertion = insertionOptions.maxDistanceInsertion;
compareGrid.insertionOptions.maxOccupancyUpdateCertainty = 0.95f;
o->insertObservationInto( &compareGrid, &robotPose );
// Save Cells offset between the two grids:
Ax = round((x_min - compareGrid.x_min) / resolution);
Ay = round((y_min - compareGrid.y_min) / resolution);
int nCellsCompared = 0;
float cellsDifference = 0;
int x0 = max(0,Ax);
int y0 = max(0,Ay);
int x1 = min(compareGrid.size_x, size_x+Ax);
int y1 = min(compareGrid.size_y, size_y+Ay);
for (int x=x0;x<x1;x+=1)
{
for (int y=y0;y<y1;y+=1)
{
float xx = compareGrid.idx2x(x);
float yy = compareGrid.idx2y(y);
float c1 = getPos(xx,yy);
float c2 = compareGrid.getCell(x,y);
if ( c2<0.45f || c2>0.55f )
{
nCellsCompared++;
if ((c1>0.5 && c2<0.5) || (c1<0.5 && c2>0.5))
cellsDifference++;
}
}
}
ret = 1 - cellsDifference / (nCellsCompared);
}
return log(ret);
}
/** Must return the action vector u.
* \param out_u The action vector which will be passed to OnTransitionModel
*/
void CRangeBearingKFSLAM2D::OnGetAction(KFArray_ACT &u) const
{
// Get odometry estimation:
CActionRobotMovement2DPtr actMov2D = m_action->getBestMovementEstimation();
CActionRobotMovement3DPtr actMov3D = m_action->getActionByClass<CActionRobotMovement3D>();
if (actMov3D)
{
u[0]=actMov3D->poseChange.mean.x();
u[1]=actMov3D->poseChange.mean.y();
u[2]=actMov3D->poseChange.mean.yaw();
}
else
if (actMov2D)
{
CPose2D estMovMean;
actMov2D->poseChange->getMean(estMovMean);
u[0]=estMovMean.x();
u[1]=estMovMean.y();
u[2]=estMovMean.phi();
}
else
{
// Left u as zeros
for (size_t i=0;i<3;i++) u[i]=0;
}
}
void SRBASolver::Compute()
{
ROS_INFO("Computing corrected poses");
corrections_.clear();
if(!rba_.get_rba_state().keyframes.empty())
{
// Use a spanning tree to estimate the global pose of every node
// starting (root) at the given keyframe:
//corrections_.resize(rba_.get_rba_state().keyframes.size());
srba_t::frameid2pose_map_t spantree;
if(curr_kf_id_ == 0)
return;
TKeyFrameID root_keyframe(0);
rba_.create_complete_spanning_tree(root_keyframe,spantree);
for (srba_t::frameid2pose_map_t::const_iterator itP = spantree.begin();itP!=spantree.end();++itP)
{
std::pair<int, karto::Pose2> id_pose;
id_pose.first = itP->first;
const CPose2D p = itP->second.pose;
karto::Pose2 pos(p.x(), p.y(), p.phi());
id_pose.second = pos;
corrections_.push_back(id_pose);
}
}
// Get the global graph and return updated poses?
//typedef std::vector<sba::Node2d, Eigen::aligned_allocator<sba::Node2d> > NodeVector;
//ROS_INFO("Calling SRBA compute");
// Do nothing here?
}
IdPoseVector& SRBASolver::GetCorrections()
{
ROS_INFO("Computing corrected poses up to %d", curr_kf_id_-1);
corrections_.clear();
if(!rba_.get_rba_state().keyframes.empty())
{
// Use a spanning tree to estimate the global pose of every node
// starting (root) at the given keyframe:
//corrections_.resize(rba_.get_rba_state().keyframes.size());
srba_t::frameid2pose_map_t spantree;
if(curr_kf_id_ == 0)
return corrections_;
TKeyFrameID root_keyframe(0);
rba_.create_complete_spanning_tree(root_keyframe,spantree);
for (srba_t::frameid2pose_map_t::const_iterator itP = spantree.begin();itP!=spantree.end();++itP)
{
std::pair<int, karto::Pose2> id_pose;
id_pose.first = itP->first;
const CPose2D p = itP->second.pose;
karto::Pose2 pos(p.x(), p.y(), p.phi());
id_pose.second = pos;
corrections_.push_back(id_pose);
}
}
return corrections_;
}
/*---------------------------------------------------------------
squareErrorVector
---------------------------------------------------------------*/
void TMatchingPairList::squareErrorVector(
const CPose2D &q,
vector_float &out_sqErrs,
vector_float &xs,
vector_float &ys ) const
{
out_sqErrs.resize( size() );
xs.resize( size() );
ys.resize( size() );
// * \f[ e_i = | x_{this} - q \oplus x_{other} |^2 \f]
const float ccos = cos(q.phi());
const float csin = sin(q.phi());
const float qx = q.x();
const float qy = q.y();
const_iterator corresp;
vector_float::iterator e_i, xx, yy;
for (corresp=begin(), e_i = out_sqErrs.begin(), xx = xs.begin(), yy = ys.begin();corresp!=end();corresp++, e_i++, xx++,yy++)
{
*xx = qx + ccos * corresp->other_x - csin * corresp->other_y;
*yy = qy + csin * corresp->other_x + ccos * corresp->other_y;
*e_i = square( corresp->this_x - *xx ) + square( corresp->this_y - *yy );
}
}
/*---------------------------------------------------------------
getMean
Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF)
---------------------------------------------------------------*/
void CPosePDFSOG::getMean(CPose2D &p) const
{
size_t N = m_modes.size();
if (N)
{
// Use an auxiliary parts. set to manage the problematic computation of the mean "PHI":
// See CPosePDFParticles::getEstimatedPose
CPosePDFParticles auxParts( N );
CPosePDFParticles::CParticleList::iterator itPart;
const_iterator it;
for (it=m_modes.begin(),itPart=auxParts.m_particles.begin();it!=m_modes.end();++it,++itPart)
{
itPart->log_w = (it)->log_w;
*itPart->d = (it)->mean;
}
auxParts.getMean(p);
return;
}
else
{
p.x(0);
p.y(0);
p.phi(0);
return;
}
}
bool MyReactiveInterface::senseObstacles(CSimplePointsMap &obstacles){
cout << "senseObstacles" << endl;
CObservation2DRangeScan laserScan;
CPose2D rPose;
CPose3D rPose3D;
getCurrentMeasures(laserScan,rPose);
rPose3D=rPose;
obstacles.insertionOptions.minDistBetweenLaserPoints=0.005f;
obstacles.insertionOptions.also_interpolate=false;
obstacles.clear();
obstacles.insertObservation(&laserScan);
// Update data for plot thread
pthread_mutex_lock(&m_mutex);
path.AddVertex(rPose.x(),rPose.y());
laserCurrentMap.loadFromRangeScan(laserScan,&rPose3D);
newScan=true;
pthread_mutex_unlock(&m_mutex);
refreshPlot();
return true;
}
// 判断路径起点，即当前位置能否找到校正路标
bool CPathAgent::StartCalibCheck(void)
{
TPose2D tpose = g_pGyro->ReadPose(); // 机器人当前位置的大地坐标
CPose2D robot(tpose);
CPose2D center2robot(m_dCalibCenterDistance, 0, 0); // 查找范围的中心点在机器人坐标系内的坐标
CPose2D center = robot + center2robot; // 查找范围的中心点在大地坐标系内的坐标
// 遍历路标，找到距离0.4米内的路标
for (int j=0; j<m_iLandMarkNum; j++)
{
TPose2D Land = m_pLandMarkPose[j];
double dis = center.distance2DTo(Land.x, Land.y);
if (dis <= 0.5) // 0.4m半径的圆范围
{
//// 这个路标点满足基本的距离条件，需要进一步判断是否在矩形ROI区域
//CPoint2D Land2Center(Land.x - center.x(), Land.y - center.y()); // 路标在center坐标系的坐标
//CPose2D TurnPose(0,0,-center.phi()); // 旋转到机器人朝向的center坐标系
//CPoint2D newLand2Center = TurnPose + Land2Center;
//if (abs(newLand2Center.x()) < 0.2) // 在矩形ROI区域
// return true;
//else
// return false;
return true;
}
}
return false;
}
/** Return one or both of the following 6x6 Jacobians, useful in graph-slam
* problems...
*/
void SE_traits<2>::jacobian_dP1DP2inv_depsilon(
const CPose2D& P1DP2inv, matrix_VxV_t* df_de1, matrix_VxV_t* df_de2)
{
if (df_de1)
{
matrix_VxV_t& J1 = *df_de1;
// This Jacobian has the structure:
// [ I_2 | -[d_t]_x ]
// Jacob1 = [ ---------+--------------- ]
// [ 0 | 1 ]
//
J1.unit(VECTOR_SIZE, 1.0);
J1(0, 2) = -P1DP2inv.y();
J1(1, 2) = P1DP2inv.x();
}
if (df_de2)
{
// This Jacobian has the structure:
// [ -R | 0 ]
// Jacob2 = [ ---------+------- ]
// [ 0 | -1 ]
//
matrix_VxV_t& J2 = *df_de2;
const double ccos = cos(P1DP2inv.phi());
const double csin = sin(P1DP2inv.phi());
const double vals[] = {-ccos, csin, 0, -csin, -ccos, 0, 0, 0, -1};
J2 = CMatrixFixedNumeric<double, 3, 3>(vals);
}
}
TPoint2D mrpt::poses::operator +(const CPose2D &pose, const TPoint2D &u)
{
const double ccos = pose.phi_cos();
const double ssin = pose.phi_sin();
return TPoint2D(
pose.x() + u.x * ccos - u.y * ssin,
pose.y() + u.x * ssin + u.y * ccos );
}
void RS_drawFromProposal( CPose2D &outSample )
{
double ang = randomGenerator.drawUniform(-M_PI,M_PI);
double R = randomGenerator.drawGaussian1D(1.0,SIGMA);
outSample.x( 1.0f - cos(ang) * R );
outSample.y( sin(ang) * R );
outSample.phi( randomGenerator.drawUniform(-M_PI,M_PI) );
}
/*---------------------------------------------------------------
The operator D="this"-b is the pose inverse compounding operator.
The resulting pose "D" is the diference between this pose and "b"
---------------------------------------------------------------*/
CPoint2D CPoint2D::operator - (const CPose2D& b) const
{
const double ccos = cos(b.phi());
const double ssin = sin(b.phi());
const double Ax = x()-b.x();
const double Ay = y()-b.y();
return CPoint2D( Ax * ccos + Ay * ssin, -Ax * ssin + Ay * ccos );
}
/*---------------------------------------------------------------
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 CPosePDFParticles::getMean(CPose2D &est_) const
{
TPose2D est(0,0,0);
CPose2D p;
size_t i,n = m_particles.size();
double phi,w,W=0;
double W_phi_R=0,W_phi_L=0;
double phi_R=0,phi_L=0;
if (!n) return;
// First: XY
// -----------------------------------
for (i=0;i<n;i++)
{
p = *m_particles[i].d;
w = exp(m_particles[i].log_w);
W += w;
est.x+= p.x() * w;
est.y+= p.y() * w;
// PHI is special:
phi = p.phi();
if (fabs(phi)>1.5707963267948966192313216916398f)
{
// LEFT HALF: 0,2pi
if (phi<0) phi = (M_2PI + phi);
phi_L += phi * w;
W_phi_L += w;
}
else
{
// RIGHT HALF: -pi,pi
phi_R += phi * w;
W_phi_R += w;
}
}
est_ = est;
est_ *= (1.0/W);
// Next: PHI
// -----------------------------------
// The mean value from each side:
if (W_phi_L>0) phi_L /= W_phi_L; // [0,2pi]
if (W_phi_R>0) phi_R /= W_phi_R; // [-pi,pi]
// Left side to [-pi,pi] again:
if (phi_L>M_PI) phi_L = phi_L - M_2PI;
// The total mean:
est_.phi( ((phi_L * W_phi_L + phi_R * W_phi_R )/(W_phi_L+W_phi_R)) );
}
/*---------------------------------------------------------------
changeCoordinatesReference
---------------------------------------------------------------*/
void CPosePDFGaussian::changeCoordinatesReference(const CPose3D &newReferenceBase_ )
{
CPose2D newReferenceBase = CPose2D(newReferenceBase_);
// The mean:
mean = CPose2D( newReferenceBase + mean );
// The covariance:
rotateCov( newReferenceBase.phi() );
}
void MyReactiveInterface::getCurrentMeasures(CObservation2DRangeScan &laserScan,CPose2D &robotPose){
cout << "getCurrentMeasures" << endl;
// Clear readings
laserScan.scan.clear();
laserScan.validRange.clear();
laserScan.aperture=M_PIf;
laserScan.rightToLeft=false;
float dist;
char valid;
ArPose punto;
//Bloquear laser
laser->lockDevice();
ArPose pose=robot->getPose();
vector<ArSensorReading> *readings=laser->getRawReadingsAsVector();
robotPose.x(pose.getX()*0.001);
robotPose.y(pose.getY()*0.001);
robotPose.phi(DEG2RAD(pose.getTh()));
for(vector<ArSensorReading>::iterator it=readings->begin(); it != readings->end(); it++)
{
if(it->getIgnoreThisReading())
{
// Establezco 10m, máximo rango del láser
dist=10000;
valid=0;
}
else
{
punto=it->getPose();
dist=pose.findDistanceTo(punto);
valid=1;
}
laserScan.scan.push_back(dist*0.001);
laserScan.validRange.push_back(valid);
}
laser->unlockDevice();
}
/*---------------------------------------------------------------
insertObservation
---------------------------------------------------------------*/
bool CWirelessPowerGridMap2D::internal_insertObservation(
const CObservation *obs,
const CPose3D *robotPose )
{
MRPT_START
CPose2D robotPose2D;
CPose3D robotPose3D;
if (robotPose)
{
robotPose2D = CPose2D(*robotPose);
robotPose3D = (*robotPose);
}
else
{
// Default values are (0,0,0)
}
if ( IS_CLASS(obs, CObservationWirelessPower ))
{
/********************************************************************
OBSERVATION TYPE: CObservationWirelessPower
********************************************************************/
const CObservationWirelessPower *o = static_cast<const CObservationWirelessPower*>( obs );
float sensorReading;
// Compute the 3D sensor pose in world coordinates:
CPose2D sensorPose = CPose2D(robotPose3D + o->sensorPoseOnRobot );
sensorReading = o->power;
// Normalization:
sensorReading = (sensorReading - insertionOptions.R_min) /( insertionOptions.R_max - insertionOptions.R_min );
// Update the gross estimates of mean/vars for the whole reading history (see IROS2009 paper):
m_average_normreadings_mean = (sensorReading + m_average_normreadings_count*m_average_normreadings_mean)/(1+m_average_normreadings_count);
m_average_normreadings_var = (square(sensorReading - m_average_normreadings_mean) + m_average_normreadings_count*m_average_normreadings_var) /(1+m_average_normreadings_count);
m_average_normreadings_count++;
// Finally, do the actual map update with that value:
this->insertIndividualReading(sensorReading, mrpt::math::TPoint2D(sensorPose.x(),sensorPose.y()) );
return true; // Done!
} // end if "CObservationWirelessPower"
return false;
MRPT_END
}
/*---------------------------------------------------------------
getEstimatedCovariance
---------------------------------------------------------------*/
void CPosePDFParticles::getCovarianceAndMean(CMatrixDouble33 &cov, CPose2D &mean) const
{
cov.zeros();
getMean(mean);
size_t i,n = m_particles.size();
double var_x=0,var_y=0,var_p=0,var_xy=0,var_xp=0,var_yp=0;
double mean_phi = mean.phi();
if (mean_phi<0) mean_phi = M_2PI + mean_phi;
double lin_w_sum = 0;
for (i=0;i<n;i++) lin_w_sum+= exp( m_particles[i].log_w );
if (lin_w_sum==0) lin_w_sum=1;
for (i=0;i<n;i++)
{
double w = exp( m_particles[i].log_w ) / lin_w_sum;
// Manage 1 PI range:
double err_x = m_particles[i].d->x() - mean.x();
double err_y = m_particles[i].d->y() - mean.y();
double err_phi = math::wrapToPi( fabs(m_particles[i].d->phi() - mean_phi) );
var_x+= square(err_x)*w;
var_y+= square(err_y)*w;
var_p+= square(err_phi)*w;
var_xy+= err_x*err_y*w;
var_xp+= err_x*err_phi*w;
var_yp+= err_y*err_phi*w;
}
if (n<2)
{
// Not enought information to estimate the variance:
}
else
{
// Unbiased estimation of variance:
cov(0,0) = var_x;
cov(1,1) = var_y;
cov(2,2) = var_p;
cov(1,0) = cov(0,1) = var_xy;
cov(2,0) = cov(0,2) = var_xp;
cov(1,2) = cov(2,1) = var_yp;
}
}
/*---------------------------------------------------------------
jacobiansPoseComposition
---------------------------------------------------------------*/
void CPosePDF::jacobiansPoseComposition(
const CPose2D &x,
const CPose2D &u,
CMatrixDouble33 &df_dx,
CMatrixDouble33 &df_du,
const bool compute_df_dx,
const bool compute_df_du )
{
const double spx = sin(x.phi());
const double cpx = cos(x.phi());
if (compute_df_dx)
{
/*
df_dx =
[ 1, 0, -sin(phi_x)*x_u-cos(phi_x)*y_u ]
[ 0, 1, cos(phi_x)*x_u-sin(phi_x)*y_u ]
[ 0, 0, 1 ]
*/
df_dx.unit(3,1.0);
const double xu = u.x();
const double yu = u.y();
df_dx.get_unsafe(0,2) = -spx*xu-cpx*yu;
df_dx.get_unsafe(1,2) = cpx*xu-spx*yu;
}
if (compute_df_du)
{
/*
df_du =
[ cos(phi_x) , -sin(phi_x) , 0 ]
[ sin(phi_x) , cos(phi_x) , 0 ]
[ 0 , 0 , 1 ]
*/
// This is the homogeneous matrix of "x":
df_du.get_unsafe(0,2) =
df_du.get_unsafe(1,2) =
df_du.get_unsafe(2,0) =
df_du.get_unsafe(2,1) = 0;
df_du.get_unsafe(2,2) = 1;
df_du.get_unsafe(0,0) = cpx;
df_du.get_unsafe(0,1) = -spx;
df_du.get_unsafe(1,0) = spx;
df_du.get_unsafe(1,1) = cpx;
}
}
CPose2D getPataMesa(){
FILE* file=fopen("patamesa.dat","r");
CPose2D pataMesa;
int nDatos(0);
if(file){
float x,y;
// Archivo existe
fscanf(file,"x:%f,y:%f",&x,&y);
pataMesa.x(x);
pataMesa.y(y);
fclose(file);
}
else{
//Archivo no existe
file=fopen("patamesa.dat","w");
for(int i=1;i<21;i++){
Detector detector;
char nombre[100];
sprintf(nombre,"/home/jplata/Eclipse/MedidasPiernas/23Mayo/laser%i.dat",i);
cout << "Fichero: " << nombre << endl;
detector.abrirFichero(nombre,false);
// Clusteres detectados
vector<Cluster> piernas=detector.clusterizar(0.1,3);
detector.printClusters(piernas);
// El ultimo cluster corresponde a la pata de la mesa en las primeras muestras
nDatos++;
pataMesa+=piernas.back().getCentro();
}
pataMesa.x(pataMesa.x()/nDatos);
pataMesa.y(pataMesa.y()/nDatos);
fprintf(file,"x:%f,y:%f",pataMesa.x(),pataMesa.y());
fclose(file);
}
return pataMesa;
}
void CVirtualWorld::LocateRobot(CPose2D &location)
{
location.phi_incr(3.14159/2);
robot->setPose(location);
double x=location.x();
double y=location.y();
win.setCameraPointingToPoint(x,y,0);
win.repaint();
}
bool MyReactiveInterface::getCurrentPoseAndSpeeds(CPose2D &curPose, float &curV,float &curW){
cout << "getCurrentPoseAndSpeeds" << endl;
ArPose pose=robot->getPose();
curPose.x(pose.getX()*0.001);
curPose.y(pose.getY()*0.001);
curPose.phi(DEG2RAD(pose.getTh()));
curV = robot->getVel() * 0.001;
curW = DEG2RAD( robot->getRotVel() );
return true;
}
/*---------------------------------------------------------------
drawSingleSample
---------------------------------------------------------------*/
void CPosePDFGaussian::drawSingleSample(CPose2D& outPart) const
{
MRPT_START
CVectorDouble v;
getRandomGenerator().drawGaussianMultivariate(v, cov);
outPart.x(mean.x() + v[0]);
outPart.y(mean.y() + v[1]);
outPart.phi(mean.phi() + v[2]);
// Range -pi,pi
outPart.normalizePhi();
MRPT_END_WITH_CLEAN_UP(
cov.saveToTextFile("__DEBUG_EXC_DUMP_drawSingleSample_COV.txt"););
/*---------------------------------------------------------------
changeCoordinatesReference
---------------------------------------------------------------*/
void CPosePDFGaussian::changeCoordinatesReference(const CPose2D &newReferenceBase )
{
// The mean:
mean = newReferenceBase + mean;
// The covariance:
rotateCov( newReferenceBase.phi() );
}
void test_to_from_2d(double x,double y, double phi)
{
const CPose2D p2d = CPose2D(x,y,phi);
const CPose3D p3d = CPose3D(p2d);
const CPose2D p2d_bis = CPose2D(p3d);
EXPECT_FLOAT_EQ( p2d.x(), p2d_bis.x() ) << "p2d: " << p2d << endl;
EXPECT_FLOAT_EQ( p2d.y(), p2d_bis.y() ) << "p2d: " << p2d << endl;
EXPECT_FLOAT_EQ( p2d.phi(), p2d_bis.phi() ) << "p2d: " << p2d << endl;
EXPECT_FLOAT_EQ( p2d.phi(), p3d.yaw() ) << "p2d: " << p2d << endl;
}
double poses_test_compose2D2(int a1, int a2)
{
const long N = 500000;
CTicTac tictac;
CPose2D a(1.0,2.0,DEG2RAD(10));
CPose2D b(8.0,-5.0,DEG2RAD(-40));
CPose2D p;
for (long i=0;i<N;i++)
{
p.composeFrom(a,b);
}
double T = tictac.Tac()/N;
dummy_do_nothing_with_string( mrpt::format("%f",p.x()) );
return T;
}
/*---------------------------------------------------------------
The operator D="this"-b is the pose inverse compounding operator.
The resulting pose "D" is the diference between this pose and "b"
---------------------------------------------------------------*/
void CPose2D::inverseComposeFrom(const CPose2D& A, const CPose2D& B )
{
B.update_cached_cos_sin();
m_coords[0] = (A.m_coords[0] - B.m_coords[0]) * B.m_cosphi + (A.m_coords[1] - B.m_coords[1]) * B.m_sinphi;
m_coords[1] =-(A.m_coords[0] - B.m_coords[0]) * B.m_sinphi + (A.m_coords[1] - B.m_coords[1]) * B.m_cosphi;
m_phi = math::wrapToPi(A.m_phi - B.m_phi);
m_cossin_uptodate=false;
}
/*---------------------------------------------------------------
squareErrorVector
---------------------------------------------------------------*/
void TMatchingPairList::squareErrorVector(const CPose2D &q, vector<float> &out_sqErrs ) const
{
out_sqErrs.resize( size() );
// * \f[ e_i = | x_{this} - q \oplus x_{other} |^2 \f]
const float ccos = cos(q.phi());
const float csin = sin(q.phi());
const float qx = q.x();
const float qy = q.y();
const_iterator corresp;
vector<float>::iterator e_i;
for (corresp=begin(), e_i = out_sqErrs.begin();corresp!=end();++corresp, ++e_i)
{
float xx = qx + ccos * corresp->other_x - csin * corresp->other_y;
float yy = qy + csin * corresp->other_x + ccos * corresp->other_y;
*e_i = square( corresp->this_x - xx ) + square( corresp->this_y - yy );
}
}
/*---------------------------------------------------------------
computeObservationLikelihood_Consensus
---------------------------------------------------------------*/
double COccupancyGridMap2D::computeObservationLikelihood_Consensus(
const CObservation *obs,
const CPose2D &takenFrom )
{
double likResult = 0;
// This function depends on the observation type:
// -----------------------------------------------------
if ( obs->GetRuntimeClass() != CLASS_ID(CObservation2DRangeScan) )
{
//THROW_EXCEPTION("This method is defined for 'CObservation2DRangeScan' classes only.");
return 1e-3;
}
// Observation is a laser range scan:
// -------------------------------------------
const CObservation2DRangeScan *o = static_cast<const CObservation2DRangeScan*>( obs );
// Insert only HORIZONTAL scans, since the grid is supposed to
// be a horizontal representation of space.
if ( ! o->isPlanarScan(insertionOptions.horizontalTolerance) ) return 0.5f; // NO WAY TO ESTIMATE NON HORIZONTAL SCANS!!
// Assure we have a 2D points-map representation of the points from the scan:
const CPointsMap *compareMap = o->buildAuxPointsMap<mrpt::maps::CPointsMap>();
// Observation is a points map:
// -------------------------------------------
size_t Denom=0;
// int Acells = 1;
TPoint2D pointGlobal,pointLocal;
// Get the points buffers:
// compareMap.getPointsBuffer( n, xs, ys, zs );
const size_t n = compareMap->size();
for (size_t i=0;i<n;i+=likelihoodOptions.consensus_takeEachRange)
{
// Get the point and pass it to global coordinates:
compareMap->getPoint(i,pointLocal);
takenFrom.composePoint(pointLocal, pointGlobal);
int cx0 = x2idx( pointGlobal.x );
int cy0 = y2idx( pointGlobal.y );
likResult += 1-getCell_nocheck(cx0,cy0);
Denom++;
}
if (Denom) likResult/=Denom;
likResult = pow(likResult, static_cast<double>( likelihoodOptions.consensus_pow ) );
return log(likResult);
}
/*---------------------------------------------------------------
composeFrom
---------------------------------------------------------------*/
void CPose2D::composeFrom(const CPose2D &A, const CPose2D &B)
{
A.update_cached_cos_sin();
// Use temporary variables for the cases (A==this) or (B==this)
const double new_x = A.m_coords[0] + B.m_coords[0] * A.m_cosphi - B.m_coords[1] * A.m_sinphi;
const double new_y = A.m_coords[1] + B.m_coords[0] * A.m_sinphi + B.m_coords[1] * A.m_cosphi;
m_coords[0] = new_x;
m_coords[1] = new_y;
m_phi = math::wrapToPi(A.m_phi + B.m_phi);
m_cossin_uptodate=false;
}
void COccupancyGridMap2D::sonarSimulator(
CObservationRange &inout_observation,
const CPose2D &robotPose,
float threshold,
float rangeNoiseStd,
float angleNoiseStd) const
{
const float free_thres = 1.0f - threshold;
const unsigned int max_ray_len = round(inout_observation.maxSensorDistance / resolution);
for (CObservationRange::iterator itR=inout_observation.begin();itR!=inout_observation.end();++itR)
{
const CPose2D sensorAbsolutePose = CPose2D( CPose3D(robotPose) + CPose3D(itR->sensorPose) );
// For each sonar cone, simulate several rays and keep the shortest distance:
ASSERT_(inout_observation.sensorConeApperture>0)
size_t nRays = round(1+ inout_observation.sensorConeApperture / DEG2RAD(1.0) );
double direction = sensorAbsolutePose.phi() - 0.5*inout_observation.sensorConeApperture;
const double Adir = inout_observation.sensorConeApperture / nRays;
float min_detected_obs=0;
for (size_t i=0;i<nRays;i++, direction+=Adir )
{
bool valid;
float sim_rang;
simulateScanRay(
sensorAbsolutePose.x(), sensorAbsolutePose.y(), direction,
sim_rang, valid,
max_ray_len, free_thres,
rangeNoiseStd, angleNoiseStd );
if (valid && (sim_rang<min_detected_obs || !i))
min_detected_obs = sim_rang;
}
// Save:
itR->sensedDistance = min_detected_obs;
}
}