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