Eigen::MatrixXd Tra_via0( double x0 , double v0 , double a0,
double xf , double vf , double af,
double smp , double tf )
/*
simple minimum jerk trajectory
x0 : position at initial state
v0 : velocity at initial state
a0 : acceleration at initial state
xf : position at final state
vf : velocity at final state
af : acceleration at final state
smp : sampling time
tf : movement time
*/
{
Eigen::MatrixXd A( 3 , 3 );
Eigen::MatrixXd B( 3 , 1 );
A << pow( tf , 3 ) , pow( tf , 4 ) , pow( tf , 5 ),
3 * pow( tf , 2 ) , 4 * pow( tf , 3 ) , 5 * pow( tf , 4 ),
6 * tf , 12 * pow( tf , 2 ) , 20 * pow( tf , 3 );
B << xf - x0 - v0 * tf - a0 * pow( tf , 2 ) / 2,
vf - v0 - a0 * tf,
af - a0 ;
Eigen::Matrix<double,3,1> C = A.inverse() * B;
double N;
N = tf / smp;
int NN = round( N + 1 );
Eigen::MatrixXd Time = Eigen::MatrixXd::Zero( NN , 1 );
Eigen::MatrixXd Tra = Eigen::MatrixXd::Zero( NN , 1 );
int i;
for ( i = 1; i <= NN; i++ )
Time.coeffRef( i - 1 , 0 ) = ( i - 1 ) * smp;
for ( i = 1; i <= NN; i++ )
{
Tra.coeffRef(i-1,0) =
x0 +
v0 * Time.coeff( i - 1 ) +
0.5 * a0 * pow( Time.coeff( i - 1 ) , 2 ) +
C.coeff( 0 , 0 ) * pow( Time.coeff( i - 1 ) , 3 ) +
C.coeff( 1 , 0 ) * pow( Time.coeff( i - 1 ) , 4 ) +
C.coeff( 2 , 0 ) * pow( Time.coeff( i - 1 ) , 5 );
}
return Tra;
}
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;
}
}
}
void GreenStrain_LIMSolver2D::computeGradient(const Eigen::Matrix<double,Eigen::Dynamic,1>& x, Eigen::Matrix<double,Eigen::Dynamic,1>& grad)
{
// green strain energy
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;
// jacobian(E) = 4(VMM'V'VMM' - VMM')
Eigen::Matrix<double,2,3> VMMT = V*MMTs.block<3,3>(0,3*t);
Eigen::Matrix<double,2,3> T = 4*(VMMT*V.transpose()*VMMT - VMMT);
for(int i=0;i<6;i++)
grad[TriangleVertexIdx.coeff(i,t)] += T.coeff(i)*Divider;
}
}
void GreenStrain_LIMSolver3D::computeGradient(const Eigen::Matrix<double,Eigen::Dynamic,1>& x, Eigen::Matrix<double,Eigen::Dynamic,1>& grad)
{
// green strain energy
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;
// jacobian(E) = 4(VMM'V'VMM' - VMM')
Eigen::Matrix<double,3,4> VMMT = V*MMTs.block<4,4>(0,4*t);
Eigen::Matrix<double,3,4> T = 4*(VMMT*V.transpose()*VMMT - VMMT);
for(int i=0;i<12;i++)
grad[TetrahedronVertexIdx.coeff(i,t)] += T.coeff(i)*Divider;
}
}
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;
}
}
}
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);
}
