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; }