Skip to content
GitLab
Explore
Sign in
Primary navigation
Search or go to…
Project
K
kf_based_terrain_analysis
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Snippets
Build
Pipelines
Jobs
Pipeline schedules
Artifacts
Deploy
Releases
Package Registry
Model registry
Operate
Environments
Terraform modules
Monitor
Incidents
Service Desk
Analyze
Value stream analytics
Contributor analytics
CI/CD analytics
Repository analytics
Model experiments
Help
Help
Support
GitLab documentation
Compare GitLab plans
Community forum
Contribute to GitLab
Provide feedback
Keyboard shortcuts
?
Snippets
Groups
Projects
Show more breadcrumbs
Iván del Pino
kf_based_terrain_analysis
Commits
cd4d5e64
Commit
cd4d5e64
authored
2 years ago
by
Iván del Pino
Browse files
Options
Downloads
Patches
Plain Diff
documenting some functions
parent
91deb9ad
No related branches found
No related tags found
No related merge requests found
Changes
1
Hide whitespace changes
Inline
Side-by-side
Showing
1 changed file
src/kf_based_terrain_analysis.cpp
+48
-6
48 additions, 6 deletions
src/kf_based_terrain_analysis.cpp
with
48 additions
and
6 deletions
src/kf_based_terrain_analysis.cpp
+
48
−
6
View file @
cd4d5e64
...
...
@@ -363,6 +363,23 @@ void CKf_Based_Terrain_Analysis::labelLocalGroundPoints(
pcl
::
PointCloud
<
pcl
::
PointXYZRGBNormal
>::
Ptr
cloud_ptr
,
std
::
vector
<
std
::
vector
<
int
>>
&
edges
,
std
::
vector
<
std
::
vector
<
kf_based_terrain_analysis_lib
::
Edge
>>
&
discarded_edges
)
{
// In this function we will check by Mahalanobis distance test (using the prior) each elevation_cloud point in the ROI of the reference being
// evaluated. Those who pass, are ground points, and will be used to update the prior. Those who pass and that are not yet labeled (have
// the class value as UNKNOWN)are also used to drive the exploration, being selected to create new references (vertices in the graph)
//
// Points classified as GROUND are changed to GROUND independently of their previous class (because points can be seen as obstacles from
// a POV, but seen as ground from another). Points classified as OBSTACLE are changed to OBSTACLE only if they were not of GROUND class.
// Either way, if a change of class is produced or if the new classification has a better score than the previous one, we mark the current reference
// (vertex in the graph) as the one which made the label. This is done to avoid searching during the whole point cloud labeling.
//
// After this, we use the GROUND classified points in the elevation cloud to update the prior
// and store the posterior in the graph
//
// Once we get the posterior, we use the points that were UNKNOWN at the beggining of this function to generate new references (vertices)
// then, we analize the vertices in the ROI and make edges to those that are seen as ground from the currently processed vertex. The CONNECTED
// _TO_ROOT flag is set in those to which we connect (it is unset by default)
//std::cout << "Starting labelLocalGroundPoints!!" << std::endl;
// if (filtering_configuration.measure_performance)
// CFunctionMonitor performance_monitor("labelLocalGroundPoints", performance_supervisor_ptr_);
...
...
@@ -657,6 +674,7 @@ void CKf_Based_Terrain_Analysis::labelGroundPoints(
pcl
::
PointCloud
<
pcl
::
PointXYZRGBNormal
>::
Ptr
references_in_ROI
(
new
pcl
::
PointCloud
<
pcl
::
PointXYZRGBNormal
>
);
//std::cout << "Extracting points in ROI..." << std::endl;
// We extract points from the elevation_cloud in the Region of Interest of the reference currently being processed
extractPointsInROI
(
lidar_configuration
,
filtering_configuration
,
ground_reference_cloud_ptr
->
points
[
reference_index
],
elevation_cloud_ptr
,
points_in_ROI
);
...
...
@@ -793,12 +811,22 @@ void CKf_Based_Terrain_Analysis::fastGenerateElevationCloud(
pcl
::
PointCloud
<
pcl
::
PointXYZRGBNormal
>::
Ptr
pcl_cloud_ptr
,
pcl
::
PointCloud
<
pcl
::
PointXYZRGBNormal
>::
Ptr
elevation_cloud
,
std
::
vector
<
std
::
vector
<
int
>>
&
correspondence_indexes
)
{
// In this function we want to create a reduced point cloud using a grid. Each point in the reduced point cloud
// will contain the lowest point of its grid cell, the values of z mean and variance, intensity mean and variance,
// and a vector with the indices of the rest of points in the cell w.r.t. the original point cloud, also it will
// be colored as OUTLIER or UNKNOWN (since ground obstacle classification is done later on the algorithm)
//if (filtering_configuration.measure_performance)
//CFunctionMonitor performance_monitor("generateElevationCloud", performance_supervisor_ptr_);
// We will use a integer matrix with sufficient size to discretize the whole point cloud (2*MAX_RANGE/resolution)^2
// This matrix is required to save memory, because it contains only one integer by cell, while the structure required
// to store the actual data is far more complex (see the struct ElevationCloudCell), so we only create ElevationCloudCell
// objects in cells that actually contain data (which are a lot less than the size of the grid)
int
number_of_rows_and_columns
=
std
::
ceil
(
2.0
*
MAX_RANGE
/
filtering_configuration
.
elevation_grid_resolution
);
//std::cout << "number_of_rows_and_columns = " << number_of_rows_and_columns << std::endl;
// We initialize the grid to unknown values
int
elevation_map
[
number_of_rows_and_columns
][
number_of_rows_and_columns
];
for
(
int
i
=
0
;
i
<
number_of_rows_and_columns
;
++
i
)
for
(
int
j
=
0
;
j
<
number_of_rows_and_columns
;
++
j
)
...
...
@@ -864,7 +892,7 @@ void CKf_Based_Terrain_Analysis::fastGenerateElevationCloud(
elevation_cloud_point
.
g
=
G_CLASS_UNKNOWN
;
elevation_cloud_point
.
b
=
B_CLASS_UNKNOWN
;
}
// TODO:
// TODO:
check this!
float
sum
=
std
::
accumulate
(
elevation_cell_vector
[
i
].
z_coordinates
.
begin
(),
elevation_cell_vector
[
i
].
z_coordinates
.
end
(),
0.0
);
float
z_mean
=
sum
/
(
float
)
elevation_cell_vector
[
i
].
z_coordinates
.
size
();
...
...
@@ -919,6 +947,16 @@ void CKf_Based_Terrain_Analysis::generateRootVertex(
const
kf_based_terrain_analysis_lib
::
FilteringConfiguration
filtering_configuration
,
pcl
::
PointCloud
<
pcl
::
PointXYZRGBNormal
>::
Ptr
ground_reference_cloud_ptr
)
{
// In this function we generate the root vertex using the z value as -1.0 * LiDAR height, the slope values are set to 0.0
// since we assume that the sensor is normal to the plane in which it lies. The uncertainties (sigma_z, sigma_roll, sigma_pitch)
// are set by parameter (roll means not the angle, but the tangent of the angle, in other words the slope in y axis, and pitch
// is the slope in x axis).
// We also set the flag "VERTEX_CONNECTED_TO_ROOT" to true, because the root is obviously connected to root! --> this flag will be passed
// through the graph building process in such a way that once it is built we can avoid to search paths to root using nodes that we already know
// that are not connected with the root (the root position is same of the sensor but at ground level, so not connected to root means the same
// as unreachable for the robot
//std::cout << "Generating root vertex" << std::endl;
// The first reference point (root vertex) is the ground in (0,0) approximately known by sensor extrinsic calibration
pcl
::
PointXYZRGBNormal
root_vertex
;
...
...
@@ -948,11 +986,10 @@ void CKf_Based_Terrain_Analysis::generateRootVertex(
root_vertex
.
data_n
[
GRND_REF_DATA_N_2_VERTEX_CLASS
]
=
VERTEX_CONNECTED_TO_ROOT
;
ground_reference_cloud_ptr
->
points
.
push_back
(
root_vertex
);
Eigen
::
Matrix3d
covariance_matrix
;
covariance_matrix
<<
filtering_configuration
.
z_initial_std_dev
*
filtering_configuration
.
z_initial_std_dev
,
0.0
,
0.0
,
0.0
,
filtering_configuration
.
pitch_initial_std_dev
*
filtering_configuration
.
pitch_initial_std_dev
,
0.0
,
0.0
,
0.0
,
filtering_configuration
.
roll_initial_std_dev
*
filtering_configuration
.
roll_initial_std_dev
;
// Eigen::Matrix3d covariance_matrix;
// covariance_matrix << filtering_configuration.z_initial_std_dev * filtering_configuration.z_initial_std_dev, 0.0, 0.0, 0.0, filtering_configuration.pitch_initial_std_dev
// * filtering_configuration.pitch_initial_std_dev, 0.0, 0.0, 0.0, filtering_configuration.roll_initial_std_dev
// * filtering_configuration.roll_initial_std_dev;
// references_covariance_matrix_vector_.push_back(covariance_matrix);
}
...
...
@@ -1062,6 +1099,11 @@ void CKf_Based_Terrain_Analysis::fastLabelPointcloudUsingGroundModel(
const
pcl
::
PointCloud
<
pcl
::
PointXYZRGBNormal
>::
Ptr
ground_reference_cloud_ptr
,
const
std
::
vector
<
std
::
vector
<
int
>>
&
edges
,
pcl
::
PointCloud
<
pcl
::
PointXYZRGBNormal
>::
Ptr
pcl_cloud_ptr
)
{
// In this function, we iterate over all points in the elevation cloud (the grid-based reduction of the original point cloud)
// for each elevation cloud point, we extract the best reference (encoded in the integer part of the float in the field
// DATA_C_2_INDEX_OF_GROUND_REF_THAT_MADE_THE_LABEL), and then using the indexes vector we check the mahalanobis distance to every
// point represented by the elevation point and set the score.
//if (filtering_configuration.measure_performance)
//CFunctionMonitor performance_monitor("labelPointcloudUsingGroundModel", performance_supervisor_ptr_);
//std::cout << "Labeling points in the original pointcloud using the posterior and the ROI indexes vectors..."
...
...
This diff is collapsed.
Click to expand it.
Preview
0%
Loading
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Save comment
Cancel
Please
register
or
sign in
to comment