//----------------------------------------------------------------------------
// Summary: define the reference member's coordinate system
// Parameters: quaternion and position vector relative to the origin
// Returns: none
//----------------------------------------------------------------------------
void dmSystem::setRefSystem(dmQuaternion quat, CartesianVector pos)
{
normalizeQuat(quat);
m_quat_ICS[0] = quat[0];
m_quat_ICS[1] = quat[1];
m_quat_ICS[2] = quat[2];
m_quat_ICS[3] = quat[3];
m_p_ICS[0] = pos[0];
m_p_ICS[1] = pos[1];
m_p_ICS[2] = pos[2];
buildRotMat(quat, m_R_ICS);
}
void Filter3D<RealT>::operator()(std::string const& id, cv::Mat& measuredTrans, cv::Mat& measuredRot)
{
//Create&insert or return the related filter
//Second set of 4 params are for Kalman filter: # of state dimensions, # of measurement dimensions,
//# of control input dimensions and float precision of internal matrices
auto pair = mFilters.emplace(std::piecewise_construct,
std::make_tuple(id),
std::make_tuple(7, 7, 3, CV_TYPE));
cv::KalmanFilter& filter = pair.first->second.filter;
cv::Vec<RealT,4>& prevQuat = pair.first->second.prevQuat;
bool& deleted = pair.first->second.deleted;
//Newly inserted or lazy-deleted
if(pair.second || deleted){
deleted = false;
initFilter(filter, prevQuat, measuredTrans, measuredRot);
}
//Already existing filter
else{
RealT* state = (RealT*)mTempState.ptr();
double* trans = (double*)measuredTrans.ptr();
//Fill state
state[0] = (RealT)trans[0]; //x
state[1] = (RealT)trans[1]; //y
state[2] = (RealT)trans[2]; //z
getQuaternion((double*)measuredRot.ptr(), state + 3);
//Do the correction step
shortestPathQuat(prevQuat);
filter.correct(mTempState).copyTo(mTempState);
normalizeQuat();
//Write state back
trans[0] = state[0]; //x
trans[1] = state[1]; //y
trans[2] = state[2]; //z
getAngleAxis(state + 3, (double*)measuredRot.ptr());
}
}
