Commit 64ec0f57 authored by Nicolas Cadart's avatar Nicolas Cadart
Browse files

Merge branch 'fix/useIsometryForTworldAndTrelative' into 'master'

Replace SLAM internal 6 DoF state vector by an homogeneous matrix.

See merge request !11
parents f4954a73 11527a5b
Pipeline #155174 failed with stage
in 24 seconds
......@@ -41,15 +41,15 @@ void RollingGrid::Clear()
}
//------------------------------------------------------------------------------
void RollingGrid::Roll(const Eigen::Matrix<double, 6, 1>& T)
void RollingGrid::Roll(const Eigen::Vector3d& T)
{
// Very basic implementation where the grid is not circular.
// This only moves VoxelGrid so that current frame can entirely fit in rolled map.
// Compute the position of the new frame center in the grid.
int frameCenterX = std::floor(T[3] / this->VoxelResolution);
int frameCenterY = std::floor(T[4] / this->VoxelResolution);
int frameCenterZ = std::floor(T[5] / this->VoxelResolution);
int frameCenterX = std::floor(T.x() / this->VoxelResolution);
int frameCenterY = std::floor(T.y() / this->VoxelResolution);
int frameCenterZ = std::floor(T.z() / this->VoxelResolution);
// Half size of the VoxelGrid, rounded up.
int halfGridSize = (this->GridSize + 1) / 2;
......@@ -158,12 +158,12 @@ void RollingGrid::Roll(const Eigen::Matrix<double, 6, 1>& T)
}
//------------------------------------------------------------------------------
RollingGrid::PointCloud::Ptr RollingGrid::Get(const Eigen::Matrix<double, 6, 1>& T)
RollingGrid::PointCloud::Ptr RollingGrid::Get(const Eigen::Vector3d& T)
{
// Compute the position of the new frame center in the grid
int frameCenterX = std::floor(T[3] / this->VoxelResolution) - (this->VoxelGridPosition[0] - this->GridSize / 2);
int frameCenterY = std::floor(T[4] / this->VoxelResolution) - (this->VoxelGridPosition[1] - this->GridSize / 2);
int frameCenterZ = std::floor(T[5] / this->VoxelResolution) - (this->VoxelGridPosition[2] - this->GridSize / 2);
int frameCenterX = std::floor(T.x() / this->VoxelResolution) - (this->VoxelGridPosition[0] - this->GridSize / 2);
int frameCenterY = std::floor(T.y() / this->VoxelResolution) - (this->VoxelGridPosition[1] - this->GridSize / 2);
int frameCenterZ = std::floor(T.z() / this->VoxelResolution) - (this->VoxelGridPosition[2] - this->GridSize / 2);
// Get sub-VoxelGrid bounds
int minX = std::max<int>(frameCenterX + this->MinPoint[0], 0);
......
......@@ -30,10 +30,10 @@ public:
RollingGrid(double posX, double posY, double posZ);
//! Roll the grid to enable adding new point cloud
void Roll(const Eigen::Matrix<double, 6, 1>& T);
void Roll(const Eigen::Vector3d& T);
//! Get points near T
PointCloud::Ptr Get(const Eigen::Matrix<double, 6, 1>& T);
PointCloud::Ptr Get(const Eigen::Vector3d& T);
//! Get all points
PointCloud::Ptr Get();
......@@ -41,9 +41,10 @@ public:
//! Add some points to the grid
void Add(const PointCloud::Ptr& pointcloud);
// Remove all points from all voxels
//! Remove all points from all voxels
void Clear();
//! Set min and max keypoints bounds of current frame
void SetMinMaxPoints(const Eigen::Vector3d& minPoint, const Eigen::Vector3d& maxPoint);
void SetGridSize(int size);
......
This diff is collapsed.
......@@ -327,17 +327,18 @@ private:
// Transformation to map the current pointcloud
// in the referential of the previous one
Eigen::Matrix<double, 6, 1> Trelative;
Eigen::VectorXd MotionParametersEgoMotion;
Eigen::Isometry3d Trelative;
std::pair<Eigen::Isometry3d, Eigen::Isometry3d> MotionParametersEgoMotion;
// Transformation to map the current pointcloud
// in the world (i.e first frame) one
Eigen::Matrix<double, 6, 1> Tworld = Eigen::Matrix<double, 6, 1>::Zero();
Eigen::Matrix<double, 6, 1> PreviousTworld = Eigen::Matrix<double, 6, 1>::Zero(); // CHECK unused ?
Eigen::VectorXd MotionParametersMapping;
Eigen::Isometry3d Tworld = Eigen::Isometry3d::Identity();
Eigen::Isometry3d PreviousTworld = Eigen::Isometry3d::Identity(); // CHECK unused ?
std::pair<Eigen::Isometry3d, Eigen::Isometry3d> MotionParametersMapping;
// Variance-Covariance matrix that estimates the
// estimation error about the 6-DoF parameters
// (DoF order : rX, rY, rZ, X, Y, Z)
Eigen::Matrix<double, 6, 6> TworldCovariance = Eigen::Matrix<double, 6, 6>::Identity();
// Represents estimated samples of the trajectory
......@@ -534,7 +535,7 @@ private:
// using the constant velocity hypothesis and the provided sensor
// position estimation
void ExpressPointInOtherReferencial(Point& p);
void ExpressPointCloudInOtherReferencial(PointCloud::Ptr pointcloud);
void ExpressPointCloudInOtherReferencial(PointCloud::Ptr& pointcloud);
// Compute the trajectory of the sensor within a frame according to the sensor
// motion model.
......@@ -553,12 +554,12 @@ private:
// (R * X + T - P).t * A * (R * X + T - P)
// Where P is the mean point of the neighborhood and A is the symmetric
// variance-covariance matrix encoding the shape of the neighborhood
int ComputeLineDistanceParameters(KDTreePCLAdaptor& kdtreePreviousEdges, Eigen::Matrix3d& R,
Eigen::Vector3d& dT, Point p, MatchingMode matchingMode);
int ComputePlaneDistanceParameters(KDTreePCLAdaptor& kdtreePreviousPlanes, Eigen::Matrix3d& R,
Eigen::Vector3d& dT, Point p, MatchingMode matchingMode);
int ComputeBlobsDistanceParameters(pcl::KdTreeFLANN<Point>::Ptr kdtreePreviousBlobs, Eigen::Matrix3d& R,
Eigen::Vector3d& dT, Point p, MatchingMode /*matchingMode*/);
int ComputeLineDistanceParameters(KDTreePCLAdaptor& kdtreePreviousEdges, const Eigen::Isometry3d& transform,
Point p, MatchingMode matchingMode);
int ComputePlaneDistanceParameters(KDTreePCLAdaptor& kdtreePreviousPlanes, const Eigen::Isometry3d& transform,
Point p, MatchingMode matchingMode);
int ComputeBlobsDistanceParameters(pcl::KdTreeFLANN<Point>::Ptr kdtreePreviousBlobs, const Eigen::Isometry3d& transform,
Point p, MatchingMode /*matchingMode*/);
// Instead of taking the k-nearest neigbors in the odometry step we will take
// specific neighbor using the particularities of the lidar sensor
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment