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
25e3629a
Commit
25e3629a
authored
Jan 08, 2019
by
Marius
Browse files
Options
Browse Files
Download
Email Patches
Plain Diff
asd
parent
b91c59ca
Changes
9
Expand all
Hide whitespace changes
Inline
Side-by-side
Showing
9 changed files
with
1273 additions
and
250 deletions
+1273
-250
.idea/workspace.xml
.idea/workspace.xml
+392
-206
CMakeLists.txt
CMakeLists.txt
+21
-0
include/registration_feature/helper.hpp
include/registration_feature/helper.hpp
+94
-0
src/er1_driver_node/main.cpp
src/er1_driver_node/main.cpp
+1
-1
src/point_processing_node/main.cpp
src/point_processing_node/main.cpp
+43
-31
src/point_processing_node_student/main.cpp
src/point_processing_node_student/main.cpp
+9
-12
src/registration_feature/helper.cpp
src/registration_feature/helper.cpp
+271
-0
src/registration_feature/main.cpp
src/registration_feature/main.cpp
+398
-0
src/registration_icp/main.cpp
src/registration_icp/main.cpp
+44
-0
No files found.
.idea/workspace.xml
View file @
25e3629a
This diff is collapsed.
Click to expand it.
CMakeLists.txt
View file @
25e3629a
...
...
@@ -250,3 +250,24 @@ add_executable(crs_point_processing_node_student ${point_processing_node_student
set_target_properties
(
crs_point_processing_node_student PROPERTIES OUTPUT_NAME point_processing_node_student PREFIX
""
)
target_link_libraries
(
crs_point_processing_node_student crs_comon
${
PCL_COMMON_LIBRARIES
}
${
PCL_IO_LIBRARIES
}
${
catkin_LIBRARIES
}
)
####################################### registration_icp #######################################
set
(
registration_icp_INCLUDE_DIR
${
CMAKE_CURRENT_LIST_DIR
}
/include/registration_icp/
)
file
(
GLOB registration_icp_HEADERS
${
registration_icp_INCLUDE_DIR
}
/*.h*
)
set
(
registration_icp_SOURCE_DIR
${
CMAKE_CURRENT_LIST_DIR
}
/src/registration_icp/
)
file
(
GLOB registration_icp_SOURCES
${
registration_icp_SOURCE_DIR
}
/*.c*
)
add_executable
(
crs_registration_icp
${
registration_icp_SOURCES
}
${
registration_icp_HEADERS
}
)
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
0 → 100644
View file @
25e3629a
#pragma once
#include <string>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <boost/assert.hpp>
#include <ros/ros.h>
using
CloudRGBA
=
pcl
::
PointCloud
<
pcl
::
PointXYZRGBA
>
;
/**
* - Has to be a
*/
class
BagFileHelper
{
public:
BagFileHelper
(
const
BagFileHelper
&
)
=
delete
;
BagFileHelper
();
~
BagFileHelper
();
void
openBagFile
(
const
std
::
string
&
filePath
);
// void reset();
// initialize all topics
bool
initTopic
(
std
::
string
topic
);
// bool getTopicSize(const std::string topic) const;
bool
hasTopicAvailableMessages
(
const
std
::
string
&
topic
)
const
;
bool
isTopicInitialized
(
const
std
::
string
&
topic
)
const
;
CloudRGBA
::
ConstPtr
getNextConstCloudRGBAForTopic
(
const
std
::
string
&
topic
);
CloudRGBA
::
Ptr
getNextCloudRGBAForTopic
(
const
std
::
string
&
topic
);
// template<class T>
// std::shared_ptr<T> getNextForTopic(const std::string& topic);
private:
/// A PIMPL class is required due to some PCL and ROS incompatibilities.
class
Data
;
std
::
unique_ptr
<
Data
>
data_
;
};
class
ROSParamHelper
{
public:
void
setROSNodePtr
(
ros
::
NodeHandlePtr
ptr
);
std
::
string
dumpString
();
bool
updateAll
();
bool
updateICPMaxCorrespondenceDistance
();
void
setICPMaxCorrespondenceDistance
(
double
value
);
double
getICPMaxCorrespondenceDistance
()
const
;
bool
updateICPMaxIterations
();
void
setICPMaxIterations
(
int
value
);
int
getICPMaxIterations
()
const
;
bool
updateICPTransformationEpsilon
();
void
setICPTransformationEpsilon
(
double
value
);
double
getICPTransformationEpsilon
()
const
;
bool
updateICPEuclidianFitnessEpsilon
();
void
setICPEuclidianFitnessEpsilon
(
double
value
);
double
getICPEuclidianFitnessEpsilon
()
const
;
bool
updateDownsampleLeafSize
();
void
setDownsampleLeafSize
(
double
value
);
double
getDownsampleLeafSize
()
const
;
private:
std
::
tuple
<
std
::
string
,
std
::
string
>
mainCloudTopic_
=
{
"/kinect2/sd/points"
,
"MainCloudTopic"
};
std
::
tuple
<
std
::
string
,
std
::
string
>
mainCloudBag_
=
{
"/home/marius/bockdata/bags/crs.bag"
,
"MainBagFile"
};
std
::
tuple
<
double
,
std
::
string
>
icpMaxCorrespondenceDistance_
=
{
0.03
,
"ICPMaxCorrespondenceDistance"
};
std
::
tuple
<
int
,
std
::
string
>
icpMaxIterations_
=
{
100
,
"ICPMaxIterations"
};
std
::
tuple
<
double
,
std
::
string
>
icpTransformationEpsilon_
=
{
0.03
,
"ICPTransformationEpsilon"
};
std
::
tuple
<
double
,
std
::
string
>
icpEuclidianFitnessEpsilon_
=
{
0.03
,
"ICPEuclidianFitnessEpsilon"
};
std
::
tuple
<
double
,
std
::
string
>
downsampleLeafSize
=
{
0.03
,
"DownsampleLeafSize"
};
ros
::
NodeHandlePtr
node_
;
};
src/er1_driver_node/main.cpp
View file @
25e3629a
...
...
@@ -72,7 +72,7 @@ int main(int argc, char** argv) {
ros
::
Subscriber
sub
=
node
.
subscribe
(
"er1_motor_commands"
,
1000
,
&
callback
);
// Connect to robot. You have to secify which robot you are using!
EvolutionRobotID
id
=
EvolutionRobotID
::
ER1
Small
;
EvolutionRobotID
id
=
EvolutionRobotID
::
ER1
Medium
;
robot
.
connect
(
id
);
// Enter ROS main loop.
...
...
src/point_processing_node/main.cpp
View file @
25e3629a
...
...
@@ -3,47 +3,59 @@
#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>
// 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
;
int
main
(
int
argc
,
char
**
argv
){
// Init ROS stuff
ros
::
init
(
argc
,
argv
,
"crs_tf_publisher"
);
// Create Transform Broadcaster
tf
::
TransformBroadcaster
br
;
// 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
());
// Init the clock helper
ClockHelper
helper
;
helper
.
init
();
helper
.
setCycleTime
(
std
::
chrono
::
seconds
(
3
));
// Specify an update rate
ros
::
Rate
rate
(
3
);
// Init voxelgrid filter and apply
pcl
::
VoxelGrid
<
pcl
::
PointXYZRGBA
>
sor
;
sor
.
setInputCloud
(
msgPtr
);
sor
.
setLeafSize
(
0.01
f
,
0.01
f
,
0.01
f
);
auto
filteredPtr
=
boost
::
make_shared
<
CloudRGBA
>
();
sor
.
filter
(
*
filteredPtr
);
// Main loop
while
(
ros
::
ok
())
{
// Get the roll value for the current time point
auto
roll
=
helper
.
getRoll
();
// Specify the origin for our transformation
auto
origin
=
tf
::
Vector3
(
0
,
0
,
0
);
// Set the header of the message. Just some shabby details ...
filteredPtr
->
header
.
frame_id
=
"filtered_frame"
;
pcl_conversions
::
toPCL
(
ros
::
Time
::
now
(),
filteredPtr
->
header
.
stamp
);
// Specify the rotation for our transformation
tf
::
Quaternion
q
;
q
.
setRPY
(
0
,
0
,
roll
);
// Publish cloud
publisher
.
publish
(
filteredPtr
);
// Print some information
ROS_INFO
(
"Published a cloud with %i points"
,
filteredPtr
->
size
());
}
int
main
(
int
argc
,
char
**
argv
){
// Init ROS stuff
ros
::
init
(
argc
,
argv
,
"crs_tf_publisher"
);
auto
node
=
ros
::
NodeHandle
();
// Bundle all information into a transformation object
tf
::
Transform
transform
;
transform
.
setOrigin
(
origin
);
transform
.
setRotation
(
q
);
// Init subscriber and set topic
subscriber
=
node
.
subscribe
<
CloudRGBA
>
(
"/kinect2/sd/points"
,
1
,
callback
);
// Publish the transformation
std
::
string
destinationFrame
=
"map"
;
std
::
string
sourceFrame
=
"kinect2_ir_optical_frame"
;
br
.
sendTransform
(
tf
::
StampedTransform
(
transform
,
ros
::
Time
::
now
(),
destinationFrame
,
sourceFrame
));
// Init publisher and set topic
publisher
=
node
.
advertise
<
CloudRGBA
>
(
"/crs/points"
,
5
);
// Sleep a little bit
rate
.
sleep
();
}
// Run!
ros
::
spin
();
return
0
;
};
\ No newline at end of file
src/point_processing_node_student/main.cpp
View file @
25e3629a
...
...
@@ -24,20 +24,20 @@ void callback(CloudRGBA::ConstPtr msgPtr) {
ROS_INFO
(
"Received a cloud with %i points"
,
msgPtr
->
size
());
auto
filteredPtr
=
boost
::
make_shared
<
CloudRGBA
>
();
// Init voxelgrid filter and apply
pcl
::
VoxelGrid
<
pcl
::
PointXYZRGBA
>
sor
;
sor
.
setInputCloud
(
msgPtr
);
sor
.
setLeafSize
(
0.01
f
,
0.01
f
,
0.01
f
);
auto
filteredPtr
=
boost
::
make_shared
<
CloudRGBA
>
();
sor
.
filter
(
*
filteredPtr
);
/* ######### <MISSING> ########## */
/* ######### <MISSING> ########## */
/* ######### <MISSING> ########## */
// Set the header of the message. Just some shabby details ...
filteredPtr
->
header
.
frame_id
=
"filtered_frame"
;
pcl_conversions
::
toPCL
(
ros
::
Time
::
now
(),
filteredPtr
->
header
.
stamp
);
// Publish cloud
publisher
.
publish
(
filteredPtr
);
// Print some information
ROS_INFO
(
"Published a cloud with %i points"
,
filteredPtr
->
size
());
...
...
@@ -45,14 +45,11 @@ void callback(CloudRGBA::ConstPtr msgPtr) {
int
main
(
int
argc
,
char
**
argv
){
// Init ROS stuff
ros
::
init
(
argc
,
argv
,
"crs_
tf_publishe
r"
);
ros
::
init
(
argc
,
argv
,
"crs_
cloud_processo
r"
);
auto
node
=
ros
::
NodeHandle
();
// Init subscriber and set topic
subscriber
=
node
.
subscribe
<
CloudRGBA
>
(
"/kinect2/sd/points"
,
1
,
callback
);
// Init publisher and set topic
publisher
=
node
.
advertise
<
CloudRGBA
>
(
"/crs/points"
,
5
);
/* ######### <MISSING> ########## */
/* ######### <MISSING> ########## */
// Run!
ros
::
spin
();
...
...
src/registration_feature/helper.cpp
0 → 100644
View file @
25e3629a
#include <registration_feature/helper.hpp>
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <unordered_map>
#include <pcl_ros/point_cloud.h>
class
BagFileHelper
::
Data
{
public:
Data
()
=
default
;
~
Data
()
=
default
;
rosbag
::
Bag
bag_
;
std
::
unordered_map
<
std
::
string
,
std
::
unique_ptr
<
rosbag
::
View
>>
views_
;
std
::
unordered_map
<
std
::
string
,
std
::
pair
<
rosbag
::
View
::
iterator
,
rosbag
::
View
::
iterator
>>
iteratorPairs_
;
};
void
BagFileHelper
::
openBagFile
(
const
std
::
string
&
filePath
)
{
// TODO: can throw exception
data_
->
bag_
.
open
(
filePath
);
}
//void BagFileHelper::reset() {
//
//}
CloudRGBA
::
ConstPtr
BagFileHelper
::
getNextConstCloudRGBAForTopic
(
const
std
::
string
&
topic
)
{
if
(
data_
->
views_
.
count
(
topic
)
==
0
)
{
return
nullptr
;
}
else
{
auto
&
[
cur
,
end
]
=
data_
->
iteratorPairs_
.
at
(
topic
);
if
(
cur
==
end
)
{
return
nullptr
;
}
else
{
auto
ptr
=
cur
->
instantiate
<
CloudRGBA
>
();
++
cur
;
return
ptr
;
}
}
}
CloudRGBA
::
Ptr
BagFileHelper
::
getNextCloudRGBAForTopic
(
const
std
::
string
&
topic
)
{
if
(
data_
->
views_
.
count
(
topic
)
==
0
)
{
return
nullptr
;
}
else
{
auto
&
[
cur
,
end
]
=
data_
->
iteratorPairs_
.
at
(
topic
);
if
(
cur
==
end
)
{
return
nullptr
;
}
else
{
auto
ptr
=
cur
->
instantiate
<
CloudRGBA
>
();
++
cur
;
return
ptr
;
}
}
}
bool
BagFileHelper
::
initTopic
(
std
::
string
topic
)
{
try
{
auto
cloudBagView_
=
std
::
make_unique
<
rosbag
::
View
>
(
data_
->
bag_
,
rosbag
::
TopicQuery
(
topic
));
if
(
cloudBagView_
->
size
()
==
0
)
{
}
auto
begin
=
cloudBagView_
->
begin
();
auto
end
=
cloudBagView_
->
end
();
data_
->
iteratorPairs_
.
emplace
(
topic
,
std
::
make_pair
(
begin
,
end
));
data_
->
views_
.
emplace
(
topic
,
std
::
move
(
cloudBagView_
));
}
catch
(
rosbag
::
BagException
&
)
{
return
false
;
}
return
true
;
}
bool
BagFileHelper
::
isTopicInitialized
(
const
std
::
string
&
topic
)
const
{
return
data_
->
views_
.
count
(
topic
)
>
0
;
}
bool
BagFileHelper
::
hasTopicAvailableMessages
(
const
std
::
string
&
topic
)
const
{
BOOST_ASSERT
(
isTopicInitialized
(
topic
));
auto
&
[
cur
,
end
]
=
data_
->
iteratorPairs_
.
at
(
topic
);
return
std
::
distance
(
cur
,
end
)
>
0
;
}
BagFileHelper
::
BagFileHelper
()
{
data_
=
std
::
make_unique
<
BagFileHelper
::
Data
>
();
}
BagFileHelper
::~
BagFileHelper
()
{
}
//+++++++++++++++++++++++++++++++
//
//if (privateNode_.hasParam("iteration_mode")) {
//std::string iterationMode;
//privateNode_.getParam("iteration_mode", iterationMode);
//auto modeOptional = IterationMode::_from_string_nocase_nothrow(iterationMode.c_str());
//if (!modeOptional) {
//std::stringstream validValues;
//auto values = IterationMode::_values();
//for (auto value: values) {
//validValues << value._to_string() << " ";
//}
//return stringify("Invalid value for <iteration_mode>. Valid values are: ", validValues.str());
//}
//appCfg.setIterationMode(*modeOptional);
//}
//####################### ICPMaxCorrespondenceDistance #######################
void
ROSParamHelper
::
setICPMaxCorrespondenceDistance
(
double
newValue
)
{
auto
&
[
value
,
key
]
=
icpMaxCorrespondenceDistance_
;
node_
->
setParam
(
key
,
newValue
);
value
=
newValue
;
}
double
ROSParamHelper
::
getICPMaxCorrespondenceDistance
()
const
{
return
std
::
get
<
0
>
(
icpMaxCorrespondenceDistance_
);
}
bool
ROSParamHelper
::
updateICPMaxCorrespondenceDistance
()
{
const
auto
&
[
defaultValue
,
key
]
=
icpMaxCorrespondenceDistance_
;
if
(
node_
->
hasParam
(
key
))
{
double
value
;
if
(
node_
->
getParam
(
key
,
value
))
{
setICPMaxCorrespondenceDistance
(
value
);
return
true
;
}
return
false
;
}
return
true
;
}
//####################### ICPMaxIterations #######################
void
ROSParamHelper
::
setICPMaxIterations
(
int
newValue
)
{
auto
&
[
value
,
key
]
=
icpMaxIterations_
;
node_
->
setParam
(
key
,
newValue
);
value
=
newValue
;
}
int
ROSParamHelper
::
getICPMaxIterations
()
const
{
return
std
::
get
<
0
>
(
icpMaxIterations_
);
}
bool
ROSParamHelper
::
updateICPMaxIterations
()
{
const
auto
&
[
defaultValue
,
key
]
=
icpMaxIterations_
;
if
(
node_
->
hasParam
(
key
))
{
int
value
;
if
(
node_
->
getParam
(
key
,
value
))
{
setICPMaxIterations
(
value
);
return
true
;
}
return
false
;
}
return
true
;
}
//####################### ICPTransformationEpsilon #######################
double
ROSParamHelper
::
getICPTransformationEpsilon
()
const
{
return
std
::
get
<
0
>
(
icpTransformationEpsilon_
);
}
void
ROSParamHelper
::
setICPTransformationEpsilon
(
double
newValue
)
{
auto
&
[
value
,
key
]
=
icpTransformationEpsilon_
;
node_
->
setParam
(
key
,
newValue
);
value
=
newValue
;
}
bool
ROSParamHelper
::
updateICPTransformationEpsilon
()
{
const
auto
&
[
defaultValue
,
key
]
=
icpTransformationEpsilon_
;
if
(
node_
->
hasParam
(
key
))
{
double
value
;
if
(
node_
->
getParam
(
key
,
value
))
{
setICPTransformationEpsilon
(
value
);
return
true
;
}
return
false
;
}
return
true
;
}
//####################### ICPEuclidianFitnessEpsilon #######################
void
ROSParamHelper
::
setICPEuclidianFitnessEpsilon
(
double
newValue
)
{
auto
&
[
value
,
key
]
=
icpEuclidianFitnessEpsilon_
;
node_
->
setParam
(
key
,
newValue
);
value
=
newValue
;
}
double
ROSParamHelper
::
getICPEuclidianFitnessEpsilon
()
const
{
return
std
::
get
<
0
>
(
icpEuclidianFitnessEpsilon_
);
}
bool
ROSParamHelper
::
updateICPEuclidianFitnessEpsilon
()
{
const
auto
&
[
defaultValue
,
key
]
=
icpEuclidianFitnessEpsilon_
;
if
(
node_
->
hasParam
(
key
))
{
double
value
;
if
(
node_
->
getParam
(
key
,
value
))
{
setICPEuclidianFitnessEpsilon
(
value
);
return
true
;
}
return
false
;
}
return
true
;
}
void
ROSParamHelper
::
setROSNodePtr
(
ros
::
NodeHandlePtr
ptr
)
{
node_
=
ptr
;
}
bool
ROSParamHelper
::
updateAll
()
{
return
updateICPEuclidianFitnessEpsilon
()
&&
updateICPMaxCorrespondenceDistance
()
&&
updateICPMaxIterations
()
&&
updateICPTransformationEpsilon
()
&&
updateDownsampleLeafSize
();
}
//####################### DownsampleLeafSize #######################
bool
ROSParamHelper
::
updateDownsampleLeafSize
()
{
const
auto
&
[
defaultValue
,
key
]
=
downsampleLeafSize
;
if
(
node_
->
hasParam
(
key
))
{
double
value
;
if
(
node_
->
getParam
(
key
,
value
))
{
setDownsampleLeafSize
(
value
);
return
true
;
}
return
false
;
}
return
true
;
}
void
ROSParamHelper
::
setDownsampleLeafSize
(
double
newValue
)
{
auto
&
[
value
,
key
]
=
downsampleLeafSize
;
node_
->
setParam
(
key
,
newValue
);
value
=
newValue
;
}
double
ROSParamHelper
::
getDownsampleLeafSize
()
const
{
return
std
::
get
<
0
>
(
downsampleLeafSize
);
}
#include <sstream>
std
::
string
ROSParamHelper
::
dumpString
()
{
std
::
stringstream
ss
;
ss
<<
"DownsampleLeadSize: "
<<
getDownsampleLeafSize
()
<<
"
\n
"
<<
"ICPMaxCorrespondenceDistance"
<<
getICPMaxCorrespondenceDistance
()
<<
"
\n
"
<<
"ICPEuclidianFitnessEpsilon"
<<
getICPEuclidianFitnessEpsilon
()
<<
"
\n
"
<<
"ICPTransformationEpsilon"
<<
getICPTransformationEpsilon
()
<<
"
\n
"
<<
"ICPMaxIterations"
<<
getICPMaxIterations
()
<<
"
\n
"
;
return
ss
.
str
();
}
src/registration_feature/main.cpp
0 → 100644
View file @
25e3629a
#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