本文整理汇总了C++中eigen::Transform类的典型用法代码示例。如果您正苦于以下问题:C++ Transform类的具体用法?C++ Transform怎么用?C++ Transform使用的例子?那么恭喜您, 这里精选的类代码示例或许可以为您提供帮助。
在下文中一共展示了Transform类的20个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于我们的系统推荐出更棒的C++代码示例。
示例1: heuristicCost
/**
* @function heuristicCost
* @brief
*/
double M_RRT::heuristicCost( Eigen::VectorXd node )
{
Eigen::Transform<double, 3, Eigen::Affine> T;
// Calculate the EE position
robinaLeftArm_fk( node, TWBase, Tee, T );
Eigen::VectorXd trans_ee = T.translation();
Eigen::VectorXd x_ee = T.rotation().col(0);
Eigen::VectorXd y_ee = T.rotation().col(1);
Eigen::VectorXd z_ee = T.rotation().col(2);
Eigen::VectorXd GH = ( goalPosition - trans_ee );
double fx1 = GH.norm() ;
GH = GH/GH.norm();
double fx2 = abs( GH.dot( x_ee ) - 1 );
double fx3 = abs( GH.dot( z_ee ) );
double heuristic = w1*fx1 + w2*fx2 + w3*fx3;
return heuristic;
}
开发者ID:ana-GT,项目名称:Lucy,代码行数:31,代码来源:M_RRT.cpp
示例2: dHomogTrans
Eigen::Matrix<Scalar, HOMOGENEOUS_TRANSFORM_SIZE, DerivedQdotToV::ColsAtCompileTime> dHomogTrans(
const Eigen::Transform<Scalar, 3, Eigen::Isometry>& T,
const Eigen::MatrixBase<DerivedS>& S,
const Eigen::MatrixBase<DerivedQdotToV>& qdot_to_v) {
const int nq_at_compile_time = DerivedQdotToV::ColsAtCompileTime;
int nq = qdot_to_v.cols();
auto qdot_to_twist = (S * qdot_to_v).eval();
const int numel = HOMOGENEOUS_TRANSFORM_SIZE;
Eigen::Matrix<Scalar, numel, nq_at_compile_time> ret(numel, nq);
const auto& Rx = T.linear().col(0);
const auto& Ry = T.linear().col(1);
const auto& Rz = T.linear().col(2);
const auto& qdot_to_omega_x = qdot_to_twist.row(0);
const auto& qdot_to_omega_y = qdot_to_twist.row(1);
const auto& qdot_to_omega_z = qdot_to_twist.row(2);
ret.template middleRows<3>(0) = -Rz * qdot_to_omega_y + Ry * qdot_to_omega_z;
ret.row(3).setZero();
ret.template middleRows<3>(4) = Rz * qdot_to_omega_x - Rx * qdot_to_omega_z;
ret.row(7).setZero();
ret.template middleRows<3>(8) = -Ry * qdot_to_omega_x + Rx * qdot_to_omega_y;
ret.row(11).setZero();
ret.template middleRows<3>(12) = T.linear() * qdot_to_twist.bottomRows(3);
ret.row(15).setZero();
return ret;
}
开发者ID:ElFeo,项目名称:drake,代码行数:33,代码来源:drakeGeometryUtil.cpp
示例3: dHomogTransInv
Eigen::Matrix<Scalar, HOMOGENEOUS_TRANSFORM_SIZE, DerivedDT::ColsAtCompileTime> dHomogTransInv(
const Eigen::Transform<Scalar, 3, Eigen::Isometry>& T,
const Eigen::MatrixBase<DerivedDT>& dT) {
const int nq_at_compile_time = DerivedDT::ColsAtCompileTime;
int nq = dT.cols();
const auto& R = T.linear();
const auto& p = T.translation();
std::array<int, 3> rows {0, 1, 2};
std::array<int, 3> R_cols {0, 1, 2};
std::array<int, 1> p_cols {3};
auto dR = getSubMatrixGradient(dT, rows, R_cols, T.Rows);
auto dp = getSubMatrixGradient(dT, rows, p_cols, T.Rows);
auto dinvT_R = transposeGrad(dR, R.rows());
auto dinvT_p = (-R.transpose() * dp - matGradMult(dinvT_R, p)).eval();
const int numel = HOMOGENEOUS_TRANSFORM_SIZE;
Eigen::Matrix<Scalar, numel, nq_at_compile_time> ret(numel, nq);
setSubMatrixGradient(ret, dinvT_R, rows, R_cols, T.Rows);
setSubMatrixGradient(ret, dinvT_p, rows, p_cols, T.Rows);
// zero out gradient of elements in last row:
const int last_row = 3;
for (int col = 0; col < T.HDim; col++) {
ret.row(last_row + col * T.Rows).setZero();
}
return ret;
}
开发者ID:ElFeo,项目名称:drake,代码行数:32,代码来源:drakeGeometryUtil.cpp
示例4: runtime_error
void TranslationRotation3D::setF(const std::vector<double> &F_in) {
if (F_in.size() != 16)
throw std::runtime_error(
"TranslationRotation3D::setF: F_in requires 16 elements");
if ((F_in.at(12) != 0.0) || (F_in.at(13) != 0.0) || (F_in.at(14) != 0.0) ||
(F_in.at(15) != 1.0))
throw std::runtime_error(
"TranslationRotation3D::setF: bottom row of F_in should be [0 0 0 1]");
Eigen::Map<const Eigen::Matrix<double, 4, 4, Eigen::RowMajor> > F_in_eig(
F_in.data());
Eigen::Transform<double, 3, Eigen::Affine> F;
F = F_in_eig;
double tmpT[3];
Eigen::Map<Eigen::Vector3d> tra_eig(tmpT);
tra_eig = F.translation();
double tmpR_mat[9];
Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor> > rot_eig(tmpR_mat);
rot_eig = F.rotation();
setT(tmpT);
setR_mat(tmpR_mat);
updateR_mat(); // for stability
}
开发者ID:CoffeRobot,项目名称:fato,代码行数:28,代码来源:translation_rotation_3d.cpp
示例5: t
void X3DConverter::startShape(const std::array<float, 12>& matrix) {
// Finding axis/angle from matrix using Eigen for its bullet proof implementation.
Eigen::Transform<float, 3, Eigen::Affine> t;
t.setIdentity();
for (unsigned int i = 0; i < 3; i++) {
for (unsigned int j = 0; j < 4; j++) {
t(i, j) = matrix[i+j*3];
}
}
Eigen::Matrix3f rotationMatrix;
Eigen::Matrix3f scaleMatrix;
t.computeRotationScaling(&rotationMatrix, &scaleMatrix);
Eigen::Quaternionf q;
Eigen::AngleAxisf aa;
q = rotationMatrix;
aa = q;
Eigen::Vector3f scale = scaleMatrix.diagonal();
Eigen::Vector3f translation = t.translation();
startNode(ID::Transform);
m_writers.back()->setSFVec3f(ID::translation, translation.x(), translation.y() , translation.z());
m_writers.back()->setSFRotation(ID::rotation, aa.axis().x(), aa.axis().y(), aa.axis().z(), aa.angle());
m_writers.back()->setSFVec3f(ID::scale, scale.x(), scale.y(), scale.z());
startNode(ID::Shape);
startNode(ID::Appearance);
startNode(ID::Material);
m_writers.back()->setSFColor<vector<float> >(ID::diffuseColor, RVMColorHelper::color(m_materials.back()));
endNode(ID::Material); // Material
endNode(ID::Appearance); // Appearance
}
开发者ID:Supporting,项目名称:pmuc,代码行数:34,代码来源:x3dconverter.cpp
示例6: wsDiff
/**
* @function wsDiff
*/
Eigen::VectorXd JG_RRT::wsDiff( Eigen::VectorXd q )
{
Eigen::Transform<double, 3, Eigen::Affine> T;
robinaLeftArm_fk( q, TWBase, Tee, T );
Eigen::VectorXd ws_diff = ( goalPosition - T.translation() );
return ws_diff;
}
开发者ID:ana-GT,项目名称:Lucy,代码行数:11,代码来源:JG_RRT.cpp
示例7:
operator Eigen::Transform<double, 3, Eigen::Affine, _Options>() const
{
Eigen::Transform<double, 3, Eigen::Affine, _Options> ret;
ret.setIdentity();
ret.rotate(this->orientation);
ret.translation() = this->position;
return ret;
}
开发者ID:maltewi,项目名称:base-types,代码行数:8,代码来源:RigidBodyState.hpp
示例8: wsDistance
/**
* @function wsDistance
*/
double goWSOrient::wsDistance( Eigen::VectorXd q )
{
Eigen::Transform<double, 3, Eigen::Affine> T;
robinaLeftArm_fk( q, TWBase, Tee, T );
Eigen::VectorXd ws_diff = ( goalPos - T.translation() );
double ws_dist = ws_diff.norm();
return ws_dist;
}
开发者ID:ana-GT,项目名称:Lucy,代码行数:12,代码来源:goWSOrient.cpp
示例9: getData
void multiKinectCalibration::getData()
{
iterationStep = 0;
// std::string relativeTo = listOfTFnames.at(0);
kinect2kinectTransform.resize(listOfTFnames.size() - 1);
ros::Rate r(2);
Eigen::Transform<double,3,Eigen::Affine> kinectReference;
ros::spinOnce();
if (!useTFonly)
{
while ( iterationStep < listOfTFnames.size() && ros::ok())
{
std::cerr << "Waiting for point clouds... \n";
r.sleep();
ros::spinOnce();
}
showPCL();
std::cerr << "Subscribe pcl done" << std::endl;
}
sleep(1.0);
std::cerr << listener.allFramesAsString() << std::endl;
// listen to all tf Frames
// get the transformation between kinects
for (std::size_t i = 0; i < listOfTFnames.size(); i++)
{
tf::StampedTransform transform;
std::string parentFrame;
listener.getParent(listOfTFnames.at(i),ros::Time(0),parentFrame);
listOfPointCloudnameHeaders.push_back(parentFrame);
std::cerr << "Lookup transform: "<< listOfTFnames.at(i) << " with parent: "<< parentFrame <<std::endl;
listener.waitForTransform(parentFrame,listOfTFnames.at(i),ros::Time(0),ros::Duration(5.0));
listener.lookupTransform(parentFrame,listOfTFnames.at(i),ros::Time(0),transform);
Eigen::Transform<double,3,Eigen::Affine> tmpTransform;
tf::transformTFToEigen(transform,tmpTransform);
// geometry_msgs::TransformStamped msg;
// transformStampedTFToMsg(transform, msg);
// Eigen::Translation<float,3> translation(msg.transform.translation.x, msg.transform.translation.y, msg.transform.translation.z);
// Eigen::Quaternion<float> rotation(msg.transform.rotation.w,
// msg.transform.rotation.x,
// msg.transform.rotation.y,
// msg.transform.rotation.z);
// std::cerr << "tmp:\n" << tmpTransform.matrix() << std::endl;
if (i == 0) kinectReference = tmpTransform;
else kinect2kinectTransform[i-1] = kinectReference * tmpTransform.inverse();
}
// std::cerr << "Kinect Ref:\n" << kinectReference.matrix() << std::endl;
}
开发者ID:fjonath1,项目名称:multi_kinect_merge,代码行数:52,代码来源:multiKinectCalibration.cpp
示例10: toGeometryMsg
void toGeometryMsg(geometry_msgs::Transform& out, const Eigen::Transform<double, 3, TransformType>& pose) {
// convert accumulated_pose_ to transformStamped
Eigen::Quaterniond rot_quat(pose.rotation());
out.translation.x = pose.translation().x();
out.translation.y = pose.translation().y();
out.translation.z = pose.translation().z();
out.rotation.x = rot_quat.x();
out.rotation.y = rot_quat.y();
out.rotation.z = rot_quat.z();
out.rotation.w = rot_quat.w();
}
开发者ID:Leeyangg,项目名称:limo,代码行数:13,代码来源:publish_helpers.hpp
示例11: perspective
Eigen::Matrix<Scalar,4,4> perspective(Scalar fovy, Scalar aspect, Scalar zNear, Scalar zFar){
Eigen::Transform<Scalar,3,Eigen::Projective> tr;
tr.matrix().setZero();
assert(aspect > 0);
assert(zFar > zNear);
Scalar radf = M_PI * fovy / 180.0;
Scalar tan_half_fovy = std::tan(radf / 2.0);
tr(0,0) = 1.0 / (aspect * tan_half_fovy);
tr(1,1) = 1.0 / (tan_half_fovy);
tr(2,2) = - (zFar + zNear) / (zFar - zNear);
tr(3,2) = - 1.0;
tr(2,3) = - (2.0 * zFar * zNear) / (zFar - zNear);
return tr.matrix();
}
开发者ID:Yiroro,项目名称:pattern-retrieval,代码行数:14,代码来源:Viewer.cpp
示例12:
template <typename PointT> void
pcl::people::GroundBasedPeopleDetectionApp<PointT>::applyTransformationGround ()
{
if (transformation_set_ && ground_coeffs_set_)
{
Eigen::Transform<float, 3, Eigen::Affine> transform;
transform = transformation_;
ground_coeffs_transformed_ = transform.matrix() * ground_coeffs_;
}
else
{
ground_coeffs_transformed_ = ground_coeffs_;
}
}
开发者ID:2php,项目名称:pcl,代码行数:14,代码来源:ground_based_people_detection_app.hpp
示例13: workspaceDist
/**
* @function workspaceDist
* @brief
*/
Eigen::VectorXd M_RRT::workspaceDist( Eigen::VectorXd node, Eigen::VectorXd ws_target )
{
Eigen::Transform<double, 3, Eigen::Affine> T;
Eigen::VectorXd diff;
// Calculate the EE position
robinaLeftArm_fk( node, TWBase, Tee, T );
Eigen::VectorXd ws_node = T.translation();
// Calculate the workspace distance to goal
diff = ( ws_target - ws_node );
return diff;
}
开发者ID:ana-GT,项目名称:Lucy,代码行数:18,代码来源:M_RRT.cpp
示例14:
/**
Transforms this lifting surface.
@param[in] transformation Affine transformation.
*/
void
LiftingSurface::transform(const Eigen::Transform<double, 3, Eigen::Affine> &transformation)
{
// Call super:
this->Surface::transform(transformation);
// Transform bisectors and wake normals:
for (int i = 0; i < n_spanwise_nodes(); i++) {
Vector3d trailing_edge_bisector = trailing_edge_bisectors.row(i);
trailing_edge_bisectors.row(i) = transformation.linear() * trailing_edge_bisector;
Vector3d wake_normal = wake_normals.row(i);
wake_normals.row(i) = transformation.linear() * wake_normal;
}
}
开发者ID:jbaayen,项目名称:vortexje,代码行数:20,代码来源:lifting-surface.cpp
示例15: renderSprites
void renderSprites() {
glUseProgram(spriteShaderProgramId);
entityManagerRef->visitEntitiesWithTypeMask(componentMask, [&](Entity<EntityManagerTypes...> &entity){
auto &aabbComponent = entity.template getComponent<AABBComponent>();
auto &transformComponent = entity.template getComponent<TransformComponent>();
Eigen::Translation<GLfloat, 3> translationMat((transformComponent.x - HALF_SCREEN_WIDTH) / HALF_SCREEN_WIDTH,
(transformComponent.y - HALF_SCREEN_HEIGHT) / HALF_SCREEN_HEIGHT,
0);
Eigen::DiagonalMatrix<GLfloat, 3> scaleMat(aabbComponent.width / SCREEN_WIDTH,
aabbComponent.height / SCREEN_HEIGHT,
1);
Eigen::Transform<GLfloat, 3, Eigen::Affine> transformMatrix = translationMat * scaleMat;
boundsSprite.render(transformMatrix.matrix());
});
}
开发者ID:joekarl,项目名称:Space-Shooter,代码行数:18,代码来源:DebugCollisionRenderSystem.hpp
示例16: return
template <typename PointT, typename Scalar> inline PointT
pcl::transformPointWithNormal (const PointT &point,
const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
{
PointT ret = point;
ret.getVector3fMap () = transform * point.getVector3fMap ();
ret.getNormalVector3fMap () = transform.rotation () * point.getNormalVector3fMap ();
return (ret);
}
开发者ID:2php,项目名称:pcl,代码行数:10,代码来源:transforms.hpp
示例17: dTransformAdjointTranspose
typename Gradient<DerivedX, DerivedDX::ColsAtCompileTime>::type dTransformAdjointTranspose(
const Eigen::Transform<Scalar, 3, Eigen::Isometry>& T,
const Eigen::MatrixBase<DerivedX>& X,
const Eigen::MatrixBase<DerivedDT>& dT,
const Eigen::MatrixBase<DerivedDX>& dX) {
assert(dT.cols() == dX.cols());
int nq = dT.cols();
const auto& R = T.linear();
const auto& p = T.translation();
std::array<int, 3> rows {0, 1, 2};
std::array<int, 3> R_cols {0, 1, 2};
std::array<int, 1> p_cols {3};
auto dR = getSubMatrixGradient(dT, rows, R_cols, T.Rows);
auto dp = getSubMatrixGradient(dT, rows, p_cols, T.Rows);
auto Rtranspose = R.transpose();
auto dRtranspose = transposeGrad(dR, R.rows());
typename Gradient<DerivedX, DerivedDX::ColsAtCompileTime>::type ret(X.size(), nq);
std::array<int, 3> Xomega_rows {0, 1, 2};
std::array<int, 3> Xv_rows {3, 4, 5};
for (int col = 0; col < X.cols(); col++) {
auto Xomega_col = X.template block<3, 1>(0, col);
auto Xv_col = X.template block<3, 1>(3, col);
std::array<int, 1> col_array {col};
auto dXomega_col = getSubMatrixGradient(dX, Xomega_rows, col_array, X.rows());
auto dXv_col = getSubMatrixGradient(dX, Xv_rows, col_array, X.rows());
auto dp_hatXv_col = (dp.colwise().cross(Xv_col) - dXv_col.colwise().cross(p)).eval();
auto Xomega_col_minus_p_cross_Xv_col = (Xomega_col - p.cross(Xv_col)).eval();
auto dXomega_transformed_col = (Rtranspose * (dXomega_col - dp_hatXv_col) + matGradMult(dRtranspose, Xomega_col_minus_p_cross_Xv_col)).eval();
auto dRtransposeXv_col = (Rtranspose * dXv_col + matGradMult(dRtranspose, Xv_col)).eval();
setSubMatrixGradient(ret, dXomega_transformed_col, Xomega_rows, col_array, X.rows());
setSubMatrixGradient(ret, dRtransposeXv_col, Xv_rows, col_array, X.rows());
}
return ret;
}
开发者ID:ElFeo,项目名称:drake,代码行数:42,代码来源:drakeGeometryUtil.cpp
示例18: plan
/**
* @function plan
* @brief
*/
bool JG_RRT::plan( const Eigen::VectorXd &_startConfig,
const Eigen::Transform<double, 3, Eigen::Affine> &_goalPose,
const std::vector< Eigen::VectorXd > &_guidingNodes,
std::vector<Eigen::VectorXd> &path )
{
/** Store information */
this->startConfig = _startConfig;
this->goalPose = _goalPose;
this->goalPosition = _goalPose.translation();
//-- Initialize the search tree
addNode( startConfig, -1 );
//-- Add the guiding nodes
addGuidingNodes( _guidingNodes );
double p;
while( goalDistVector[activeNode] > distanceThresh )
{
//-- Generate the probability
p = RANDNM( 0.0, 1.0 );
//-- Apply either extension to goal (J) or random connection
if( p < p_goal )
{
if( extendGoal() == true )
{
break;
}
}
else
{
tryStep(); /*extendRandom();*/
}
// Check if we are still inside
if( configVector.size() > maxNodes )
{ cout<<"-- Exceeded "<<maxNodes<<" allowed - ws_dist: "<<goalDistVector[rankingVector[0]]<<endl;
break;
}
}
//-- If a path is found
if( goalDistVector[activeNode] < distanceThresh )
{ tracePath( activeNode, path );
cout<<"JG - Got a path! - nodes: "<<path.size()<<" tree size: "<<configVector.size()<<endl;
return true;
}
else
{ cout<<"--(!) JG :No successful path found! "<<endl;
return false;
}
}
开发者ID:ana-GT,项目名称:Lucy,代码行数:58,代码来源:JG_RRT.cpp
示例19: BasicPlan
/**
* @function Basic_M_RRT
* @brief
*/
bool M_RRT::BasicPlan( std::vector<Eigen::VectorXd> &path,
Eigen::VectorXd &_startConfig,
Eigen::Transform<double, 3, Eigen::Affine> &_goalPose,
Eigen::Transform<double, 3, Eigen::Affine> _TWBase,
Eigen::Transform<double, 3, Eigen::Affine> _Tee )
{
printf("Basic Plan \n");
/** Store information */
this->startConfig = _startConfig;
this->goalPose = _goalPose;
this->goalPosition = _goalPose.translation();
this->TWBase = _TWBase;
this->Tee = _Tee;
//-- Initialize the search tree
addNode( startConfig, -1 );
//-- Calculate the heuristicThreshold
heuristicThresh = w1*distThresh + w2*abs( cos( xAlignThresh ) - 1 ) +w3*abs( cos( yAlignThresh ) );
//-- Let's start the loop
double p;
double heuristic = heuristicVector[0];
int gc = 0; int rc = 0;
while( heuristic > heuristicThresh )
{
//-- Probability
p = rand()%100;
//-- Either extends towards goal or random
if( p < pGoal )
{ printf("Goal \n");extendGoal(); gc++; }
else
{ printf("Random \n"); extendRandom(); rc++;}
//-- If bigger than maxNodes, get out loop
if( maxNodes > 0 && configVector.size() > maxNodes )
{
cout<<"** Exceeded "<<maxNodes<<" MinCost: "<<heuristicVector[minHeuristicIndex()]<<"MinRankingCost: "<<heuristicVector[rankingVector[0]]<<endl;
printf("Goal counter: %d, random counter: %d \n", gc, rc );
return false; }
heuristic = heuristicVector[ rankingVector[0] ];
}
printf("Goal counter: %d, random counter: %d \n", gc, rc );
printf( "-- Plan successfully generated with %d nodes \n", configVector.size() );
tracePath( activeNode, path );
return true;
}
开发者ID:ana-GT,项目名称:Lucy,代码行数:54,代码来源:M_RRT.cpp
示例20:
//DEPRECATED???
double
NDTMatcherFeatureD2D::scoreNDT(std::vector<NDTCell*> &sourceNDT, NDTMap &targetNDT,
Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor>& T)
{
NUMBER_OF_ACTIVE_CELLS = 0;
double score_here = 0;
double det = 0;
bool exists = false;
NDTCell *cell;
Eigen::Matrix3d covCombined, icov;
Eigen::Vector3d meanFixed;
Eigen::Vector3d meanMoving;
Eigen::Matrix3d R = T.rotation();
std::vector<std::pair<unsigned int, double> > scores;
for(unsigned int j=0; j<_corr.size(); j++)
{
unsigned int i = _corr[j].second;
if (_corr[j].second >= (int)sourceNDT.size())
{
std::cout << "second correspondance : " << _corr[j].second << ", " << sourceNDT.size() << std::endl;
}
if (sourceNDT[i] == NULL)
{
std::cout << "sourceNDT[i] == NULL!" << std::endl;
}
meanMoving = T*sourceNDT[i]->getMean();
cell = targetNDT.getCellIdx(_corr[j].first);
{
if(cell == NULL)
{
std::cout << "cell== NULL!!!" << std::endl;
}
else
{
if(cell->hasGaussian_)
{
meanFixed = cell->getMean();
covCombined = cell->getCov() + R.transpose()*sourceNDT[i]->getCov()*R;
covCombined.computeInverseAndDetWithCheck(icov,det,exists);
if(!exists) continue;
double l = (meanMoving-meanFixed).dot(icov*(meanMoving-meanFixed));
if(l*0 != 0) continue;
if(l > 120) continue;
double sh = -lfd1*(exp(-lfd2*l/2));
if(fabsf(sh) > 1e-10)
{
NUMBER_OF_ACTIVE_CELLS++;
}
scores.push_back(std::pair<unsigned int, double>(j, sh));
score_here += sh;
//score_here += l;
}
}
}
}
if (_trimFactor == 1.)
{
return score_here;
}
else
{
// Determine the score value
if (scores.empty()) // TODO, this happens(!), why??!??
return score_here;
score_here = 0.;
unsigned int index = static_cast<unsigned int>(_trimFactor * (scores.size() - 1));
// std::nth_element (scores.begin(), scores.begin()+index, scores.end(), sort_scores()); //boost::bind(&std::pair<unsigned int, double>::second, _1) < boost::bind(&std::pair<unsigned int, double>::second, _2));
std::nth_element (scores.begin(), scores.begin()+index, scores.end(), boost::bind(&std::pair<unsigned int, double>::second, _1) < boost::bind(&std::pair<unsigned int, double>::second, _2));
std::fill(_goodCorr.begin(), _goodCorr.end(), false);
// std::cout << "_goodCorr.size() : " << _goodCorr.size() << " scores.size() : " << scores.size() << " index : " << index << std::endl;
for (unsigned int i = 0; i < _goodCorr.size(); i++)
{
if (i <= index)
{
score_here += scores[i].second;
_goodCorr[scores[i].first] = true;
}
}
return score_here;
}
}
开发者ID:vbillys,项目名称:velo_mapping,代码行数:88,代码来源:ndt_matcher_d2d_feature.cpp
注:本文中的eigen::Transform类示例由纯净天空整理自Github/MSDocs等源码及文档管理平台,相关代码片段筛选自各路编程大神贡献的开源项目,源码版权归原作者所有,传播和使用请参考对应项目的License;未经允许,请勿转载。 |
请发表评论