/* * You can use GPU for that * */ void Transform_Cloud ( matrix <Point_XYZI> & In_Cloud, matrix <Point_XYZI> & Out_Cloud, Eigen::Affine3f & Transform ) { if ( & In_Cloud != & Out_Cloud ) Out_Cloud.Set_Dimensions ( In_Cloud.Get_Dimensions() ); // Dataset might contain NaNs and Infs, so check for them first, // otherwise we get errors during the multiplication (?) for (size_t i = 0; i < In_Cloud.Get_Num_Of_Elem (); ++i) { XYZI cur_point = In_Cloud.data[i]; if ( ! isfinite (cur_point.x) || ! isfinite (cur_point.y) || ! isfinite (cur_point.z) ) continue; // FIXME - rewrite - // too complex // use SSE ??? Eigen::Matrix<float, 3, 1> pt ( cur_point.x, cur_point.y, cur_point.z ); XYZI result; result.x = static_cast<float> (Transform (0, 0) * pt.coeffRef (0) + Transform (0, 1) * pt.coeffRef (1) + Transform (0, 2) * pt.coeffRef (2) + Transform (0, 3)); result.y = static_cast<float> (Transform (1, 0) * pt.coeffRef (0) + Transform (1, 1) * pt.coeffRef (1) + Transform (1, 2) * pt.coeffRef (2) + Transform (1, 3)); result.z = static_cast<float> (Transform (2, 0) * pt.coeffRef (0) + Transform (2, 1) * pt.coeffRef (1) + Transform (2, 2) * pt.coeffRef (2) + Transform (2, 3)); result.w = cur_point.w; Out_Cloud.data[i] = result; } }
template <typename PointT, typename Scalar> void pcl::transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, const std::vector<int> &indices, pcl::PointCloud<PointT> &cloud_out, const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform, bool copy_all_fields) { size_t npts = indices.size (); // In order to transform the data, we need to remove NaNs cloud_out.is_dense = cloud_in.is_dense; cloud_out.header = cloud_in.header; cloud_out.width = static_cast<int> (npts); cloud_out.height = 1; cloud_out.points.resize (npts); cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_; cloud_out.sensor_origin_ = cloud_in.sensor_origin_; if (cloud_in.is_dense) { // If the dataset is dense, simply transform it! for (size_t i = 0; i < npts; ++i) { // Copy fields first, then transform xyz data if (copy_all_fields) cloud_out.points[i] = cloud_in.points[indices[i]]; //cloud_out.points[i].getVector3fMap () = transform*cloud_out.points[i].getVector3fMap (); Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[indices[i]].x, cloud_in[indices[i]].y, cloud_in[indices[i]].z); cloud_out[i].x = static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3)); cloud_out[i].y = static_cast<float> (transform (1, 0) * pt.coeffRef (0) + transform (1, 1) * pt.coeffRef (1) + transform (1, 2) * pt.coeffRef (2) + transform (1, 3)); cloud_out[i].z = static_cast<float> (transform (2, 0) * pt.coeffRef (0) + transform (2, 1) * pt.coeffRef (1) + transform (2, 2) * pt.coeffRef (2) + transform (2, 3)); } } else { // Dataset might contain NaNs and Infs, so check for them first, // otherwise we get errors during the multiplication (?) for (size_t i = 0; i < npts; ++i) { if (copy_all_fields) cloud_out.points[i] = cloud_in.points[indices[i]]; if (!pcl_isfinite (cloud_in.points[indices[i]].x) || !pcl_isfinite (cloud_in.points[indices[i]].y) || !pcl_isfinite (cloud_in.points[indices[i]].z)) continue; //cloud_out.points[i].getVector3fMap () = transform*cloud_out.points[i].getVector3fMap (); Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[indices[i]].x, cloud_in[indices[i]].y, cloud_in[indices[i]].z); cloud_out[i].x = static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3)); cloud_out[i].y = static_cast<float> (transform (1, 0) * pt.coeffRef (0) + transform (1, 1) * pt.coeffRef (1) + transform (1, 2) * pt.coeffRef (2) + transform (1, 3)); cloud_out[i].z = static_cast<float> (transform (2, 0) * pt.coeffRef (0) + transform (2, 1) * pt.coeffRef (1) + transform (2, 2) * pt.coeffRef (2) + transform (2, 3)); } } }
template <typename PointT, typename Scalar> inline unsigned int pcl::computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud, Eigen::Matrix<Scalar, 3, 3> &covariance_matrix) { // create the buffer on the stack which is much faster than using cloud[indices[i]] and centroid as a buffer Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor>::Zero (); unsigned int point_count; if (cloud.is_dense) { point_count = static_cast<unsigned int> (cloud.size ()); // For each point in the cloud for (size_t i = 0; i < point_count; ++i) { accu [0] += cloud[i].x * cloud[i].x; accu [1] += cloud[i].x * cloud[i].y; accu [2] += cloud[i].x * cloud[i].z; accu [3] += cloud[i].y * cloud[i].y; accu [4] += cloud[i].y * cloud[i].z; accu [5] += cloud[i].z * cloud[i].z; } } else { point_count = 0; for (size_t i = 0; i < cloud.size (); ++i) { if (!isFinite (cloud[i])) continue; accu [0] += cloud[i].x * cloud[i].x; accu [1] += cloud[i].x * cloud[i].y; accu [2] += cloud[i].x * cloud[i].z; accu [3] += cloud[i].y * cloud[i].y; accu [4] += cloud[i].y * cloud[i].z; accu [5] += cloud[i].z * cloud[i].z; ++point_count; } } if (point_count != 0) { accu /= static_cast<Scalar> (point_count); covariance_matrix.coeffRef (0) = accu [0]; covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = accu [1]; covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = accu [2]; covariance_matrix.coeffRef (4) = accu [3]; covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = accu [4]; covariance_matrix.coeffRef (8) = accu [5]; } return (point_count); }
template <typename PointT, typename Scalar> inline unsigned int pcl::computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud, const std::vector<int> &indices, Eigen::Matrix<Scalar, 3, 3> &covariance_matrix) { // create the buffer on the stack which is much faster than using cloud[indices[i]] and centroid as a buffer Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor>::Zero (); unsigned int point_count; if (cloud.is_dense) { point_count = static_cast<unsigned int> (indices.size ()); for (std::vector<int>::const_iterator iIt = indices.begin (); iIt != indices.end (); ++iIt) { //const PointT& point = cloud[*iIt]; accu [0] += cloud[*iIt].x * cloud[*iIt].x; accu [1] += cloud[*iIt].x * cloud[*iIt].y; accu [2] += cloud[*iIt].x * cloud[*iIt].z; accu [3] += cloud[*iIt].y * cloud[*iIt].y; accu [4] += cloud[*iIt].y * cloud[*iIt].z; accu [5] += cloud[*iIt].z * cloud[*iIt].z; } } else { point_count = 0; for (std::vector<int>::const_iterator iIt = indices.begin (); iIt != indices.end (); ++iIt) { if (!isFinite (cloud[*iIt])) continue; ++point_count; accu [0] += cloud[*iIt].x * cloud[*iIt].x; accu [1] += cloud[*iIt].x * cloud[*iIt].y; accu [2] += cloud[*iIt].x * cloud[*iIt].z; accu [3] += cloud[*iIt].y * cloud[*iIt].y; accu [4] += cloud[*iIt].y * cloud[*iIt].z; accu [5] += cloud[*iIt].z * cloud[*iIt].z; } } if (point_count != 0) { accu /= static_cast<Scalar> (point_count); covariance_matrix.coeffRef (0) = accu [0]; covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = accu [1]; covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = accu [2]; covariance_matrix.coeffRef (4) = accu [3]; covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = accu [4]; covariance_matrix.coeffRef (8) = accu [5]; } return (point_count); }
template <typename PointT, typename Scalar> void pcl::transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, pcl::PointCloud<PointT> &cloud_out, const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform, bool copy_all_fields) { if (&cloud_in != &cloud_out) { cloud_out.header = cloud_in.header; cloud_out.is_dense = cloud_in.is_dense; cloud_out.width = cloud_in.width; cloud_out.height = cloud_in.height; cloud_out.points.reserve (cloud_in.points.size ()); if (copy_all_fields) cloud_out.points.assign (cloud_in.points.begin (), cloud_in.points.end ()); else cloud_out.points.resize (cloud_in.points.size ()); cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_; cloud_out.sensor_origin_ = cloud_in.sensor_origin_; } if (cloud_in.is_dense) { // If the dataset is dense, simply transform it! for (size_t i = 0; i < cloud_out.points.size (); ++i) { //cloud_out.points[i].getVector3fMap () = transform * cloud_in.points[i].getVector3fMap (); Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[i].x, cloud_in[i].y, cloud_in[i].z); cloud_out[i].x = static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3)); cloud_out[i].y = static_cast<float> (transform (1, 0) * pt.coeffRef (0) + transform (1, 1) * pt.coeffRef (1) + transform (1, 2) * pt.coeffRef (2) + transform (1, 3)); cloud_out[i].z = static_cast<float> (transform (2, 0) * pt.coeffRef (0) + transform (2, 1) * pt.coeffRef (1) + transform (2, 2) * pt.coeffRef (2) + transform (2, 3)); } } else { // Dataset might contain NaNs and Infs, so check for them first, // otherwise we get errors during the multiplication (?) for (size_t i = 0; i < cloud_out.points.size (); ++i) { if (!pcl_isfinite (cloud_in.points[i].x) || !pcl_isfinite (cloud_in.points[i].y) || !pcl_isfinite (cloud_in.points[i].z)) continue; //cloud_out.points[i].getVector3fMap () = transform * cloud_in.points[i].getVector3fMap (); Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[i].x, cloud_in[i].y, cloud_in[i].z); cloud_out[i].x = static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3)); cloud_out[i].y = static_cast<float> (transform (1, 0) * pt.coeffRef (0) + transform (1, 1) * pt.coeffRef (1) + transform (1, 2) * pt.coeffRef (2) + transform (1, 3)); cloud_out[i].z = static_cast<float> (transform (2, 0) * pt.coeffRef (0) + transform (2, 1) * pt.coeffRef (1) + transform (2, 2) * pt.coeffRef (2) + transform (2, 3)); } } }
template <typename PointT, typename Scalar> void pcl::transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, const std::vector<int> &indices, pcl::PointCloud<PointT> &cloud_out, const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform) { size_t npts = indices.size (); // In order to transform the data, we need to remove NaNs cloud_out.is_dense = cloud_in.is_dense; cloud_out.header = cloud_in.header; cloud_out.width = static_cast<int> (npts); cloud_out.height = 1; cloud_out.points.resize (npts); cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_; cloud_out.sensor_origin_ = cloud_in.sensor_origin_; // If the data is dense, we don't need to check for NaN if (cloud_in.is_dense) { for (size_t i = 0; i < cloud_out.points.size (); ++i) { //cloud_out.points[i].getVector3fMap() = transform * cloud_in.points[i].getVector3fMap (); Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[indices[i]].x, cloud_in[indices[i]].y, cloud_in[indices[i]].z); cloud_out[i].x = static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3)); cloud_out[i].y = static_cast<float> (transform (1, 0) * pt.coeffRef (0) + transform (1, 1) * pt.coeffRef (1) + transform (1, 2) * pt.coeffRef (2) + transform (1, 3)); cloud_out[i].z = static_cast<float> (transform (2, 0) * pt.coeffRef (0) + transform (2, 1) * pt.coeffRef (1) + transform (2, 2) * pt.coeffRef (2) + transform (2, 3)); // Rotate normals //cloud_out.points[i].getNormalVector3fMap() = transform.rotation () * cloud_in.points[i].getNormalVector3fMap (); Eigen::Matrix<Scalar, 3, 1> nt (cloud_in[indices[i]].normal_x, cloud_in[indices[i]].normal_y, cloud_in[indices[i]].normal_z); cloud_out[i].normal_x = static_cast<float> (transform (0, 0) * nt.coeffRef (0) + transform (0, 1) * nt.coeffRef (1) + transform (0, 2) * nt.coeffRef (2)); cloud_out[i].normal_y = static_cast<float> (transform (1, 0) * nt.coeffRef (0) + transform (1, 1) * nt.coeffRef (1) + transform (1, 2) * nt.coeffRef (2)); cloud_out[i].normal_z = static_cast<float> (transform (2, 0) * nt.coeffRef (0) + transform (2, 1) * nt.coeffRef (1) + transform (2, 2) * nt.coeffRef (2)); } } // Dataset might contain NaNs and Infs, so check for them first. else { for (size_t i = 0; i < cloud_out.points.size (); ++i) { if (!pcl_isfinite (cloud_in.points[indices[i]].x) || !pcl_isfinite (cloud_in.points[indices[i]].y) || !pcl_isfinite (cloud_in.points[indices[i]].z)) continue; //cloud_out.points[i].getVector3fMap() = transform * cloud_in.points[i].getVector3fMap (); Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[indices[i]].x, cloud_in[indices[i]].y, cloud_in[indices[i]].z); cloud_out[i].x = static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3)); cloud_out[i].y = static_cast<float> (transform (1, 0) * pt.coeffRef (0) + transform (1, 1) * pt.coeffRef (1) + transform (1, 2) * pt.coeffRef (2) + transform (1, 3)); cloud_out[i].z = static_cast<float> (transform (2, 0) * pt.coeffRef (0) + transform (2, 1) * pt.coeffRef (1) + transform (2, 2) * pt.coeffRef (2) + transform (2, 3)); // Rotate normals //cloud_out.points[i].getNormalVector3fMap() = transform.rotation () * cloud_in.points[i].getNormalVector3fMap (); Eigen::Matrix<Scalar, 3, 1> nt (cloud_in[indices[i]].normal_x, cloud_in[indices[i]].normal_y, cloud_in[indices[i]].normal_z); cloud_out[i].normal_x = static_cast<float> (transform (0, 0) * nt.coeffRef (0) + transform (0, 1) * nt.coeffRef (1) + transform (0, 2) * nt.coeffRef (2)); cloud_out[i].normal_y = static_cast<float> (transform (1, 0) * nt.coeffRef (0) + transform (1, 1) * nt.coeffRef (1) + transform (1, 2) * nt.coeffRef (2)); cloud_out[i].normal_z = static_cast<float> (transform (2, 0) * nt.coeffRef (0) + transform (2, 1) * nt.coeffRef (1) + transform (2, 2) * nt.coeffRef (2)); } } }
template <typename PointT, typename Scalar> void pcl::transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, pcl::PointCloud<PointT> &cloud_out, const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform) { if (&cloud_in != &cloud_out) { // Note: could be replaced by cloud_out = cloud_in cloud_out.header = cloud_in.header; cloud_out.width = cloud_in.width; cloud_out.height = cloud_in.height; cloud_out.is_dense = cloud_in.is_dense; cloud_out.points.reserve (cloud_out.points.size ()); cloud_out.points.assign (cloud_in.points.begin (), cloud_in.points.end ()); } // If the data is dense, we don't need to check for NaN if (cloud_in.is_dense) { for (size_t i = 0; i < cloud_out.points.size (); ++i) { //cloud_out.points[i].getVector3fMap() = transform * cloud_in.points[i].getVector3fMap (); Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[i].x, cloud_in[i].y, cloud_in[i].z); cloud_out[i].x = static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3)); cloud_out[i].y = static_cast<float> (transform (1, 0) * pt.coeffRef (0) + transform (1, 1) * pt.coeffRef (1) + transform (1, 2) * pt.coeffRef (2) + transform (1, 3)); cloud_out[i].z = static_cast<float> (transform (2, 0) * pt.coeffRef (0) + transform (2, 1) * pt.coeffRef (1) + transform (2, 2) * pt.coeffRef (2) + transform (2, 3)); // Rotate normals (WARNING: transform.rotation () uses SVD internally!) //cloud_out.points[i].getNormalVector3fMap() = transform.rotation () * cloud_in.points[i].getNormalVector3fMap (); Eigen::Matrix<Scalar, 3, 1> nt (cloud_in[i].normal_x, cloud_in[i].normal_y, cloud_in[i].normal_z); cloud_out[i].normal_x = static_cast<float> (transform (0, 0) * nt.coeffRef (0) + transform (0, 1) * nt.coeffRef (1) + transform (0, 2) * nt.coeffRef (2)); cloud_out[i].normal_y = static_cast<float> (transform (1, 0) * nt.coeffRef (0) + transform (1, 1) * nt.coeffRef (1) + transform (1, 2) * nt.coeffRef (2)); cloud_out[i].normal_z = static_cast<float> (transform (2, 0) * nt.coeffRef (0) + transform (2, 1) * nt.coeffRef (1) + transform (2, 2) * nt.coeffRef (2)); } } // Dataset might contain NaNs and Infs, so check for them first. else { for (size_t i = 0; i < cloud_out.points.size (); ++i) { if (!pcl_isfinite (cloud_in.points[i].x) || !pcl_isfinite (cloud_in.points[i].y) || !pcl_isfinite (cloud_in.points[i].z)) continue; //cloud_out.points[i].getVector3fMap() = transform * cloud_in.points[i].getVector3fMap (); Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[i].x, cloud_in[i].y, cloud_in[i].z); cloud_out[i].x = static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3)); cloud_out[i].y = static_cast<float> (transform (1, 0) * pt.coeffRef (0) + transform (1, 1) * pt.coeffRef (1) + transform (1, 2) * pt.coeffRef (2) + transform (1, 3)); cloud_out[i].z = static_cast<float> (transform (2, 0) * pt.coeffRef (0) + transform (2, 1) * pt.coeffRef (1) + transform (2, 2) * pt.coeffRef (2) + transform (2, 3)); // Rotate normals //cloud_out.points[i].getNormalVector3fMap() = transform.rotation () * cloud_in.points[i].getNormalVector3fMap (); Eigen::Matrix<Scalar, 3, 1> nt (cloud_in[i].normal_x, cloud_in[i].normal_y, cloud_in[i].normal_z); cloud_out[i].normal_x = static_cast<float> (transform (0, 0) * nt.coeffRef (0) + transform (0, 1) * nt.coeffRef (1) + transform (0, 2) * nt.coeffRef (2)); cloud_out[i].normal_y = static_cast<float> (transform (1, 0) * nt.coeffRef (0) + transform (1, 1) * nt.coeffRef (1) + transform (1, 2) * nt.coeffRef (2)); cloud_out[i].normal_z = static_cast<float> (transform (2, 0) * nt.coeffRef (0) + transform (2, 1) * nt.coeffRef (1) + transform (2, 2) * nt.coeffRef (2)); } } }
void GreenStrain_LIMSolver2D::computeHessian(const Eigen::Matrix<double,Eigen::Dynamic,1>& x, const Eigen::Matrix<double*,Eigen::Dynamic,1>& hess) { // green strain tensor energy Eigen::Matrix<double,2,3> S; for(int t=0;t<mesh->Triangles->rows();t++) { Eigen::Vector2d A(x[TriangleVertexIdx.coeff(0,t)],x[TriangleVertexIdx.coeff(1,t)]); Eigen::Vector2d B(x[TriangleVertexIdx.coeff(2,t)],x[TriangleVertexIdx.coeff(3,t)]); Eigen::Vector2d C(x[TriangleVertexIdx.coeff(4,t)],x[TriangleVertexIdx.coeff(5,t)]); Eigen::Matrix<double,2,3> V; V.col(0) = A; V.col(1) = B; V.col(2) = C; // hessian(E) = 4*r_x'*((SMM'V'V+VMM'*(V'S+SV))*MM' - SMM')*c_x Eigen::Matrix3d VTV = V.transpose()*V; Eigen::Matrix3d MMT = MMTs.block<3,3>(0,3*t); Eigen::Matrix<double,2,3> VMMT = V*MMT; Eigen::Matrix3d MMTVTV = MMT*VTV; int numElem = 0; for(int r=0;r<6;r++) { S = Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic>::Zero(2,3); S.coeffRef(r) = 1; Eigen::Matrix<double,2,3> Temp = 4*((S*MMTVTV + VMMT*(V.transpose()*S+S.transpose()*V))*MMT - S*MMT); for(int c=r;c<6;c++) *denseHessianCoeffs(numElem++,t) += Temp.coeff(c)*Divider; } } }
/* ctor for cholesky function * * Stores varis for A * Instantiates and stores varis for L * Instantiates and stores dummy vari for * upper triangular part of var result returned * in cholesky_decompose function call * * variRefL aren't on the chainable * autodiff stack, only used for storage * and computation. Note that varis for * L are constructed externally in * cholesky_decompose. * * @param matrix A * @param matrix L, cholesky factor of A * */ cholesky_decompose_v_vari(const Eigen::Matrix<var, -1, -1>& A, const Eigen::Matrix<double, -1, -1>& L_A) : vari(0.0), M_(A.rows()), variRefA_(ChainableStack::memalloc_.alloc_array<vari*> (A.rows() * (A.rows() + 1) / 2)), variRefL_(ChainableStack::memalloc_.alloc_array<vari*> (A.rows() * (A.rows() + 1) / 2)) { size_t accum = 0; size_t accum_i = accum; for (size_type j = 0; j < M_; ++j) { for (size_type i = j; i < M_; ++i) { accum_i += i; size_t pos = j + accum_i; variRefA_[pos] = A.coeffRef(i, j).vi_; variRefL_[pos] = new vari(L_A.coeffRef(i, j), false); } accum += j; accum_i = accum; } }
ValueType compare(const Eigen::Matrix<ValueType, _Rows, _Cols, _Option> &mat, const ValueType value, const ValueType eps = 1e-12) { ValueType v; int num_eq = 0; if (_Option == Eigen::RowMajor) { for (int i = 0; i < mat.rows(); ++i) { for (int j = 0; j < mat.cols(); ++j) { v = mat.coeffRef(i, j); if (compare(v, value, eps)) ++num_eq; } } } else { for (int j = 0; j < mat.cols(); ++j) { for (int i = 0; i < mat.rows(); ++i) { v = mat.coeffRef(i, j); if (compare(v, value, eps)) ++num_eq; } } } return num_eq; }
inline Eigen::Matrix<typename boost::math::tools::promote_args<T1, T2>::type, Eigen::Dynamic, 1> csr_matrix_times_vector(const int& m, const int& n, const Eigen::Matrix<T1, Eigen::Dynamic, 1>& w, const std::vector<int>& v, const std::vector<int>& u, const Eigen::Matrix<T2, Eigen::Dynamic, 1>& b) { typedef typename boost::math::tools::promote_args<T1, T2>::type result_t; check_positive("csr_matrix_times_vector", "m", m); check_positive("csr_matrix_times_vector", "n", n); check_size_match("csr_matrix_times_vector", "n", n, "b", b.size()); check_size_match("csr_matrix_times_vector", "m", m, "u", u.size() - 1); check_size_match("csr_matrix_times_vector", "w", w.size(), "v", v.size()); check_size_match("csr_matrix_times_vector", "u/z", u[m - 1] + csr_u_to_z(u, m - 1) - 1, "v", v.size()); for (unsigned int i = 0; i < v.size(); ++i) check_range("csr_matrix_times_vector", "v[]", n, v[i]); Eigen::Matrix<result_t, Eigen::Dynamic, 1> result(m); result.setZero(); for (int row = 0; row < m; ++row) { int idx = csr_u_to_z(u, row); int row_end_in_w = (u[row] - stan::error_index::value) + idx; int i = 0; // index into dot-product segment entries. Eigen::Matrix<result_t, Eigen::Dynamic, 1> b_sub(idx); b_sub.setZero(); for (int nze = u[row] - stan::error_index::value; nze < row_end_in_w; ++nze, ++i) { check_range("csr_matrix_times_vector", "j", n, v[nze]); b_sub.coeffRef(i) = b.coeffRef(v[nze] - stan::error_index::value); } // loop skipped when z is zero. Eigen::Matrix<T1, Eigen::Dynamic, 1> w_sub(w.segment(u[row] - stan::error_index::value, idx)); result.coeffRef(row) = dot_product(w_sub, b_sub); } return result; }
pcl::PointNormal transformPoint( const pcl::PointNormal & point, const Transform & transform) { pcl::PointNormal ret; Eigen::Matrix<float, 3, 1> pt (point.x, point.y, point.z); ret.x = static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3)); ret.y = static_cast<float> (transform (1, 0) * pt.coeffRef (0) + transform (1, 1) * pt.coeffRef (1) + transform (1, 2) * pt.coeffRef (2) + transform (1, 3)); ret.z = static_cast<float> (transform (2, 0) * pt.coeffRef (0) + transform (2, 1) * pt.coeffRef (1) + transform (2, 2) * pt.coeffRef (2) + transform (2, 3)); // Rotate normals Eigen::Matrix<float, 3, 1> nt (point.normal_x, point.normal_y, point.normal_z); ret.normal_x = static_cast<float> (transform (0, 0) * nt.coeffRef (0) + transform (0, 1) * nt.coeffRef (1) + transform (0, 2) * nt.coeffRef (2)); ret.normal_y = static_cast<float> (transform (1, 0) * nt.coeffRef (0) + transform (1, 1) * nt.coeffRef (1) + transform (1, 2) * nt.coeffRef (2)); ret.normal_z = static_cast<float> (transform (2, 0) * nt.coeffRef (0) + transform (2, 1) * nt.coeffRef (1) + transform (2, 2) * nt.coeffRef (2)); return ret; }
void GreenStrain_LIMSolver3D::computeHessian(const Eigen::Matrix<double,Eigen::Dynamic,1>& x, const Eigen::Matrix<double*,Eigen::Dynamic,1>& hess) { // green strain tensor energy Eigen::Matrix<double,3,4> S; for(int t=0;t<mesh->Tetrahedra->rows();t++) { Eigen::Vector3d A(x[TetrahedronVertexIdx.coeff(0,t)],x[TetrahedronVertexIdx.coeff(1,t)],x[TetrahedronVertexIdx.coeff(2,t)]); Eigen::Vector3d B(x[TetrahedronVertexIdx.coeff(3,t)],x[TetrahedronVertexIdx.coeff(4,t)],x[TetrahedronVertexIdx.coeff(5,t)]); Eigen::Vector3d C(x[TetrahedronVertexIdx.coeff(6,t)],x[TetrahedronVertexIdx.coeff(7,t)],x[TetrahedronVertexIdx.coeff(8,t)]); Eigen::Vector3d D(x[TetrahedronVertexIdx.coeff(9,t)],x[TetrahedronVertexIdx.coeff(10,t)],x[TetrahedronVertexIdx.coeff(11,t)]); Eigen::Matrix<double,3,4> V; V.col(0) = A; V.col(1) = B; V.col(2) = C; V.col(3) = D; // hessian(E) = 4*r_x'*((SMM'V'V+VMM'*(V'S+SV))*MM' - SMM')*c_x Eigen::Matrix<double,4,4> VTV = V.transpose()*V; Eigen::Matrix<double,4,4> MMT = MMTs.block<4,4>(0,4*t); Eigen::Matrix<double,3,4> VMMT = V*MMT; Eigen::Matrix<double,4,4> MMTVTV = MMT*VTV; int numElem = 0; for(int r=0;r<12;r++) { S = Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic>::Zero(3,4); S.coeffRef(r) = 1; Eigen::Matrix<double,3,4> Temp = 4*((S*MMTVTV + VMMT*(V.transpose()*S+S.transpose()*V))*MMT - S*MMT); for(int c=r;c<12;c++) *denseHessianCoeffs(numElem++,t) += Temp.coeff(c)*Divider; } } }
vectview<typename Base::scalar, T::DOF> subvector(Eigen::Matrix<typename Base::scalar, Base::DOF, 1>& vec, SubManifold<T, idx> Base::*) { return &vec.coeffRef(idx); }
void serialization_test() { std::string file("test"); bool tbIn = true,tbOut; char tcIn = 't',tcOut; unsigned char tucIn = 'u',tucOut; short tsIn = 6,tsOut; int tiIn = -10,tiOut; unsigned int tuiIn = 10,tuiOut; float tfIn = 1.0005,tfOut; double tdIn = 1.000000005,tdOut; int* tinpIn = NULL,*tinpOut = NULL; float* tfpIn = new float,*tfpOut = NULL; *tfpIn = 1.11101; std::string tstrIn("test12345"),tstrOut; Test2 tObjIn,tObjOut; int ti = 2; tObjIn.ti = &ti; Test1 test1,test2,test3; test1.ts = "100"; test2.ts = "200"; test3.ts = "300"; Test1 testA, testC; testA.tt = &test1; testA.ts = "test123"; testA.tvt.push_back(&test2); testA.tvt.push_back(&test3); Test1 testB = testA; testB.ts = "400"; testB.tvt.pop_back(); std::pair<int,bool> tPairIn(10,true); std::pair<int,bool> tPairOut; std::vector<int> tVector1In ={1,2,3,4,5}; std::vector<int> tVector1Out; std::pair<int,bool> p1(10,1); std::pair<int,bool> p2(1,0); std::pair<int,bool> p3(10000,1); std::vector<std::pair<int,bool> > tVector2In ={p1,p2,p3}; std::vector<std::pair<int,bool> > tVector2Out; std::set<std::pair<int,bool> > tSetIn ={p1,p2,p3}; std::set<std::pair<int,bool> > tSetOut; std::map<int,bool> tMapIn ={p1,p2,p3}; std::map<int,bool> tMapOut; Eigen::Matrix<float,3,3> tDenseMatrixIn; tDenseMatrixIn << Eigen::Matrix<float,3,3>::Random(); tDenseMatrixIn.coeffRef(0,0) = 1.00001; Eigen::Matrix<float,3,3> tDenseMatrixOut; Eigen::Matrix<float,3,3,Eigen::RowMajor> tDenseRowMatrixIn; tDenseRowMatrixIn << Eigen::Matrix<float,3,3,Eigen::RowMajor>::Random(); Eigen::Matrix<float,3,3,Eigen::RowMajor> tDenseRowMatrixOut; Eigen::SparseMatrix<double> tSparseMatrixIn; tSparseMatrixIn.resize(3,3); tSparseMatrixIn.insert(0,0) = 1.3; tSparseMatrixIn.insert(1,1) = 10.2; tSparseMatrixIn.insert(2,2) = 100.1; tSparseMatrixIn.finalize(); Eigen::SparseMatrix<double> tSparseMatrixOut; // binary serialization igl::serialize(tbIn,file); igl::deserialize(tbOut,file); assert(tbIn == tbOut); igl::serialize(tcIn,file); igl::deserialize(tcOut,file); assert(tcIn == tcOut); igl::serialize(tucIn,file); igl::deserialize(tucOut,file); assert(tucIn == tucOut); igl::serialize(tsIn,file); igl::deserialize(tsOut,file); assert(tsIn == tsOut); igl::serialize(tiIn,file); igl::deserialize(tiOut,file); assert(tiIn == tiOut); igl::serialize(tuiIn,file); igl::deserialize(tuiOut,file); assert(tuiIn == tuiOut); igl::serialize(tfIn,file); igl::deserialize(tfOut,file); assert(tfIn == tfOut); igl::serialize(tdIn,file); igl::deserialize(tdOut,file); assert(tdIn == tdOut); igl::serialize(tinpIn,file); igl::deserialize(tinpOut,file); assert(tinpIn == tinpOut); igl::serialize(tfpIn,file); igl::deserialize(tfpOut,file); assert(*tfpIn == *tfpOut); tfpOut = NULL; igl::serialize(tstrIn,file); igl::deserialize(tstrOut,file); assert(tstrIn == tstrOut); // updating igl::serialize(tbIn,"tb",file,true); igl::serialize(tcIn,"tc",file); igl::serialize(tiIn,"ti",file); tiIn++; igl::serialize(tiIn,"ti",file); tiIn++; igl::serialize(tiIn,"ti",file); igl::deserialize(tbOut,"tb",file); igl::deserialize(tcOut,"tc",file); igl::deserialize(tiOut,"ti",file); assert(tbIn == tbOut); assert(tcIn == tcOut); assert(tiIn == tiOut); igl::serialize(tsIn,"tsIn",file,true); igl::serialize(tVector1In,"tVector1In",file); igl::serialize(tVector2In,"tsIn",file); igl::deserialize(tVector2Out,"tsIn",file); for(unsigned int i=0;i<tVector2In.size();i++) { assert(tVector2In[i].first == tVector2Out[i].first); assert(tVector2In[i].second == tVector2Out[i].second); } tVector2Out.clear(); igl::serialize(tObjIn,file); igl::deserialize(tObjOut,file); assert(tObjIn.tc == tObjOut.tc); assert(*tObjIn.ti == *tObjOut.ti); for(unsigned int i=0;i<tObjIn.tvb.size();i++) assert(tObjIn.tvb[i] == tObjOut.tvb[i]); tObjOut.ti = NULL; igl::serialize(tPairIn,file); igl::deserialize(tPairOut,file); assert(tPairIn.first == tPairOut.first); assert(tPairIn.second == tPairOut.second); igl::serialize(tVector1In,file); igl::deserialize(tVector1Out,file); for(unsigned int i=0;i<tVector1In.size();i++) assert(tVector1In[i] == tVector1Out[i]); igl::serialize(tVector2In,file); igl::deserialize(tVector2Out,file); for(unsigned int i=0;i<tVector2In.size();i++) { assert(tVector2In[i].first == tVector2Out[i].first); assert(tVector2In[i].second == tVector2Out[i].second); } igl::serialize(tSetIn,file); igl::deserialize(tSetOut,file); assert(tSetIn.size() == tSetOut.size()); igl::serialize(tMapIn,file); igl::deserialize(tMapOut,file); assert(tMapIn.size() == tMapOut.size()); igl::serialize(tDenseMatrixIn,file); igl::deserialize(tDenseMatrixOut,file); assert((tDenseMatrixIn - tDenseMatrixOut).sum() == 0); igl::serialize(tDenseRowMatrixIn,file); igl::deserialize(tDenseRowMatrixOut,file); assert((tDenseRowMatrixIn - tDenseRowMatrixOut).sum() == 0); igl::serialize(tSparseMatrixIn,file); igl::deserialize(tSparseMatrixOut,file); assert((tSparseMatrixIn - tSparseMatrixOut).sum() == 0); igl::serialize(testB,file); igl::deserialize(testC,file); assert(testB.ts == testC.ts); assert(testB.tvt.size() == testC.tvt.size()); for(unsigned int i=0;i<testB.tvt.size();i++) { assert(testB.tvt[i]->ts == testC.tvt[i]->ts); assert(testB.tvt[i]->tvt.size() == testC.tvt[i]->tvt.size()); assert(testB.tvt[i]->tt == testC.tvt[i]->tt); } assert(testB.tt->ts == testC.tt->ts); assert(testB.tt->tvt.size() == testC.tt->tvt.size()); assert(testB.tt->tt == testC.tt->tt); testC = Test1(); // big data test /*std::vector<std::vector<float> > bigDataIn,bigDataOut; for(unsigned int i=0;i<10000;i++) { std::vector<float> v; for(unsigned int j=0;j<10000;j++) { v.push_back(j); } bigDataIn.push_back(v); } igl::Timer timer; timer.start(); igl::serialize(bigDataIn,file); timer.stop(); std::cout << "ser: " << timer.getElapsedTimeInMilliSec() << std::endl; timer.start(); igl::deserialize(bigDataOut,file); timer.stop(); std::cout << "des: " << timer.getElapsedTimeInMilliSec() << std::endl; char c; std::cin >> c; */ // xml serialization igl::serialize_xml(tbIn,file); igl::deserialize_xml(tbOut,file); assert(tbIn == tbOut); igl::serialize_xml(tcIn,file); igl::deserialize_xml(tcOut,file); assert(tcIn == tcOut); igl::serialize_xml(tucIn,file); igl::deserialize_xml(tucOut,file); assert(tucIn == tucOut); igl::serialize_xml(tsIn,file); igl::deserialize_xml(tsOut,file); assert(tsIn == tsOut); igl::serialize_xml(tiIn,file); igl::deserialize_xml(tiOut,file); assert(tiIn == tiOut); igl::serialize_xml(tuiIn,file); igl::deserialize_xml(tuiOut,file); assert(tuiIn == tuiOut); igl::serialize_xml(tfIn,file); igl::deserialize_xml(tfOut,file); assert(tfIn == tfOut); igl::serialize_xml(tdIn,file); igl::deserialize_xml(tdOut,file); assert(tdIn == tdOut); igl::serialize_xml(tinpIn,file); igl::deserialize_xml(tinpOut,file); assert(tinpIn == tinpOut); igl::serialize_xml(tfpIn,file); igl::deserialize_xml(tfpOut,file); assert(*tfpIn == *tfpOut); igl::serialize_xml(tstrIn,file); igl::deserialize_xml(tstrOut,file); assert(tstrIn == tstrOut); // updating igl::serialize_xml(tbIn,"tb",file,false,true); igl::serialize_xml(tcIn,"tc",file); igl::serialize_xml(tiIn,"ti",file); tiIn++; igl::serialize_xml(tiIn,"ti",file); tiIn++; igl::serialize_xml(tiIn,"ti",file); igl::deserialize_xml(tbOut,"tb",file); igl::deserialize_xml(tcOut,"tc",file); igl::deserialize_xml(tiOut,"ti",file); assert(tbIn == tbOut); assert(tcIn == tcOut); assert(tiIn == tiOut); igl::serialize_xml(tsIn,"tsIn",file,false,true); igl::serialize_xml(tVector1In,"tVector1In",file); igl::serialize_xml(tVector2In,"tsIn",file); igl::deserialize_xml(tVector2Out,"tsIn",file); for(unsigned int i=0;i<tVector2In.size();i++) { assert(tVector2In[i].first == tVector2Out[i].first); assert(tVector2In[i].second == tVector2Out[i].second); } tVector2Out.clear(); // binarization igl::serialize_xml(tVector2In,"tVector2In",file,true); igl::deserialize_xml(tVector2Out,"tVector2In",file); for(unsigned int i=0;i<tVector2In.size();i++) { assert(tVector2In[i].first == tVector2Out[i].first); assert(tVector2In[i].second == tVector2Out[i].second); } igl::serialize_xml(tObjIn,file); igl::deserialize_xml(tObjOut,file); assert(tObjIn.tc == tObjOut.tc); assert(*tObjIn.ti == *tObjOut.ti); for(unsigned int i=0;i<tObjIn.tvb.size();i++) assert(tObjIn.tvb[i] == tObjOut.tvb[i]); igl::serialize_xml(tPairIn,file); igl::deserialize_xml(tPairOut,file); assert(tPairIn.first == tPairOut.first); assert(tPairIn.second == tPairOut.second); igl::serialize_xml(tVector1In,file); igl::deserialize_xml(tVector1Out,file); for(unsigned int i=0;i<tVector1In.size();i++) assert(tVector1In[i] == tVector1Out[i]); igl::serialize_xml(tVector2In,file); igl::deserialize_xml(tVector2Out,file); for(unsigned int i=0;i<tVector2In.size();i++) { assert(tVector2In[i].first == tVector2Out[i].first); assert(tVector2In[i].second == tVector2Out[i].second); } igl::serialize_xml(tSetIn,file); igl::deserialize_xml(tSetOut,file); assert(tSetIn.size() == tSetOut.size()); igl::serialize_xml(tMapIn,file); igl::deserialize_xml(tMapOut,file); assert(tMapIn.size() == tMapOut.size()); igl::serialize_xml(tDenseMatrixIn,file); igl::deserialize_xml(tDenseMatrixOut,file); assert((tDenseMatrixIn - tDenseMatrixOut).sum() == 0); igl::serialize_xml(tDenseRowMatrixIn,file); igl::deserialize_xml(tDenseRowMatrixOut,file); assert((tDenseRowMatrixIn - tDenseRowMatrixOut).sum() == 0); igl::serialize_xml(tSparseMatrixIn,file); igl::deserialize_xml(tSparseMatrixOut,file); assert((tSparseMatrixIn - tSparseMatrixOut).sum() == 0); igl::serialize_xml(testB,file); igl::deserialize_xml(testC,file); assert(testB.ts == testC.ts); assert(testB.tvt.size() == testC.tvt.size()); for(unsigned int i=0;i<testB.tvt.size();i++) { assert(testB.tvt[i]->ts == testC.tvt[i]->ts); assert(testB.tvt[i]->tvt.size() == testC.tvt[i]->tvt.size()); assert(testB.tvt[i]->tt == testC.tvt[i]->tt); } assert(testB.tt->ts == testC.tt->ts); assert(testB.tt->tvt.size() == testC.tt->tvt.size()); assert(testB.tt->tt == testC.tt->tt); // big data test /*std::vector<std::vector<float> > bigDataIn,bigDataOut; for(unsigned int i=0;i<10000;i++) { std::vector<float> v; for(unsigned int j=0;j<10000;j++) { v.push_back(j); } bigDataIn.push_back(v); } igl::Timer timer; timer.start(); igl::serialize_xml(bigDataIn,"bigDataIn",file,igl::SERIALIZE_BINARY); timer.stop(); std::cout << "ser: " << timer.getElapsedTimeInMilliSec() << std::endl; timer.start(); igl::deserialize_xml(bigDataOut,"bigDataIn",file); timer.stop(); std::cout << "des: " << timer.getElapsedTimeInMilliSec() << std::endl; char c; std::cin >> c;*/ std::cout << "All tests run successfully!\n"; }
template<typename PointT> void pcl::search::OrganizedNeighbor<PointT>::estimateProjectionMatrix () { // internally we calculate with double but store the result into float matrices. typedef double Scalar; projection_matrix_.setZero (); if (input_->height == 1 || input_->width == 1) { PCL_ERROR ("[pcl::%s::estimateProjectionMatrix] Input dataset is not organized!\n", getName ().c_str ()); return; } // we just want to use every 16th column and row -> skip = 2^4 const unsigned int skip = input_->width >> 4; Eigen::Matrix<Scalar, 4, 4> A = Eigen::Matrix<Scalar, 4, 4>::Zero (); Eigen::Matrix<Scalar, 4, 4> B = Eigen::Matrix<Scalar, 4, 4>::Zero (); Eigen::Matrix<Scalar, 4, 4> C = Eigen::Matrix<Scalar, 4, 4>::Zero (); Eigen::Matrix<Scalar, 4, 4> D = Eigen::Matrix<Scalar, 4, 4>::Zero (); for (unsigned yIdx = 0, idx = 0; yIdx < input_->height; yIdx += skip, idx += input_->width * (skip-1)) { for (unsigned xIdx = 0; xIdx < input_->width; xIdx += skip, idx += skip) { const PointT& point = input_->points[idx]; if (isFinite (point)) { Scalar xx = point.x * point.x; Scalar xy = point.x * point.y; Scalar xz = point.x * point.z; Scalar yy = point.y * point.y; Scalar yz = point.y * point.z; Scalar zz = point.z * point.z; Scalar xx_yy = xIdx * xIdx + yIdx * yIdx; A.coeffRef (0) += xx; A.coeffRef (1) += xy; A.coeffRef (2) += xz; A.coeffRef (3) += point.x; A.coeffRef (5) += yy; A.coeffRef (6) += yz; A.coeffRef (7) += point.y; A.coeffRef (10) += zz; A.coeffRef (11) += point.z; A.coeffRef (15) += 1.0; B.coeffRef (0) -= xx * xIdx; B.coeffRef (1) -= xy * xIdx; B.coeffRef (2) -= xz * xIdx; B.coeffRef (3) -= point.x * xIdx; B.coeffRef (5) -= yy * xIdx; B.coeffRef (6) -= yz * xIdx; B.coeffRef (7) -= point.y * xIdx; B.coeffRef (10) -= zz * xIdx; B.coeffRef (11) -= point.z * xIdx; B.coeffRef (15) -= xIdx; C.coeffRef (0) -= xx * yIdx; C.coeffRef (1) -= xy * yIdx; C.coeffRef (2) -= xz * yIdx; C.coeffRef (3) -= point.x * yIdx; C.coeffRef (5) -= yy * yIdx; C.coeffRef (6) -= yz * yIdx; C.coeffRef (7) -= point.y * yIdx; C.coeffRef (10) -= zz * yIdx; C.coeffRef (11) -= point.z * yIdx; C.coeffRef (15) -= yIdx; D.coeffRef (0) += xx * xx_yy; D.coeffRef (1) += xy * xx_yy; D.coeffRef (2) += xz * xx_yy; D.coeffRef (3) += point.x * xx_yy; D.coeffRef (5) += yy * xx_yy; D.coeffRef (6) += yz * xx_yy; D.coeffRef (7) += point.y * xx_yy; D.coeffRef (10) += zz * xx_yy; D.coeffRef (11) += point.z * xx_yy; D.coeffRef (15) += xx_yy; } } } makeSymmetric(A); makeSymmetric(B); makeSymmetric(C); makeSymmetric(D); Eigen::Matrix<Scalar, 12, 12> X = Eigen::Matrix<Scalar, 12, 12>::Zero (); X.topLeftCorner<4,4> () = A; X.block<4,4> (0, 8) = B; X.block<4,4> (8, 0) = B; X.block<4,4> (4, 4) = A; X.block<4,4> (4, 8) = C; X.block<4,4> (8, 4) = C; X.block<4,4> (8, 8) = D; Eigen::SelfAdjointEigenSolver<Eigen::Matrix<Scalar, 12, 12> > ei_symm(X); Eigen::Matrix<Scalar, 12, 12> eigen_vectors = ei_symm.eigenvectors(); // check whether the residual MSE is low. If its high, the cloud was not captured from a projective device. Eigen::Matrix<Scalar, 1, 1> residual_sqr = eigen_vectors.col (0).transpose () * X * eigen_vectors.col (0); if ( residual_sqr.coeff (0) > eps_ * A.coeff (15)) { PCL_ERROR ("[pcl::%s::radiusSearch] Input dataset is not from a projective device!\n", getName ().c_str ()); return; } projection_matrix_.coeffRef (0) = eigen_vectors.coeff (0); projection_matrix_.coeffRef (1) = eigen_vectors.coeff (12); projection_matrix_.coeffRef (2) = eigen_vectors.coeff (24); projection_matrix_.coeffRef (3) = eigen_vectors.coeff (36); projection_matrix_.coeffRef (4) = eigen_vectors.coeff (48); projection_matrix_.coeffRef (5) = eigen_vectors.coeff (60); projection_matrix_.coeffRef (6) = eigen_vectors.coeff (72); projection_matrix_.coeffRef (7) = eigen_vectors.coeff (84); projection_matrix_.coeffRef (8) = eigen_vectors.coeff (96); projection_matrix_.coeffRef (9) = eigen_vectors.coeff (108); projection_matrix_.coeffRef (10) = eigen_vectors.coeff (120); projection_matrix_.coeffRef (11) = eigen_vectors.coeff (132); if (projection_matrix_.coeff (0) < 0) projection_matrix_ *= -1.0; // get left 3x3 sub matrix, which contains K * R, with K = camera matrix = [[fx s cx] [0 fy cy] [0 0 1]] // and R being the rotation matrix KR_ = projection_matrix_.topLeftCorner <3, 3> (); // precalculate KR * KR^T needed by calculations during nn-search KR_KRT_ = KR_ * KR_.transpose (); }
template <typename PointT> double pcl::estimateProjectionMatrix ( typename pcl::PointCloud<PointT>::ConstPtr cloud, Eigen::Matrix<float, 3, 4, Eigen::RowMajor>& projection_matrix, const std::vector<int>& indices) { // internally we calculate with double but store the result into float matrices. typedef double Scalar; projection_matrix.setZero (); if (cloud->height == 1 || cloud->width == 1) { PCL_ERROR ("[pcl::estimateProjectionMatrix] Input dataset is not organized!\n"); return (-1.0); } Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor> A = Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor>::Zero (); Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor> B = Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor>::Zero (); Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor> C = Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor>::Zero (); Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor> D = Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor>::Zero (); pcl::ConstCloudIterator <PointT> pointIt (*cloud, indices); while (pointIt) { unsigned yIdx = pointIt.getCurrentPointIndex () / cloud->width; unsigned xIdx = pointIt.getCurrentPointIndex () % cloud->width; const PointT& point = *pointIt; if (pcl_isfinite (point.x)) { Scalar xx = point.x * point.x; Scalar xy = point.x * point.y; Scalar xz = point.x * point.z; Scalar yy = point.y * point.y; Scalar yz = point.y * point.z; Scalar zz = point.z * point.z; Scalar xx_yy = xIdx * xIdx + yIdx * yIdx; A.coeffRef (0) += xx; A.coeffRef (1) += xy; A.coeffRef (2) += xz; A.coeffRef (3) += point.x; A.coeffRef (5) += yy; A.coeffRef (6) += yz; A.coeffRef (7) += point.y; A.coeffRef (10) += zz; A.coeffRef (11) += point.z; A.coeffRef (15) += 1.0; B.coeffRef (0) -= xx * xIdx; B.coeffRef (1) -= xy * xIdx; B.coeffRef (2) -= xz * xIdx; B.coeffRef (3) -= point.x * static_cast<double>(xIdx); B.coeffRef (5) -= yy * xIdx; B.coeffRef (6) -= yz * xIdx; B.coeffRef (7) -= point.y * static_cast<double>(xIdx); B.coeffRef (10) -= zz * xIdx; B.coeffRef (11) -= point.z * static_cast<double>(xIdx); B.coeffRef (15) -= xIdx; C.coeffRef (0) -= xx * yIdx; C.coeffRef (1) -= xy * yIdx; C.coeffRef (2) -= xz * yIdx; C.coeffRef (3) -= point.x * static_cast<double>(yIdx); C.coeffRef (5) -= yy * yIdx; C.coeffRef (6) -= yz * yIdx; C.coeffRef (7) -= point.y * static_cast<double>(yIdx); C.coeffRef (10) -= zz * yIdx; C.coeffRef (11) -= point.z * static_cast<double>(yIdx); C.coeffRef (15) -= yIdx; D.coeffRef (0) += xx * xx_yy; D.coeffRef (1) += xy * xx_yy; D.coeffRef (2) += xz * xx_yy; D.coeffRef (3) += point.x * xx_yy; D.coeffRef (5) += yy * xx_yy; D.coeffRef (6) += yz * xx_yy; D.coeffRef (7) += point.y * xx_yy; D.coeffRef (10) += zz * xx_yy; D.coeffRef (11) += point.z * xx_yy; D.coeffRef (15) += xx_yy; } ++pointIt; } // while pcl::common::internal::makeSymmetric (A); pcl::common::internal::makeSymmetric (B); pcl::common::internal::makeSymmetric (C); pcl::common::internal::makeSymmetric (D); Eigen::Matrix<Scalar, 12, 12, Eigen::RowMajor> X = Eigen::Matrix<Scalar, 12, 12, Eigen::RowMajor>::Zero (); X.topLeftCorner<4,4> ().matrix () = A; X.block<4,4> (0, 8).matrix () = B; X.block<4,4> (8, 0).matrix () = B; X.block<4,4> (4, 4).matrix () = A; X.block<4,4> (4, 8).matrix () = C; X.block<4,4> (8, 4).matrix () = C; X.block<4,4> (8, 8).matrix () = D; Eigen::SelfAdjointEigenSolver<Eigen::Matrix<Scalar, 12, 12, Eigen::RowMajor> > ei_symm (X); Eigen::Matrix<Scalar, 12, 12, Eigen::RowMajor> eigen_vectors = ei_symm.eigenvectors (); // check whether the residual MSE is low. If its high, the cloud was not captured from a projective device. Eigen::Matrix<Scalar, 1, 1> residual_sqr = eigen_vectors.col (0).transpose () * X * eigen_vectors.col (0); double residual = residual_sqr.coeff (0); projection_matrix.coeffRef (0) = static_cast <float> (eigen_vectors.coeff (0)); projection_matrix.coeffRef (1) = static_cast <float> (eigen_vectors.coeff (12)); projection_matrix.coeffRef (2) = static_cast <float> (eigen_vectors.coeff (24)); projection_matrix.coeffRef (3) = static_cast <float> (eigen_vectors.coeff (36)); projection_matrix.coeffRef (4) = static_cast <float> (eigen_vectors.coeff (48)); projection_matrix.coeffRef (5) = static_cast <float> (eigen_vectors.coeff (60)); projection_matrix.coeffRef (6) = static_cast <float> (eigen_vectors.coeff (72)); projection_matrix.coeffRef (7) = static_cast <float> (eigen_vectors.coeff (84)); projection_matrix.coeffRef (8) = static_cast <float> (eigen_vectors.coeff (96)); projection_matrix.coeffRef (9) = static_cast <float> (eigen_vectors.coeff (108)); projection_matrix.coeffRef (10) = static_cast <float> (eigen_vectors.coeff (120)); projection_matrix.coeffRef (11) = static_cast <float> (eigen_vectors.coeff (132)); if (projection_matrix.coeff (0) < 0) projection_matrix *= -1.0; return (residual); }