void
WRATHRadialGradientValue::
update_pack_values(void)
{
vec2 delta_p(m_p1 - m_p0);
float delta_r(m_r1 - m_r0);
float recipA;
recipA= dot(delta_p, delta_p) - delta_r*delta_r;
m_A=recipA!=0.0f?
1.0f/recipA:
0.0f;
m_A_delta_p= m_A*delta_p;
m_A_r0_delta_r= m_A*m_r0*delta_r;
m_r0_r0=m_r0*m_r0;
}
void learning() {
std::vector<double> delta_p(k), delta_q(k);
for(auto & kv : usr_dct) {
p[kv.second] = random_double_list(k, 0.1);
usr_bias[kv.second] = 0.1 * random_double();
}
for(auto & kv : item_dct) {
q[kv.second] = random_double_list(k, 0.1);
item_bias[kv.second] = 0.1 * random_double();
}
std::cout << "init done" << std::endl;
auto learning_lambda = [&] (const std::string & uid,
const std::string & iid,
double rating) {
double e = rating - estimate(uid, iid);
// compute delta
for(int i = 0; i < k; ++i) {
delta_p[i] = alpha * (2 * e * q[iid][i] - beta * p[uid][i]);
delta_q[i] = alpha * (2 * e * p[uid][i] - beta * q[iid][i]);
}
// update with delta
for(int i = 0; i < k; ++i) {
p[uid][i] += delta_p[i];
q[iid][i] += delta_q[i];
}
// update bias
usr_bias[uid] += alpha * (2 * e - beta * usr_bias[uid]);
item_bias[iid] += alpha * (2 * e - beta * item_bias[iid]);
};
// learning
for(int rd = 0; rd < rounds; ++rd) {
std::cout << "rd:" << rd << " started" << std::endl;
rating_graph.traverse(learning_lambda);
} // end for
}
template<typename PointSource, typename PointTarget> void
pcl::NormalDistributionsTransform<PointSource, PointTarget>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess)
{
nr_iterations_ = 0;
converged_ = false;
double gauss_c1, gauss_c2, gauss_d3;
// Initializes the guassian fitting parameters (eq. 6.8) [Magnusson 2009]
gauss_c1 = 10 * (1 - outlier_ratio_);
gauss_c2 = outlier_ratio_ / pow (resolution_, 3);
gauss_d3 = -log (gauss_c2);
gauss_d1_ = -log ( gauss_c1 + gauss_c2 ) - gauss_d3;
gauss_d2_ = -2 * log ((-log ( gauss_c1 * exp ( -0.5 ) + gauss_c2 ) - gauss_d3) / gauss_d1_);
if (guess != Eigen::Matrix4f::Identity ())
{
// Initialise final transformation to the guessed one
final_transformation_ = guess;
// Apply guessed transformation prior to search for neighbours
transformPointCloud (output, output, guess);
}
// Initialize Point Gradient and Hessian
point_gradient_.setZero ();
point_gradient_.block<3, 3>(0, 0).setIdentity ();
point_hessian_.setZero ();
Eigen::Transform<float, 3, Eigen::Affine, Eigen::ColMajor> eig_transformation;
eig_transformation.matrix () = final_transformation_;
// Convert initial guess matrix to 6 element transformation vector
Eigen::Matrix<double, 6, 1> p, delta_p, score_gradient;
Eigen::Vector3f init_translation = eig_transformation.translation ();
Eigen::Vector3f init_rotation = eig_transformation.rotation ().eulerAngles (0, 1, 2);
p << init_translation (0), init_translation (1), init_translation (2),
init_rotation (0), init_rotation (1), init_rotation (2);
Eigen::Matrix<double, 6, 6> hessian;
double score = 0;
double delta_p_norm;
// Calculate derivates of initial transform vector, subsequent derivative calculations are done in the step length determination.
score = computeDerivatives (score_gradient, hessian, output, p);
while (!converged_)
{
// Store previous transformation
previous_transformation_ = transformation_;
// Solve for decent direction using newton method, line 23 in Algorithm 2 [Magnusson 2009]
Eigen::JacobiSVD<Eigen::Matrix<double, 6, 6> > sv (hessian, Eigen::ComputeFullU | Eigen::ComputeFullV);
// Negative for maximization as opposed to minimization
delta_p = sv.solve (-score_gradient);
//Calculate step length with guarnteed sufficient decrease [More, Thuente 1994]
delta_p_norm = delta_p.norm ();
if (delta_p_norm == 0 || delta_p_norm != delta_p_norm)
{
trans_probability_ = score / static_cast<double> (input_->points.size ());
converged_ = delta_p_norm == delta_p_norm;
return;
}
delta_p.normalize ();
delta_p_norm = computeStepLengthMT (p, delta_p, delta_p_norm, step_size_, transformation_epsilon_ / 2, score, score_gradient, hessian, output);
delta_p *= delta_p_norm;
transformation_ = (Eigen::Translation<float, 3> (static_cast<float> (delta_p (0)), static_cast<float> (delta_p (1)), static_cast<float> (delta_p (2))) *
Eigen::AngleAxis<float> (static_cast<float> (delta_p (3)), Eigen::Vector3f::UnitX ()) *
Eigen::AngleAxis<float> (static_cast<float> (delta_p (4)), Eigen::Vector3f::UnitY ()) *
Eigen::AngleAxis<float> (static_cast<float> (delta_p (5)), Eigen::Vector3f::UnitZ ())).matrix ();
p = p + delta_p;
// Update Visualizer (untested)
if (update_visualizer_ != 0)
update_visualizer_ (output, std::vector<int>(), *target_, std::vector<int>() );
if (nr_iterations_ > max_iterations_ ||
(nr_iterations_ && (std::fabs (delta_p_norm) < transformation_epsilon_)))
{
converged_ = true;
}
nr_iterations_++;
}
// Store transformation probability. The realtive differences within each scan registration are accurate
// but the normalization constants need to be modified for it to be globally accurate
trans_probability_ = score / static_cast<double> (input_->points.size ());
}
