Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Gepetto
aicp_mapping
Commits
5ae8f9e9
Commit
5ae8f9e9
authored
Apr 30, 2019
by
Maurice Fallon
Browse files
put pose file reader into a different class
parent
e4ce499c
Changes
2
Hide whitespace changes
Inline
Side-by-side
aicp_core/include/aicp_utils/poseFileReader.hpp
0 → 100644
View file @
5ae8f9e9
class
IsometryWithTime
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
public:
IsometryWithTime
(){
pose
=
Eigen
::
Isometry3d
::
Identity
();
sec
=
0
;
nsec
=
0
;
counter
=
0
;
}
IsometryWithTime
(
Eigen
::
Isometry3d
pose_in
,
int
sec_in
,
int
nsec_in
,
int
counter_in
){
pose
=
pose_in
;
sec
=
sec_in
;
nsec
=
nsec_in
;
counter
=
counter_in
;
}
~
IsometryWithTime
(){
}
Eigen
::
Isometry3d
pose
;
int
sec
;
int
nsec
;
int
counter
;
};
class
PoseFileReader
{
public:
PoseFileReader
(){
}
~
PoseFileReader
(){
}
void
readPoseFile
(
std
::
string
file_name
,
std
::
vector
<
IsometryWithTime
>
&
world_to_body_poses
){
// Read the input poses file:
ifstream
in
(
file_name
);
vector
<
vector
<
double
>>
fields
;
if
(
in
)
{
string
line
;
while
(
getline
(
in
,
line
))
{
if
(
line
.
at
(
0
)
==
'#'
){
continue
;
}
stringstream
sep
(
line
);
string
field
;
fields
.
push_back
(
vector
<
double
>
());
while
(
getline
(
sep
,
field
,
','
))
{
fields
.
back
().
push_back
(
stod
(
field
));
}
}
}
for
(
auto
row
:
fields
)
{
Eigen
::
Isometry3d
world_to_body
=
Eigen
::
Isometry3d
::
Identity
();
world_to_body
.
translation
()
<<
row
[
3
],
row
[
4
],
row
[
5
];
world_to_body
.
rotate
(
Eigen
::
Quaterniond
(
row
[
9
],
row
[
6
],
row
[
7
],
row
[
8
])
);
int
counter
=
int
(
row
[
0
]);
int
sec
=
int
(
row
[
1
]);
int
nsec
=
int
(
row
[
2
]);
IsometryWithTime
world_to_body_pose
=
IsometryWithTime
(
world_to_body
,
sec
,
nsec
,
counter
);
world_to_body_poses
.
push_back
(
world_to_body_pose
);
}
}
};
\ No newline at end of file
aicp_core/src/registration/app.cpp
View file @
5ae8f9e9
...
...
@@ -5,6 +5,7 @@
#include
"aicp_utils/timing.hpp"
#include
"aicp_utils/common.hpp"
#include
"aicp_utils/poseFileReader.hpp"
namespace
aicp
{
...
...
@@ -246,49 +247,25 @@ void App::runAicpPipeline(pcl::PointCloud<pcl::PointXYZ>::Ptr& reference_prefilt
}
void
App
::
processFromFile
(
std
::
string
file_path
){
std
::
cout
<<
"starting processFromFile
\n
"
;
// Rea
ch the input
poses file:
// Rea
d all
poses
csv
file:
std
::
stringstream
ss
;
ss
<<
file_path
<<
"/aicp_input_poses.csv"
;
ifstream
in
(
ss
.
str
()
);
vector
<
vector
<
double
>>
fields
;
if
(
in
)
{
string
line
;
while
(
getline
(
in
,
line
))
{
if
(
line
.
at
(
0
)
==
'#'
){
continue
;
}
stringstream
sep
(
line
);
string
field
;
fields
.
push_back
(
vector
<
double
>
());
while
(
getline
(
sep
,
field
,
','
))
{
fields
.
back
().
push_back
(
stod
(
field
));
}
}
}
// Read each point cloud and consecutively feed to AICP
for
(
auto
row
:
fields
)
{
int
counter
=
int
(
row
[
0
]);
int
sec
=
int
(
row
[
1
]);
int
nsec
=
int
(
row
[
2
]);
int64_t
utime
=
0
;
Eigen
::
Isometry3d
world_to_body_
=
Eigen
::
Isometry3d
::
Identity
();
world_to_body_
.
translation
()
<<
row
[
3
],
row
[
4
],
row
[
5
];
world_to_body_
.
rotate
(
Eigen
::
Quaterniond
(
row
[
9
],
row
[
6
],
row
[
7
],
row
[
8
])
);
std
::
vector
<
IsometryWithTime
>
world_to_body_poses
;
PoseFileReader
reader
;
reader
.
readPoseFile
(
ss
.
str
(),
world_to_body_poses
);
// Process each cloud successively
for
(
auto
pose_with_time
:
world_to_body_poses
)
{
std
::
stringstream
ss2
;
ss2
<<
file_path
<<
"/cloud_"
<<
counter
<<
"_"
<<
sec
<<
"_"
<<
nsec
<<
".pcd"
;
ss2
<<
file_path
<<
"/cloud_"
<<
pose_with_time
.
counter
<<
"_"
<<
pose_with_time
.
sec
<<
"_"
<<
pose_with_time
.
nsec
<<
".pcd"
;
std
::
cout
<<
ss2
.
str
()
<<
"
\n
"
;
pcl
::
PointCloud
<
pcl
::
PointXYZ
>::
Ptr
accumulated_cloud
(
new
pcl
::
PointCloud
<
pcl
::
PointXYZ
>
);
int64_t
utime
=
pose_with_time
.
sec
*
1E6
+
pose_with_time
.
nsec
;
pcl
::
PointCloud
<
pcl
::
PointXYZ
>::
Ptr
accumulated_cloud
(
new
pcl
::
PointCloud
<
pcl
::
PointXYZ
>
);
if
(
pcl
::
io
::
loadPCDFile
<
pcl
::
PointXYZ
>
(
ss2
.
str
(),
*
accumulated_cloud
)
==
-
1
){
std
::
cout
<<
"Couldn't read file "
<<
ss2
.
str
()
<<
"
\n
"
;
return
;
...
...
@@ -296,7 +273,7 @@ void App::processFromFile(std::string file_path){
AlignedCloudPtr
current_cloud
(
new
AlignedCloud
(
utime
,
accumulated_cloud
,
world_to_body_
));
pose_with_time
.
pose
));
processCloud
(
current_cloud
);
}
}
...
...
Write
Preview
Supports
Markdown
0%
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!
Cancel
Please
register
or
sign in
to comment