void ffff(const CVectorDouble &x,const CQuaternionDouble &Q, CVectorDouble &OUT)
{
OUT.resize(3);
CQuaternionDouble q(x[0],x[1],x[2],x[3]);
q.normalize();
q.rpy(OUT[2],OUT[1],OUT[0]);
}
/*---------------------------------------------------------------
getAsVector
---------------------------------------------------------------*/
void CPose2D::getAsVector(CVectorDouble &v) const
{
v.resize(3);
v[0]=m_coords[0];
v[1]=m_coords[1];
v[2]=m_phi;
}
// The error function F(x):
void myFunction(
const CVectorDouble& x, const CVectorDouble& y, CVectorDouble& out_f)
{
out_f.resize(1);
// 1-cos(x+1) *cos(x*y+1)
out_f[0] = 1 - cos(x[0] + 1) * cos(x[0] * x[1] + 1);
}
/** Returns a 1x7 vector with [x y z qr qx qy qz] */
void CPose3DQuat::getAsVector(CVectorDouble &v) const
{
v.resize(7);
v[0] = m_coords[0];
v[1] = m_coords[1];
v[2] = m_coords[2];
v[3] = m_quat[0];
v[4] = m_quat[1];
v[5] = m_quat[2];
v[6] = m_quat[3];
}
/*---------------------------------------------------------------
getCurrentState
---------------------------------------------------------------*/
void CRangeBearingKFSLAM::getCurrentState(
CPose3DQuatPDFGaussian& out_robotPose,
std::vector<TPoint3D>& out_landmarksPositions,
std::map<unsigned int, CLandmark::TLandmarkID>& out_landmarkIDs,
CVectorDouble& out_fullState, CMatrixDouble& out_fullCovariance) const
{
MRPT_START
ASSERT_(size_t(m_xkk.size()) >= get_vehicle_size());
// Copy xyz+quat: (explicitly unroll the loop)
out_robotPose.mean.m_coords[0] = m_xkk[0];
out_robotPose.mean.m_coords[1] = m_xkk[1];
out_robotPose.mean.m_coords[2] = m_xkk[2];
out_robotPose.mean.m_quat[0] = m_xkk[3];
out_robotPose.mean.m_quat[1] = m_xkk[4];
out_robotPose.mean.m_quat[2] = m_xkk[5];
out_robotPose.mean.m_quat[3] = m_xkk[6];
// and cov:
m_pkk.extractMatrix(0, 0, out_robotPose.cov);
// Landmarks:
ASSERT_(((m_xkk.size() - get_vehicle_size()) % get_feature_size()) == 0);
size_t i, nLMs = (m_xkk.size() - get_vehicle_size()) / get_feature_size();
out_landmarksPositions.resize(nLMs);
for (i = 0; i < nLMs; i++)
{
out_landmarksPositions[i].x =
m_xkk[get_vehicle_size() + i * get_feature_size() + 0];
out_landmarksPositions[i].y =
m_xkk[get_vehicle_size() + i * get_feature_size() + 1];
out_landmarksPositions[i].z =
m_xkk[get_vehicle_size() + i * get_feature_size() + 2];
} // end for i
// IDs:
out_landmarkIDs = m_IDs.getInverseMap(); // m_IDs_inverse;
// Full state:
out_fullState.resize(m_xkk.size());
for (KFVector::Index i = 0; i < m_xkk.size(); i++)
out_fullState[i] = m_xkk[i];
// Full cov:
out_fullCovariance = m_pkk;
MRPT_END
}
/*---------------------------------------------------------------
getCurrentState
---------------------------------------------------------------*/
void CRangeBearingKFSLAM2D::getCurrentState(
CPosePDFGaussian &out_robotPose,
std::vector<TPoint2D> &out_landmarksPositions,
std::map<unsigned int,CLandmark::TLandmarkID> &out_landmarkIDs,
CVectorDouble &out_fullState,
CMatrixDouble &out_fullCovariance ) const
{
MRPT_START
ASSERT_(m_xkk.size()>=3);
// Set 6D pose mean:
out_robotPose.mean = CPose2D(m_xkk[0],m_xkk[1],m_xkk[2]);
// and cov:
CMatrixTemplateNumeric<kftype> COV(3,3);
m_pkk.extractMatrix(0,0,COV);
out_robotPose.cov = COV;
// Landmarks:
ASSERT_( ((m_xkk.size() - 3) % 2)==0 );
size_t i,nLMs = (m_xkk.size() - 3) / 2;
out_landmarksPositions.resize(nLMs);
for (i=0;i<nLMs;i++)
{
out_landmarksPositions[i].x = m_xkk[3+i*2+0];
out_landmarksPositions[i].y = m_xkk[3+i*2+1];
} // end for i
// IDs:
out_landmarkIDs = m_IDs.getInverseMap(); //m_IDs_inverse;
// Full state:
out_fullState.resize( m_xkk.size() );
for (KFVector::Index i=0;i<m_xkk.size();i++)
out_fullState[i] = m_xkk[i];
// Full cov:
out_fullCovariance = m_pkk;
MRPT_END
}
/*---------------------------------------------------------------
interpolate
---------------------------------------------------------------*/
CPose3D & CPose3DInterpolator::interpolate( mrpt::system::TTimeStamp t, CPose3D &out_interp, bool &out_valid_interp ) const
{
TTimePosePair p1, p2, p3, p4;
// Invalid?
if (t==INVALID_TIMESTAMP)
{
out_valid_interp = false;
return out_interp;
}
// Out of range?
const_iterator it_ge1 = m_path.lower_bound( t );
// Exact match?
if( it_ge1 != m_path.end() && it_ge1->first == t )
{
out_interp = it_ge1->second;
out_valid_interp = true;
return out_interp;
}
// Are we in the beginning or the end of the path?
if( it_ge1 == m_path.end() || it_ge1 == m_path.begin() )
{
out_valid_interp = false;
out_interp.setFromValues(0,0,0);
return out_interp;
} // end
p3 = *it_ge1; // Third pair
++it_ge1;
if( it_ge1 == m_path.end() )
{
out_valid_interp = false;
out_interp.setFromValues(0,0,0);
return out_interp;
}
p4 = *(it_ge1); // Fourth pair
--it_ge1;
p2 = *(--it_ge1); // Second pair
if( it_ge1 == m_path.begin() )
{
out_valid_interp = false;
out_interp.setFromValues(0,0,0);
return out_interp;
}
p1 = *(--it_ge1); // First pair
// Test if the difference between the desired timestamp and the next timestamp is lower than a certain (configurable) value
if( maxTimeInterpolation > 0 &&
( (p4.first - p3.first)/1e7 > maxTimeInterpolation ||
(p3.first - p2.first)/1e7 > maxTimeInterpolation ||
(p2.first - p1.first)/1e7 > maxTimeInterpolation ))
{
out_valid_interp = false;
out_interp.setFromValues(0,0,0);
return out_interp;
}
// Do interpolation:
// ------------------------------------------
// First Previous point: p1
// Second Previous point: p2
// First Next point: p3
// Second Next point: p4
// Time where to interpolate: t
double td = mrpt::system::timestampTotime_t(t);
CVectorDouble ts;
ts.resize(4);
ts[0] = mrpt::system::timestampTotime_t(p1.first);
ts[1] = mrpt::system::timestampTotime_t(p2.first);
ts[2] = mrpt::system::timestampTotime_t(p3.first);
ts[3] = mrpt::system::timestampTotime_t(p4.first);
CVectorDouble X,Y,Z,yaw,pitch,roll;
X.resize(4); Y.resize(4); Z.resize(4);
X[0] = p1.second.x(); Y[0] = p1.second.y(); Z[0] = p1.second.z();
X[1] = p2.second.x(); Y[1] = p2.second.y(); Z[1] = p2.second.z();
X[2] = p3.second.x(); Y[2] = p3.second.y(); Z[2] = p3.second.z();
X[3] = p4.second.x(); Y[3] = p4.second.y(); Z[3] = p4.second.z();
yaw.resize(4); pitch.resize(4); roll.resize(4);
yaw[0] = p1.second.yaw(); pitch[0] = p1.second.pitch(); roll[0] = p1.second.roll();
yaw[1] = p2.second.yaw(); pitch[1] = p2.second.pitch(); roll[1] = p2.second.roll();
yaw[2] = p3.second.yaw(); pitch[2] = p3.second.pitch(); roll[2] = p3.second.roll();
yaw[3] = p4.second.yaw(); pitch[3] = p4.second.pitch(); roll[3] = p4.second.roll();
unwrap2PiSequence(yaw);
unwrap2PiSequence(pitch);
unwrap2PiSequence(roll);
// Target interpolated values:
double int_x,int_y,int_z,int_yaw,int_pitch,int_roll;
switch (m_method)
{
case imSpline:
{
// ---------------------------------------
// SPLINE INTERPOLATION
// ---------------------------------------
int_x = math::spline(td, ts, X);
int_y = math::spline(td, ts, Y);
int_z = math::spline(td, ts, Z);
int_yaw = math::spline(td, ts, yaw, true ); // Wrap 2pi
int_pitch = math::spline(td, ts, pitch, true );
int_roll = math::spline(td, ts, roll, true );
}
break;
case imLinear2Neig:
{
int_x = math::interpolate2points(td, ts[1],X[1],ts[2],X[2]);
int_y = math::interpolate2points(td, ts[1],Y[1],ts[2],Y[2]);
int_z = math::interpolate2points(td, ts[1],Z[1],ts[2],Z[2]);
int_yaw = math::interpolate2points(td, ts[1],yaw[1],ts[2],yaw[2], true ); // Wrap 2pi
int_pitch = math::interpolate2points(td, ts[1],pitch[1],ts[2],pitch[2], true );
int_roll = math::interpolate2points(td, ts[1],roll[1],ts[2],roll[2], true );
}
break;
case imLinear4Neig:
{
int_x = math::leastSquareLinearFit(td, ts, X);
int_y = math::leastSquareLinearFit(td, ts, Y);
int_z = math::leastSquareLinearFit(td, ts, Z);
int_yaw = math::leastSquareLinearFit(td, ts, yaw, true ); // Wrap 2pi
int_pitch = math::leastSquareLinearFit(td, ts, pitch, true );
int_roll = math::leastSquareLinearFit(td, ts, roll, true );
}
break;
case imSSLLLL:
{
int_x = math::spline(td, ts, X);
int_y = math::spline(td, ts, Y);
int_z = math::leastSquareLinearFit(td, ts, Z);
int_yaw = math::leastSquareLinearFit(td, ts, yaw, true ); // Wrap 2pi
int_pitch = math::leastSquareLinearFit(td, ts, pitch, true );
int_roll = math::leastSquareLinearFit(td, ts, roll, true );
}
break;
case imSSLSLL:
{
int_x = math::spline(td, ts, X);
int_y = math::spline(td, ts, Y);
int_z = math::leastSquareLinearFit(td, ts, Z);
int_yaw = math::spline(td, ts, yaw, true ); // Wrap 2pi
int_pitch = math::leastSquareLinearFit(td, ts, pitch, true );
int_roll = math::leastSquareLinearFit(td, ts, roll, true );
}
break;
case imLinearSlerp:
{
int_x = math::interpolate2points(td, ts[1],X[1],ts[2],X[2]);
int_y = math::interpolate2points(td, ts[1],Y[1],ts[2],Y[2]);
int_z = math::interpolate2points(td, ts[1],Z[1],ts[2],Z[2]);
const CPose3D aux1(0,0,0,yaw[1],pitch[1],roll[1]);
const CPose3D aux2(0,0,0,yaw[2],pitch[2],roll[2]);
CPose3D q_interp;
const double ratio = (td-ts[1])/(ts[2]-ts[1]);
mrpt::math::slerp(aux1,aux2, ratio, q_interp);
q_interp.getYawPitchRoll(int_yaw,int_pitch,int_roll);
}
break;
case imSplineSlerp:
{
int_x = math::spline(td, ts, X);
int_y = math::spline(td, ts, Y);
int_z = math::spline(td, ts, Z);
const CPose3D aux1(0,0,0,yaw[1],pitch[1],roll[1]);
const CPose3D aux2(0,0,0,yaw[2],pitch[2],roll[2]);
CPose3D q_interp;
const double ratio = (td-ts[1])/(ts[2]-ts[1]);
mrpt::math::slerp(aux1,aux2, ratio, q_interp);
q_interp.getYawPitchRoll(int_yaw,int_pitch,int_roll);
}
break;
default: THROW_EXCEPTION("Unknown value for interpolation method!");
}; // end switch
out_interp.setFromValues(int_x, int_y, int_z, int_yaw, int_pitch, int_roll);
out_valid_interp = true;
return out_interp;
} // end interpolate
