本文整理汇总了C++中Matrix3f::sum方法的典型用法代码示例。如果您正苦于以下问题：C++ Matrix3f::sum方法的具体用法？C++ Matrix3f::sum怎么用？C++ Matrix3f::sum使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类Matrix3f的用法示例。
在下文中一共展示了Matrix3f::sum方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: verifyPoseHypothesesAndRefine
double CCubicGrids::verifyPoseHypothesesAndRefine(CRGBDFrame::tp_ptr pCurFrame_, CRGBDFrame::tp_ptr pPrevFrame_, vector<Eigen::Affine3f>& v_k_hypothese_poses, int nICPMethod_, int nStage_,
Matrix3f* pRw_, Vector3f* pTw_, int* p_best_idx_){
//assert the hypotheses list is not empty
double dICPEnergy = numeric_limits<double>::max();
if (v_k_hypothese_poses.empty()){
//using the ICP to refine each hypotheses and store their alignment score
//do ICP as one of pose hypotheses
Matrix3f eimRw; Vector3f eivTw;
//refine R,t
Eigen::Vector4i eivIter;
pPrevFrame_->getRnt(&*pRw_, &*pTw_);//use previous R,t as initial pose
{
short asICPIterations[4] = { 8, 4, 3, 2 };
dICPEnergy = icpFrm2Frm(pCurFrame_, pPrevFrame_, asICPIterations, &*pRw_, &*pTw_, &eivIter);
}
pCurFrame_->setRTw(*pRw_, *pTw_);
*p_best_idx_ = -1;
return dICPEnergy;
}
//using the ICP to refine each hypotheses and store their alignment score
vector<Matrix3f> v_refined_R;
vector<Vector3f> v_refined_t;
Mat energy_ICP;
energy_ICP.create(1, v_k_hypothese_poses.size(), CV_64FC1); energy_ICP.setTo(numeric_limits<double>::max());
for (int i = 0; i < v_k_hypothese_poses.size(); i++)
{
const short asICPIterations[4] = { 0, 0, 1, 1 };
Matrix3f eimRw; Vector3f eivTw;
btl::utility::convertPrj2Rnt(v_k_hypothese_poses[i], &eimRw, &eivTw);
float s = eimRw.sum();
if (fabs(s) < 0.0001 || boost::math::isnan<float>(s)){
energy_ICP.at<double>(0, i) = numeric_limits<double>::max();
v_refined_R.push_back(Matrix3f::Zero());
v_refined_t.push_back(Vector3f::Zero());
continue;
}
//ICP -- refine R,t
Eigen::Vector4i eivIter;
switch (nICPMethod_){
case btl::Frm_2_Frm_ICP:
if (nStage_ == btl::Relocalisation_Only){
//get virtual frame as previous, if current frame is a lost one, no previous frame can be used for refinement,
//therefore a virtual frame is required here.
pPrevFrame_->setRTw(eimRw, eivTw);
rayCast(&*pPrevFrame_,1);
}
energy_ICP.at<double>(0, i) = icpFrm2Frm(pCurFrame_, pPrevFrame_, asICPIterations, &eimRw, &eivTw, &eivIter);
//PRINT(energy_ICP.at<double>( 0, i ));
break;
default:
PRINTSTR("Failure - unrecognized ICP method.");
break;
}
if (energy_ICP.at<double>(0, i) != energy_ICP.at<double>(0, i) || energy_ICP.at<double>(0, i) < 0 )
energy_ICP.at<double>(0, i) = numeric_limits<double>::max();
v_refined_R.push_back(eimRw);
v_refined_t.push_back(eivTw);
}
//use the best hypotheses
//Mat SortedIdx;
//cv::sortIdx(energy_ICP, SortedIdx, CV_SORT_EVERY_ROW + CV_SORT_ASCENDING);
//PRINT(energy_ICP);
int best_k;// = SortedIdx.at<int>(0, 0);
for (best_k = 0; best_k < v_k_hypothese_poses.size(); best_k++){
dICPEnergy = energy_ICP.at<double>(0, best_k);
if (dICPEnergy >= 0.f) break;
}
for (int i = best_k + 1; i < v_k_hypothese_poses.size(); i++){
if (energy_ICP.at<double>(0, i) < dICPEnergy && energy_ICP.at<double>(0, i) >= 0.f) {
best_k = i;
dICPEnergy = energy_ICP.at<double>(0, i);
}
}
//PRINT(best_k);
best_k >= v_k_hypothese_poses.size() ? 0 : best_k;
*p_best_idx_ = best_k;
//show refined pose
*pRw_ = v_refined_R[best_k];
*pTw_ = v_refined_t[best_k];
pCurFrame_->setRTw(*pRw_, *pTw_);
//final refinement
if (true){
double dICPEnergyFinal = numeric_limits<double>::max();
Matrix3f eimRw = *pRw_; Vector3f eivTw = *pTw_;
float s = eimRw.sum();
if (fabs(s) > 0.0001 && !boost::math::isnan<float>(s)){
//ICP -- refine R,t
Eigen::Vector4i eivIter;
switch (nICPMethod_){
case Frm_2_Frm_ICP:
//if (nStage_ == btl::Relocalisation_Only)
{
//.........这里部分代码省略.........