本文整理汇总了C++中mrpt::square方法的典型用法代码示例。如果您正苦于以下问题：C++ mrpt::square方法的具体用法？C++ mrpt::square怎么用？C++ mrpt::square使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类mrpt的用法示例。
在下文中一共展示了mrpt::square方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: filter
void CPointCloudFilterByDistance::filter(
/** [in,out] The input pointcloud, which will be modified upon return after
filtering. */
mrpt::maps::CPointsMap* pc,
/** [in] The timestamp of the input pointcloud */
const mrpt::system::TTimeStamp pc_timestamp,
/** [in] If nullptr, the PC is assumed to be given in global coordinates.
Otherwise, it will be transformed from local coordinates to global using
this transformation. */
const mrpt::poses::CPose3D& cur_pc_pose,
/** [in,out] additional in/out parameters */
TExtraFilterParams* params)
{
using namespace mrpt::poses;
using namespace mrpt::math;
using mrpt::square;
MRPT_START;
ASSERT_(pc_timestamp != INVALID_TIMESTAMP);
ASSERT_(pc != nullptr);
CSimplePointsMap::Ptr original_pc =
mrpt::make_aligned_shared<CSimplePointsMap>();
original_pc->copyFrom(*pc);
// 1) Filter:
// ---------------------
const size_t N = pc->size();
std::vector<bool> deletion_mask;
deletion_mask.assign(N, false);
size_t del_count = 0;
// get reference, previous PC:
ASSERT_(options.previous_keyframes >= 1);
bool can_do_filter = true;
std::vector<FrameInfo*> prev_pc; // (options.previous_keyframes, nullptr);
{
auto it = m_last_frames.rbegin();
for (int i = 0;
i < options.previous_keyframes && it != m_last_frames.rend();
++i, ++it)
{
prev_pc.push_back(&it->second);
}
}
if (prev_pc.size() < static_cast<size_t>(options.previous_keyframes))
{
can_do_filter = false;
}
else
{
for (int i = 0; can_do_filter && i < options.previous_keyframes; ++i)
{
if (mrpt::system::timeDifference(
m_last_frames.rbegin()->first, pc_timestamp) >
options.too_old_seconds)
{
can_do_filter = false; // A required keyframe is too old
break;
}
}
}
if (can_do_filter)
{
// Reference poses of each PC:
// Previous: prev_pc.pose
mrpt::aligned_std_vector<CPose3D> rel_poses;
for (int k = 0; k < options.previous_keyframes; ++k)
{
const CPose3D rel_pose = cur_pc_pose - prev_pc[k]->pose;
rel_poses.push_back(rel_pose);
}
// The idea is that we can now find matches between pt{i} in time_{k},
// composed with rel_pose
// with the local points in time_{k-1}.
std::vector<TPoint3D> pt_km1(options.previous_keyframes);
for (size_t i = 0; i < N; i++)
{
// get i-th point in time=k:
TPoint3Df ptf_k;
pc->getPointFast(i, ptf_k.x, ptf_k.y, ptf_k.z);
const TPoint3D pt_k = TPoint3D(ptf_k);
// Point, referred to time=k-1 frame of reference
for (int k = 0; k < options.previous_keyframes; ++k)
rel_poses[k].composePoint(pt_k, pt_km1[k]);
// Look for neighbors in "time=k"
std::vector<TPoint3D> neig_k;
std::vector<float> neig_sq_dist_k;
pc->kdTreeNClosestPoint3D(
pt_k, 2 /*num queries*/, neig_k, neig_sq_dist_k);
// Look for neighbors in "time=k-i"
//.........这里部分代码省略.........