std::vector<FullPose3D> PnPRansac::modelFromMinimalSet(const std::vector<int> &constraintIndices, const std::vector<int> &constraintSubindices) { assert(constraintIndices.size()==4); std::vector<cv::Point3f> refp(4); std::vector<cv::Point2f> imgp(4); for(int i=0; i<4; ++i) { const int idx = constraintIndices[i]; const int sidx = constraintSubindices[i]; const FeatureMatch &match = *mMatches3D[idx]; refp[i] = match.measurement.getFeature().getPosition(); imgp[i] = cvutils::NormalizePoint(match.measurement.getPositionXns()[sidx]); } std::vector<FullPose3D> solutions; cv::Vec3d rvec,tvec; if(cv::solvePnP(refp, imgp, cv::Matx33f::eye(), cv::noArray(), rvec, tvec, false, CV_P3P)) { cv::Matx33d R; cv::Rodrigues(rvec, R); solutions.push_back(FullPose3D(R, tvec)); } return solutions; }
BasepointPixmap OverlayTextLine::render(int w, int h, const QColor& col, const QPoint& bp) const { BasepointPixmap img(w, h, bp); img.fill(Qt::transparent); QPainter imgp(&img); imgp.setRenderHint(QPainter::Antialiasing); imgp.setRenderHint(QPainter::TextAntialiasing); imgp.setBackground(QColor(0,0,0,0)); imgp.setCompositionMode(QPainter::CompositionMode_SourceOver); QColor qc(col); qc.setAlpha(255); imgp.setBrush(qc); imgp.setPen(QPen(Qt::black, fEdge, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin)); imgp.drawPath(qpp); imgp.setPen(Qt::NoPen); imgp.drawPath(qpp); img.iAscent = iroundf(fAscent + 0.5f); img.iDescent = iroundf(fDescent + 0.5f); return img; }
/** * 重绘 * 实现只出现垂直滚动条的重载的画图事件处理方法。 * */ void TaskBallMapWidget::paintEvent(QPaintEvent * event ) { // qLogx()<< __FUNCTION__ <<this->mBallBit.size()<<this->mBallMap.size()<<this->mCurrentTaskId; QBrush brush(QColor(0, 0, 0)); //依靠currtaskid 判断是否需要重绘 if (this->mCurrentTaskId < 0) { return; } if (this->mBallBit.size() <= 0) { qLogx()<<__FUNCTION__<<QString("No bit set,"); // leave the widget raw clear return; } //计算图象应该具有的宽度,splitter是以线性变化,而图形则必须以非线性变化,需要对取到的窗口宽度进行圆整 int width = this->mp->size().width() - 20 ; if (width % (this->mBallRadius*2) != 0 ) { width = ( (width / (this->mBallRadius*2)) ) * ( this->mBallRadius * 2) ; } else { width = (width / (this->mBallRadius*2)) * ( this->mBallRadius * 2) ; } //窗口的高度。 int height = this->mBallMap.height() ; if (width != this->mBallMap.width()) { //qLogx()<< "change to width:" << width ; //需要改变图象大小。宽度 this->mBallMap = QImage(width , this->mBallMap.height() , QImage::Format_ARGB32_Premultiplied); //this->mBallMap = this->mBallMap.scaledToWidth( width ) ; //this->resize(width,height); this->setFixedWidth( width ); } if (this->mBallBit.size() != (width/(this->mBallRadius*2)) * (height/(this->mBallRadius*2))) { height = (this->mBallRadius*2) * (this->mBallBit.size())/((width/(this->mBallRadius*2))); if (height % (this->mBallRadius*2) != 0 ) { height = ( 1+ (height / (this->mBallRadius*2)) ) * ( this->mBallRadius * 2); } else { height = ( height / (this->mBallRadius*2) ) * ( this->mBallRadius * 2); } //qLogx()<< "change to height: " << height ; //需要改变图象大小。高度。 //this->mBallMap = this->mBallMap.scaledToHeight( (this->mCurrentLength / (this->mBlockSize))/((width/(this->mBallRadius*2)) ) * ( this->mBallRadius*2 )); this->mBallMap = QImage(width, height, QImage::Format_ARGB32_Premultiplied); //this->mBallMap = this->mBallMap.scaledToHeight(height); //this->resize(width,height); this->setFixedHeight(height); //设置本窗口构件的大小,使得外层的滚动构件能适当的添加或者删除滚动条。 } // ball ball 48 0 // 0 true 600 0 // qLogx()<<__FUNCTION__<<this->mBallBit.size()<<this->mBallMap.isNull()<<width<<height; if (this->mBallMap.isNull()) { /* if not check, show this error: QPainter::begin: Paint device returned engine == 0, type: 3 QPainter::setRenderHint: Painter must be active to set rendering hints QPainter::setBrush: Painter not active QPainter::drawRects: Painter not active QPainter::setBrush: Painter not active QPainter::setBrush: Painter not actiev */ // this->mBallMap = QImage(mp->width(), mp->height(), QImage::Format_ARGB32_Premultiplied); return; } QPainter imgp(&this->mBallMap) ; imgp.setRenderHint(QPainter::Antialiasing, true); imgp.setBrush(brush) ; imgp.drawRect(0, 0, this->mBallMap.width(), this->mBallMap.height()); //更改前景颜色。 brush.setColor(QColor(0,255,0)); imgp.setBrush(brush); ////////// QRadialGradient radgrad ; //梯度法 imgp.setBrush(radgrad) ; int row = 0 , col = 0 ; //当前行,列 int mrow , mcol ; //最大行,列 int srow , scol ; //当前球块所在行,列。 mcol = width / (this->mBallRadius*2) ; mrow = this->mBallBit.size() % mcol == 0 ? (this->mBallBit.size() / mcol) : (this->mBallBit.size() / mcol + 1); int mtb = this->mBallBit.size(); //所有球的个数。 // qLogx()<<"rows:"<<mrow<<"cols:"<<mcol; for (int row = 0; row < mrow; row++) { int sballno; for (int col=0; col < mcol ; col ++) { int bx, by, bw, bh; sballno = row * mcol + col; //本球块在整个文件块中的序号。 if (sballno >= this->mBallBit.size()) { break; } if (this->mBallBit.testBit(sballno) == false) { continue; } bx = col * this->mBallRadius * 2; //本求块的右上角X值。 by = row * this->mBallRadius * 2; //本求块的右上角Y值。 //qLogx()<<"x,y,w"<<bx<<by<<(this->mBallRadius*2); radgrad.setCenter(bx+this->mBallRadius, by + this->mBallRadius); //梯度参数设置 radgrad.setRadius(this->mBallRadius * 2 ); radgrad.setFocalPoint(bx+this->mBallRadius/2,by+this->mBallRadius/2); radgrad.setColorAt(0.0,QColor(255,255,255)); radgrad.setColorAt(0.5,QColor(0,255,0)); radgrad.setColorAt(1.0,QColor(0,0,0)); imgp.setBrush(radgrad); imgp.drawEllipse(bx,by,this->mBallRadius*2 , this->mBallRadius*2); } } //输出到屏幕。 QPainter scrp(this); scrp.drawImage(0, 0, this->mBallMap); }
std::vector<EssentialRansacModel> EssentialRansac::modelFromMinimalSet(const std::vector<int> &constraintIndices, const std::vector<int> &constraintSubindices) { ProfileSection ss("essential::modelFrom5point"); assert(constraintIndices.size()==6); std::vector<cv::Point2f> refp(5); std::vector<cv::Point2f> imgp(5); for(int i=0; i<5; ++i) { const int idx = constraintIndices[i]; const int sidx = constraintSubindices[i]; //XXX this will break with fish-eye lenses, but cv::findEssentialMat() can only take 2D points refp[i] = mRefXnPointsNormalized[idx]; imgp[i] = mImageXnPointsNormalized[idx][sidx]; } auto &extraMatch = mReferenceFrameMatches->at(constraintIndices[5]); //One extra match to validate the model //EssentialReprojectionError2D extraErrorFunctor(&extraMatch.sourceMeasurement->getCamera(), extraMatch.sourceMeasurement->getUniquePositionXn(), extraMatch.measurement.getOctave(), mCamera, extraMatch.measurement.getPositionXns()); EpipolarSegmentErrorForPose extraErrorFunctor(*extraMatch.sourceMeasurement, extraMatch.measurement, (float)FLAGS_MinDepth); //Find essential: five point algorithm cv::Mat1f Eall ; { ProfileSection ss("essential::findEssential"); Eall = cv::findEssentialMat(refp, imgp, 1, cv::Point2d(0,0)); } const int modelCount = Eall.rows / 3; cv::Matx34f P1 = cv::Matx34f::eye(); std::vector<EssentialRansacModel> models; for(int i=0; i<modelCount; ++i) { cv::Matx33f essential = Eall.rowRange(i*3,i*3+3); if(std::isnan(essential(0,0))) { //Replace with identity essential = cv::Matx33f(0,0,0, 0,0,-1, 0,1,0); } //Decompose essential cv::Matx33f R[2]; cv::Vec3f t; cv::decomposeEssentialMat(essential, R[0], R[1], t); for(int kr=0; kr<2; ++kr) { for(int kt=0; kt<2; ++kt) { cv::Vec3f tk; if(kt==0) tk = t; else tk = -t; bool isGoodModel = true; //////////////////////////////////////////// //Ensure a proper essential matrix // It turns out that opencv's 5 point algorithm might return // a matrix that is not a valid essential matrix (i.e. full rank). // Also, rebuild it to get correct sign (very important) const cv::Matx33f essentialk = EssentialUtils::EssentialFromPose(R[kr], tk); //Traingulate the 5 points used cv::Matx34f P2 = cvutils::CatRT(R[kr], tk);; cv::Mat1f p4mat; cv::triangulatePoints(P1,P2,refp,imgp,p4mat); //Make sure all 5 points are in front for(int j=0; j<5; ++j) { float z = p4mat(2,j); float w = p4mat(3,j); if(std::signbit(z) != std::signbit(w)) { isGoodModel = false; break; } } //Change the reference frame of the pose cv::Matx33f worldR; cv::Vec3f worldT; //Eval the 6th point if (isGoodModel) { worldR = R[kr] * mReferenceFramePose.getRotationRef(); worldT = R[kr] * mReferenceFramePose.getTranslationRef() + tk; std::vector<float> residuals; extraErrorFunctor.computeAllResiduals(worldR, worldT, residuals); MatchReprojectionErrors errors; CeresUtils::ResidualsToErrors<EpipolarSegmentError::kResidualsPerItem>(extraErrorFunctor.getPointCount(), residuals, mOutlierErrorThresholdSq, errors); if(!errors.isInlier) { //DTSLAM_LOG << "Model failed because of extra point\n"; isGoodModel = false; } } if(isGoodModel) { //Model is good //Add model models.emplace_back(essentialk, FullPose3D(worldR,worldT)); } } } } //DTSLAM_LOG << "Model count: " << models.size() << "\n"; return std::move(models); }