Skip to content

Commit

Permalink
fix: add a param use line search (#46)
Browse files Browse the repository at this point in the history 
* Added a parameter "use_line_search"

Signed-off-by: Shintaro Sakoda <[email protected]>

* Added default value use_line_search=false

Signed-off-by: Shintaro Sakoda <[email protected]>

---------

Signed-off-by: Shintaro Sakoda <[email protected]>
  • Loading branch information
@SakodaShintaro
SakodaShintaro authored Mar 15, 2024
1 parent 44ea634 commit 110d4e5
Show file tree
Hide file tree
Showing 3 changed files with 144 additions and 170 deletions.
155 changes: 70 additions & 85 deletions include/multigrid_pclomp/multigrid_ndt_omp_impl.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -151,6 +151,7 @@ MultiGridNormalDistributionsTransform<PointSource, PointTarget>::MultiGridNormal
params_.search_method = DIRECT7;
params_.num_threads = omp_get_max_threads();
params_.regularization_scale_factor = 0.0f;
params_.use_line_search = false;

double gauss_c1, gauss_c2;

Expand Down Expand Up @@ -879,91 +880,75 @@ double MultiGridNormalDistributionsTransform<PointSource, PointTarget>::computeS
// If you have any suggestions on how to solve these issues, we would appreciate it if you could contribute.
// --------------------------------------------------------------------------------------------------------------------------------

// // Calculate phi(alpha_t)
// double phi_t = -score;
// // Calculate phi'(alpha_t)
// double d_phi_t = -(score_gradient.dot (step_dir));

// // Calculate psi(alpha_t)
// double psi_t = auxiliaryFunction_PsiMT (a_t, phi_t, phi_0, d_phi_0, mu);
// // Calculate psi'(alpha_t)
// double d_psi_t = auxiliaryFunction_dPsiMT (d_phi_t, d_phi_0, mu);

// // Iterate until max number of iterations, interval convergence or a value satisfies the sufficient decrease, Equation 1.1, and curvature condition, Equation 1.2 [More, Thuente 1994]
// while (!interval_converged && step_iterations < max_step_iterations && !(psi_t <= 0 /*Sufficient Decrease*/ && d_phi_t <= -nu * d_phi_0 /*Curvature Condition*/))
// {
// // Use auxiliary function if interval I is not closed
// if (open_interval)
// {
// a_t = trialValueSelectionMT (a_l, f_l, g_l,
// a_u, f_u, g_u,
// a_t, psi_t, d_psi_t);
// }
// else
// {
// a_t = trialValueSelectionMT (a_l, f_l, g_l,
// a_u, f_u, g_u,
// a_t, phi_t, d_phi_t);
// }

// a_t = std::min (a_t, step_max);
// a_t = std::max (a_t, step_min);

// x_t = x + step_dir * a_t;

// final_transformation_ = (Eigen::Translation<float, 3> (static_cast<float> (x_t (0)), static_cast<float> (x_t (1)), static_cast<float> (x_t (2))) *
// Eigen::AngleAxis<float> (static_cast<float> (x_t (3)), Eigen::Vector3f::UnitX ()) *
// Eigen::AngleAxis<float> (static_cast<float> (x_t (4)), Eigen::Vector3f::UnitY ()) *
// Eigen::AngleAxis<float> (static_cast<float> (x_t (5)), Eigen::Vector3f::UnitZ ())).matrix ();

// // New transformed point cloud
// // Done on final cloud to prevent wasted computation
// transformPointCloud (*input_, trans_cloud, final_transformation_);

// // Updates score, gradient. Values stored to prevent wasted computation.
// score = computeDerivatives (score_gradient, hessian, trans_cloud, x_t, false);

// // Calculate phi(alpha_t+)
// phi_t = -score;
// // Calculate phi'(alpha_t+)
// d_phi_t = -(score_gradient.dot (step_dir));

// // Calculate psi(alpha_t+)
// psi_t = auxiliaryFunction_PsiMT (a_t, phi_t, phi_0, d_phi_0, mu);
// // Calculate psi'(alpha_t+)
// d_psi_t = auxiliaryFunction_dPsiMT (d_phi_t, d_phi_0, mu);

// // Check if I is now a closed interval
// if (open_interval && (psi_t <= 0 && d_psi_t >= 0))
// {
// open_interval = false;

// // Converts f_l and g_l from psi to phi
// f_l = f_l + phi_0 - mu * d_phi_0 * a_l;
// g_l = g_l + mu * d_phi_0;

// // Converts f_u and g_u from psi to phi
// f_u = f_u + phi_0 - mu * d_phi_0 * a_u;
// g_u = g_u + mu * d_phi_0;
// }

// if (open_interval)
// {
// // Update interval end points using Updating Algorithm [More, Thuente 1994]
// interval_converged = updateIntervalMT (a_l, f_l, g_l,
// a_u, f_u, g_u,
// a_t, psi_t, d_psi_t);
// }
// else
// {
// // Update interval end points using Modified Updating Algorithm [More, Thuente 1994]
// interval_converged = updateIntervalMT (a_l, f_l, g_l,
// a_u, f_u, g_u,
// a_t, phi_t, d_phi_t);
// }

// step_iterations++;
// }
if(params_.use_line_search) {
// Calculate phi(alpha_t)
double phi_t = -score;
// Calculate phi'(alpha_t)
double d_phi_t = -(score_gradient.dot(step_dir));

// Calculate psi(alpha_t)
double psi_t = auxiliaryFunction_PsiMT(a_t, phi_t, phi_0, d_phi_0, mu);
// Calculate psi'(alpha_t)
double d_psi_t = auxiliaryFunction_dPsiMT(d_phi_t, d_phi_0, mu);

// Iterate until max number of iterations, interval convergence or a value satisfies the sufficient decrease, Equation 1.1, and curvature condition, Equation 1.2 [More, Thuente 1994]
while(!interval_converged && step_iterations < max_step_iterations && !(psi_t <= 0 /*Sufficient Decrease*/ && d_phi_t <= -nu * d_phi_0 /*Curvature Condition*/)) {
// Use auxiliary function if interval I is not closed
if(open_interval) {
a_t = trialValueSelectionMT(a_l, f_l, g_l, a_u, f_u, g_u, a_t, psi_t, d_psi_t);
} else {
a_t = trialValueSelectionMT(a_l, f_l, g_l, a_u, f_u, g_u, a_t, phi_t, d_phi_t);
}

a_t = std::min(a_t, step_max);
a_t = std::max(a_t, step_min);

x_t = x + step_dir * a_t;

final_transformation_ = (Eigen::Translation<float, 3>(static_cast<float>(x_t(0)), static_cast<float>(x_t(1)), static_cast<float>(x_t(2))) * Eigen::AngleAxis<float>(static_cast<float>(x_t(3)), Eigen::Vector3f::UnitX()) * Eigen::AngleAxis<float>(static_cast<float>(x_t(4)), Eigen::Vector3f::UnitY()) * Eigen::AngleAxis<float>(static_cast<float>(x_t(5)), Eigen::Vector3f::UnitZ()))
.matrix();

// New transformed point cloud
// Done on final cloud to prevent wasted computation
transformPointCloud(*input_, trans_cloud, final_transformation_);

// Updates score, gradient. Values stored to prevent wasted computation.
score = computeDerivatives(score_gradient, hessian, trans_cloud, x_t, false);

// Calculate phi(alpha_t+)
phi_t = -score;
// Calculate phi'(alpha_t+)
d_phi_t = -(score_gradient.dot(step_dir));

// Calculate psi(alpha_t+)
psi_t = auxiliaryFunction_PsiMT(a_t, phi_t, phi_0, d_phi_0, mu);
// Calculate psi'(alpha_t+)
d_psi_t = auxiliaryFunction_dPsiMT(d_phi_t, d_phi_0, mu);

// Check if I is now a closed interval
if(open_interval && (psi_t <= 0 && d_psi_t >= 0)) {
open_interval = false;

// Converts f_l and g_l from psi to phi
f_l = f_l + phi_0 - mu * d_phi_0 * a_l;
g_l = g_l + mu * d_phi_0;

// Converts f_u and g_u from psi to phi
f_u = f_u + phi_0 - mu * d_phi_0 * a_u;
g_u = g_u + mu * d_phi_0;
}

if(open_interval) {
// Update interval end points using Updating Algorithm [More, Thuente 1994]
interval_converged = updateIntervalMT(a_l, f_l, g_l, a_u, f_u, g_u, a_t, psi_t, d_psi_t);
} else {
// Update interval end points using Modified Updating Algorithm [More, Thuente 1994]
interval_converged = updateIntervalMT(a_l, f_l, g_l, a_u, f_u, g_u, a_t, phi_t, d_phi_t);
}

step_iterations++;
}
}

// If inner loop was run then hessian needs to be calculated.
// Hessian is unnecessary for step length determination but gradients are required
Expand Down
155 changes: 70 additions & 85 deletions include/pclomp/ndt_omp_impl.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -55,6 +55,7 @@ pclomp::NormalDistributionsTransform<PointSource, PointTarget>::NormalDistributi
params_.search_method = DIRECT7;
params_.num_threads = omp_get_max_threads();
params_.regularization_scale_factor = 0.0f;
params_.use_line_search = false;

double gauss_c1, gauss_c2;

Expand Down Expand Up @@ -833,91 +834,75 @@ double pclomp::NormalDistributionsTransform<PointSource, PointTarget>::computeSt
// If you have any suggestions on how to solve these issues, we would appreciate it if you could contribute.
// --------------------------------------------------------------------------------------------------------------------------------

// // Calculate phi(alpha_t)
// double phi_t = -score;
// // Calculate phi'(alpha_t)
// double d_phi_t = -(score_gradient.dot (step_dir));

// // Calculate psi(alpha_t)
// double psi_t = auxiliaryFunction_PsiMT (a_t, phi_t, phi_0, d_phi_0, mu);
// // Calculate psi'(alpha_t)
// double d_psi_t = auxiliaryFunction_dPsiMT (d_phi_t, d_phi_0, mu);

// // Iterate until max number of iterations, interval convergence or a value satisfies the sufficient decrease, Equation 1.1, and curvature condition, Equation 1.2 [More, Thuente 1994]
// while (!interval_converged && step_iterations < max_step_iterations && !(psi_t <= 0 /*Sufficient Decrease*/ && d_phi_t <= -nu * d_phi_0 /*Curvature Condition*/))
// {
// // Use auxiliary function if interval I is not closed
// if (open_interval)
// {
// a_t = trialValueSelectionMT (a_l, f_l, g_l,
// a_u, f_u, g_u,
// a_t, psi_t, d_psi_t);
// }
// else
// {
// a_t = trialValueSelectionMT (a_l, f_l, g_l,
// a_u, f_u, g_u,
// a_t, phi_t, d_phi_t);
// }

// a_t = std::min (a_t, step_max);
// a_t = std::max (a_t, step_min);

// x_t = x + step_dir * a_t;

// final_transformation_ = (Eigen::Translation<float, 3> (static_cast<float> (x_t (0)), static_cast<float> (x_t (1)), static_cast<float> (x_t (2))) *
// Eigen::AngleAxis<float> (static_cast<float> (x_t (3)), Eigen::Vector3f::UnitX ()) *
// Eigen::AngleAxis<float> (static_cast<float> (x_t (4)), Eigen::Vector3f::UnitY ()) *
// Eigen::AngleAxis<float> (static_cast<float> (x_t (5)), Eigen::Vector3f::UnitZ ())).matrix ();

// // New transformed point cloud
// // Done on final cloud to prevent wasted computation
// transformPointCloud (*input_, trans_cloud, final_transformation_);

// // Updates score, gradient. Values stored to prevent wasted computation.
// score = computeDerivatives (score_gradient, hessian, trans_cloud, x_t, false);

// // Calculate phi(alpha_t+)
// phi_t = -score;
// // Calculate phi'(alpha_t+)
// d_phi_t = -(score_gradient.dot (step_dir));

// // Calculate psi(alpha_t+)
// psi_t = auxiliaryFunction_PsiMT (a_t, phi_t, phi_0, d_phi_0, mu);
// // Calculate psi'(alpha_t+)
// d_psi_t = auxiliaryFunction_dPsiMT (d_phi_t, d_phi_0, mu);

// // Check if I is now a closed interval
// if (open_interval && (psi_t <= 0 && d_psi_t >= 0))
// {
// open_interval = false;

// // Converts f_l and g_l from psi to phi
// f_l = f_l + phi_0 - mu * d_phi_0 * a_l;
// g_l = g_l + mu * d_phi_0;

// // Converts f_u and g_u from psi to phi
// f_u = f_u + phi_0 - mu * d_phi_0 * a_u;
// g_u = g_u + mu * d_phi_0;
// }

// if (open_interval)
// {
// // Update interval end points using Updating Algorithm [More, Thuente 1994]
// interval_converged = updateIntervalMT (a_l, f_l, g_l,
// a_u, f_u, g_u,
// a_t, psi_t, d_psi_t);
// }
// else
// {
// // Update interval end points using Modified Updating Algorithm [More, Thuente 1994]
// interval_converged = updateIntervalMT (a_l, f_l, g_l,
// a_u, f_u, g_u,
// a_t, phi_t, d_phi_t);
// }

// step_iterations++;
// }
if(params_.use_line_search) {
// Calculate phi(alpha_t)
double phi_t = -score;
// Calculate phi'(alpha_t)
double d_phi_t = -(score_gradient.dot(step_dir));

// Calculate psi(alpha_t)
double psi_t = auxiliaryFunction_PsiMT(a_t, phi_t, phi_0, d_phi_0, mu);
// Calculate psi'(alpha_t)
double d_psi_t = auxiliaryFunction_dPsiMT(d_phi_t, d_phi_0, mu);

// Iterate until max number of iterations, interval convergence or a value satisfies the sufficient decrease, Equation 1.1, and curvature condition, Equation 1.2 [More, Thuente 1994]
while(!interval_converged && step_iterations < max_step_iterations && !(psi_t <= 0 /*Sufficient Decrease*/ && d_phi_t <= -nu * d_phi_0 /*Curvature Condition*/)) {
// Use auxiliary function if interval I is not closed
if(open_interval) {
a_t = trialValueSelectionMT(a_l, f_l, g_l, a_u, f_u, g_u, a_t, psi_t, d_psi_t);
} else {
a_t = trialValueSelectionMT(a_l, f_l, g_l, a_u, f_u, g_u, a_t, phi_t, d_phi_t);
}

a_t = std::min(a_t, step_max);
a_t = std::max(a_t, step_min);

x_t = x + step_dir * a_t;

final_transformation_ = (Eigen::Translation<float, 3>(static_cast<float>(x_t(0)), static_cast<float>(x_t(1)), static_cast<float>(x_t(2))) * Eigen::AngleAxis<float>(static_cast<float>(x_t(3)), Eigen::Vector3f::UnitX()) * Eigen::AngleAxis<float>(static_cast<float>(x_t(4)), Eigen::Vector3f::UnitY()) * Eigen::AngleAxis<float>(static_cast<float>(x_t(5)), Eigen::Vector3f::UnitZ()))
.matrix();

// New transformed point cloud
// Done on final cloud to prevent wasted computation
transformPointCloud(*input_, trans_cloud, final_transformation_);

// Updates score, gradient. Values stored to prevent wasted computation.
score = computeDerivatives(score_gradient, hessian, trans_cloud, x_t, false);

// Calculate phi(alpha_t+)
phi_t = -score;
// Calculate phi'(alpha_t+)
d_phi_t = -(score_gradient.dot(step_dir));

// Calculate psi(alpha_t+)
psi_t = auxiliaryFunction_PsiMT(a_t, phi_t, phi_0, d_phi_0, mu);
// Calculate psi'(alpha_t+)
d_psi_t = auxiliaryFunction_dPsiMT(d_phi_t, d_phi_0, mu);

// Check if I is now a closed interval
if(open_interval && (psi_t <= 0 && d_psi_t >= 0)) {
open_interval = false;

// Converts f_l and g_l from psi to phi
f_l = f_l + phi_0 - mu * d_phi_0 * a_l;
g_l = g_l + mu * d_phi_0;

// Converts f_u and g_u from psi to phi
f_u = f_u + phi_0 - mu * d_phi_0 * a_u;
g_u = g_u + mu * d_phi_0;
}

if(open_interval) {
// Update interval end points using Updating Algorithm [More, Thuente 1994]
interval_converged = updateIntervalMT(a_l, f_l, g_l, a_u, f_u, g_u, a_t, psi_t, d_psi_t);
} else {
// Update interval end points using Modified Updating Algorithm [More, Thuente 1994]
interval_converged = updateIntervalMT(a_l, f_l, g_l, a_u, f_u, g_u, a_t, phi_t, d_phi_t);
}

step_iterations++;
}
}

// If inner loop was run then hessian needs to be calculated.
// Hessian is unnecessary for step length determination but gradients are required
Expand Down
4 changes: 4 additions & 0 deletions include/pclomp/ndt_struct.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -89,6 +89,10 @@ struct NdtParams {
NeighborSearchMethod search_method;
int num_threads;
float regularization_scale_factor;

// line search is false by default
// "use_lines_search = true" is not tested well
bool use_line_search = false;
};

} // namespace pclomp
Expand Down

0 comments on commit 110d4e5

Please sign in to comment.