C++ Matrix3d::data方法代码示例

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;
    }