Eigen::Matrix3d createRotationMatrix(double x, double y, double z, double w) {
KDL::Rotation kdl_rotaion = KDL::Rotation::Quaternion(x, y, z, w);
Eigen::Matrix3d result;
for(int i = 0; i < 9; i++) {
result.data()[i] = kdl_rotaion.data[i];
}
result.transposeInPlace();
return result;
}
int main(int , char** )
{
for (int k = 0; k < 100000; ++k) {
// create a random rotation matrix by sampling a random 3d vector
// that will be used in axis-angle representation to create the matrix
Eigen::Vector3d rotAxisAngle = Vector3d::Random();
rotAxisAngle += Vector3d::Random();
Eigen::AngleAxisd rotation(rotAxisAngle.norm(), rotAxisAngle.normalized());
Eigen::Matrix3d Re = rotation.toRotationMatrix();
// our analytic function which we want to evaluate
Matrix<double, 3 , 9> dq_dR;
compute_dq_dR (dq_dR,
Re(0,0),Re(1,0),Re(2,0),
Re(0,1),Re(1,1),Re(2,1),
Re(0,2),Re(1,2),Re(2,2));
// compute the Jacobian using AD
Matrix<double, 3 , 9, Eigen::RowMajor> dq_dR_AD;
typedef ceres::internal::AutoDiff<RotationMatrix2QuaternionManifold, double, 9> AutoDiff_Dq_DR;
double *parameters[] = { Re.data() };
double *jacobians[] = { dq_dR_AD.data() };
double value[3];
RotationMatrix2QuaternionManifold rot2quat;
AutoDiff_Dq_DR::Differentiate(rot2quat, parameters, 3, value, jacobians);
// compare the two Jacobians
const double allowedDifference = 1e-6;
for (int i = 0; i < dq_dR.rows(); ++i) {
for (int j = 0; j < dq_dR.cols(); ++j) {
double d = fabs(dq_dR_AD(i,j) - dq_dR(i,j));
if (d > allowedDifference) {
cerr << "\ndetected difference in the Jacobians" << endl;
cerr << PVAR(Re) << endl << endl;
cerr << PVAR(dq_dR_AD) << endl << endl;
cerr << PVAR(dq_dR) << endl << endl;
return 1;
}
}
}
cerr << "+";
}
return 0;
}
/** \brief compute the 3 eigen values and eigenvectors for a 3x3 covariance matrix
* \param covariance_matrix a 3x3 covariance matrix in eigen2::matrix3d format
* \param eigen_values the resulted eigenvalues in eigen2::vector3d
* \param eigen_vectors a 3x3 matrix in eigen2::matrix3d format, containing each eigenvector on a new line
*/
bool
eigen_cov (Eigen::Matrix3d covariance_matrix, Eigen::Vector3d &eigen_values, Eigen::Matrix3d &eigen_vectors)
{
char jobz = 'V'; // 'V': Compute eigenvalues and eigenvectors
char uplo = 'U'; // 'U': Upper triangle of A is stored
int n = 3, lda = 3, info = -1;
int lwork = 3 * n - 1;
double *work = new double[lwork];
for (int i = 0; i < 3; i++)
for (int j = 0; j < 3; j++)
eigen_vectors (i, j) = covariance_matrix (i, j);
dsyev_ (&jobz, &uplo, &n, eigen_vectors.data (), &lda, eigen_values.data (), work, &lwork, &info);
delete work;
return (info == 0);
}
bool updatePose( Node *root, Camera *camera )
{
ceres::Problem problem;
Node *node = camera->node;
double params[6];
Eigen::Map<Eigen::Vector3d> translationvec(params);
translationvec = node->pose.translation();
ceres::RotationMatrixToAngleAxis( node->pose.so3().matrix().data(), params+3 );
ceres::LossFunction *lossFunction = new ceres::HuberLoss( 4.0 );
Calibration *calibration = camera->calibration;
ElementList::iterator pointit;
for ( pointit = root->points.begin(); pointit != root->points.end(); pointit++ )
{
Point *point = (Point*)pointit->second;
if ( !point->tracked ) continue;
Eigen::Vector3d XYZ = project(point->position);
ReprojectionError *reproj_error = new ReprojectionError(XYZ,
calibration->focal,
point->location[0]-calibration->center[0],
point->location[1]-calibration->center[1]);
ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction<ReprojectionError, 2, 6>(reproj_error);
problem.AddResidualBlock(cost_function, lossFunction, params );
}
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
// std::cout << summary.FullReport() << "\n";
bool success = ( summary.termination_type != ceres::FAILURE );
if ( success )
{
node->pose.translation() = translationvec;
Eigen::Matrix3d R;
ceres::AngleAxisToRotationMatrix(params+3, R.data());
node->pose.so3() = Sophus::SO3d(R);
}
// update tracked flag
for ( pointit = root->points.begin(); pointit != root->points.end(); pointit++ )
{
Point *point = (Point*)pointit->second;
if ( !point->tracked ) continue;
// check re-projection error
Eigen::Vector2d proj = calibration->focal * project( node->pose * project(point->position) ) + calibration->center;
Eigen::Vector2d diff = proj - point->location.cast<double>();
double err = diff.norm();
if ( err > 16.0 )
{
point->tracked = false;
}
}
return success;
}
