本文整理汇总了C++中KeyFrame::MakeKeyFrame_Rest方法的典型用法代码示例。如果您正苦于以下问题：C++ KeyFrame::MakeKeyFrame_Rest方法的具体用法？C++ KeyFrame::MakeKeyFrame_Rest怎么用？C++ KeyFrame::MakeKeyFrame_Rest使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类KeyFrame的用法示例。
在下文中一共展示了KeyFrame::MakeKeyFrame_Rest方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: InitFromCalibImage
// Initialize the map
bool MapMakerCalib::InitFromCalibImage(CalibImageTaylor& calibImage, double dSquareSize, std::string cameraName,
TooN::SE3<>& se3TrackerPose)
{
// Create a new MKF
MultiKeyFrame* pMKF = new MultiKeyFrame;
pMKF->mse3BaseFromWorld = calibImage.mse3CamFromWorld; // set mkf pose to be pose from tracker
pMKF->mse3BaseFromWorld.get_translation() *= dSquareSize; // scale it
pMKF->mbFixed = false;
// Create a new KF (there will only be one for now)
KeyFrame* pKF = new KeyFrame(pMKF, cameraName);
pMKF->mmpKeyFrames[cameraName] = pKF;
pKF->mse3CamFromWorld = pMKF->mse3BaseFromWorld; // same as parent MKF
pKF->mse3CamFromBase = TooN::SE3<>(); // set relative pose to identity;
pKF->mbActive = true;
pKF->MakeKeyFrame_Lite(calibImage.mImage, true);
pKF->MakeKeyFrame_Rest();
// Create MapPoints where the calib image corners are
// Testing to see if we should create points at all levels or just level 0
for (int l = 0; l < LEVELS; ++l)
{
if (l >= 1)
break;
int nLevelScale = LevelScale(l);
for (unsigned i = 0; i < calibImage.mvGridCorners.size(); ++i)
{
MapPoint* pNewPoint = new MapPoint;
pNewPoint->mv3WorldPos.slice<0, 2>() = dSquareSize * CVD::vec(calibImage.mvGridCorners[i].mirGridPos);
pNewPoint->mv3WorldPos[2] = 0.0; // on z=0 plane
pNewPoint->mbFixed = true; // the calibration pattern is fixed
pNewPoint->mbOptimized = true; // since it won't move it's already in its optimal location
// Patch source stuff:
pNewPoint->mpPatchSourceKF = pKF;
pNewPoint->mnSourceLevel = l;
pNewPoint->mv3Normal_NC = TooN::makeVector(0, 0, -1);
TooN::Vector<2> v2RootPos = calibImage.mvGridCorners[i].mParams.v2Pos;
// Same code as in MapMakerServerBase::AddPointEpipolar
pNewPoint->mirCenter = CVD::ir_rounded(LevelNPos(v2RootPos, l));
pNewPoint->mv3Center_NC = mmCameraModels[cameraName].UnProject(v2RootPos);
pNewPoint->mv3OneRightFromCenter_NC =
mmCameraModels[cameraName].UnProject(v2RootPos + CVD::vec(CVD::ImageRef(nLevelScale, 0)));
pNewPoint->mv3OneDownFromCenter_NC =
mmCameraModels[cameraName].UnProject(v2RootPos + CVD::vec(CVD::ImageRef(0, nLevelScale)));
normalize(pNewPoint->mv3Center_NC);
normalize(pNewPoint->mv3OneDownFromCenter_NC);
normalize(pNewPoint->mv3OneRightFromCenter_NC);
pNewPoint->RefreshPixelVectors();
mMap.mlpPoints.push_back(pNewPoint);
// Create a measurement of the point
Measurement* pMeas = new Measurement;
pMeas->eSource = Measurement::SRC_ROOT;
pMeas->v2RootPos = v2RootPos;
pMeas->nLevel = l;
pMeas->bSubPix = true;
pKF->mmpMeasurements[pNewPoint] = pMeas;
pNewPoint->mMMData.spMeasurementKFs.insert(pKF);
}
}
mMap.mlpMultiKeyFrames.push_back(pMKF);
/*
{
ROS_DEBUG("After creating points, before optimization: ");
int i=0;
double dErrorSum = 0;
for(MapPointPtrList::iterator it = mMap.mlpPoints.begin(); it != mMap.mlpPoints.end(); ++it, ++i)
{
std::cout<<"Point pos: "<<(*it)->mv3WorldPos;
TooN::Vector<3> v3Cam = pKF->mse3CamFromWorld * (*it)->mv3WorldPos;
TooN::Vector<2> v2Image = mmCameraModels[cameraName].Project(v3Cam);
std::cout<<" Reprojected: "<<v2Image<<" Original: "<<calibImage.mvGridCorners[i].mParams.v2Pos;
std::cout<<std::endl;
TooN::Vector<2> v2Error = v2Image - calibImage.mvGridCorners[i].mParams.v2Pos;
dErrorSum += v2Error * v2Error;
}
std::cout<<"Error sum: "<<dErrorSum<<" mean: "<<dErrorSum/i<<std::endl;
}
*/
mBundleAdjuster.SetNotConverged();
int nSanityCounter = 0;
//.........这里部分代码省略.........