本文整理汇总了C++中SE3::transformation方法的典型用法代码示例。如果您正苦于以下问题：C++ SE3::transformation方法的具体用法？C++ SE3::transformation怎么用？C++ SE3::transformation使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类SE3的用法示例。
在下文中一共展示了SE3::transformation方法的2个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: testApply
static bool testApply()
{
bool b, ret;
// apply delta:
Eigen::Matrix<double, 6, 1> delta = Eigen::Matrix<double, 6, 1>::Zero();
Eigen::Matrix<double, 4, 4> expectedT = Eigen::Matrix<double, 4, 4>::Identity();
Eigen::Matrix<double, 4, 4> diff;
SE3<double> pose;
pose.set( delta );
delta[ 0 ] = Math::deg2Rad( 1.5 );
delta[ 1 ] = Math::deg2Rad( 1.1 );
delta[ 2 ] = Math::deg2Rad( 1.6 );
delta[ 3 ] = 1;
delta[ 4 ] = 1;
delta[ 5 ] = 1;
pose.apply( delta );
expectedT( 0, 3 ) = delta[ 3 ];
expectedT( 1, 3 ) = delta[ 4 ];
expectedT( 2, 3 ) = delta[ 5 ];
Eigen::Matrix<double, 3, 1> axis = delta.segment<3>( 0 );
double angle = axis.norm(); axis /= angle;
expectedT.block<3, 3>( 0, 0 ) = Eigen::AngleAxis<double>( angle, axis ).toRotationMatrix();
diff = expectedT - pose.transformation();
ret = b = ( diff.array().abs().sum() / 12 < 0.001 );
if( !b ){
std::cout << expectedT << std::endl;
std::cout << pose.transformation() << std::endl;
std::cout << "avg SAD: " << diff.array().abs().sum() / 12 << std::endl;
}
pose.apply( -delta );
expectedT.setIdentity();
b &= ( ( expectedT - pose.transformation() ).array().abs().sum() / 12 < 0.0001 );
CVTTEST_PRINT( "apply", b );
ret &= b;
return ret;
}示例2: testInitAndSet
static bool testInitAndSet()
{
bool ret = true;
SE3<double> pose;
bool b;
b = ( pose.transformation() == Eigen::Matrix<double, 4, 4>::Identity() );
ret &= b;
CVTTEST_PRINT( "Initialization", b );
pose.set( 0, 0, 0, 100, 200, 300 );
b = ( pose.transformation()( 0, 3 ) == 100 ) &&
( pose.transformation()( 1, 3 ) == 200 ) &&
( pose.transformation()( 2, 3 ) == 300 ) &&
( pose.transformation()( 3, 3 ) == 1 ) &&
( pose.transformation()( 2, 2 ) == 1 ) &&
( pose.transformation()( 1, 1 ) == 1 ) &&
( pose.transformation()( 0, 0 ) == 1 );
pose.set( Math::deg2Rad( 90.0 ), 0, 0, 0, 0, 0 );
b &= ( pose.transformation()( 0, 3 ) == 0 );
b &= ( pose.transformation()( 1, 3 ) == 0 );
b &= ( pose.transformation()( 2, 3 ) == 0 );
b &= ( pose.transformation()( 3, 3 ) == 1 );
b &= ( pose.transformation()( 0, 0 ) == 1 );
b &= ( pose.transformation()( 0, 1 ) == 0 );
b &= ( pose.transformation()( 0, 2 ) == 0 );
b &= ( pose.transformation()( 1, 0 ) == 0 );
b &= ( pose.transformation()( 2, 0 ) == 0 );
b &= ( pose.transformation()( 1, 2 ) + 1 < 0.0000001 );
b &= ( pose.transformation()( 2, 1 ) - 1 < 0.0000001 );
b &= ( Math::abs( pose.transformation()( 2, 2 ) ) < 0.0000001 );
b &= ( Math::abs( pose.transformation()( 1, 1 ) ) < 0.0000001 );
CVTTEST_PRINT( "Set", b );
ret &= b;
return ret;
}