Eigen::VectorXd operator()(const F& obj, Eigen::VectorXd x) const {
Eigen::VectorXd g, d;
Eigen::MatrixXd G;
LineSearch lineSearch;
obj(x, g);
obj(x, G);
DEBUG_PRINT(x);
DEBUG_PRINT(g);
DEBUG_PRINT(G);
DEBUG_PRINT(G.ldlt().solve(g));
while(g.norm() > 10e-10) {
#if DEBUG
std::cout << std::string(20, '-') << std::endl;
#endif
d = G.ldlt().solve(-g);
DEBUG_PRINT(G.ldlt().solve(-g).transpose());
DEBUG_PRINT(G.ldlt().vectorD());
Real a = 1.;
a = lineSearch(obj, x, d, a);
DEBUG_PRINT(a);
DEBUG_PRINT(obj(x));
x = x + a * d;
obj(x, g);
obj(x, G);
DEBUG_PRINT(x.transpose());
DEBUG_PRINT(g.transpose());
// DEBUG_PRINT(G);
}
return x;
}
void Registration::ICPEnergyMinimization(const Point3DSet* pRef, const Point3DSet* pOrigin, const MagicMath::HomoMatrix4* pTransInit,
std::vector<int>& sampleIndex, std::vector<int>& correspondIndex, MagicMath::HomoMatrix4* pTransDelta)
{
//DebugLog << "Registration::ICPEnergyMinimization" << std::endl;
int pcNum = sampleIndex.size();
Eigen::MatrixXd matA(pcNum, 6);
Eigen::VectorXd vecB(pcNum, 1);
for (int i = 0; i < pcNum; i++)
{
MagicMath::Vector3 norRef = pRef->GetPoint(correspondIndex.at(i))->GetNormal();
MagicMath::Vector3 posRef = pRef->GetPoint(correspondIndex.at(i))->GetPosition();
MagicMath::Vector3 posPC = pTransInit->TransformPoint( pOrigin->GetPoint(sampleIndex.at(i))->GetPosition() );
vecB(i) = (posRef - posPC) * norRef;
MagicMath::Vector3 coffTemp = posPC.CrossProduct(norRef);
matA(i, 0) = coffTemp[0];
matA(i, 1) = coffTemp[1];
matA(i, 2) = coffTemp[2];
matA(i, 3) = norRef[0];
matA(i, 4) = norRef[1];
matA(i, 5) = norRef[2];
}
Eigen::MatrixXd matAT = matA.transpose();
Eigen::MatrixXd matCoefA = matAT * matA;
Eigen::MatrixXd vecCoefB = matAT * vecB;
Eigen::VectorXd res = matCoefA.ldlt().solve(vecCoefB);
pTransDelta->Unit();
pTransDelta->SetValue(0, 1, -res(2));
pTransDelta->SetValue(0, 2, res(1));
pTransDelta->SetValue(0, 3, res(3));
pTransDelta->SetValue(1, 0, res(2));
pTransDelta->SetValue(1, 2, -res(0));
pTransDelta->SetValue(1, 3, res(4));
pTransDelta->SetValue(2, 0, -res(1));
pTransDelta->SetValue(2, 1, res(0));
pTransDelta->SetValue(2, 3, res(5));
//print result
/*DebugLog << "MatA: " << std::endl;
DebugLog << 1 << " " << -res(2) << " " << res(1) << " " << res(3) << std::endl;
DebugLog << res(2) << " " << 1 << " " << -res(0) << " " << res(4) << std::endl;
DebugLog << -res(1) << " " << res(0) << " " << 1 << " " << res(5) << std::endl;*/
}
void TensorJTree::LearnSpectralParameters()
{
for (int i = 0; i < node_list->size(); ++i)
{
node_list->at(i)->MarkMultModes();
FindObservationPartitions(i);
}
for (int i = 0; i < node_list->size(); ++i)
{
node_list->at(i)->MarkObservableRepresentation();
}
for (int i = 0; i < node_list->size(); ++i)
{
cout << i;
cout << "\n";
node_list->at(i)->ComputeObservableRepresentation();
}
for (int i = 0; i < node_list->size(); ++i)
{
node_list->at(i)->FinalizeObservableRepresentation();
}
for (int i = 0; i < node_list->size(); ++i)
{
cout << i;
cout << "\n";
vector<double> big_forward_vec;
// big_forward_vec.reserve(10000);
Matrix big_back_matrix;
// big_back_matrix.Reserve(10000);
bool invModesExist = node_list->at(i)->InvModesExists();
if (invModesExist)
{
vector<Tensor*>& forward_list = node_list->at(i)->GetForwardList();
vector<Tensor*>& extra_list = node_list->at(i)->GetExtraList();
Tensor& X = node_list->at(i)->GetTransformTensor().GetTensor();
vector<int> result_dims = X.Dims();
assert(forward_list.size() == extra_list.size());
vector<int> inv_group_modes = node_list->at(i)->GetInvModeGroup();
vector<int> inv_modes = node_list->at(i)->GetInvModes();
for (int j = 0; j < forward_list.size(); ++j)
{
Tensor& forward_tensor = *(forward_list[j]);
Tensor& back_tensor = *(extra_list[j]);
if (j == 0)
{
Tensor::CreateLinearSystem(big_forward_vec, big_back_matrix, X, back_tensor, forward_tensor, inv_group_modes, inv_modes);
}
else
{
vector<double> forward_vec;
Matrix back_matrix;
Tensor::CreateLinearSystem(forward_vec, back_matrix, X, back_tensor, forward_tensor, inv_group_modes, inv_modes);
VectorPlus::Concat(big_forward_vec, forward_vec);
big_back_matrix.MatrixConcat(back_matrix);
}
}
Eigen::MatrixXd A(big_back_matrix.Dim(0), big_back_matrix.Dim(1));
for (int j = 0; j < big_back_matrix.Dim(0); ++j)
{
for (int k = 0; k < big_back_matrix.Dim(1); ++k)
{
A(j,k) = big_back_matrix.At(j,k);
}
}
Eigen::VectorXd b(big_forward_vec.size());
for (int j = 0; j < big_forward_vec.size(); ++j)
{
b(j) = big_forward_vec.at(j);
}
//cout << "yee";
Eigen::MatrixXd newA = A.transpose() * A;
Eigen::MatrixXd newb = A.transpose() * b;
Eigen::VectorXd res = newA.ldlt().solve(newb);
//cout << "yeeldlt solved";
//Eigen::VectorXd res = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
//cout << "yeesvdsolved";
// solve the linear system here
Tensor result_tensor(result_dims);
for (int j = 0; j < res.size(); ++j)
{
result_tensor.Set(j, res(j));
}
Tensor& curr_result = node_list->at(i)->GetTransformTensor().GetTensor();
// assert(Tensor::Diff(result_tensor, curr_result) < .0001);
node_list->at(i)->SetTransformTensor(result_tensor);
}
}
// average by computing template and see what happens
// hack it so we change the template
/* template_to_cliques = new vector<vector<int>*>();
for (int i = 0; i < NumCliques(); ++i)
{
template_to_cliques->push_back(new vector<int>(1,i));
} */
/*for (int i = 0; i < template_to_cliques->size(); ++i)
{
vector<int>& i_cliques = *(template_to_cliques->at(i));
vector<double> big_forward_vec;
Matrix big_back_matrix;
vector<int> result_dims;
bool invModesExist = node_list->at(i_cliques[0])->InvModesExists();
if (invModesExist)
{
for (int j = 0; j < i_cliques.size(); ++j)
{
if (node_list->at(i_cliques[0])->is_invalid())
continue;
Tensor& forward_tensor = node_list->at(i_cliques[j])->GetForwardTensor();
Tensor& back_tensor = node_list->at(i_cliques[j])->GetUExtraTensor();
Tensor& X = node_list->at(i_cliques[j])->GetTransformTensor().GetTensor();
vector<int>& inv_group_modes = node_list->at(i_cliques[j])->GetInvModeGroup();
vector<int>& inv_modes = node_list->at(i_cliques[j])->GetInvModes();
if (j == 0)
{
Tensor::CreateLinearSystem(big_forward_vec, big_back_matrix, X, back_tensor, forward_tensor, inv_group_modes, inv_modes);
result_dims = X.Dims();
}
else
{
vector<double> forward_vec;
Matrix back_matrix;
Tensor::CreateLinearSystem(forward_vec, back_matrix, X, back_tensor, forward_tensor, inv_group_modes, inv_modes);
VectorPlus::Concat(big_forward_vec, forward_vec);
big_back_matrix.MatrixConcat(back_matrix);
}
}
Eigen::MatrixXd A(big_back_matrix.Dim(0), big_back_matrix.Dim(1));
for (int i = 0; i < big_back_matrix.Dim(0); ++i)
{
for (int j = 0; j < big_back_matrix.Dim(1); ++j)
{
A(i,j) = big_back_matrix.At(i,j);
}
}
Eigen::VectorXd b(big_forward_vec.size());
for (int i = 0; i < big_forward_vec.size(); ++i)
{
b(i) = big_forward_vec.at(i);
}
Eigen::VectorXd res = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
// solve the linear system here
Tensor result_tensor(result_dims);
for (int i = 0; i < res.size(); ++i)
{
result_tensor.Set(i, res(i));
}
Tensor& curr_result = node_list->at(i_cliques[0])->GetTransformTensor().GetTensor();
// assert(Tensor::Diff(result_tensor, curr_result) < .0001);
for (int j = 0; j < i_cliques.size(); ++j)
{
node_list->at(i_cliques[j])->SetTransformTensor(result_tensor);
}
}
} */
/* for (int i = 0; i < template_to_cliques->size(); ++i)
{
vector<int>& i_cliques = *(template_to_cliques->at(i));
TensorCPT* avg_tensor = new TensorCPT(node_list->at(i_cliques[0])->GetTransformTensor());
for (int j = 1; j < i_cliques.size(); ++j)
{
TensorCPT::Add(*avg_tensor, node_list->at(i_cliques[j])->GetTransformTensor());
}
avg_tensor->Divide(i_cliques.size());
for (int j = 0; j < i_cliques.size(); ++j)
{
node_list->at(i_cliques[j])->SetTransformTensor(avg_tensor->GetTensor());
}
delete(avg_tensor);
} */
}
void TensorJTree::LearnTemplateSpectralParameters()
{
for (int i = 0; i < node_list->size(); ++i)
{
node_list->at(i)->MarkMultModes();
FindObservationPartitions(i);
}
for (int i = 0; i < node_list->size(); ++i)
{
node_list->at(i)->MarkObservableRepresentation();
}
for (int i = 0; i < node_list->size(); ++i)
{
cout << i;
cout << "\n";
node_list->at(i)->ComputeObservableRepresentation();
}
cout << "yeee";
cout << "\n";
for (int i = 0; i < node_list->size(); ++i)
{
cout << i;
cout << "\n";
node_list->at(i)->FinalizeTemplateObservableRepresentation();
}
cout << "yeee";
cout << "\n";
/* template_to_cliques = new vector<vector<int>*>();
for (int i = 0; i < NumCliques(); ++i)
{
template_to_cliques->push_back(new vector<int>(1,i));
} */
cout << "yeee";
cout << "\n";
for (int i = 0; i < template_to_cliques->size(); ++i)
{
vector<int>& i_cliques = *(template_to_cliques->at(i));
vector<double> big_forward_vec;
Matrix big_back_matrix;
vector<int> result_dims;
bool invModesExist = node_list->at(i_cliques[0])->InvModesExists();
if (invModesExist)
{
for (int j = 0; j < i_cliques.size(); ++j)
{
cout << j;
cout << "\n";
if (node_list->at(i_cliques[0])->is_invalid())
{
assert(0);
continue;
}
int index = i_cliques[j];
vector<Tensor*>& forward_list = node_list->at(index)->GetForwardList();
vector<Tensor*>& extra_list = node_list->at(index)->GetExtraList();
Tensor& X = node_list->at(index)->GetTransformTensor().GetTensor();
if (j == 0)
{
result_dims = X.Dims();
}
assert(forward_list.size() == extra_list.size());
vector<int> inv_group_modes = node_list->at(index)->GetInvModeGroup();
vector<int> inv_modes = node_list->at(index)->GetInvModes();
for (int k = 0; k < forward_list.size(); ++k)
{
Tensor& forward_tensor = *(forward_list[k]);
Tensor& back_tensor = *(extra_list[k]);
if (j == 0 && k == 0)
{
Tensor::CreateLinearSystem(big_forward_vec, big_back_matrix, X, back_tensor, forward_tensor, inv_group_modes, inv_modes);
}
else
{
vector<double> forward_vec;
Matrix back_matrix;
cout << "yeebeforesystem";
Tensor::CreateLinearSystem(forward_vec, back_matrix, X, back_tensor, forward_tensor, inv_group_modes, inv_modes);
cout << "yeeaftersystem";
VectorPlus::Concat(big_forward_vec, forward_vec);
big_back_matrix.MatrixConcat(back_matrix);
cout << "yeeafterconcat";
}
}
node_list->at(index)->DeleteForwardList();
node_list->at(index)->DeleteExtraList();
}
cout << big_back_matrix.Dim(0);
cout << "\n";
cout << big_back_matrix.Dim(1);
cout << "\n";
Eigen::MatrixXd A(big_back_matrix.Dim(0), big_back_matrix.Dim(1));
cout << "allocatedbigone";
for (int j = 0; j < big_back_matrix.Dim(0); ++j)
{
for (int k = 0; k < big_back_matrix.Dim(1); ++k)
{
(A)(j,k) = big_back_matrix.At(j,k);
}
}
Eigen::VectorXd b(big_forward_vec.size());
for (int j = 0; j < big_forward_vec.size(); ++j)
{
b(j) = big_forward_vec.at(j);
}
cout << "before solve";
cout << "\n";
Eigen::MatrixXd newA = A.transpose() * (A);
Eigen::MatrixXd newb = A.transpose() * b;
Eigen::VectorXd res = newA.ldlt().solve(newb);
cout << "after solve";
cout << "\n";
// solve the linear system here
Tensor result_tensor(result_dims);
for (int j = 0; j < res.size(); ++j)
{
result_tensor.Set(j, res(j));
}
Tensor& curr_result = node_list->at(i_cliques[0])->GetTransformTensor().GetTensor();
// assert(Tensor::Diff(result_tensor, curr_result) < .0001);
for (int j = 0; j < i_cliques.size(); ++j)
{
node_list->at(i_cliques[j])->SetTransformTensor(result_tensor);
}
}
}
}
