/*---------------------------------------------------------------
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)) );
}
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;
}
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_;
}
/*---------------------------------------------------------------
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;
}
}
/** 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);
}
}
/*---------------------------------------------------------------
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 );
}
}
/** 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;
}
}
/*---------------------------------------------------------------
changeCoordinatesReference
---------------------------------------------------------------*/
void CPosePDFGaussian::changeCoordinatesReference(const CPose2D &newReferenceBase )
{
// The mean:
mean = newReferenceBase + mean;
// The covariance:
rotateCov( newReferenceBase.phi() );
}
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?
}
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 );
}
/*---------------------------------------------------------------
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();
}
/*---------------------------------------------------------------
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;
}
}
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"););
/*---------------------------------------------------------------
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 );
}
}
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;
}
}
/*----------------------------------------------------------------------------
ICP_Method_Classic
----------------------------------------------------------------------------*/
CPosePDFPtr CICP::ICP_Method_Classic(
const mrpt::maps::CMetricMap *m1,
const mrpt::maps::CMetricMap *mm2,
const CPosePDFGaussian &initialEstimationPDF,
TReturnInfo &outInfo )
{
MRPT_START
// The result can be either a Gaussian or a SOG:
CPosePDFPtr resultPDF;
CPosePDFGaussianPtr gaussPdf;
CPosePDFSOGPtr SOG;
size_t nCorrespondences=0;
bool keepApproaching;
CPose2D grossEst = initialEstimationPDF.mean;
mrpt::utils::TMatchingPairList correspondences,old_correspondences;
CPose2D lastMeanPose;
// Assure the class of the maps:
const mrpt::maps::CMetricMap *m2 = 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 = CPosePDFGaussian::Create();
// First gross approximation:
gaussPdf->mean = grossEst;
// Initial thresholds:
mrpt::maps::TMatchingParams matchParams;
mrpt::maps::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
{
// ------------------------------------------------------
// Find the matching (for a points map)
// ------------------------------------------------------
matchParams.angularDistPivotPoint = TPoint3D(gaussPdf->mean.x(),gaussPdf->mean.y(),0); // Pivot point for angular measurements
m1->determineMatching2D(
m2, // The other map
gaussPdf->mean, // The other map pose
correspondences,
matchParams, matchExtraResults);
nCorrespondences = correspondences.size();
// ***DEBUG***
// correspondences.dumpToFile("debug_correspondences.txt");
if ( !nCorrespondences )
{
// Nothing we can do !!
keepApproaching = false;
}
else
{
// Compute the estimated pose.
// (Method from paper of J.Gonzalez, Martinez y Morales)
// ----------------------------------------------------------------------
mrpt::math::TPose2D est_mean;
mrpt::tfest::se2_l2(correspondences, est_mean);
gaussPdf->mean = est_mean;
// 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(math::wrapToPi(lastMeanPose.phi()-gaussPdf->mean.phi()))>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)
{
switch(options.ICP_covariance_method)
{
// ----------------------------------------------
// METHOD: MSE linear estimation
// ----------------------------------------------
case icpCovLinealMSE:
mrpt::tfest::se2_l2(correspondences, *gaussPdf );
// Scale covariance:
gaussPdf->cov *= options.covariance_varPoints;
break;
// ----------------------------------------------
// METHOD: Method from Oxford MRG's "OXSMV2"
//
// It is the equivalent covariance resulting
// from a Levenberg-Maquardt optimization stage.
// ----------------------------------------------
case icpCovFiniteDifferences:
{
Eigen::Matrix<double,3,Eigen::Dynamic> D(3,nCorrespondences);
const TPose2D transf( gaussPdf->mean );
double ccos = cos(transf.phi);
double csin = sin(transf.phi);
double w1,w2,w3;
double q1,q2,q3;
double A,B;
double Axy = 0.01;
// Fill out D:
double rho2 = square( options.kernel_rho );
mrpt::utils::TMatchingPairList::iterator it;
size_t i;
for (i=0, it=correspondences.begin();i<nCorrespondences; ++i, ++it)
{
float other_x_trans = transf.x + ccos * it->other_x - csin * it->other_y;
float other_y_trans = transf.y + csin * it->other_x + ccos * it->other_y;
// Jacobian: dR2_dx
// --------------------------------------
w1= other_x_trans-Axy;
q1= kernel( square(it->this_x - w1)+ square( it->this_y - other_y_trans ), rho2 );
w2= other_x_trans;
q2= kernel( square(it->this_x - w2)+ square( it->this_y - other_y_trans ), rho2 );
w3= other_x_trans+Axy;
q3= kernel( square(it->this_x - w3)+ square( it->this_y - other_y_trans ), rho2 );
//interpolate
A=( (q3-q2)/((w3-w2)*(w3-w1)) ) - ( (q1-q2)/((w1-w2)*(w3-w1)) );
B=( (q1-q2)+(A*((w2*w2)-(w1*w1))) )/(w1-w2);
D(0,i) = (2*A*other_x_trans)+B;
// Jacobian: dR2_dy
// --------------------------------------
w1= other_y_trans-Axy;
q1= kernel( square(it->this_x - other_x_trans)+ square( it->this_y - w1 ), rho2 );
w2= other_y_trans;
q2= kernel( square(it->this_x - other_x_trans)+ square( it->this_y - w2 ), rho2 );
w3= other_y_trans+Axy;
q3= kernel( square(it->this_x - other_x_trans)+ square( it->this_y - w3 ), rho2 );
//interpolate
A=( (q3-q2)/((w3-w2)*(w3-w1)) ) - ( (q1-q2)/((w1-w2)*(w3-w1)) );
B=( (q1-q2)+(A*((w2*w2)-(w1*w1))) )/(w1-w2);
D(1,i) = (2*A*other_y_trans)+B;
// Jacobian: dR_dphi
// --------------------------------------
D(2,i) = D(0,i) * ( -csin * it->other_x - ccos * it->other_y ) +
D(1,i) * ( ccos * it->other_x - csin * it->other_y );
} // end for each corresp.
// COV = ( D*D^T + lamba*I )^-1
CMatrixDouble33 DDt = D*D.transpose();
for (i=0;i<3;i++) DDt( i,i ) += 1e-6; // Just to make sure the matrix is not singular, while not changing its covariance significantly.
DDt.inv(gaussPdf->cov);
}
break;
default:
THROW_EXCEPTION_FMT("Invalid value for ICP_covariance_method: %i", static_cast<int>(options.ICP_covariance_method));
}
}
outInfo.goodness = matchExtraResults.correspondencesRatio;
if (!nCorrespondences || options.skip_quality_calculation)
{
outInfo.quality = matchExtraResults.correspondencesRatio;
}
else
{
// Compute a crude estimate of the quality of scan matching at this local minimum:
// -----------------------------------------------------------------------------------
float Axy = matchParams.maxDistForCorrespondence*0.9f;
TPose2D P0( gaussPdf->mean );
TPose2D PX1(P0); PX1.x -= Axy;
TPose2D PX2(P0); PX2.x += Axy;
TPose2D PY1(P0); PY1.y -= Axy;
TPose2D PY2(P0); PY2.y += Axy;
matchParams.angularDistPivotPoint = TPoint3D( gaussPdf->mean.x(),gaussPdf->mean.y(),0);
m1->determineMatching2D(
m2, // The other map
P0, // The other map pose
correspondences,
matchParams, matchExtraResults);
const float E0 = matchExtraResults.correspondencesRatio;
m1->determineMatching2D(
m2, // The other map
PX1, // The other map pose
correspondences,
matchParams, matchExtraResults);
const float EX1 = matchExtraResults.correspondencesRatio;
m1->determineMatching2D(
m2, // The other map
PX2, // The other map pose
correspondences,
matchParams, matchExtraResults);
const float EX2 = matchExtraResults.correspondencesRatio;
m1->determineMatching2D(
m2, // The other map
PY1, // The other map pose
correspondences,
matchParams, matchExtraResults);
const float EY1 = matchExtraResults.correspondencesRatio;
m1->determineMatching2D(
m2, // The other map
PY2, // The other map pose
correspondences,
matchParams, matchExtraResults);
const float EY2 = matchExtraResults.correspondencesRatio;
outInfo.quality= -max(EX1-E0, max(EX2-E0, max(EY1-E0 , EY2-E0 ) ) )/(E0+1e-1);
}
} // end of "if m2 is not empty"
// We'll return a CPosePDFGaussian or a CPosePDFSOG if RANSAC is enabled:
// -----------------------------------------------------------------------
// RANSAC?
if (options.doRANSAC)
{
mrpt::tfest::TSE2RobustParams params;
params.ransac_minSetSize = options.ransac_minSetSize;
params.ransac_maxSetSize = options.ransac_maxSetSize;
params.ransac_mahalanobisDistanceThreshold = options.ransac_mahalanobisDistanceThreshold;
params.ransac_nSimulations = options.ransac_nSimulations;
params.ransac_fuseByCorrsMatch = options.ransac_fuseByCorrsMatch;
params.ransac_fuseMaxDiffXY = options.ransac_fuseMaxDiffXY;
params.ransac_fuseMaxDiffPhi = options.ransac_fuseMaxDiffPhi;
params.ransac_algorithmForLandmarks = false;
mrpt::tfest::TSE2RobustResult results;
mrpt::tfest::se2_l2_robust(correspondences, options.normalizationStd, params, results);
SOG = CPosePDFSOG::Create();
*SOG = results.transformation;
// And return the SOG:
resultPDF = SOG;
}
else
{
// Return the gaussian distribution:
resultPDF = gaussPdf;
}
return resultPDF;
MRPT_END
}
/*----------------------------------------------------------------------------
ICP_Method_LM
----------------------------------------------------------------------------*/
CPosePDFPtr CICP::ICP_Method_LM(
const mrpt::maps::CMetricMap *mm1,
const mrpt::maps::CMetricMap *m2,
const CPosePDFGaussian &initialEstimationPDF,
TReturnInfo &outInfo )
{
MRPT_START
// The result can be either a Gaussian or a SOG:
size_t nCorrespondences=0;
bool keepIteratingICP;
CPose2D grossEst = initialEstimationPDF.mean;
TMatchingPairList correspondences,old_correspondences;
CPose2D lastMeanPose;
std::vector<float> other_xs_trans,other_ys_trans; // temporary container of "other" map (map2) transformed by "q"
CMatrixFloat dJ_dq; // The jacobian
CPose2D q; // The updated 2D transformation estimate
CPose2D q_new;
const bool onlyUniqueRobust = options.onlyUniqueRobust;
const bool onlyKeepTheClosest = options.onlyClosestCorrespondences;
// Assure the class of the maps:
ASSERT_(mm1->GetRuntimeClass()->derivedFrom(CLASS_ID(CPointsMap)));
const CPointsMap *m1 = static_cast<const CPointsMap*>(mm1);
// Asserts:
// -----------------
ASSERT_( options.ALFA>0 && options.ALFA<1 );
// The algorithm output auxiliar info:
// -------------------------------------------------
outInfo.nIterations = 0;
outInfo.goodness = 1.0f;
TMatchingParams matchParams;
TMatchingExtraResults matchExtraResults;
matchParams.maxDistForCorrespondence = options.thresholdDist; // Distance threshold
matchParams.maxAngularDistForCorrespondence = options.thresholdAng; // Angular threshold
matchParams.angularDistPivotPoint = TPoint3D(q.x(),q.y(),0); // Pivot point for angular measurements
matchParams.onlyKeepTheClosest = onlyKeepTheClosest;
matchParams.onlyUniqueRobust = onlyUniqueRobust;
matchParams.decimation_other_map_points = options.corresponding_points_decimation;
// The gaussian PDF to estimate:
// ------------------------------------------------------
// First gross approximation:
q = grossEst;
// For LM inverse
CMatrixFixedNumeric<float,3,3> C;
CMatrixFixedNumeric<float,3,3> C_inv; // This will keep the cov. matrix at the end
// Asure maps are not empty!
// ------------------------------------------------------
if ( !m2->isEmpty() )
{
matchParams.offset_other_map_points = 0;
// ------------------------------------------------------
// The ICP loop
// ------------------------------------------------------
do
{
// ------------------------------------------------------
// Find the matching (for a points map)
// ------------------------------------------------------
m1->determineMatching2D(
m2, // The other map
q, // The other map pose
correspondences,
matchParams, matchExtraResults);
nCorrespondences = correspondences.size();
if ( !nCorrespondences )
{
// Nothing we can do !!
keepIteratingICP = false;
}
else
{
// Compute the estimated pose through iterative least-squares: Levenberg-Marquardt
// ----------------------------------------------------------------------
dJ_dq.setSize(3,nCorrespondences); // The jacobian of the error function wrt the transformation q
double lambda = options.LM_initial_lambda; // The LM parameter
double ccos = cos(q.phi());
double csin = sin(q.phi());
double w1,w2,w3;
double q1,q2,q3;
double A,B;
const double Axy = options.Axy_aprox_derivatives; // For approximating the derivatives
// Compute at once the square errors for each point with the current "q" and the transformed points:
std::vector<float> sq_errors;
correspondences.squareErrorVector( q, sq_errors, other_xs_trans,other_ys_trans);
double OSE_initial = math::sum( sq_errors );
// Compute "dJ_dq"
// ------------------------------------
double rho2 = square( options.kernel_rho );
mrpt::utils::TMatchingPairList::iterator it;
std::vector<float>::const_iterator other_x_trans,other_y_trans;
size_t i;
for (i=0,
it=correspondences.begin(),
other_x_trans = other_xs_trans.begin(),
other_y_trans = other_ys_trans.begin();
i<nCorrespondences;
++i, ++it,++other_x_trans,++other_y_trans )
{
// Jacobian: dJ_dx
// --------------------------------------
//#define ICP_DISTANCES_TO_LINE
#ifndef ICP_DISTANCES_TO_LINE
w1= *other_x_trans-Axy;
q1 = m1->squareDistanceToClosestCorrespondence( w1, *other_y_trans );
q1= kernel( q1, rho2 );
w2= *other_x_trans;
q2 = m1->squareDistanceToClosestCorrespondence( w2, *other_y_trans );
q2= kernel( q2, rho2 );
w3= *other_x_trans+Axy;
q3 = m1->squareDistanceToClosestCorrespondence( w3, *other_y_trans );
q3= kernel( q3, rho2 );
#else
// The distance to the line that interpolates the TWO closest points:
float x1,y1, x2,y2, d1,d2;
m1->kdTreeTwoClosestPoint2D(
*other_x_trans, *other_y_trans, // The query
x1, y1, // Closest point #1
x2, y2, // Closest point #2
d1,d2);
w1= *other_x_trans-Axy;
q1 = math::closestSquareDistanceFromPointToLine( w1, *other_y_trans, x1,y1, x2,y2 );
q1= kernel( q1, rho2 );
w2= *other_x_trans;
q2 = math::closestSquareDistanceFromPointToLine( w2, *other_y_trans, x1,y1, x2,y2 );
q2= kernel( q2, rho2 );
w3= *other_x_trans+Axy;
q3 = math::closestSquareDistanceFromPointToLine( w3, *other_y_trans, x1,y1, x2,y2 );
q3= kernel( q3, rho2 );
#endif
//interpolate
A=( (q3-q2)/((w3-w2)*(w3-w1)) ) - ( (q1-q2)/((w1-w2)*(w3-w1)) );
B=( (q1-q2)+(A*((w2*w2)-(w1*w1))) )/(w1-w2);
dJ_dq.get_unsafe(0,i) = (2*A* *other_x_trans)+B;
// Jacobian: dJ_dy
// --------------------------------------
w1= *other_y_trans-Axy;
#ifdef ICP_DISTANCES_TO_LINE
q1 = math::closestSquareDistanceFromPointToLine( *other_x_trans, w1, x1,y1, x2,y2 );
q1= kernel( q1, rho2 );
#else
q1= kernel( square(it->this_x - *other_x_trans)+ square( it->this_y - w1 ), rho2 );
#endif
w2= *other_y_trans;
// q2 is alreay computed from above!
//q2 = m1->squareDistanceToClosestCorrespondence( *other_x_trans, w2 );
//q2= kernel( square(it->this_x - *other_x_trans)+ square( it->this_y - w2 ), rho2 );
w3= *other_y_trans+Axy;
#ifdef ICP_DISTANCES_TO_LINE
q3 = math::closestSquareDistanceFromPointToLine( *other_x_trans, w3, x1,y1, x2,y2 );
q3= kernel( q3, rho2 );
#else
q3= kernel( square(it->this_x - *other_x_trans)+ square( it->this_y - w3 ), rho2 );
#endif
//interpolate
A=( (q3-q2)/((w3-w2)*(w3-w1)) ) - ( (q1-q2)/((w1-w2)*(w3-w1)) );
B=( (q1-q2)+(A*((w2*w2)-(w1*w1))) )/(w1-w2);
dJ_dq.get_unsafe(1,i) = (2*A* *other_y_trans)+B;
// Jacobian: dR_dphi
// --------------------------------------
dJ_dq.get_unsafe(2,i) = dJ_dq.get_unsafe(0,i) * ( -csin * it->other_x - ccos * it->other_y ) +
dJ_dq.get_unsafe(1,i) * ( ccos * it->other_x - csin * it->other_y );
} // end for each corresp.
// Now we have the Jacobian in dJ_dq.
// Compute the Hessian matrix H = dJ_dq * dJ_dq^T
CMatrixFloat H_(3,3);
H_.multiply_AAt(dJ_dq);
CMatrixFixedNumeric<float,3,3> H = CMatrixFixedNumeric<float,3,3>(H_);
bool keepIteratingLM = true;
// ---------------------------------------------------
// Iterate the inner LM loop until convergence:
// ---------------------------------------------------
q_new = q;
std::vector<float> new_sq_errors, new_other_xs_trans, new_other_ys_trans;
size_t nLMiters = 0;
const size_t maxLMiters = 100;
while ( keepIteratingLM && ++nLMiters<maxLMiters)
{
// The LM heuristic is:
// x_{k+1} = x_k - ( H + \lambda diag(H) )^-1 * grad(J)
// grad(J) = dJ_dq * e (vector of errors)
C = H;
for (i=0;i<3;i++)
C(i,i) *= (1+lambda); // Levenberg-Maquardt heuristic
C_inv = C.inv();
// LM_delta = C_inv * dJ_dq * sq_errors
Eigen::VectorXf dJsq, LM_delta;
dJ_dq.multiply_Ab( Eigen::Map<Eigen::VectorXf>(&sq_errors[0],sq_errors.size()), dJsq );
C_inv.multiply_Ab(dJsq,LM_delta);
q_new.x( q.x() - LM_delta[0] );
q_new.y( q.y() - LM_delta[1] );
q_new.phi( q.phi() - LM_delta[2] );
// Compute the new square errors:
// ---------------------------------------
correspondences.squareErrorVector(
q_new,
new_sq_errors,
new_other_xs_trans,
new_other_ys_trans);
float OSE_new = math::sum( new_sq_errors );
bool improved = OSE_new < OSE_initial;
#if 0 // Debuggin'
cout << "_____________" << endl;
cout << "q -> q_new : " << q << " -> " << q_new << endl;
printf("err: %f -> %f lambda: %e\n", OSE_initial ,OSE_new, lambda );
cout << "\\/J = "; utils::operator <<(cout,dJsq); cout << endl;
mrpt::system::pause();
#endif
keepIteratingLM =
fabs(LM_delta[0])>options.minAbsStep_trans ||
fabs(LM_delta[1])>options.minAbsStep_trans ||
fabs(LM_delta[2])>options.minAbsStep_rot;
if(improved)
{
//If resids have gone down, keep change and make lambda smaller by factor of 10
lambda/=10;
q=q_new;
OSE_initial = OSE_new;
}
else
{
// Discard movement and try with larger lambda:
lambda*=10;
}
} // end iterative LM
#if 0 // Debuggin'
cout << "ICP loop: " << lastMeanPose << " -> " << q << " LM iters: " << nLMiters << " threshold: " << matchParams.maxDistForCorrespondence << endl;
#endif
// --------------------------------------------------------
// now the conditions for the outer ICP loop
// --------------------------------------------------------
keepIteratingICP = true;
if (fabs(lastMeanPose.x()-q.x())<options.minAbsStep_trans &&
fabs(lastMeanPose.y()-q.y())<options.minAbsStep_trans &&
fabs( math::wrapToPi( lastMeanPose.phi()-q.phi()) )<options.minAbsStep_rot)
{
matchParams.maxDistForCorrespondence *= options.ALFA;
matchParams.maxAngularDistForCorrespondence *= options.ALFA;
if (matchParams.maxDistForCorrespondence < options.smallestThresholdDist )
keepIteratingICP = false;
if (++matchParams.offset_other_map_points>=options.corresponding_points_decimation)
matchParams.offset_other_map_points=0;
}
lastMeanPose = q;
} // end of "else, there are correspondences"
// Next iteration:
outInfo.nIterations++;
if (outInfo.nIterations >= options.maxIterations && matchParams.maxDistForCorrespondence>options.smallestThresholdDist)
{
matchParams.maxDistForCorrespondence *= options.ALFA;
}
} while ( (keepIteratingICP && outInfo.nIterations<options.maxIterations) ||
(outInfo.nIterations >= options.maxIterations && matchParams.maxDistForCorrespondence>options.smallestThresholdDist) );
outInfo.goodness = matchExtraResults.correspondencesRatio;
//if (!options.skip_quality_calculation)
{
outInfo.quality= matchExtraResults.correspondencesRatio;
}
} // end of "if m2 is not empty"
return CPosePDFGaussianPtr( new CPosePDFGaussian(q, C_inv.cast<double>() ) );
MRPT_END
}
bool mrpt::poses::operator!=(const CPose2D &p1,const CPose2D &p2)
{
return (p1.x()!=p2.x())||(p1.y()!=p2.y())||(p1.phi()!=p2.phi());
}
double CPose2D::distance2DFrobeniusTo( const CPose2D & p) const
{
return std::sqrt(square(p.x()-x())+square(p.y()-y())+4*(1-cos(p.phi()-phi())));
}
bool mrpt::poses::operator==(const CPose2D &p1,const CPose2D &p2)
{
return (p1.x()==p2.x())&&(p1.y()==p2.y())&&(p1.phi()==p2.phi());
}
/*---------------------------------------------------------------
getEstimatedPoseState
---------------------------------------------------------------*/
TExtendedCPose2D CPosePDFParticlesExtended::getEstimatedPoseState() const
{
TExtendedCPose2D est;
CPose2D p;
size_t i,n = m_particles.size();
double phi;
double w,W=0;
double W_phi_R=0,W_phi_L=0;
double phi_R=0,phi_L=0;
if (!n) return est;
est.state.resize( m_particles[0].d->state.size() );
for (i=0;i<n;i++) W += exp(m_particles[i].log_w);
if (W==0) W=1;
// First: XY
// -----------------------------------
for (i=0;i<n;i++)
{
p = m_particles[i].d->pose;
w = exp(m_particles[i].log_w) / W;
est.pose.x_incr( (p.x() * w));
est.pose.y_incr( (p.y() * w));
vector<double> auxVec (m_particles[i].d->state);
auxVec *= w;
est.state += auxVec;
// 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.pose *= (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.pose.phi( ((phi_L * W_phi_L + phi_R * W_phi_R )/(W_phi_L+W_phi_R)) );
return est;
}
/* --------------------------------------------------------
thread_path_tracking
Kalman-Filter tracking of the current robot state
-------------------------------------------------------- */
void CPRRTNavigator::thread_path_tracking()
{
// Set highest priority
mrpt::system::changeCurrentProcessPriority(ppVeryHigh);
mrpt::system::changeThreadPriority( mrpt::system::getCurrentThreadHandle(),tpHighest );
cout << "[CPRRTNavigator:thread_path_tracking] Thread alive.\n";
const double DESIRED_RATE = 15.0;
const double DESIRED_PERIOD = 1.0/DESIRED_RATE;
TTimeStamp tim_last_iter = INVALID_TIMESTAMP;
// Buffered data:
TPathData next_planned_point; // target
TTimeStamp planned_path_time = INVALID_TIMESTAMP;
while (!m_closingThreads)
{
if ( !m_initialized ) // Do nothing until we're loaded and ready.
{
mrpt::system::sleep(100);
continue;
}
// Acquire the latest path plan --------------
{
CCriticalSectionLocker lock(& m_planned_path_cs );
if (m_planned_path_time==INVALID_TIMESTAMP || m_planned_path.empty())
{
// There's no plan: Stop the robot:
planned_path_time = INVALID_TIMESTAMP;
}
else // Only update our copy if needed:
{
planned_path_time = m_planned_path_time;
next_planned_point = m_planned_path.front();
}
} // end of CS
// Path following ------------------------------
if (planned_path_time == INVALID_TIMESTAMP)
{
// Stop the robot, now.
onMotionCommand(0,0);
}
else
{
const bool ignore_trg_heading = INVALID_HEADING==next_planned_point.p.phi;
// Acquire the current estimate of the robot state:
// ----------------------------------------------------
TPose2D robotPose;
float robot_v,robot_w;
if (!m_robotStateFilter.getCurrentEstimate(robotPose,robot_v,robot_w)) // Thread-safe call
{
// Error: Stop the robot, now.
onMotionCommand(0,0);
}
else
{ // we have proper localization
// ==============================================================================
// We want to approach to "next_planned_point"
// - Apply a sequence of tests to discover the shortests & easiest movement
// to that target.
// ==============================================================================
// Compute target in coordinates relative to the robot just now:
const CPose2D trg_rel = CPose2D(next_planned_point.p) - CPose2D(robotPose);
// if (next_planned_point.p.y==-1) {
// int a=0;
// }
// Current distances to target:
const double trg_dist_lin = trg_rel.norm();
const double trg_dist_ang = std::abs(trg_rel.phi());
// Remaining Estimated Time for Arrival
double ETA_target;
if (std::abs(robot_v)>1e-4)
ETA_target = std::max(0.05, (trg_dist_lin-params.pathtrack.radius_checkpoints)/std::abs(robot_v) );
else if (std::abs(robot_w)>1e-4)
ETA_target = trg_dist_ang/std::abs(robot_w);
else ETA_target = 1000.0;
//cout << format("ETA: %f\n",ETA_target);
//cout << format("d_lin=%f d_ang=%f\n",trg_dist_lin,RAD2DEG(trg_dist_ang));
// If we have reached one point, remove it from the front of the list:
if ((trg_dist_lin<params.pathtrack.radius_checkpoints || trg_dist_lin<std::abs(robot_v)*3*DESIRED_PERIOD )&&
(ignore_trg_heading || trg_dist_ang<DEG2RAD(10) || trg_dist_ang<std::abs(robot_w)*3*DESIRED_PERIOD )
)
{
CCriticalSectionLocker lock(& m_planned_path_cs );
if (m_planned_path_time==planned_path_time)
m_planned_path.pop_front();
}
else
{
// ------------------------------------------------------------------------------------
// CASE 1: Direct arc.
// (tx,ty,ta) all are compatible with a direct arc from our current pose
// ------------------------------------------------------------------------------------
bool good_cmd_found = false;
double good_cmd_v=0,good_cmd_w=0;
{
// predicted heading at (tx,ty):
double predict_phi;
double cmd_v=0,cmd_w=0;
if (trg_rel.y()==0)
{ // In real-world conditions this might never happen, but just in case:
predict_phi = 0;
cmd_v = next_planned_point.max_v * sign(trg_rel.x());
cmd_w = 0;
}
else
{
const double R = square(trg_dist_lin)/(2*trg_rel.y());
// predicted heading at (tx,ty):
predict_phi = atan2(trg_rel.x(),R-trg_rel.y());
cmd_v = next_planned_point.max_v * sign(trg_rel.x());
cmd_w = cmd_v/R;
}
// Good enough?
if (ignore_trg_heading ||
std::abs( wrapToPi(trg_rel.phi()-predict_phi))<DEG2RAD(7) )
{
good_cmd_found = true;
good_cmd_v=cmd_v;
good_cmd_w=cmd_w;
}
} // end case 1
// ------------------------------------------
// Scale velocities to the actual ranges:
// ------------------------------------------
if (good_cmd_found)
{
// SCALE: For maximum segment speeds:
// ----------------------------------------
if (std::abs(good_cmd_v)>next_planned_point.max_v)
{
const double K = next_planned_point.max_v / std::abs(good_cmd_v);
good_cmd_v *= K;
good_cmd_w *= K;
}
if (std::abs(good_cmd_w)>next_planned_point.max_w)
{
const double K = next_planned_point.max_w / std::abs(good_cmd_w);
good_cmd_v *= K;
good_cmd_w *= K;
}
// SCALE: Take into account the desired velocities when arriving at the target.
// ------------------------------------------------------------------------------
const double Av_to_trg = next_planned_point.trg_v - robot_v;
const double min_At_to_change_to_trg_v = std::abs(Av_to_trg)/params.max_accel_v;
if (good_cmd_v!=0 && ETA_target<=min_At_to_change_to_trg_v)
{
// We have to adapt velocity so we can get to the target right:
const double new_v = next_planned_point.trg_v - (ETA_target/min_At_to_change_to_trg_v)*Av_to_trg;
const double K = new_v / std::abs(good_cmd_v);
good_cmd_v *= K;
good_cmd_w *= K;
}
// SCALE: Is the command compatible with the maximum accelerations, wrt the current speed??
// ----------------------------------------
const double max_Av = params.max_accel_v * 0.2 /*sec*/;
const double max_Aw = params.max_accel_w * 0.2 /*sec*/;
if ( std::abs( good_cmd_v - robot_v )>max_Av )
{
if (good_cmd_v!=0)
{
const double rho = good_cmd_w/good_cmd_v;
good_cmd_v = (good_cmd_v>robot_v) ? robot_v+max_Av : robot_v-max_Av;
good_cmd_w = rho*good_cmd_v;
}
}
if ( std::abs( good_cmd_w - robot_w )>max_Aw )
{
if (good_cmd_w!=0)
{
const double R = good_cmd_v/good_cmd_w;
good_cmd_w = (good_cmd_w>robot_w) ? robot_w+max_Aw : robot_w-max_Aw;
good_cmd_v = R*good_cmd_w;
}
}
// Ok, send the command:
// ----------------------------------------
onMotionCommand(good_cmd_v,good_cmd_w);
}
else
{
// No valid motion found??!!
// TODO: Raise some error.
onMotionCommand(0,0);
}
} // end we haven't reached the target
} // end we have proper localization
} // end do path following
// Run at XX Hz -------------------------------
const TTimeStamp tim_now = now();
int delay_ms;
if (tim_last_iter != INVALID_TIMESTAMP)
{
const double tim_since_last = mrpt::system::timeDifference(tim_last_iter,tim_now);
delay_ms = std::max(1, round( 1000.0*(DESIRED_PERIOD-tim_since_last) ) );
}
else delay_ms = 20;
tim_last_iter = tim_now; // for the next iter.
mrpt::system::sleep(delay_ms); // do the delay
};
cout << "[CPRRTNavigator:thread_path_tracking] Exit.\n";
}
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-beacons - 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 );
randomGenerator.randomize();
int i;
char auxStr[2000];
// Set default values:
int nBeacons = 3;
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 odometryBias_Y = 0;
float minSensorDistance = 0;
float maxSensorDistance = 10;
float stdError = 0.04f;
bool circularPath = true;
int squarePathLength=40;
float ratio_outliers = 0;
float ratio_outliers_first_step = 0;
float outlier_uniform_min=0;
float outlier_uniform_max=5.0;
// Load params from INI:
MRPT_LOAD_CONFIG_VAR(nBeacons,int, ini,"Params");
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(squarePathLength,int,ini,"Params");
MRPT_LOAD_CONFIG_VAR(ratio_outliers,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(ratio_outliers_first_step,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(outlier_uniform_min,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(outlier_uniform_max,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(min_x,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(max_x,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(min_y,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(max_y,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(min_z,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(odometryNoiseXY_std,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(odometryBias_Y,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(minSensorDistance,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(maxSensorDistance,float, ini,"Params");
MRPT_LOAD_CONFIG_VAR(stdError,float, ini,"Params");
// Create out dir:
mrpt::system::createDirectory(outDir);
// ---------------------------------------------
// Create the point-beacons:
// ---------------------------------------------
printf("Creating beacon map...");
mrpt::slam::CBeaconMap beaconMap;
for (i=0;i<nBeacons;i++)
{
CBeacon b;
CPoint3D pt3D;
// Random coordinates:
pt3D.x( randomGenerator.drawUniform(min_x,max_x) );
pt3D.y( randomGenerator.drawUniform(min_y,max_y) );
pt3D.z( randomGenerator.drawUniform(min_z,max_z) );
// Add:
b.m_typePDF=CBeacon::pdfMonteCarlo;
b.m_locationMC.setSize(1,pt3D);
b.m_ID = i;
beaconMap.push_back(b);
}
os::sprintf(auxStr,sizeof(auxStr),"%s/outSimMap.txt",outDir.c_str());
beaconMap.saveToTextFile(auxStr);
printf("Done!\n");
//beaconMap.simulateBeaconReadings( );
// ---------------------------------------------
// Simulate:
// ---------------------------------------------
CActionRobotMovement2D::TMotionModelOptions opts;
CPoint3D null3D(0,0,0);
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 = DEG2RAD( 0.002f );
os::sprintf(auxStr,sizeof(auxStr),"%s/%s",outDir.c_str(),outFile.c_str());
CFileOutputStream fil(auxStr);
CPose2D realPose;
CPose2D incPose;
int stopSteps = 4;
for (i=0;i<nSteps;i++)
{
printf("Generating step %i...",i);
CSensoryFrame SF;
CActionCollection acts;
CActionRobotMovement2D act;
CPose3D pose3D( realPose );
if (i>=stopSteps)
{
if (circularPath)
{
// Circular path:
float Ar = DEG2RAD(5);
incPose = CPose2D(0.20f*cos(Ar),0.20f*sin(Ar),Ar);
}
else
{
// Square path:
if ( (i % squarePathLength) > (squarePathLength-5) )
incPose = CPose2D(0,0,DEG2RAD(90.0f/4));
else incPose = CPose2D(0.20f,0,0);
}
}
else incPose = CPose2D(0,0,0);
// Simulate observations:
CObservationBeaconRangesPtr obs=CObservationBeaconRanges::Create();
obs->minSensorDistance=minSensorDistance;
obs->maxSensorDistance=maxSensorDistance;
obs->stdError=stdError;
beaconMap.simulateBeaconReadings( pose3D,null3D,*obs );
// Corrupt with ourliers:
float probability_corrupt = i==0 ? ratio_outliers_first_step : ratio_outliers;
for (size_t q=0;q<obs->sensedData.size();q++)
{
if ( randomGenerator.drawUniform(0.0f,1.0f) < probability_corrupt )
{
obs->sensedData[q].sensedDistance += randomGenerator.drawUniform( outlier_uniform_min,outlier_uniform_max );
if (obs->sensedData[q].sensedDistance<0)
obs->sensedData[q].sensedDistance = 0;
}
}
std::cout << obs->sensedData.size() << " beacons at range";
SF.push_back( obs );
// Simulate actions:
CPose2D incOdo( incPose );
if (incPose.x()!=0 || incPose.y()!=0 || incPose.phi()!=0)
{
incOdo.x_incr( randomGenerator.drawGaussian1D_normalized() * odometryNoiseXY_std );
incOdo.y_incr( odometryBias_Y + randomGenerator.drawGaussian1D_normalized() * odometryNoiseXY_std );
}
act.computeFromOdometry( incOdo, opts);
acts.insert( act );
// Save:
fil << SF << acts;
// Next pose:
realPose = realPose + incPose;
printf("\n");
}
cout << "Data saved to directory: " << outDir << endl;
}
catch(std::exception &e)
{
std::cout << e.what();
}
catch(...)
{
std::cout << "Untyped exception!";
}
}
void SRBASolver::publishGraphVisualization(visualization_msgs::MarkerArray &marray)
{
ROS_INFO("Visualizing");
// Vertices are round, red spheres
visualization_msgs::Marker m;
m.header.frame_id = relative_map_frame_;
m.header.stamp = ros::Time::now();
m.id = 0;
m.ns = "karto";
m.type = visualization_msgs::Marker::ARROW;
m.pose.position.x = 0.0;
m.pose.position.y = 0.0;
m.pose.position.z = 0.0;
m.scale.x = 0.15;
m.scale.y = 0.15;
m.scale.z = 0.15;
m.color.r = 1.0;
m.color.g = 0;
m.color.b = 0.0;
m.color.a = 1.0;
m.lifetime = ros::Duration(0);
// Odometry edges are opaque blue line strips
visualization_msgs::Marker edge;
edge.header.frame_id = relative_map_frame_;
edge.header.stamp = ros::Time::now();
edge.action = visualization_msgs::Marker::ADD;
edge.ns = "karto";
edge.id = 0;
edge.type = visualization_msgs::Marker::LINE_STRIP;
edge.scale.x = 0.1;
edge.scale.y = 0.1;
edge.scale.z = 0.1;
edge.color.a = 1.0;
edge.color.r = 0.0;
edge.color.g = 0.0;
edge.color.b = 1.0;
// Loop edges are purple, opacity depends on backend state
visualization_msgs::Marker loop_edge;
loop_edge.header.frame_id = relative_map_frame_;
loop_edge.header.stamp = ros::Time::now();
loop_edge.action = visualization_msgs::Marker::ADD;
loop_edge.ns = "spanning_tree";
loop_edge.id = 0;
loop_edge.type = visualization_msgs::Marker::LINE_STRIP;
loop_edge.scale.x = 0.1;
loop_edge.scale.y = 0.1;
loop_edge.scale.z = 0.1;
loop_edge.color.a = 1.0;
loop_edge.color.r = 1.0;
loop_edge.color.g = 0.0;
loop_edge.color.b = 1.0;
visualization_msgs::Marker node_text;
node_text.header.frame_id = relative_map_frame_;
node_text.header.stamp = ros::Time::now();
node_text.action = visualization_msgs::Marker::ADD;
node_text.ns = "karto";
node_text.id = 0;
node_text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
node_text.scale.z = 0.3;
node_text.color.a = 1.0;
node_text.color.r = 1.0;
node_text.color.g = 1.0;
node_text.color.b = 1.0;
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:
srba_t::frameid2pose_map_t spantree;
if(curr_kf_id_ == 0)
return;
TKeyFrameID root_keyframe(curr_kf_id_ -1 );
rba_.create_complete_spanning_tree(root_keyframe,spantree);
int id = 0;
for (srba_t::frameid2pose_map_t::const_iterator itP = spantree.begin();itP!=spantree.end();++itP)
{
if (root_keyframe==itP->first) continue;
const CPose2D p = itP->second.pose;
// Add the vertex to the marker array
m.id = id;
m.pose.position.x = p.x();
m.pose.position.y = p.y();
geometry_msgs::Quaternion q = tf::createQuaternionMsgFromYaw(p.phi());
m.pose.orientation = q;
marray.markers.push_back(visualization_msgs::Marker(m));
id++;
node_text.id = id;
node_text.text= boost::lexical_cast<std::string>(itP->first);
node_text.pose.position.x = p.x()+0.15;
node_text.pose.position.y = p.y()+0.15;
marray.markers.push_back(visualization_msgs::Marker(node_text));
id++;
}
for (srba_t::rba_problem_state_t::k2k_edges_deque_t::const_iterator itEdge = rba_.get_rba_state().k2k_edges.begin();
itEdge!=rba_.get_rba_state().k2k_edges.end();++itEdge)
{
CPose2D p1, p2;
if(itEdge->from != root_keyframe)
{
srba_t::frameid2pose_map_t::const_iterator itN1 = spantree.find(itEdge->from);
if(itN1==spantree.end())
continue;
p1 = itN1->second.pose;
}
if(itEdge->to != root_keyframe)
{
srba_t::frameid2pose_map_t::const_iterator itN2 = spantree.find(itEdge->to);
if(itN2==spantree.end())
continue;
p2 = itN2->second.pose;
}
geometry_msgs::Point pt1, pt2;
pt1.x = p1.x();
pt1.y = p1.y();
pt2.x = p2.x();
pt2.y = p2.y();
loop_edge.points.clear();
loop_edge.points.push_back(pt1);
loop_edge.points.push_back(pt2);
loop_edge.id = id;
marray.markers.push_back(visualization_msgs::Marker(loop_edge));
id++;
}
// Render landmark as pose constraint
// For each KF: check all its "observations"
for (srba_t::frameid2pose_map_t::const_iterator it=spantree.begin();it!=spantree.end();++it)
{
const TKeyFrameID kf_id = it->first;
const srba_t::pose_flag_t & pf = it->second;
const typename srba_t::keyframe_info &kfi = rba_.get_rba_state().keyframes[kf_id];
for (size_t i=0;i<kfi.adjacent_k2f_edges.size();i++)
{
const srba_t::k2f_edge_t * k2f = kfi.adjacent_k2f_edges[i];
const TKeyFrameID other_kf_id = k2f->feat_rel_pos->id_frame_base;
if (kf_id==other_kf_id)
continue; // It's not an constraint with ANOTHER keyframe
// Is the other KF in the spanning tree?
srba_t::frameid2pose_map_t::const_iterator other_it=spantree.find(other_kf_id);
if (other_it==spantree.end()) continue;
const srba_t::pose_flag_t & other_pf = other_it->second;
// Add edge between the two KFs to represent the pose constraint:
mrpt::poses::CPose2D p1 = mrpt::poses::CPose2D(pf.pose); // Convert to 3D
mrpt::poses::CPose2D p2 = mrpt::poses::CPose2D(other_pf.pose);
geometry_msgs::Point pt1, pt2;
pt1.x = p1.x();
pt1.y = p1.y();
pt2.x = p2.x();
pt2.y = p2.y();
edge.points.clear();
edge.points.push_back(pt1);
edge.points.push_back(pt2);
edge.id = id;
marray.markers.push_back(visualization_msgs::Marker(edge));
id++;
}
} // end for each KF
// Delete any excess markers whose ids haven't been reclaimed
m.action = visualization_msgs::Marker::DELETE;
for (; id < marker_count_; id++)
{
m.id = id;
marray.markers.push_back(visualization_msgs::Marker(m));
}
marker_count_ = marray.markers.size();
}
else
ROS_INFO("Graph is empty");
}
