Skip to content
GitLab
Projects
Groups
Snippets
Help
Loading...
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
C
crs
Project overview
Project overview
Details
Activity
Releases
Repository
Repository
Files
Commits
Branches
Tags
Contributors
Graph
Compare
Issues
0
Issues
0
List
Boards
Labels
Service Desk
Milestones
Merge Requests
0
Merge Requests
0
CI / CD
CI / CD
Pipelines
Jobs
Schedules
Operations
Operations
Incidents
Environments
Packages & Registries
Packages & Registries
Container Registry
Analytics
Analytics
CI / CD
Repository
Value Stream
Wiki
Wiki
Snippets
Snippets
Members
Members
Collapse sidebar
Close sidebar
Activity
Graph
Create a new issue
Jobs
Commits
Issue Boards
Open sidebar
Marius Bock
crs
Commits
5546665b
Commit
5546665b
authored
Jan 09, 2019
by
Marius
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
asd
parent
0e52f806
Changes
6
Expand all
Hide whitespace changes
Inline
Side-by-side
Showing
6 changed files
with
344 additions
and
625 deletions
+344
-625
.idea/workspace.xml
.idea/workspace.xml
+184
-193
CMakeLists.txt
CMakeLists.txt
+0
-10
include/registration_icp/helper.hpp
include/registration_icp/helper.hpp
+1
-4
src/registration_feature/main.cpp
src/registration_feature/main.cpp
+0
-398
src/registration_icp/helper.cpp
src/registration_icp/helper.cpp
+6
-2
src/registration_icp/main.cpp
src/registration_icp/main.cpp
+153
-18
No files found.
.idea/workspace.xml
View file @
5546665b
This diff is collapsed.
Click to expand it.
CMakeLists.txt
View file @
5546665b
...
...
@@ -261,13 +261,3 @@ add_executable(crs_registration_icp ${registration_icp_SOURCES} ${registration_i
set_target_properties
(
crs_registration_icp PROPERTIES OUTPUT_NAME registration_icp PREFIX
""
)
target_link_libraries
(
crs_registration_icp crs_comon
${
PCL_COMMON_LIBRARIES
}
${
PCL_IO_LIBRARIES
}
${
catkin_LIBRARIES
}
)
####################################### registration_feature #######################################
set
(
registration_feature_INCLUDE_DIR
${
CMAKE_CURRENT_LIST_DIR
}
/include/registration_feature/
)
file
(
GLOB registration_feature_HEADERS
${
registration_feature_INCLUDE_DIR
}
/*.h*
)
set
(
registration_feature_SOURCE_DIR
${
CMAKE_CURRENT_LIST_DIR
}
/src/registration_feature/
)
file
(
GLOB registration_feature_SOURCES
${
registration_feature_SOURCE_DIR
}
/*.c*
)
add_executable
(
crs_registration_feature
${
registration_feature_SOURCES
}
${
registration_feature_HEADERS
}
)
set_target_properties
(
crs_registration_feature PROPERTIES OUTPUT_NAME registration_feature PREFIX
""
)
target_link_libraries
(
crs_registration_feature crs_comon
${
PCL_COMMON_LIBRARIES
}
${
PCL_IO_LIBRARIES
}
${
catkin_LIBRARIES
}
)
\ No newline at end of file
include/registration_
feature
/helper.hpp
→
include/registration_
icp
/helper.hpp
View file @
5546665b
...
...
@@ -46,6 +46,7 @@ private:
class
ROSParamHelper
{
public:
ROSParamHelper
();
void
setROSNodePtr
(
ros
::
NodeHandlePtr
ptr
);
...
...
@@ -86,9 +87,5 @@ private:
std
::
tuple
<
double
,
std
::
string
>
downsampleLeafSize
=
{
0.03
,
"DownsampleLeafSize"
};
ros
::
NodeHandlePtr
node_
;
};
src/registration_feature/main.cpp
deleted
100644 → 0
View file @
0e52f806
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <geometry_msgs/Pose.h>
#include <chrono>
#include <comon/clockhelper.hpp>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/passthrough.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/ia_ransac.h>
#include <pcl/features/pfh.h>
#include <pcl/features/fpfh.h>
#include <pcl/features/normal_3d.h>
#include <Eigen/Geometry>
#include <pcl/common/transforms.h>
#include <registration_feature/helper.hpp>
//FPFH feature parameters
//FPFH allignment parameters
ROSParamHelper
paramHelper
;
//ICP allingment parameters
const
int
ICP_MAX_ITERATIONS
=
50
;
const
double
ICP_MAX_CORRESPONDENCE_DISTANCE
=
0.25
;
const
double
ICP_TRANSFORMATION_EPSILON
=
1e-6
;
const
double
ICP_EUCLIDEAN_FITNESS_EPSILON
=
1e-5
;
enum
Method
{
NONE
,
FPFH
};
using
namespace
std
;
using
namespace
pcl
;
// Less typing
using
CloudRGBA
=
pcl
::
PointCloud
<
pcl
::
PointXYZRGBA
>
;
// We are going to publish clouds ... Therefore we need a publisher!
ros
::
Publisher
publisher
;
// We are going to receive clouds ... Therefore we need a subscriber!
ros
::
Subscriber
subscriber
;
PointCloud
<
FPFHSignature33
>::
Ptr
getFeaturesFPFH
(
CloudRGBA
::
Ptr
cloud
,
PointCloud
<
Normal
>::
Ptr
normals
,
double
radius
)
{
PointCloud
<
FPFHSignature33
>::
Ptr
features
=
PointCloud
<
FPFHSignature33
>::
Ptr
(
new
PointCloud
<
FPFHSignature33
>
);
search
::
KdTree
<
pcl
::
PointXYZRGBA
>::
Ptr
search_method_ptr
=
search
::
KdTree
<
PointXYZRGBA
>::
Ptr
(
new
search
::
KdTree
<
PointXYZRGBA
>
);
FPFHEstimation
<
PointXYZRGBA
,
Normal
,
FPFHSignature33
>
fpfh_est
;
fpfh_est
.
setInputCloud
(
cloud
);
fpfh_est
.
setInputNormals
(
normals
);
fpfh_est
.
setSearchMethod
(
search_method_ptr
);
fpfh_est
.
setRadiusSearch
(
radius
);
fpfh_est
.
compute
(
*
features
);
#ifdef DEBUG
cout
<<
"Computing features using radius: "
<<
radius
<<
endl
;
#endif
return
features
;
}
PointCloud
<
Normal
>::
Ptr
getNormals
(
CloudRGBA
::
Ptr
incloud
,
CloudRGBA
::
Ptr
fullcloud
,
double
normalsRadius
)
{
//ADAPTED FROM RUSU TUTORIAL
// Create the normal estimation class, and pass the input dataset to it
NormalEstimation
<
PointXYZRGBA
,
Normal
>
ne
;
ne
.
setInputCloud
(
incloud
);
// Pass the original data (before downsampling) as the search surface
ne
.
setSearchSurface
(
fullcloud
);
// Create an empty kdtree representation, and pass it to the normal estimation object.
// Its content will be filled inside the object, based on the given surface dataset.
pcl
::
search
::
KdTree
<
PointXYZRGBA
>::
Ptr
tree
(
new
pcl
::
search
::
KdTree
<
PointXYZRGBA
>
());
ne
.
setSearchMethod
(
tree
);
// Output datasets
PointCloud
<
Normal
>::
Ptr
cloud_normals
(
new
PointCloud
<
Normal
>
);
// Use all neighbors in a sphere of radius 3cm
ne
.
setRadiusSearch
(
normalsRadius
);
// Compute the features
ne
.
compute
(
*
cloud_normals
);
return
cloud_normals
;
}
CloudRGBA
::
Ptr
downsample
(
CloudRGBA
::
Ptr
frame
,
float
downsample_size
)
{
CloudRGBA
::
Ptr
sampled_frame
(
new
CloudRGBA
);
if
(
downsample_size
==
0
)
{
*
sampled_frame
=
*
frame
;
return
sampled_frame
;
}
// Downsample using voxelgrid
VoxelGrid
<
PointXYZRGBA
>
downsample_filter
;
downsample_filter
.
setLeafSize
(
downsample_size
,
downsample_size
,
downsample_size
);
downsample_filter
.
setInputCloud
(
frame
);
downsample_filter
.
filter
(
*
sampled_frame
);
return
sampled_frame
;
}
struct
Params
{
double
sacMaxCorrespondenceDist
=
0.3
;
double
sacMinSampleDist
=
0.2
;
int
sacMaxIterations
=
1000
;
float
downsampleSize
=
0.3
;
float
fpfhRadius
=
0.3
;
double
normalsRadius
=
0.2
;
};
Eigen
::
Matrix4f
registerFrame
(
CloudRGBA
::
Ptr
frame
,
CloudRGBA
::
Ptr
model
,
Params
params
)
{
Eigen
::
Matrix4f
transformation
=
Eigen
::
Matrix4f
::
Identity
();
//Downsample the input clouds
CloudRGBA
::
Ptr
sampled_model
=
downsample
(
model
,
params
.
downsampleSize
);
CloudRGBA
::
Ptr
sampled_frame
=
downsample
(
frame
,
params
.
downsampleSize
);
//Compute normals
PointCloud
<
Normal
>::
Ptr
model_normals
=
getNormals
(
sampled_model
,
model
,
params
.
normalsRadius
);
PointCloud
<
Normal
>::
Ptr
frame_normals
=
getNormals
(
sampled_frame
,
frame
,
params
.
normalsRadius
);
//Compute FPFH features
PointCloud
<
FPFHSignature33
>::
Ptr
model_features
=
getFeaturesFPFH
(
sampled_model
,
model_normals
,
params
.
fpfhRadius
);
PointCloud
<
FPFHSignature33
>::
Ptr
frame_features
=
getFeaturesFPFH
(
sampled_frame
,
frame_normals
,
params
.
fpfhRadius
);
//Initialize allignment method
SampleConsensusInitialAlignment
<
PointXYZRGBA
,
PointXYZRGBA
,
FPFHSignature33
>
sac_ia
;
sac_ia
.
setInputCloud
(
sampled_frame
);
sac_ia
.
setSourceFeatures
(
frame_features
);
sac_ia
.
setInputTarget
(
sampled_model
);
sac_ia
.
setTargetFeatures
(
model_features
);
//Set parameters for allignment and RANSAC
sac_ia
.
setMaxCorrespondenceDistance
(
params
.
sacMaxCorrespondenceDist
);
sac_ia
.
setMinSampleDistance
(
params
.
sacMinSampleDist
);
sac_ia
.
setMaximumIterations
(
params
.
sacMaxIterations
);
//Allign frame using FPFH features
sac_ia
.
align
(
*
sampled_frame
);
//std::cout << "Done! MSE: " << sac_ia.getFitnessScore() << endl;
//Get the transformation
transformation
=
sac_ia
.
getFinalTransformation
();
// if (true) {
// IterativeClosestPoint<PointXYZRGBA, PointXYZRGBA> icp;
// icp.setInputCloud(sampled_frame);
// icp.setInputTarget(sampled_model);
//
// //Set the ICP parameters
// icp.setMaxCorrespondenceDistance(icp_max_dist);
// icp.setMaximumIterations(ICP_MAX_ITERATIONS);
// icp.setTransformationEpsilon(ICP_TRANSFORMATION_EPSILON);
// icp.setEuclideanFitnessEpsilon(ICP_EUCLIDEAN_FITNESS_EPSILON);
//
// //Refine the allignment using ICP
// icp.align(*sampled_frame);
//
// transformation = icp.getFinalTransformation() * transformation;
// }
//pcl::transformPointCloud(*frame, *frame, transformation);
return
transformation
;
}
class
Algorithm
{
public:
void
setup
()
{
}
CloudRGBA
::
ConstPtr
gatherCloud
()
{
auto
cloudPtr
=
ros
::
topic
::
waitForMessage
<
CloudRGBA
>
(
cloudTopic_
,
ros
::
Duration
(
5.0
));
return
cloudPtr
;
}
private:
std
::
string
cloudTopic_
=
"/kinect2/sd/points"
;
std
::
string
bagFile
=
"/kinect2/sd/points"
;
};
// The callback function. It gets called whenever we receive a new point loucd.
void
callback
(
CloudRGBA
::
ConstPtr
msgPtr
)
{
// Print some information
ROS_INFO
(
"Received a cloud with %i points"
,
msgPtr
->
size
());
}
int
main
(
int
argc
,
char
**
argv
)
{
// Init ROS stuff
ros
::
init
(
argc
,
argv
,
"crs_tf_publisher"
);
auto
node
=
ros
::
NodeHandle
();
BagFileHelper
helper
;
paramHelper
.
setROSNodePtr
(
boost
::
make_shared
<
ros
::
NodeHandle
>
(
"ICSParams"
));
helper
.
openBagFile
(
"/home/marius/bockdata/bags/room2.bag"
);
auto
cloudTopic
=
"/kinect2/sd/points"
;
helper
.
initTopic
(
cloudTopic
);
// Init publisher and set topic
publisher
=
node
.
advertise
<
CloudRGBA
>
(
"/crs/points"
,
5
);
auto
completeCloud
=
boost
::
make_shared
<
CloudRGBA
>
();
auto
transformedCloud
=
boost
::
make_shared
<
CloudRGBA
>
();
auto
transformedCloud2
=
boost
::
make_shared
<
CloudRGBA
>
();
CloudRGBA
::
Ptr
newCloud
,
lastCloud
;
ros
::
Rate
rate
(
0.5
);
auto
completeTransform
=
Eigen
::
Matrix4f
();
completeTransform
.
setIdentity
();
paramHelper
.
updateAll
();
double
downsampleSize
=
paramHelper
.
getDownsampleLeafSize
();
while
(
helper
.
hasTopicAvailableMessages
(
cloudTopic
)
&&
ros
::
ok
())
{
std
::
string
input
;
// std::cin >> input;
paramHelper
.
updateAll
();
newCloud
=
helper
.
getNextCloudRGBAForTopic
(
cloudTopic
);
ROS_INFO
(
"Running with:
\n
%s"
,
paramHelper
.
dumpString
().
c_str
());
if
(
lastCloud
!=
nullptr
)
{
Eigen
::
Matrix4f
transform
=
Eigen
::
Matrix4f
::
Identity
();
auto
model
=
lastCloud
;
auto
frame
=
newCloud
;
downsampleSize
=
paramHelper
.
getDownsampleLeafSize
();
double
normalRadius
=
0.3
;
//Downsample the input clouds
CloudRGBA
::
Ptr
sampled_model
=
downsample
(
model
,
downsampleSize
);
CloudRGBA
::
Ptr
sampled_frame
=
downsample
(
frame
,
downsampleSize
);
//Compute normals
// PointCloud<Normal>::Ptr model_normals = getNormals(sampled_model, model, normalRadius);
// PointCloud<Normal>::Ptr frame_normals = getNormals(sampled_frame, frame, normalRadius);
IterativeClosestPoint
<
PointXYZRGBA
,
PointXYZRGBA
>
icp
;
icp
.
setInputCloud
(
sampled_frame
);
icp
.
setInputTarget
(
sampled_model
);
//Set the ICP parameters
icp
.
setMaxCorrespondenceDistance
(
paramHelper
.
getICPMaxCorrespondenceDistance
());
icp
.
setMaximumIterations
(
paramHelper
.
getICPMaxIterations
());
icp
.
setTransformationEpsilon
(
paramHelper
.
getICPTransformationEpsilon
());
icp
.
setEuclideanFitnessEpsilon
(
paramHelper
.
getICPEuclidianFitnessEpsilon
());
//Refine the allignment using ICP
icp
.
align
(
*
sampled_frame
);
auto
conv
=
icp
.
getConvergeCriteria
();
auto
criteria
=
conv
->
getConvergenceState
();
switch
(
criteria
)
{
case
pcl
::
registration
::
DefaultConvergenceCriteria
<
float
>::
CONVERGENCE_CRITERIA_TRANSFORM
:
ROS_INFO
(
"CONVERGENCE_CRITERIA_TRANSFORM"
);
break
;
case
pcl
::
registration
::
DefaultConvergenceCriteria
<
float
>::
CONVERGENCE_CRITERIA_ABS_MSE
:
ROS_INFO
(
"CONVERGENCE_CRITERIA_ABS_MSE"
);
break
;
case
pcl
::
registration
::
DefaultConvergenceCriteria
<
float
>::
CONVERGENCE_CRITERIA_ITERATIONS
:
ROS_INFO
(
"CONVERGENCE_CRITERIA_ITERATIONS"
);
break
;
case
pcl
::
registration
::
DefaultConvergenceCriteria
<
float
>::
CONVERGENCE_CRITERIA_NO_CORRESPONDENCES
:
ROS_INFO
(
"CONVERGENCE_CRITERIA_NO_CORRESPONDENCES"
);
break
;
case
pcl
::
registration
::
DefaultConvergenceCriteria
<
float
>::
CONVERGENCE_CRITERIA_NOT_CONVERGED
:
ROS_INFO
(
"CONVERGENCE_CRITERIA_NOT_CONVERGED"
);
break
;
case
pcl
::
registration
::
DefaultConvergenceCriteria
<
float
>::
CONVERGENCE_CRITERIA_REL_MSE
:
ROS_INFO
(
"CONVERGENCE_CRITERIA_REL_MSE"
);
break
;
default:
ROS_INFO
(
"CONVERGENCE unknown ..."
);
break
;
}
transform
=
icp
.
getFinalTransformation
();
completeTransform
=
transform
*
completeTransform
;
pcl
::
transformPointCloud
(
*
newCloud
,
*
transformedCloud
,
completeTransform
);
//pcl::transformPointCloud(*newCloud, *transformedCloud, transform.inverse());
*
completeCloud
+=
*
transformedCloud
;
pcl_conversions
::
toPCL
(
ros
::
Time
::
now
(),
completeCloud
->
header
.
stamp
);
completeCloud
->
header
.
frame_id
=
"kinect2_ir_optical_frame"
;
publisher
.
publish
(
completeCloud
);
lastCloud
=
newCloud
;
}
else
{
lastCloud
=
downsample
(
newCloud
,
downsampleSize
);
}
}
return
0
;
}
// Params params;
// params.sacMaxCorrespondenceDist = 0.3;
// params.sacMinSampleDist = 0.5;
// params.sacMaxIterations = 1000;
// params.downsampleSize = 0.1;
// params.fpfhRadius = 0.3;
// params.normalsRadius = 0.2;
// //Compute FPFH features
// PointCloud<FPFHSignature33>::Ptr model_features = getFeaturesFPFH(sampled_model, model_normals, params.fpfhRadius);
// PointCloud<FPFHSignature33>::Ptr frame_features = getFeaturesFPFH(sampled_frame, frame_normals, params.fpfhRadius);
//
// //Initialize allignment method
// SampleConsensusInitialAlignment<PointXYZRGBA, PointXYZRGBA, FPFHSignature33> sac_ia;
// sac_ia.setInputCloud(sampled_frame);
// sac_ia.setSourceFeatures(frame_features);
// sac_ia.setInputTarget(sampled_model);
// sac_ia.setTargetFeatures(model_features);
//
// //Set parameters for allignment and RANSAC
// sac_ia.setMaxCorrespondenceDistance(params.sacMaxCorrespondenceDist);
// sac_ia.setMinSampleDistance(params.sacMinSampleDist);
// sac_ia.setMaximumIterations(params.sacMaxIterations);
//
// //Allign frame using FPFH features
// sac_ia.align(*sampled_frame);
// //std::cout << "Done! MSE: " << sac_ia.getFitnessScore() << endl;
//
// //Get the transformation
// transform = sac_ia.getFinalTransformation();
// for (int i = 0; i < transformedCloud->size(); ++i) {
// (*transformedCloud)[i].r = 255;
// (*transformedCloud)[i].g = 0;
// (*transformedCloud)[i].b = 0;
// }
//
// for (int i = 0; i < lastCloud->size(); ++i) {
// (*lastCloud)[i].r = 0;
// (*lastCloud)[i].g = 255;
// (*lastCloud)[i].b = 0;
// }
// // *completeCloud = *lastCloud + *transformedCloud;
//
// for (int i = 0; i < transformedCloud2->size(); ++i) {
// (*transformedCloud2)[i].r = 0;
// (*transformedCloud2)[i].g = 0;
// (*transformedCloud2)[i].b = 255;
// }
//
// for (int i = 0; i < newCloud->size(); ++i) {
// (*newCloud)[i].r = 0;
// (*newCloud)[i].g = 123;
// (*newCloud)[i].b = 123;
// }
//
// *completeCloud = *lastCloud;
// *completeCloud += *newCloud;
// *completeCloud += *transformedCloud;
// *completeCloud += *transformedCloud2;
\ No newline at end of file
src/registration_
feature
/helper.cpp
→
src/registration_
icp
/helper.cpp
View file @
5546665b
#include <registration_
feature
/helper.hpp>
#include <registration_
icp
/helper.hpp>
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <unordered_map>
...
...
@@ -95,7 +95,7 @@ bool BagFileHelper::hasTopicAvailableMessages(const std::string &topic) const {
}
BagFileHelper
::
BagFileHelper
()
{
data_
=
std
::
make_unique
<
BagFileHelper
::
Data
>
();
data_
=
std
::
make_unique
<
BagFileHelper
::
Data
>
();
}
BagFileHelper
::~
BagFileHelper
()
{
...
...
@@ -269,3 +269,7 @@ std::string ROSParamHelper::dumpString() {
<<
"ICPMaxIterations"
<<
getICPMaxIterations
()
<<
"
\n
"
;
return
ss
.
str
();
}
ROSParamHelper
::
ROSParamHelper
()
{
setROSNodePtr
(
boost
::
make_shared
<
ros
::
NodeHandle
>
(
"ICSParams"
));
}
src/registration_icp/main.cpp
View file @
5546665b
...
...
@@ -7,38 +7,173 @@
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/passthrough.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/ia_ransac.h>
#include <pcl/features/pfh.h>
#include <pcl/features/fpfh.h>
#include <pcl/features/normal_3d.h>
#include <Eigen/Geometry>
#include <pcl/common/transforms.h>
#include <registration_icp/helper.hpp>
// Less typing
using
CloudRGBA
=
pcl
::
PointCloud
<
pcl
::
PointXYZRGBA
>
;
using
IterativeClosestPoint
=
pcl
::
IterativeClosestPoint
<
pcl
::
PointXYZRGBA
,
pcl
::
PointXYZRGBA
>
;
using
Criteria
=
pcl
::
registration
::
DefaultConvergenceCriteria
<
float
>
;
// We are going to publish clouds ... Therefore we need a publisher!
ros
::
Publisher
publisher
;
// Convert a convergence criteria to a string. You can ignore it!
const
char
*
getCriteria
(
IterativeClosestPoint
&
icp
)
{
auto
criteria
=
icp
.
getConvergeCriteria
()
->
getConvergenceState
();
switch
(
criteria
)
{
case
Criteria
::
CONVERGENCE_CRITERIA_TRANSFORM
:
return
"CONVERGENCE_CRITERIA_TRANSFORM"
;
case
Criteria
::
CONVERGENCE_CRITERIA_ABS_MSE
:
return
"CONVERGENCE_CRITERIA_ABS_MSE"
;
case
Criteria
::
CONVERGENCE_CRITERIA_ITERATIONS
:
return
"CONVERGENCE_CRITERIA_ITERATIONS"
;
case
Criteria
::
CONVERGENCE_CRITERIA_NO_CORRESPONDENCES
:
return
"CONVERGENCE_CRITERIA_NO_CORRESPONDENCES"
;
case
Criteria
::
CONVERGENCE_CRITERIA_NOT_CONVERGED
:
return
"CONVERGENCE_CRITERIA_NOT_CONVERGED"
;
case
Criteria
::
CONVERGENCE_CRITERIA_REL_MSE
:
return
"CONVERGENCE_CRITERIA_REL_MSE"
;
default:
return
"CONVERGENCE_CRITERIA unknown ..."
;
}
}
// We are going to receive clouds ... Therefore we need a subscriber!
ros
::
Subscriber
subscriber
;
// We are going to publish clouds ... Therefore we need a publisher!
ros
::
Publisher
completeCloudPublisher
;
// The callback function. It gets called whenever we receive a new point loucd.
void
callback
(
CloudRGBA
::
ConstPtr
msgPtr
)
{
// Print some information
ROS_INFO
(
"Received a cloud with %i points"
,
msgPtr
->
size
());
CloudRGBA
::
Ptr
downsample
(
CloudRGBA
::
Ptr
frame
,
float
leafSize
)
{
CloudRGBA
::
Ptr
downsampledCloud
(
new
CloudRGBA
);
if
(
leafSize
==
0
)
{
*
downsampledCloud
=
*
frame
;
return
downsampledCloud
;
}
// Downsample using voxelgrid
pcl
::
VoxelGrid
<
pcl
::
PointXYZRGBA
>
downsample_filter
;
downsample_filter
.
setLeafSize
(
leafSize
,
leafSize
,
leafSize
);
downsample_filter
.
setInputCloud
(
frame
);
downsample_filter
.
filter
(
*
downsampledCloud
);
return
downsampledCloud
;
}
int
main
(
int
argc
,
char
**
argv
){
// Init ROS stuff
ros
::
init
(
argc
,
argv
,
"crs_tf_publisher"
);
int
main
(
int
argc
,
char
**
argv
)
{
// The name of this node
std
::
string
nodeName
=
"crs_tf_publisher"
;
// The used bag file
std
::
string
bagFilePath
=
"/home/marius/bockdata/bags/room2.bag"
;
// The topic we subscribe to
std
::
string
cloudSubscribeTopic
=
"/kinect2/sd/points"
;
// The topic we advertise the complete registered point clouds to
std
::
string
cloudAdvertiseTopic
=
"/crs/points"
;
// The topic we subscribe to
std
::
string
completeCloudFrame
=
"kinect2_ir_optical_frame"
;
// Some ICP Parameters. DO NOT CHANGE THEM!
double
ICPMaxCorrespondenceDistance
=
0.2
;
double
ICPTransformationEpsilon
=
0.00001
;
double
ICPEuclidianFitnessEpsilon
=
0.1
;
// The leaf size of the VoxelGrid Filter
double
ICPDownsampleLeafSize
=
0.1
;
// The maximal of ICP iterations
int
ICPMaxIterations
=
20
;
// Init Basic ROS stuff
ros
::
init
(
argc
,
argv
,
nodeName
);
auto
node
=
ros
::
NodeHandle
();
// Init subscriber and set topic
subscriber
=
node
.
subscribe
<
CloudRGBA
>
(
"/kinect2/sd/points"
,
1
,
callback
);
// We are going to advertise the result cloud
completeCloudPublisher
=
node
.
advertise
<
CloudRGBA
>
(
cloudAdvertiseTopic
,
5
);
// The ROS Bag Helper.
BagFileHelper
helper
;
helper
.
openBagFile
(
bagFilePath
);
helper
.
initTopic
(
cloudSubscribeTopic
);
// Create an empty cloud that is going to contain ALL registered point clouds
auto
completeCloud
=
boost
::
make_shared
<
CloudRGBA
>
();
// Create a matrix that is going to contain the transformation from the CURRENT frame to the FIRST Model
// At the beginning it is an identiy matrix -> no transformation at all
Eigen
::
Matrix4f
completeTransform
;
completeTransform
.
setIdentity
();
// Variables for the model and the frame
CloudRGBA
::
Ptr
model
,
frame
;
// The main loop runs as long as there are new clouds avaiable and [CRTL + c] has not been pressed
while
(
helper
.
hasTopicAvailableMessages
(
cloudSubscribeTopic
)
&&
ros
::
ok
())
{
// Gather the next cloud from our bag file
frame
=
helper
.
getNextCloudRGBAForTopic
(
cloudSubscribeTopic
);
if
(
model
==
nullptr
)
{
// Since it is the first loop iteration we do not have a model yet.
// The current frame will be the model for the next frame
model
=
frame
;
}
else
{
// Downsample the input clouds
auto
downsampledModel
=
downsample
(
model
,
ICPDownsampleLeafSize
);
auto
downsampledFrame
=
downsample
(
frame
,
ICPDownsampleLeafSize
);
// Create the ICP object
IterativeClosestPoint
icp
;
// Init publisher and set topic
publisher
=
node
.
advertise
<
CloudRGBA
>
(
"/crs/points"
,
5
);
// Set the ICP parameters
icp
.
setMaxCorrespondenceDistance
(
ICPMaxCorrespondenceDistance
);
icp
.
setMaximumIterations
(
ICPMaxIterations
);
icp
.
setTransformationEpsilon
(
ICPTransformationEpsilon
);
icp
.
setEuclideanFitnessEpsilon
(
ICPEuclidianFitnessEpsilon
);
// Run!
ros
::
spin
();
// Set ICP data sources
icp
.
setInputCloud
(
downsampledFrame
);
icp
.
setInputTarget
(
downsampledModel
);
// Run ICP Alligment
icp
.
align
(
*
downsampledFrame
);