本文整理汇总了C++中Transformation::compose方法的典型用法代码示例。如果您正苦于以下问题：C++ Transformation::compose方法的具体用法？C++ Transformation::compose怎么用？C++ Transformation::compose使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类Transformation的用法示例。
在下文中一共展示了Transformation::compose方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: Ransac
void Odometry::Ransac()
{
assert(observationVec.size() == cloud.size());
int numPoints = observationVec.size();
inlierMask.resize(numPoints);
const int numIterMax = 25;
const Transformation<double> initialPose = TorigBase;
int bestInliers = 0;
//TODO add a termination criterion
for (unsigned int iteration = 0; iteration < numIterMax; iteration++)
{
Transformation<double> pose = initialPose;
int maxIdx = observationVec.size();
//choose three points at random
int idx1m = rand() % maxIdx;
int idx2m, idx3m;
do
{
idx2m = rand() % maxIdx;
} while (idx2m == idx1m);
do
{
idx3m = rand() % maxIdx;
} while (idx3m == idx1m or idx3m == idx2m);
//solve an optimization problem
Problem problem;
for (auto i : {idx1m, idx2m, idx3m})
{
CostFunction * costFunc = new OdometryError(cloud[i],
observationVec[i], TbaseCam, camera);
problem.AddResidualBlock(costFunc, NULL,
pose.transData(), pose.rotData());
}
Solver::Options options;
options.linear_solver_type = ceres::DENSE_SCHUR;
Solver::Summary summary;
options.max_num_iterations = 5;
Solve(options, &problem, &summary);
//count inliers
vector<Vector3d> XcamVec(numPoints);
Transformation<double> TorigCam = pose.compose(TbaseCam);
TorigCam.inverseTransform(cloud, XcamVec);
vector<Vector2d> projVec(numPoints);
camera.projectPointCloud(XcamVec, projVec);
vector<bool> currentInlierMask(numPoints, false);
int countInliers = 0;
for (unsigned int i = 0; i < numPoints; i++)
{
Vector2d err = observationVec[i] - projVec[i];
if (err.norm() < 2)
{
currentInlierMask[i] = true;
countInliers++;
}
}
//keep the best hypothesis
if (countInliers > bestInliers)
{
//TODO copy in a bettegit lor way
inlierMask = currentInlierMask;
bestInliers = countInliers;
TorigBase = pose;
}
}
}