// points1, points2,
void computeRot(vector<double>& template_vextex, vector<double>& vertex,
vector<vector<double> >& template_nbor_vertices, vector<vector<double > >& nbor_vertices,
vector<unsigned int>& neighbors, vector<double>& weights,
vector<double>& output_rot, bool deform)
{
int num_neighbors = neighbors.size();
MatrixXd Xt(3, num_neighbors), X(3, num_neighbors);
Map<VectorXd> w(weights.data(), num_neighbors);
Eigen::VectorXd w_normalized = w/w.sum();
Eigen::Vector3d Xt_mean, X_mean;
for(int i = 0; i < num_neighbors; ++i)
{
for(int j = 0; j < 3; ++j)
{
Xt(j,i) = template_vextex[j] - template_nbor_vertices[ neighbors[i] ][j];
X(j,i) = vertex[j] - nbor_vertices[ neighbors[i] ][j];
if(deform)
X(j,i) += Xt(j,i);
}
}
// std::cout << Xt << endl;
// std::cout << X << endl;
for(int i=0; i<3; ++i) {
Xt_mean(i) = (Xt.row(i).array()*w_normalized.transpose().array()).sum();
X_mean(i) = (X.row(i).array()*w_normalized.transpose().array()).sum();
}
Xt.colwise() -= Xt_mean;
X.colwise() -= X_mean;
// std::cout << Xt_mean << endl;
// std::cout << X_mean << endl;
// std::cout << Xt << endl;
// std::cout << X << endl;
// std::cout << w_normalized << endl;
Eigen::Matrix3d sigma = Xt * w_normalized.asDiagonal() * X.transpose();
Eigen::JacobiSVD<Eigen::Matrix3d> svd(sigma, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::Matrix3d Rot;
if(svd.matrixU().determinant()*svd.matrixV().determinant() < 0.0) {
Eigen::Vector3d S = Eigen::Vector3d::Ones(); S(2) = -1.0;
Rot = svd.matrixV()*S.asDiagonal()*svd.matrixU().transpose();
} else {
Rot = svd.matrixV()*svd.matrixU().transpose();
}
// ceres::RotationMatrixToAngleAxis(&Rot(0, 0), &output_rot[0]);
Eigen::AngleAxisd angleAxis(Rot);
for(int i = 0; i < 3; ++i)
output_rot[i] = angleAxis.axis()[i] * angleAxis.angle();
}
/*! \brief Analytic evaluation of anisotropic liquid Green's function and its derivatives
*
* \f[
* \begin{align}
* \phantom{G(\vect{r},\vect{r}^\prime)}
* &\begin{aligned}
* G(\vect{r},\vect{r}^\prime) = \frac{1}{4\pi\sqrt{\det{\bm{\diel}}}\sqrt{(\vect{r}-\vect{r}^\prime)^t\bm{\diel}^{-1}(\vect{r}-\vect{r}^\prime)}}
* \end{aligned}\\
* &\begin{aligned}
* \pderiv{}{{\vect{n}_{\vect{r}^\prime}}}G(\vect{r},\vect{r}^\prime) = \frac{[\bm{\diel}^{-1}(\vect{r}-\vect{r}^\prime)]\cdot \vect{n}_{\vect{r}^\prime}}{4\pi\sqrt{\det{\bm{\diel}}}
* [(\vect{r}-\vect{r}^\prime)^t\bm{\diel}^{-1}(\vect{r}-\vect{r}^\prime)]^{\frac{3}{2}}}
* \end{aligned}\\
* &\begin{aligned}
* \pderiv{}{{\vect{n}_{\vect{r}}}}G(\vect{r},\vect{r}^\prime) = -\frac{[\bm{\diel}^{-1}(\vect{r}-\vect{r}^\prime)]\cdot \vect{n}_{\vect{r}}}{4\pi\sqrt{\det{\bm{\diel}}}
* [(\vect{r}-\vect{r}^\prime)^t\bm{\diel}^{-1}(\vect{r}-\vect{r}^\prime)]^{\frac{3}{2}}}
* \end{aligned}\\
* &\begin{aligned}
* \frac{\partial^2}{\partial{\vect{n}_{\vect{r}}}\partial{\vect{n}_{\vect{r}^\prime}}}G(\vect{r},\vect{r}^\prime) &=
* \frac{\vect{n}_{\vect{r}}\cdot[\bm{\diel}^{-1} \vect{n}_{\vect{r}^\prime}]}{4\pi\sqrt{\det{\bm{\diel}}}
* [(\vect{r}-\vect{r}^\prime)^t\bm{\diel}^{-1}(\vect{r}-\vect{r}^\prime)]^{\frac{3}{2}}} \\
* &-3\frac{\lbrace[\bm{\diel}^{-1}(\vect{r}-\vect{r}^\prime)]\cdot \vect{n}_{\vect{r}}\rbrace\lbrace[\bm{\diel}^{-1}(\vect{r}-\vect{r}^\prime)]\cdot \vect{n}_{\vect{r}^\prime}\rbrace}{4\pi\sqrt{\det{\bm{\diel}}}
* [(\vect{r}-\vect{r}^\prime)^t\bm{\diel}^{-1}(\vect{r}-\vect{r}^\prime)]^{\frac{5}{2}}}
* \end{aligned}
* \end{align}
* \f]
*/
inline Eigen::Array4d analyticAnisotropicLiquid(const Eigen::Vector3d & epsilon,
const Eigen::Vector3d & euler,
const Eigen::Vector3d & spNormal,
const Eigen::Vector3d & sp,
const Eigen::Vector3d & ppNormal, const Eigen::Vector3d & pp) {
Eigen::Array4d result = Eigen::Array4d::Zero();
Eigen::Matrix3d epsilonInv, R;
eulerRotation(R, euler);
Eigen::Vector3d scratch;
scratch << (1.0/epsilon(0)), (1.0/epsilon(1)), (1.0/epsilon(2));
epsilonInv = R * scratch.asDiagonal() * R.transpose();
double detEps = epsilon(0) * epsilon(1) * epsilon(2);
Eigen::Vector3d right = epsilonInv * (sp - pp);
Eigen::Vector3d left = (sp - pp).transpose();
double distance = std::sqrt(left.dot(right));
double distance_3 = std::pow(distance, 3);
double distance_5 = std::pow(distance, 5);
// Value of the function
result(0) = 1.0 / (std::sqrt(detEps) * distance);
// Value of the directional derivative wrt probe
result(1) = right.dot(ppNormal) / (std::sqrt(detEps) * distance_3);
// Directional derivative wrt source
result(2) = - right.dot(spNormal) / (std::sqrt(detEps) * distance_3);
// Value of the Hessian
Eigen::Vector3d eps_ppNormal = epsilonInv * ppNormal;
result(3) = spNormal.dot(eps_ppNormal) / (std::sqrt(detEps) * distance_3)
- 3 * (right.dot(spNormal)) * (right.dot(ppNormal)) / (std::sqrt(detEps) * distance_5);
return result;
}
int main() {
//-> create and fill synthetic data
int n_params = 7;
Eigen::VectorXd u(n_params);
Eigen::MatrixXd pa, pb;
create_data(pa, pb);
// <-
// -> cost function
CMAES::cost_type fcost = [&](double *params, int n_params) {
Eigen::Quaterniond q = Eigen::Quaterniond(params[0], params[1], params[2], params[3]).normalized();
Eigen::Vector3d s = Eigen::Vector3d(params[4], params[5], params[6]).cwiseAbs();
params[0] = q.w();
params[1] = q.x();
params[2] = q.y();
params[3] = q.z();
params[4] = s(0);
params[5] = s(1);
params[6] = s(2);
Eigen::Matrix3d rotation = q.toRotationMatrix();
Eigen::Matrix3d scale = s.asDiagonal();
Eigen::MatrixXd y = rotation*scale*pa;
double cost = (pb - y).squaredNorm();
return cost;
};
CMAES::Engine cmaes;
Eigen::VectorXd x0(n_params);
x0 << 1, 0, 0, 0, 1, 1, 1;
double c = fcost(x0.data(), n_params);
std::cout << "x0: " << x0.transpose() << " fcost(x0): " << c << std::endl;
double sigma0 = 1;
Solution sol = cmaes.fmin(x0, n_params, sigma0, 6, 999, fcost);
std::cout << "\nf_best: " << sol.f << "\nparams_best: " << sol.params.transpose() << std::endl;
return 0;
}
// points1, points2,
void computeRot(vector<double>& template_vextex, vector<double>& vertex,
vector<vector<double> >& template_nbor_vertices, vector<vector<double > >& nbor_vertices,
vector<unsigned int>& neighbors, vector<double>& weights,
vector<double>& output_rot, bool deform)
{
int num_neighbors = neighbors.size();
MatrixXd Xt(3, num_neighbors), X(3, num_neighbors);
for(int i = 0; i < num_neighbors; ++i)
{
for(int j = 0; j < 3; ++j)
{
Xt(j,i) = weights[i] * (
template_vextex[j] - template_nbor_vertices[ neighbors[i] ][j]);
X(j,i) = weights[i] * (
vertex[j] - nbor_vertices[ neighbors[i] ][j]);
if(deform)
X(j,i) += Xt(j,i);
}
}
Eigen::Matrix3d sigma = Xt * X.transpose();
Eigen::JacobiSVD<Eigen::Matrix3d> svd(sigma, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::Matrix3d Rot;
if(svd.matrixU().determinant()*svd.matrixV().determinant() < 0.0) {
Eigen::Vector3d S = Eigen::Vector3d::Ones(); S(2) = -1.0;
Rot = svd.matrixV()*S.asDiagonal()*svd.matrixU().transpose();
} else {
Rot = svd.matrixV()*svd.matrixU().transpose();
}
// ceres::RotationMatrixToAngleAxis(&Rot(0, 0), &output_rot[0]);
Eigen::AngleAxisd angleAxis(Rot);
for(int i = 0; i < 3; ++i)
output_rot[i] = angleAxis.axis()[i] * angleAxis.angle();
}
