void InstrumentRenderer::drawSingleDetector(size_t detIndex, bool picking) {
const auto &compInfo = m_actor.componentInfo();
if (picking)
m_pickColors[detIndex].paint();
else
m_colors[detIndex].paint();
glPushMatrix();
// Translate
auto pos = compInfo.position(detIndex);
if (!pos.nullVector())
glTranslated(pos[0], pos[1], pos[2]);
// Rotate
auto rot = compInfo.rotation(detIndex);
if (!rot.isNull()) {
double deg, ax0, ax1, ax2;
rot.getAngleAxis(deg, ax0, ax1, ax2);
glRotated(deg, ax0, ax1, ax2);
}
// Scale
auto scale = compInfo.scaleFactor(detIndex);
if (scale != Mantid::Kernel::V3D(1, 1, 1))
glScaled(scale[0], scale[1], scale[2]);
compInfo.shape(detIndex).draw();
glPopMatrix();
}
void InstrumentRenderer::drawRectangularBank(size_t bankIndex, bool picking) {
const auto &compInfo = m_actor.componentInfo();
glPushMatrix();
auto bank = compInfo.quadrilateralComponent(bankIndex);
auto pos = compInfo.position(bank.bottomLeft);
auto scale = compInfo.scaleFactor(bankIndex);
glTranslated(pos.X(), pos.Y(), pos.Z());
glScaled(scale[0], scale[1], scale[2]);
auto rot = compInfo.rotation(bankIndex);
if (!(rot.isNull())) {
double deg, ax0, ax1, ax2;
rot.getAngleAxis(deg, ax0, ax1, ax2);
glRotated(deg, ax0, ax1, ax2);
}
auto ti = m_reverseTextureIndexMap[bankIndex];
auto &tex = m_textures[ti];
tex.bindTextures(picking);
tex.uploadTextures(picking);
BankRenderingHelpers::renderRectangularBank(compInfo, bankIndex);
tex.unbindTextures();
glPopMatrix();
}
void Filter3D<RealT>::operator()(typename Chilitags3D_<RealT>::TagPoseMap& tags)
{
//TODO: These have to be double precision for rodrigues
cv::Mat predictedRot(3,1,CV_64F);
cv::Matx33d tempRotMat;
for(auto& kfq : mFilters){
if(kfq.second.deleted)
continue;
cv::KalmanFilter& filter = kfq.second.filter;
//Calculate weighted covariance estimate trace, decide to discard or not
RealT trace = 0.0f;
for(int i=0;i<7;i++)
trace += filter.errorCovPost.at<RealT>(i,i)/mCovScales.at<RealT>(i);
trace /= 7.0f;
if(trace > mPersistence){
kfq.second.deleted = true;
continue;
}
//Do prediction step
if(mQChanged)
mQ.copyTo(filter.processNoiseCov);
if(mRChanged)
mR.copyTo(filter.measurementNoiseCov);
mF.copyTo(filter.transitionMatrix);
mB.copyTo(filter.controlMatrix);
filter.predict(mControl).copyTo(mTempState);
//Convert quaternion rotation to angle-axis rotation
getAngleAxis((RealT*)mTempState.ptr() + 3, (double*)predictedRot.ptr());
//Convert angle-axis to 3x3 rotation matrix
cv::Rodrigues(predictedRot, tempRotMat);
//Write back updated pose
tags[kfq.first] = {
(RealT)tempRotMat(0,0), (RealT)tempRotMat(0,1), (RealT)tempRotMat(0,2), mTempState.at<RealT>(0),
(RealT)tempRotMat(1,0), (RealT)tempRotMat(1,1), (RealT)tempRotMat(1,2), mTempState.at<RealT>(1),
(RealT)tempRotMat(2,0), (RealT)tempRotMat(2,1), (RealT)tempRotMat(2,2), mTempState.at<RealT>(2),
0, 0, 0, 1
};
}
mQChanged = false;
mRChanged = false;
}
void InstrumentRenderer::drawGridBank(size_t bankIndex, bool picking) {
const auto &compInfo = m_actor.componentInfo();
glPushMatrix();
auto firstLayer = compInfo.children(bankIndex)[0];
auto bank = compInfo.quadrilateralComponent(firstLayer);
auto pos = compInfo.position(bank.bottomLeft);
auto scale = compInfo.scaleFactor(bankIndex);
glTranslated(pos.X(), pos.Y(), pos.Z());
glScaled(scale[0], scale[1], scale[2]);
auto rot = compInfo.rotation(bankIndex);
if (!(rot.isNull())) {
double deg, ax0, ax1, ax2;
rot.getAngleAxis(deg, ax0, ax1, ax2);
glRotated(deg, ax0, ax1, ax2);
}
auto ti = m_reverseTextureIndexMap[bankIndex];
auto &tex = m_textures[ti];
tex.bindTextures(picking);
if (m_isUsingLayers) { // Render single layer
tex.uploadTextures(picking);
BankRenderingHelpers::renderGridBankLayer(compInfo, bankIndex, m_layer);
BankRenderingHelpers::renderGridBankOutline(compInfo, bankIndex);
} else { // Render 6 faces representing grid box
tex.uploadTextures(picking, detail::GridTextureFace::Front);
BankRenderingHelpers::renderGridBankFull(compInfo, bankIndex,
detail::GridTextureFace::Front);
tex.uploadTextures(picking, detail::GridTextureFace::Back);
BankRenderingHelpers::renderGridBankFull(compInfo, bankIndex,
detail::GridTextureFace::Back);
tex.uploadTextures(picking, detail::GridTextureFace::Left);
BankRenderingHelpers::renderGridBankFull(compInfo, bankIndex,
detail::GridTextureFace::Left);
tex.uploadTextures(picking, detail::GridTextureFace::Right);
BankRenderingHelpers::renderGridBankFull(compInfo, bankIndex,
detail::GridTextureFace::Right);
tex.uploadTextures(picking, detail::GridTextureFace::Top);
BankRenderingHelpers::renderGridBankFull(compInfo, bankIndex,
detail::GridTextureFace::Top);
tex.uploadTextures(picking, detail::GridTextureFace::Bottom);
BankRenderingHelpers::renderGridBankFull(compInfo, bankIndex,
detail::GridTextureFace::Bottom);
}
tex.unbindTextures();
glPopMatrix();
}
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());
}
}
void poseJacobian(const Eigen::Matrix3d& rotation, Eigen::Matrix<double, 6, 6>& jac, const double rot_angle_threshold)
{
Eigen::Matrix3d i3 = Eigen::Matrix3d::Identity();
// convert rotation matrix into axis-angle representation
double rot_angle;
Eigen::Vector3d rot_axis;
getAngleAxis(rotation.transpose(), rot_angle, rot_axis);
// create the rotation jacobian
Eigen::Matrix3d L_theta_u;
double sinc_part;
sinc_part = sva::sinc(rot_angle)/std::pow(sva::sinc(rot_angle/2.), 2);
Eigen::Matrix3d axis_antisym;
getSkewSym(rot_axis, axis_antisym);
L_theta_u = i3 - rot_angle*0.5*axis_antisym + (1-(sinc_part))*axis_antisym*axis_antisym;
jac << L_theta_u, Eigen::Matrix3d::Zero(),
Eigen::Matrix3d::Zero(), rotation.transpose();
}
void InstrumentRenderer::drawTube(size_t bankIndex, bool picking) {
const auto &compInfo = m_actor.componentInfo();
glPushMatrix();
auto pos = compInfo.position(bankIndex);
auto rot = compInfo.rotation(bankIndex);
auto scale = compInfo.scaleFactor(bankIndex);
glTranslated(pos.X(), pos.Y(), pos.Z());
glScaled(scale[0], scale[1], scale[2]);
double deg, ax0, ax1, ax2;
rot.getAngleAxis(deg, ax0, ax1, ax2);
glRotated(deg, ax0, ax1, ax2);
auto ti = m_reverseTextureIndexMap[bankIndex];
auto &tex = m_textures[ti];
glColor3f(1.0f, 1.0f, 1.0f);
tex.bindTextures(picking);
tex.uploadTextures(picking);
compInfo.shape(bankIndex).draw();
tex.unbindTextures();
glPopMatrix();
}
Vector3f Arm::getPoint() {
return getAngleAxis(R_body_to_world) * length;
}
