Eigen::Vector3d& VertexPointXYZCov::normal(){
Eigen::Matrix3d eigvectors;
Eigen::Vector3d eigvalues;
if(!_hasNormal && _cov != Eigen::Matrix3d::Identity()){
pcl::eigen33<Matrix3d, Vector3d> (_cov, eigvectors, eigvalues);
_normal = Vector3d(0.0, 0.0, 0.0);
double eig_sum = eigvalues.sum();
if(eig_sum != 0){
if(eigvalues(0) < eigvalues(1) && eigvalues(0) < eigvalues(2)){
_normal = Vector3d(eigvectors(0,0), eigvectors(1,0), eigvectors(2,0));
}
if(eigvalues(1) < eigvalues(0) && eigvalues(1) < eigvalues(2)){
_normal = Vector3d(eigvectors(0,1), eigvectors(1,1), eigvectors(2,1));
}
if(eigvalues(2) < eigvalues(0) && eigvalues(2) < eigvalues(1)){
_normal = Vector3d(eigvectors(0,2), eigvectors(1,2), eigvectors(2,2));
}
//check direction of normal
Vector3d laserPoint(_estimate[0], _estimate[1], _estimate[2]);
Vector3d beam = (parentVertex()->estimate().translation() - laserPoint);
if(beam.dot(_normal) < 0){
_normal = -_normal;
}
}
_hasNormal = true;
}
return _normal;
}
void VertexPointXYZCov::updateNormal(Eigen::Matrix3d& cov){
Eigen::Matrix3d eigvectors;
Eigen::Vector3d eigvalues;
if(cov != Eigen::Matrix3d::Identity()){
pcl::eigen33<Matrix3d, Vector3d> (cov, eigvectors, eigvalues);
_normal = Vector3d(0.0, 0.0, 0.0);
double eig_sum = eigvalues.sum();
if(eig_sum != 0){
if(eigvalues(0) < eigvalues(1) && eigvalues(0) < eigvalues(2)){
_normal = Vector3d(eigvectors(0,0), eigvectors(1,0), eigvectors(2,0));
}
if(eigvalues(1) < eigvalues(0) && eigvalues(1) < eigvalues(2)){
_normal = Vector3d(eigvectors(0,1), eigvectors(1,1), eigvectors(2,1));
}
if(eigvalues(2) < eigvalues(0) && eigvalues(2) < eigvalues(1)){
_normal = Vector3d(eigvectors(0,2), eigvectors(1,2), eigvectors(2,2));
}
//check direction of normal
Eigen::Vector3d laserPoint(_estimate[0], _estimate[1], _estimate[2]);
Eigen::Vector3d beam = (parentVertex()->estimate().translation() - laserPoint);
if(beam.dot(_normal) < 0){
_normal = -_normal;
}
// Eigen::Matrix3d coordinateFrameRotation;
// Eigen::Vector3d yDirection(0,1,0);
// coordinateFrameRotation.row(2) = _normal;
// yDirection = yDirection - _normal(1)*_normal;
// yDirection.normalize();/// need to check if y is close to 0
// coordinateFrameRotation.row(1) = yDirection;
// coordinateFrameRotation.row(0) = _normal.cross(yDirection);
//
// _cov = Matrix3d::Identity();
// _cov(0,0) = 0.0001; ///Maybe we should make this resolution dependent
// _cov(1,1) = 0.0001; ///Maybe we should make this resolution dependent
// _cov(2,2) = 0.0001; ///Maybe we should make this resolution dependent
// _cov = coordinateFrameRotation.transpose()*_cov*coordinateFrameRotation;
_normal = parentVertex()->estimate().linear().inverse() * _normal; //rotate in local coordinate frame
}
_hasNormal = true;
}
}
void TrajectoryAnalyzer::analyze(const Eigen::MatrixXd & nma_covariance,
std::vector<std::shared_ptr<ProteinSegment>> & protein_segments,
const double & temperature) {
LOGD << "Fitting protein segments with NMA covariance matrix and computing force constants.";
for (std::shared_ptr<ProteinSegment> const & protein_segment : protein_segments) {
Eigen::MatrixXd covariance = nma_covariance.block((protein_segment->get_start_residuum_nr() - 1) * 3,
(protein_segment->get_start_residuum_nr() - 1) * 3,
protein_segment->get_size() * 3,
protein_segment->get_size() * 3);
Eigen::VectorXd displacement_vector = covariance.diagonal().array().sqrt().matrix();
Eigen::MatrixXd kk_matrix = Eigen::MatrixXd::Zero(protein_segment->get_size(), protein_segment->get_size());
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eig(covariance);
for (int i = 0; i < protein_segment->get_size(); i++) {
for (int j = 0; j < i; j++) {
Eigen::Vector3d kk = Eigen::Vector3d::Zero();
Eigen::Vector3d d = displacement_vector.segment<3>(3 * i)-displacement_vector.segment<3>(3 * j);
for(int k = (protein_segment->get_size() > 2 ? 6 : 5); k < protein_segment->get_size() * 3; k++) {
kk += Eigen::Vector3d(
eig.eigenvectors()(3 * i + 0, k) * eig.eigenvectors()(3 * j + 0, k),
eig.eigenvectors()(3 * i + 1, k) * eig.eigenvectors()(3 * j + 1, k),
eig.eigenvectors()(3 * i + 2, k) * eig.eigenvectors()(3 * j + 2, k))
/ eig.eigenvalues()[k];
}
kk_matrix(i, j) = -kk.sum() * 8.31 * temperature * 0.001;
kk_matrix(j, i) = d.norm();
}
}
protein_segment->add_force_constant(kk_matrix);
protein_segment->add_displacement_vector(displacement_vector);
protein_segment->add_mean_square_fluctuation(covariance);
}
LOGD << "Finished analyzing trajectory";
}
void operator() (Image<value_type>& dt_img)
{
/* check mask */
if (mask_img.valid()) {
assign_pos_of (dt_img, 0, 3).to (mask_img);
if (!mask_img.value())
return;
}
/* input dt */
Eigen::Matrix<double, 6, 1> dt;
for (auto l = Loop (3) (dt_img); l; ++l)
dt[dt_img.index(3)] = dt_img.value();
/* output adc */
if (adc_img.valid()) {
assign_pos_of (dt_img, 0, 3).to (adc_img);
adc_img.value() = DWI::tensor2ADC(dt);
}
double fa = 0.0;
if (fa_img.valid() || (vector_img.valid() && (modulate == 1)))
fa = DWI::tensor2FA(dt);
/* output fa */
if (fa_img.valid()) {
assign_pos_of (dt_img, 0, 3).to (fa_img);
fa_img.value() = fa;
}
bool need_eigenvalues = value_img.valid() || (vector_img.valid() && (modulate == 2)) || ad_img.valid() || rd_img.valid() || cl_img.valid() || cp_img.valid() || cs_img.valid();
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> es;
if (need_eigenvalues || vector_img.valid()) {
Eigen::Matrix3d M;
M (0,0) = dt[0];
M (1,1) = dt[1];
M (2,2) = dt[2];
M (0,1) = M (1,0) = dt[3];
M (0,2) = M (2,0) = dt[4];
M (1,2) = M (2,1) = dt[5];
es = Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d>(M, vector_img.valid() ? Eigen::ComputeEigenvectors : Eigen::EigenvaluesOnly);
}
Eigen::Vector3d eigval;
if (need_eigenvalues)
eigval = es.eigenvalues();
/* output value */
if (value_img.valid()) {
assign_pos_of (dt_img, 0, 3).to (value_img);
if (vals.size() > 1) {
auto l = Loop(3)(value_img);
for (size_t i = 0; i < vals.size(); i++) {
value_img.value() = eigval(3-vals[i]); l++;
}
} else {
value_img.value() = eigval(3-vals[0]);
}
}
/* output ad */
if (ad_img.valid()) {
assign_pos_of (dt_img, 0, 3).to (ad_img);
ad_img.value() = eigval(2);
}
/* output rd */
if (rd_img.valid()) {
assign_pos_of (dt_img, 0, 3).to (rd_img);
rd_img.value() = (eigval(1) + eigval(0)) / 2;
}
/* output shape measures */
if (cl_img.valid() || cp_img.valid() || cs_img.valid()) {
double eigsum = eigval.sum();
if (eigsum != 0.0) {
if (cl_img.valid()) {
assign_pos_of (dt_img, 0, 3).to (cl_img);
cl_img.value() = (eigval(2) - eigval(1)) / eigsum;
}
if (cp_img.valid()) {
assign_pos_of (dt_img, 0, 3).to (cp_img);
cp_img.value() = 2.0 * (eigval(1) - eigval(0)) / eigsum;
}
if (cs_img.valid()) {
assign_pos_of (dt_img, 0, 3).to (cs_img);
cs_img.value() = 3.0 * eigval(0) / eigsum;
}
}
}
/* output vector */
if (vector_img.valid()) {
Eigen::Matrix3d eigvec = es.eigenvectors();
assign_pos_of (dt_img, 0, 3).to (vector_img);
auto l = Loop(3)(vector_img);
for (size_t i = 0; i < vals.size(); i++) {
double fact = 1.0;
if (modulate == 1)
fact = fa;
else if (modulate == 2)
fact = eigval(3-vals[i]);
vector_img.value() = eigvec(0,3-vals[i])*fact; l++;
vector_img.value() = eigvec(1,3-vals[i])*fact; l++;
vector_img.value() = eigvec(2,3-vals[i])*fact; l++;
}
}
}