//---------------------------------------------------------------------------- // 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()); } }