Skip to content
GitLab
Explore
Sign in
Primary navigation
Search or go to…
Project
L
laser
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Snippets
Deploy
Releases
Model registry
Monitor
Incidents
Service Desk
Analyze
Value stream analytics
Contributor 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
mobile_robotics
wolf_projects
wolf_lib
plugins
laser
Commits
bd4f9511
Commit
bd4f9511
authored
2 years ago
by
Joan Solà Ortega
Browse files
Options
Downloads
Patches
Plain Diff
Make test with a for loop to make more KFs
parent
7255c15e
No related branches found
Branches containing commit
No related tags found
1 merge request
!41
Draft: Resolve "New branch laser 3d"
Changes
1
Hide whitespace changes
Inline
Side-by-side
Showing
1 changed file
test/gtest_processor_odom_icp_3d.cpp
+45
-99
45 additions, 99 deletions
test/gtest_processor_odom_icp_3d.cpp
with
45 additions
and
99 deletions
test/gtest_processor_odom_icp_3d.cpp
+
45
−
99
View file @
bd4f9511
...
@@ -39,7 +39,6 @@
...
@@ -39,7 +39,6 @@
#include
<core/utils/utils_gtest.h>
#include
<core/utils/utils_gtest.h>
#include
<core/utils/logging.h>
#include
<core/utils/logging.h>
// pcl includes
// pcl includes
#include
<pcl/point_cloud.h>
#include
<pcl/point_cloud.h>
#include
<pcl/io/pcd_io.h>
#include
<pcl/io/pcd_io.h>
...
@@ -60,121 +59,68 @@ void loadData(std::string fname, pcl::PointCloud<pcl::PointXYZ>& cloud)
...
@@ -60,121 +59,68 @@ void loadData(std::string fname, pcl::PointCloud<pcl::PointXYZ>& cloud)
class
Test_ProcessorOdomIcp3D_yaml
:
public
testing
::
Test
class
Test_ProcessorOdomIcp3D_yaml
:
public
testing
::
Test
{
{
public:
public:
ProblemPtr
P
;
ProblemPtr
P
;
SensorLaser3dPtr
S
;
SensorLaser3dPtr
S
;
ProcessorOdomIcp3dPtr
p
;
ProcessorOdomIcp3dPtr
p
;
FrameBasePtr
F0
,
F1
,
F
;
FrameBasePtr
F0
,
F1
,
F
;
CaptureLaser3dPtr
C0
,
C1
,
C2
,
C3
;
CaptureLaser3dPtr
C0
,
C1
,
C2
,
C3
;
VectorXd
data
;
VectorXd
data
;
MatrixXd
data_cov
;
MatrixXd
data_cov
;
protected:
protected:
void
SetUp
()
override
void
SetUp
()
override
{
{
WOLF_INFO
(
laser_root_dir
);
WOLF_INFO
(
laser_root_dir
);
ParserYaml
parser
=
ParserYaml
(
"problem_odom_icp_3d.yaml"
,
laser_root_dir
+
"/test/yaml/"
);
ParserYaml
parser
=
ParserYaml
(
"problem_odom_icp_3d.yaml"
,
laser_root_dir
+
"/test/yaml/"
);
ParamsServer
server
=
ParamsServer
(
parser
.
getParams
());
ParamsServer
server
=
ParamsServer
(
parser
.
getParams
());
P
=
Problem
::
autoSetup
(
server
);
P
=
Problem
::
autoSetup
(
server
);
S
=
std
::
static_pointer_cast
<
SensorLaser3d
>
(
P
->
getHardware
()
->
getSensorList
().
front
());
S
=
std
::
static_pointer_cast
<
SensorLaser3d
>
(
P
->
getHardware
()
->
getSensorList
().
front
());
p
=
std
::
static_pointer_cast
<
ProcessorOdomIcp3d
>
(
S
->
getProcessorList
().
front
());
p
=
std
::
static_pointer_cast
<
ProcessorOdomIcp3d
>
(
S
->
getProcessorList
().
front
());
P
->
print
(
4
,
1
,
1
,
1
);
P
->
print
(
4
,
1
,
1
,
1
);
/*
data = VectorXd::Zero(12); // [ a, w, f, t ]
data_cov = MatrixXd::Identity(12, 12);
C = std::make_shared<CaptureMotion>("CaptureMotion", 0.0, S, data, data_cov, nullptr);
C->process();
C0 = std::static_pointer_cast<CaptureMotion>(p->getOrigin());
F0 = C0->getFrame();
ASSERT_EQ(C0->getTimeStamp(), F0->getTimeStamp());
ASSERT_EQ(C0->getTimeStamp(), F0->getCaptureOf(S)->getTimeStamp());
C = std::make_shared<CaptureMotion>("CaptureMotion", 1.0, S, data, data_cov, nullptr);
C->process();
C1 = std::static_pointer_cast<CaptureMotion>(p->getLast());
F1 = C1->getFrame();
F1->link(P); // by the face
VectorXd delta_preint(VectorXd::Zero(19));
delta_preint.head<3>() = -0.5 * gravity();
delta_preint.segment<3>(3) = -gravity();
delta_preint.segment<3>(6) = -0.5 * gravity();
delta_preint.segment<3>(9) = -gravity();
delta_preint(18) = 1;
MatrixXd delta_preint_cov(MatrixXd::Identity(18, 18));
VectorXd calib_preint(VectorXd::Zero(13));
MatrixXd jacobian_calib(MatrixXd::Zero(18, 13));
f1 = FeatureBase::emplace<FeatureMotion>(
C1, "FeatureMotion", delta_preint, delta_preint_cov, calib_preint, jacobian_calib);
c1 = FactorBase::emplace<FactorForceTorqueInertialDynamics>(f1, f1, C0, p, false);
F1->getStateBlock('P')->setState(Vector3d(0, 0, 0));
F1->getStateBlock('V')->setState(Vector3d(0, 0, 0));
F1->getStateBlock('O')->setState(Vector4d(0, 0, 0, 1));
F1->getStateBlock('L')->setState(Vector3d(0, 0, 0));
*/
}
}
};
};
TEST
(
Init
,
register_in_factories
)
TEST
(
Init
,
register_in_factories
)
{
{
FactorySensor
::
registerCreator
(
"SensorLaser3d"
,
SensorLaser3d
::
create
);
FactorySensor
::
registerCreator
(
"SensorLaser3d"
,
SensorLaser3d
::
create
);
AutoConfFactorySensor
::
registerCreator
(
"SensorLaser3d"
,
SensorLaser3d
::
create
);
AutoConfFactorySensor
::
registerCreator
(
"SensorLaser3d"
,
SensorLaser3d
::
create
);
FactoryProcessor
::
registerCreator
(
"ProcessorOdomIcp3d"
,
ProcessorOdomIcp3d
::
create
);
FactoryProcessor
::
registerCreator
(
"ProcessorOdomIcp3d"
,
ProcessorOdomIcp3d
::
create
);
AutoConfFactoryProcessor
::
registerCreator
(
"ProcessorOdomIcp3d"
,
ProcessorOdomIcp3d
::
create
);
AutoConfFactoryProcessor
::
registerCreator
(
"ProcessorOdomIcp3d"
,
ProcessorOdomIcp3d
::
create
);
}
}
TEST_F
(
Test_ProcessorOdomIcp3D_yaml
,
align_known_transform
)
TEST_F
(
Test_ProcessorOdomIcp3D_yaml
,
process_make_keyframes_always_same_pcl
)
{
{
// Load data
std
::
vector
<
VectorComposite
>
X
(
10
);
pcl
::
PointCloud
<
pcl
::
PointXYZ
>::
Ptr
pcl_ref0
=
pcl
::
make_shared
<
pcl
::
PointCloud
<
pcl
::
PointXYZ
>>
();
unsigned
int
i
=
0
;
loadData
(
laser_root_dir
+
"/test/data/1.pcd"
,
*
pcl_ref0
);
for
(
TimeStamp
t
=
0
;
t
<
6
;
t
+=
0.5
)
// will make one KF every 2 captures
C0
=
std
::
make_shared
<
CaptureLaser3d
>
(
0.0
,
S
,
pcl_ref0
);
{
C0
->
process
();
// Load pointcloud from file
pcl
::
PointCloud
<
pcl
::
PointXYZ
>::
Ptr
pcl_ref0
=
pcl
::
make_shared
<
pcl
::
PointCloud
<
pcl
::
PointXYZ
>>
();
VectorComposite
x0
=
P
->
getState
();
loadData
(
laser_root_dir
+
"/test/data/1.pcd"
,
*
pcl_ref0
);
// Load data
// make Capture and process
pcl
::
PointCloud
<
pcl
::
PointXYZ
>::
Ptr
pcl_ref1
=
pcl
::
make_shared
<
pcl
::
PointCloud
<
pcl
::
PointXYZ
>>
();
C0
=
std
::
make_shared
<
CaptureLaser3d
>
(
t
,
S
,
pcl_ref0
);
loadData
(
laser_root_dir
+
"/test/data/2.pcd"
,
*
pcl_ref1
);
C0
->
process
();
C1
=
std
::
make_shared
<
CaptureLaser3d
>
(
1.0
,
S
,
pcl_ref1
);
C1
->
process
();
// store state at integer timestamps (where the KFs are)
if
(
std
::
fabs
(
t
.
get
()
-
(
double
)
i
)
<
0.1
)
VectorComposite
x1
=
P
->
getState
();
{
X
[
i
]
=
p
->
getLast
()
->
getFrame
()
->
getState
(
"PO"
);
// Load data
i
++
;
pcl
::
PointCloud
<
pcl
::
PointXYZ
>::
Ptr
pcl_ref2
=
pcl
::
make_shared
<
pcl
::
PointCloud
<
pcl
::
PointXYZ
>>
();
}
loadData
(
laser_root_dir
+
"/test/data/2.pcd"
,
*
pcl_ref2
);
}
C2
=
std
::
make_shared
<
CaptureLaser3d
>
(
2.0
,
S
,
pcl_ref2
);
C2
->
process
();
P
->
print
(
4
,
1
,
1
,
1
);
VectorComposite
x2
=
P
->
getState
();
ASSERT_MATRIX_APPROX
(
X
.
at
(
0
).
vector
(
"PO"
),
X
.
at
(
1
).
vector
(
"PO"
),
1e-8
);
// pointclouds are the same
ASSERT_MATRIX_APPROX
(
X
.
at
(
0
).
vector
(
"PO"
),
X
.
at
(
2
).
vector
(
"PO"
),
1e-8
);
// pointclouds are the same
// Load data
ASSERT_MATRIX_APPROX
(
X
.
at
(
0
).
vector
(
"PO"
),
X
.
at
(
3
).
vector
(
"PO"
),
1e-8
);
// pointclouds are the same
pcl
::
PointCloud
<
pcl
::
PointXYZ
>::
Ptr
pcl_ref3
=
pcl
::
make_shared
<
pcl
::
PointCloud
<
pcl
::
PointXYZ
>>
();
ASSERT_MATRIX_APPROX
(
X
.
at
(
0
).
vector
(
"PO"
),
X
.
at
(
4
).
vector
(
"PO"
),
1e-8
);
// pointclouds are the same
loadData
(
laser_root_dir
+
"/test/data/2.pcd"
,
*
pcl_ref3
);
ASSERT_MATRIX_APPROX
(
X
.
at
(
0
).
vector
(
"PO"
),
X
.
at
(
5
).
vector
(
"PO"
),
1e-8
);
// pointclouds are the same
C3
=
std
::
make_shared
<
CaptureLaser3d
>
(
3.0
,
S
,
pcl_ref3
);
C3
->
process
();
VectorComposite
x3
=
P
->
getState
();
ASSERT_MATRIX_APPROX
(
x1
.
vector
(
"PO"
),
x2
.
vector
(
"PO"
),
1e-8
);
// pointclouds 1 and 2 are the same
ASSERT_MATRIX_APPROX
(
x2
.
vector
(
"PO"
),
x3
.
vector
(
"PO"
),
1e-8
);
// pointclouds 2 and 3 are the same
P
->
print
(
4
,
1
,
1
,
1
);
}
}
// // Test to see if the constructor works (not yaml files)
// // Test to see if the constructor works (not yaml files)
...
@@ -332,7 +278,7 @@ TEST_F(Test_ProcessorOdomIcp3D_yaml, align_known_transform)
...
@@ -332,7 +278,7 @@ TEST_F(Test_ProcessorOdomIcp3D_yaml, align_known_transform)
// ASSERT_MATRIX_APPROX(res_exp, res, 1e-8);
// ASSERT_MATRIX_APPROX(res_exp, res, 1e-8);
// }
// }
int
main
(
int
argc
,
char
**
argv
)
int
main
(
int
argc
,
char
**
argv
)
{
{
testing
::
InitGoogleTest
(
&
argc
,
argv
);
testing
::
InitGoogleTest
(
&
argc
,
argv
);
//::testing::GTEST_FLAG(filter) = "Test_FactorForceTorqueInertialDynamics_yaml.residual";
//::testing::GTEST_FLAG(filter) = "Test_FactorForceTorqueInertialDynamics_yaml.residual";
...
...
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