Go to the documentation of this file.
45 SLAMScanWrapper::SLAMScanWrapper(
ScanPtr scan)
46 : m_scan(scan), m_deltaPose(
Transformd::Identity())
58 #pragma omp parallel for schedule(static)
97 double sqDist = minDistance * minDistance;
102 if (
m_points[cur].squaredNorm() <= sqDist)
117 double sqDist = maxDistance * maxDistance;
122 if (
m_points[cur].squaredNorm() >= sqDist)
145 return (
pose() * extended).block<3, 1>(0, 0);
160 return m_scan->registration;
170 return m_scan->poseEstimation;
175 return pose().block<3, 1>(0, 3);
196 for (
const std::pair<Transformd, FrameUse>&
frame :
m_frames)
198 for (
int i = 0; i < 16; i++)
200 out <<
frame.first(i) <<
" ";
203 out << (int)
frame.second << endl;
boost::shared_array< float > floatArr
void setMaxDistance(double maxDistance)
Reduces the Scan by removing all Points farther away than maxDistance to the origin.
const Transformd & initialPose() const
Returns the initial Pose of the Scan.
void addFrame(FrameUse use=FrameUse::UNUSED)
Adds a new animation Frame with the current Pose.
const Transformd & deltaPose() const
Returns the difference between pose() and initialPose()
std::vector< Vector3f > m_points
virtual void transform(const Transformd &transform, bool writeFrame=true, FrameUse use=FrameUse::UPDATED)
Applies a relative Transformation to the Scan.
int octreeReduce(Vector3f *points, int n, double voxelSize, int maxLeafSize)
Reduces a Point Cloud using an Octree with a minimum Voxel size.
Transform< double > Transformd
4x4 double precision transformation matrix
Vector3d getPosition() const
Get the Position of the Scan. Can also be obtained from pose()
void writeFrame(const Transform< T > &transform, const boost::filesystem::path &framesOut)
Writes a Eigen transformation into a .frames file.
const std::pair< Transformd, FrameUse > & frame(size_t index) const
Returns a Frame consisting of a Pose and a FrameUse.
Eigen::Vector3f Vector3f
Eigen 3D vector, single precision.
const Vector3f & rawPoint(size_t index) const
Returns the Point at the specified index in local Coordinates.
void setMinDistance(double minDistance)
Reduces the Scan by removing all Points closer than minDistance to the origin.
void writeFrames(std::string path) const
Writes the Frames to the specified location.
Eigen::Vector3d Vector3d
Eigen 3D vector, double precision.
FrameUse
Annotates the use of a Scan when creating an slam6D .frames file.
ScanPtr innerScan()
Access to the Scan that this instance is wrapped around.
size_t frameCount() const
Returns the number of Frames generated.
void reduce(double voxelSize, int maxLeafSize)
Reduces the Scan using Octree Reduction.
Eigen::Vector4d Vector4d
Eigen 4D vector, double precision.
size_t numPoints() const
Returns the number of Points in the Scan.
void trim()
Reduces Memory usage by getting rid of Points removed with reduction Methods.
virtual Vector3d point(size_t index) const
Returns the Point at the specified index in global Coordinates.
const Transformd & pose() const
Returns the current Pose of the Scan.
std::vector< std::pair< Transformd, FrameUse > > m_frames
std::shared_ptr< Scan > ScanPtr
Shared pointer to scans.
lvr2
Author(s): Thomas Wiemann
, Sebastian Pütz , Alexander Mock , Lars Kiesow , Lukas Kalbertodt , Tristan Igelbrink , Johan M. von Behren , Dominik Feldschnieders , Alexander Löhr
autogenerated on Wed Mar 2 2022 00:37:25