/*
* 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);
}
