template <typename PointSource, typename PointTarget> inline void
pcl::registration::TransformationEstimationPointToPlaneLLS<PointSource, PointTarget>::
estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
const std::vector<int> &indices_src,
const pcl::PointCloud<PointTarget> &cloud_tgt,
const std::vector<int> &indices_tgt,
Eigen::Matrix4f &transformation_matrix)
{
size_t nr_points = indices_src.size ();
if (indices_tgt.size () != nr_points)
{
PCL_ERROR ("[pcl::TransformationEstimationPointToPlaneLLS::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", (unsigned long)indices_src.size (), (unsigned long)indices_tgt.size ());
return;
}
// Approximate as a linear least squares problem
Eigen::MatrixXf A (nr_points, 6);
Eigen::MatrixXf b (nr_points, 1);
for (size_t i = 0; i < nr_points; ++i)
{
const float & sx = cloud_src.points[indices_src[i]].x;
const float & sy = cloud_src.points[indices_src[i]].y;
const float & sz = cloud_src.points[indices_src[i]].z;
const float & dx = cloud_tgt.points[indices_tgt[i]].x;
const float & dy = cloud_tgt.points[indices_tgt[i]].y;
const float & dz = cloud_tgt.points[indices_tgt[i]].z;
const float & nx = cloud_tgt.points[indices_tgt[i]].normal[0];
const float & ny = cloud_tgt.points[indices_tgt[i]].normal[1];
const float & nz = cloud_tgt.points[indices_tgt[i]].normal[2];
A (i, 0) = nz*sy - ny*sz;
A (i, 1) = nx*sz - nz*sx;
A (i, 2) = ny*sx - nx*sy;
A (i, 3) = nx;
A (i, 4) = ny;
A (i, 5) = nz;
b (i, 0) = nx*dx + ny*dy + nz*dz - nx*sx - ny*sy - nz*sz;
}
// Solve A*x = b
Eigen::VectorXf x = A.colPivHouseholderQr ().solve (b);
// Construct the transformation matrix from x
constructTransformationMatrix (x (0), x (1), x (2), x (3), x (4), x (5), transformation_matrix);
}
template <typename PointSource, typename PointTarget> inline void
pcl::registration::TransformationEstimationPointToPlaneLLS<PointSource, PointTarget>::
estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
const pcl::PointCloud<PointTarget> &cloud_tgt,
const pcl::Correspondences &correspondences,
Eigen::Matrix4f &transformation_matrix)
{
size_t nr_points = correspondences.size ();
// Approximate as a linear least squares problem
Eigen::MatrixXf A (nr_points, 6);
Eigen::MatrixXf b (nr_points, 1);
for (size_t i = 0; i < nr_points; ++i)
{
const int & src_idx = correspondences[i].index_query;
const int & tgt_idx = correspondences[i].index_match;
const float & sx = cloud_src.points[src_idx].x;
const float & sy = cloud_src.points[src_idx].y;
const float & sz = cloud_src.points[src_idx].z;
const float & dx = cloud_tgt.points[tgt_idx].x;
const float & dy = cloud_tgt.points[tgt_idx].y;
const float & dz = cloud_tgt.points[tgt_idx].z;
const float & nx = cloud_tgt.points[tgt_idx].normal[0];
const float & ny = cloud_tgt.points[tgt_idx].normal[1];
const float & nz = cloud_tgt.points[tgt_idx].normal[2];
A (i, 0) = nz*sy - ny*sz;
A (i, 1) = nx*sz - nz*sx;
A (i, 2) = ny*sx - nx*sy;
A (i, 3) = nx;
A (i, 4) = ny;
A (i, 5) = nz;
b (i, 0) = nx*dx + ny*dy + nz*dz - nx*sx - ny*sy - nz*sz;
}
// Solve A*x = b
Eigen::VectorXf x = A.colPivHouseholderQr ().solve (b);
// Construct the transformation matrix from x
constructTransformationMatrix (x (0), x (1), x (2), x (3), x (4), x (5), transformation_matrix);
}
template<typename PointT> void
pcl::registration::LUM<PointT>::compute ()
{
int n = static_cast<int> (getNumVertices ());
if (n < 2)
{
PCL_ERROR("[pcl::registration::LUM::compute] The slam graph needs at least 2 vertices.\n");
return;
}
for (int i = 0; i < max_iterations_; ++i)
{
// Linearized computation of C^-1 and C^-1*D and convergence checking for all edges in the graph (results stored in slam_graph_)
typename SLAMGraph::edge_iterator e, e_end;
for (boost::tuples::tie (e, e_end) = edges (*slam_graph_); e != e_end; ++e)
computeEdge (*e);
// Declare matrices G and B
Eigen::MatrixXf G = Eigen::MatrixXf::Zero (6 * (n - 1), 6 * (n - 1));
Eigen::VectorXf B = Eigen::VectorXf::Zero (6 * (n - 1));
// Start at 1 because 0 is the reference pose
for (int vi = 1; vi != n; ++vi)
{
for (int vj = 0; vj != n; ++vj)
{
// Attempt to use the forward edge, otherwise use backward edge, otherwise there was no edge
Edge e;
bool present1, present2;
boost::tuples::tie (e, present1) = edge (vi, vj, *slam_graph_);
if (!present1)
{
boost::tuples::tie (e, present2) = edge (vj, vi, *slam_graph_);
if (!present2)
continue;
}
// Fill in elements of G and B
if (vj > 0)
G.block (6 * (vi - 1), 6 * (vj - 1), 6, 6) = -(*slam_graph_)[e].cinv_;
G.block (6 * (vi - 1), 6 * (vi - 1), 6, 6) += (*slam_graph_)[e].cinv_;
B.segment (6 * (vi - 1), 6) += (present1 ? 1 : -1) * (*slam_graph_)[e].cinvd_;
}
}
// Computation of the linear equation system: GX = B
// TODO investigate accuracy vs. speed tradeoff and find the best solving method for our type of linear equation (sparse)
Eigen::VectorXf X = G.colPivHouseholderQr ().solve (B);
// Update the poses
float sum = 0.0;
for (int vi = 1; vi != n; ++vi)
{
Eigen::Vector6f difference_pose = static_cast<Eigen::Vector6f> (-incidenceCorrection (getPose (vi)).inverse () * X.segment (6 * (vi - 1), 6));
sum += difference_pose.norm ();
setPose (vi, getPose (vi) + difference_pose);
}
// Convergence check
if (sum <= convergence_threshold_ * static_cast<float> (n - 1))
return;
}
}