Skip to content

Commit 70b7b2d

Browse files
doudoudoudou
authored
Merge pull request #85 from envire/new_transformation_type
New transformation type
2 parents a24cde6 + ddd893d commit 70b7b2d

File tree

4 files changed

+237
-0
lines changed

4 files changed

+237
-0
lines changed

‎src/CMakeLists.txt‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -30,6 +30,7 @@ rock_library(
3030
samples/Sonar.cpp
3131
samples/SonarBeam.cpp
3232
samples/SonarScan.cpp
33+
samples/PoseWithCovariance.cpp
3334
HEADERS
3435
Angle.hpp
3536
CircularBuffer.hpp
@@ -79,6 +80,7 @@ rock_library(
7980
samples/Sonar.hpp
8081
samples/SonarBeam.hpp
8182
samples/SonarScan.hpp
83+
samples/PoseWithCovariance.hpp
8284
samples/Wrench.hpp
8385
samples/Wrenches.hpp
8486
templates/TimeStamped.hpp

‎src/samples/PoseWithCovariance.cpp‎

Lines changed: 75 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,75 @@
1+
#include "PoseWithCovariance.hpp"
2+
3+
namespace base { namespace samples {
4+
5+
PoseWithCovariance::PoseWithCovariance()
6+
{
7+
8+
}
9+
10+
PoseWithCovariance::PoseWithCovariance(const TransformWithCovariance& transform) :
11+
transform(transform.translation, transform.orientation, transform.cov)
12+
{
13+
14+
}
15+
16+
PoseWithCovariance::PoseWithCovariance(const RigidBodyState& rbs) : time(rbs.time),
17+
frame_id(rbs.targetFrame), object_frame_id(rbs.sourceFrame), transform(rbs.position, rbs.orientation)
18+
{
19+
transform.cov << rbs.cov_position, Eigen::Matrix3d::Zero(),
20+
Eigen::Matrix3d::Zero(), rbs.cov_orientation;
21+
}
22+
23+
PoseWithCovariance PoseWithCovariance::operator*(const PoseWithCovariance& pose) const
24+
{
25+
// compose new pose
26+
PoseWithCovariance new_pose;
27+
new_pose.time.microseconds = std::max(this->time.microseconds, pose.time.microseconds);
28+
new_pose.object_frame_id = pose.object_frame_id;
29+
new_pose.frame_id = this->frame_id;
30+
new_pose.transform = this->transform * pose.transform;
31+
return new_pose;
32+
}
33+
34+
void PoseWithCovariance::setTransform(const TransformWithCovariance& transform)
35+
{
36+
this->transform = transform;
37+
}
38+
39+
void PoseWithCovariance::setTransform(const Eigen::Affine3d& transform)
40+
{
41+
this->transform.setTransform(transform);
42+
}
43+
44+
const TransformWithCovariance& PoseWithCovariance::getTransformWithCovariance() const
45+
{
46+
return transform;
47+
}
48+
49+
Eigen::Affine3d PoseWithCovariance::getTransform() const
50+
{
51+
return transform.getTransform();
52+
}
53+
54+
const TransformWithCovariance::Covariance& PoseWithCovariance::getCovariance() const
55+
{
56+
return transform.getCovariance();
57+
}
58+
59+
RigidBodyState PoseWithCovariance::toRigidBodyState() const
60+
{
61+
base::samples::RigidBodyState rbs;
62+
rbs.time = time;
63+
rbs.targetFrame = frame_id;
64+
rbs.sourceFrame = object_frame_id;
65+
rbs.position = transform.translation;
66+
rbs.orientation = transform.orientation;
67+
if(transform.hasValidCovariance())
68+
{
69+
rbs.cov_position = transform.getTranslationCov();
70+
rbs.cov_orientation = transform.getOrientationCov();
71+
}
72+
return rbs;
73+
}
74+
75+
}}

‎src/samples/PoseWithCovariance.hpp‎

Lines changed: 80 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,80 @@
1+
#ifndef BASE_SAMPLES_POSE_WITH_COVARIANCE_HPP
2+
#define BASE_SAMPLES_POSE_WITH_COVARIANCE_HPP
3+
4+
#include <string>
5+
#include <base/Time.hpp>
6+
#include <base/TransformWithCovariance.hpp>
7+
#include <base/samples/RigidBodyState.hpp>
8+
9+
namespace base { namespace samples {
10+
11+
/**
12+
* Represents a 3D pose with uncertainty of the frame 'object_frame_id' in the frame 'frame_id'.
13+
* A frame ID shall be globally unique and correspond to a coordinate system.
14+
* Following the [Rock's conventions](http://rock.opendfki.de/wiki/WikiStart/Standards)
15+
* coordinate systems are right handed with X forward, Y left and Z up.
16+
*
17+
* The pose \f$ ^{reference}_{object}T \f$ in this structure transforms a vector \f$ v_{object} \f$
18+
* from the object frame to the reference frame:
19+
* \f$ v_{reference} = ^{reference}_{object}T \cdot v_{object} \f$
20+
* Example of a transformation chain:
21+
* \f$ ^{world}_{sensor}T = ^{world}_{body}T \cdot ^{body}_{sensor}T \f$
22+
*/
23+
class PoseWithCovariance
24+
{
25+
public:
26+
/** Reference timestamp of the pose sample */
27+
base::Time time;
28+
29+
/** ID of the reference frame where this pose is expressed in */
30+
std::string frame_id;
31+
32+
/** ID of the object frame defined by this pose */
33+
std::string object_frame_id;
34+
35+
/** Pose of the object frame in the reference frame */
36+
base::TransformWithCovariance transform;
37+
38+
public:
39+
/** Default constructor */
40+
PoseWithCovariance();
41+
42+
/** Initializes type from a TransformWithCovariance */
43+
explicit PoseWithCovariance(const base::TransformWithCovariance& transform);
44+
45+
/** Initializes type from a RigidBodyState */
46+
explicit PoseWithCovariance(const base::samples::RigidBodyState& rbs);
47+
48+
/** Performs a composition of this pose with a given pose.
49+
* The result is another pose with result = this * pose.
50+
* The frame ID's will be set accordingly (e.g. a_in_b = c_in_b * a_in_c).
51+
* Note that this method doesn't check if the composition
52+
* is frame wise valid.
53+
*/
54+
PoseWithCovariance operator*(const PoseWithCovariance& pose) const;
55+
56+
/** Sets the transformation as TransformWithCovariance */
57+
void setTransform(const base::TransformWithCovariance& transform);
58+
59+
/** Sets the transformation as Eigen::Affine3d */
60+
void setTransform(const Eigen::Affine3d& transform);
61+
62+
/** Returns transformation as TransformWithCovariance */
63+
const base::TransformWithCovariance& getTransformWithCovariance() const;
64+
65+
/** Returns transformation as Eigen::Affine3d */
66+
Eigen::Affine3d getTransform() const;
67+
68+
/** Returns the 6x6 covariance matrix */
69+
const base::TransformWithCovariance::Covariance& getCovariance() const;
70+
71+
/** Converts the type to a RigidBodyState.
72+
* Note that the cross-covariances will be lost,
73+
* since the RigidBodyState does not store them.
74+
*/
75+
base::samples::RigidBodyState toRigidBodyState() const;
76+
};
77+
78+
}}
79+
80+
#endif

‎test/test.cpp‎

Lines changed: 80 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -32,6 +32,7 @@
3232
#include <base/samples/DepthMap.hpp>
3333
#include <base/TransformWithCovariance.hpp>
3434
#include <base/samples/Sonar.hpp>
35+
#include <base/samples/PoseWithCovariance.hpp>
3536
#include <base/Temperature.hpp>
3637
#include <base/Time.hpp>
3738
#include <base/TimeMark.hpp>
@@ -1525,6 +1526,85 @@ BOOST_AUTO_TEST_CASE( transform_with_covariance )
15251526
BOOST_CHECK( t1.getCovariance().isApprox( t1r.getCovariance(), sigma ) );
15261527
}
15271528

1529+
BOOST_AUTO_TEST_CASE( pose_with_covariance )
1530+
{
1531+
base::samples::RigidBodyState rbs;
1532+
rbs.time = base::Time::fromSeconds(1000);
1533+
rbs.sourceFrame = "laser";
1534+
rbs.targetFrame = "body";
1535+
rbs.position = base::Vector3d(1,2,3);
1536+
rbs.cov_position = 0.1 * base::Matrix3d::Identity();
1537+
rbs.orientation = Eigen::AngleAxisd(0.5*M_PI,Eigen::Vector3d::UnitZ()) *
1538+
Eigen::AngleAxisd(-0.2*M_PI,Eigen::Vector3d::UnitY()) *
1539+
Eigen::AngleAxisd(0.1*M_PI,Eigen::Vector3d::UnitX());
1540+
rbs.cov_orientation = 0.02 * base::Matrix3d::Identity();
1541+
1542+
base::samples::PoseWithCovariance t(rbs);
1543+
BOOST_CHECK(rbs.time == t.time);
1544+
BOOST_CHECK(rbs.sourceFrame == t.object_frame_id);
1545+
BOOST_CHECK(rbs.targetFrame == t.frame_id);
1546+
BOOST_CHECK(rbs.position.isApprox(t.transform.translation, 1e-12));
1547+
BOOST_CHECK(rbs.cov_position.isApprox(t.transform.getTranslationCov(), 1e-12));
1548+
BOOST_CHECK(rbs.orientation.isApprox(t.transform.orientation, 1e-12));
1549+
BOOST_CHECK(rbs.cov_orientation.isApprox(t.transform.getOrientationCov(), 1e-12));
1550+
1551+
base::samples::RigidBodyState rbs2 = t.toRigidBodyState();
1552+
BOOST_CHECK(rbs.time == rbs2.time);
1553+
BOOST_CHECK(rbs.sourceFrame == rbs2.sourceFrame);
1554+
BOOST_CHECK(rbs.targetFrame == rbs2.targetFrame);
1555+
BOOST_CHECK(rbs.position.isApprox(rbs2.position, 1e-12));
1556+
BOOST_CHECK(rbs.cov_position.isApprox(rbs2.cov_position, 1e-12));
1557+
BOOST_CHECK(rbs.orientation.isApprox(rbs2.orientation, 1e-12));
1558+
BOOST_CHECK(rbs.cov_orientation.isApprox(rbs2.cov_orientation, 1e-12));
1559+
1560+
// test operator*
1561+
base::Matrix6d lt1;
1562+
lt1 <<
1563+
0.1, 0.0, 0.0, 0.0, 0.0, 0.0,
1564+
0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
1565+
0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
1566+
0.0, 0.0, 0.0, -3.0, 0.0, 0.0,
1567+
0.0, 0.0, 0.0, 0.0, -2.0, 0.0,
1568+
0.0, 0.0, 0.0, 0.0, 0.0, -1.0;
1569+
base::samples::PoseWithCovariance body_in_world(base::TransformWithCovariance(
1570+
Eigen::Affine3d(Eigen::Translation3d(Eigen::Vector3d(1,0,0)) * Eigen::AngleAxisd( M_PI/2.0, Eigen::Vector3d::UnitX()) ),
1571+
lt1 ));
1572+
body_in_world.time = base::Time();
1573+
1574+
base::Matrix6d lt2;
1575+
lt2 <<
1576+
0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
1577+
0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
1578+
0.0, 0.0, 0.2, 0.0, 0.0, 0.0,
1579+
0.0, 0.0, 0.0, 1.0, 0.0, 0.0,
1580+
0.0, 0.0, 0.0, 0.0, 2.0, 0.0,
1581+
0.0, 0.0, 0.0, 0.0, 0.0, 3.0;
1582+
base::samples::PoseWithCovariance sensor_in_body(base::TransformWithCovariance(
1583+
Eigen::Affine3d(Eigen::Translation3d(Eigen::Vector3d(0,1,2)) * Eigen::AngleAxisd( M_PI/2.0, Eigen::Vector3d::UnitY()) ),
1584+
lt2 ));
1585+
sensor_in_body.time = base::Time::now();
1586+
1587+
body_in_world.frame_id = "world";
1588+
body_in_world.object_frame_id = "body";
1589+
sensor_in_body.object_frame_id = "sensor";
1590+
sensor_in_body.frame_id = "body";
1591+
base::samples::PoseWithCovariance sensor_in_world = body_in_world * sensor_in_body;
1592+
1593+
// check composition
1594+
BOOST_CHECK(sensor_in_world.object_frame_id == sensor_in_body.object_frame_id);
1595+
BOOST_CHECK(sensor_in_world.frame_id == body_in_world.frame_id);
1596+
BOOST_CHECK(sensor_in_world.time == sensor_in_body.time);
1597+
1598+
base::samples::PoseWithCovariance body_in_world_r(sensor_in_world.transform.compositionInv(sensor_in_body.transform));
1599+
base::samples::PoseWithCovariance sensor_in_body_r(sensor_in_world.transform.preCompositionInv(body_in_world.transform));
1600+
1601+
BOOST_CHECK( body_in_world.getTransform().matrix().isApprox( body_in_world_r.getTransform().matrix(), 1e-12 ) );
1602+
BOOST_CHECK( body_in_world.getCovariance().isApprox( body_in_world_r.getCovariance(), 1e-12 ) );
1603+
1604+
BOOST_CHECK( sensor_in_body.getTransform().matrix().isApprox( sensor_in_body_r.getTransform().matrix(), 1e-12 ) );
1605+
BOOST_CHECK( sensor_in_body.getCovariance().isApprox( sensor_in_body_r.getCovariance(), 1e-12 ) );
1606+
}
1607+
15281608
#ifdef SISL_FOUND
15291609
#include <base/geometry/spline.h>
15301610
BOOST_AUTO_TEST_CASE( spline_to_points )

0 commit comments

Comments
 (0)