/*---------------------------------------------------------------
			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);
}
示例#2
0
/** 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;
	}
}
示例#3
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?
}
示例#4
0
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_;
}
示例#5
0
/*---------------------------------------------------------------
						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 );
	}
}
示例#6
0
/*---------------------------------------------------------------
						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;

}
示例#8
0
// 判断路径起点,即当前位置能否找到校正路标
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;
}
示例#9
0
/** 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);
	}
}
示例#10
0
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 );
}
示例#11
0
文件: test.cpp 项目: bcsdleweev/mrpt
	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) );
	}
示例#12
0
/*---------------------------------------------------------------
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 );
}
示例#13
0
/*---------------------------------------------------------------
						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)) );
}
示例#14
0
/*---------------------------------------------------------------
						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();


}
示例#16
0
/*---------------------------------------------------------------
						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
}
示例#17
0
/*---------------------------------------------------------------
						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;

	}
}
示例#18
0
/*---------------------------------------------------------------
					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;

}
示例#20
0
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;

}
示例#22
0
/*---------------------------------------------------------------
					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"););
示例#23
0
/*---------------------------------------------------------------
						changeCoordinatesReference
 ---------------------------------------------------------------*/
void  CPosePDFGaussian::changeCoordinatesReference(const CPose2D &newReferenceBase )
{
	// The mean:
	mean = newReferenceBase + mean;
	// The covariance:
	rotateCov( newReferenceBase.phi() );
}
示例#24
0
	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;
	}
示例#25
0
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;
}
示例#26
0
/*---------------------------------------------------------------
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;
}
示例#27
0
/*---------------------------------------------------------------
						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);
}
示例#29
0
/*---------------------------------------------------------------
				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;
	}
}