本文整理汇总了C++中SE3::hessian方法的典型用法代码示例。如果您正苦于以下问题：C++ SE3::hessian方法的具体用法？C++ SE3::hessian怎么用？C++ SE3::hessian使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类SE3的用法示例。
在下文中一共展示了SE3::hessian方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: testHessian
static bool testHessian()
{
Eigen::Matrix<double, 6, 1> delta = Eigen::Matrix<double, 6, 1>::Zero();
Eigen::Matrix<double, 24, 6> hN, hA;
SE3<double> pose;
pose.set( Math::deg2Rad( 10.0 ), Math::deg2Rad( 40.0 ), Math::deg2Rad( -120.0 ), -100.0, 200.0, 300.0 );
Eigen::Matrix<double, 3, 3> K( Eigen::Matrix<double, 3, 3>::Zero() );
K( 0, 0 ) = 650.0; K( 0, 2 ) = 320.0;
K( 1, 1 ) = 650.0; K( 1, 2 ) = 240.0;
K( 2, 2 ) = 1.0;
Eigen::Matrix<double, 3, 1> point;
Eigen::Matrix<double, 3, 1> p, ff, fb, bf, bb, xxf, xxb, hess;
point[ 0 ] = 16;
point[ 1 ] = 80;
point[ 2 ] = 13;
pose.transform( p, point );
double h = 0.0001;
for( size_t i = 0; i < 6; i++ ){
for( size_t j = 0; j < 6; j++ ){
delta.setZero();
if( i == j ){
// +
delta[ j ] = h;
pose.apply( delta );
pose.transform( xxf, point );
pose.apply( -delta );
delta[ j ] = -h;
pose.apply( delta );
pose.transform( xxb, point );
pose.apply( -delta );
hess = ( xxb - 2 * p + xxf ) / ( h*h );
} else {
delta[ i ] = h;
delta[ j ] = h;
pose.apply( delta );
pose.transform( ff, point );
pose.apply( -delta );
delta[ i ] = h;
delta[ j ] = -h;
pose.apply( delta );
pose.transform( fb, point );
pose.apply( -delta );
delta[ i ] = -h;
delta[ j ] = h;
pose.apply( delta );
pose.transform( bf, point );
pose.apply( -delta );
delta[ i ] = -h;
delta[ j ] = -h;
pose.apply( delta );
pose.transform( bb, point );
pose.apply( -delta );
hess = ( ff - bf - fb + bb ) / ( 4 * h * h );
}
hN( 4 * i , j ) = hess[ 0 ];
hN( 4 * i + 1 , j ) = hess[ 1 ];
hN( 4 * i + 2 , j ) = hess[ 2 ];
hN( 4 * i + 3 , j ) = 0.0;
}
}
pose.hessian( hA, p );
bool b, ret = true;
Eigen::Matrix<double, 24, 6> jDiff;
jDiff = hN - hA;
b = ( jDiff.array().abs().sum() / ( double )( jDiff.rows() * jDiff.cols() ) ) < 0.00001;
CVTTEST_PRINT( "Pose Hessian", b );
if( !b ){
std::cout << "Analytic:\n" << hA << std::endl;
std::cout << "Numeric:\n" << hN << std::endl;
std::cout << "Difference:\n" << jDiff << std::endl;
}
ret &= b;
return ret;
}