void addEdge(int index0, int index1) {
GrPoint p = fVertices[index0];
GrPoint q = fVertices[index1];
fMatrix.mapPoints(&p, 1);
fMatrix.mapPoints(&q, 1);
p = sanitizePoint(p);
q = sanitizePoint(q);
if (p == q) return;
GrDrawState::Edge edge = computeEdge(p, q, 1.0f);
fEdges[index0 * 2 + 1] = edge;
fEdges[index1 * 2] = edge;
}
static size_t computeEdgesAndIntersect(const GrMatrix& matrix,
const GrMatrix& inverse,
GrPoint* vertices,
size_t numVertices,
GrEdgeArray* edges,
float sign) {
if (numVertices < 3) {
return 0;
}
matrix.mapPoints(vertices, numVertices);
if (sign == 0.0f) {
sign = isCCW(vertices, numVertices) ? -1.0f : 1.0f;
}
GrPoint p = sanitizePoint(vertices[numVertices - 1]);
for (size_t i = 0; i < numVertices; ++i) {
GrPoint q = sanitizePoint(vertices[i]);
if (p == q) {
continue;
}
GrDrawState::Edge edge = computeEdge(p, q, sign);
edge.fZ += 0.5f; // Offset by half a pixel along the tangent.
*edges->append() = edge;
p = q;
}
int count = edges->count();
if (count == 0) {
return 0;
}
GrDrawState::Edge prev_edge = edges->at(0);
for (int i = 0; i < count; ++i) {
GrDrawState::Edge edge = edges->at(i < count - 1 ? i + 1 : 0);
if (parallel(edge, prev_edge)) {
// 3 points are collinear; offset by half the tangent instead
vertices[i].fX -= edge.fX * 0.5f;
vertices[i].fY -= edge.fY * 0.5f;
} else {
vertices[i] = prev_edge.intersect(edge);
}
inverse.mapPoints(&vertices[i], 1);
prev_edge = edge;
}
return edges->count();
}
bool Optimize::eval_jac_g(Ipopt::Index n, const Ipopt::Number *x, bool new_x,
Ipopt::Index m, Ipopt::Index nele_jac,
Ipopt::Index *iRow, Ipopt::Index *jCol, Ipopt::Number *values)
{
UNUSED(n);
UNUSED(m);
UNUSED(new_x);
Ipopt::Number e[3];
int idx_nv, idx_pv;
int idx = 0;
if (values == NULL)
{
int i = 0, size = edges.size();
for (; i < size; i++)
{
idx_pv = idxVertexEdges[i].first;
idx_nv = idxVertexEdges[i].second;
if (idx_pv >= 0)
{
iRow[idx] = i;
iRow[idx + 1] = i;
iRow[idx + 2] = i;
jCol[idx] = 3 * idx_pv;
jCol[idx + 1] = 3 * idx_pv + 1;
jCol[idx + 2] = 3 * idx_pv + 2;
idx += 3;
}
if (idx_nv >= 0)
{
iRow[idx] = i;
iRow[idx + 1] = i;
iRow[idx + 2] = i;
jCol[idx] = 3 * idx_nv;
jCol[idx + 1] = 3 * idx_nv + 1;
jCol[idx + 2] = 3 * idx_nv + 2;
idx += 3;
}
}
int ref;
int end = crosses.size();
i = 0;
for (; i < end; ++i)
{
for (ref = 1; ref < refSize; ++ref)
{
setJacGPos_cross(ref, size, i, idx, iRow, jCol);
}
}
for (; idx < nele_jac; idx++)
{
iRow[idx] = 0;
jCol[idx] = 0;
}
}
else
{
if (updateCross(x) == false) return false;
setZeros(values, nele_jac);
int i = 0, size = edges.size();
for (; i < size; i++)
{
computeEdge(i, x, e);
idx_pv = idxVertexEdges[i].first;
idx_nv = idxVertexEdges[i].second;
if (idx_pv >= 0)
{
multiplyByScale(e, -2, values + idx);
idx += 3;
}
if (idx_nv >= 0)
{
multiplyByScale(e, 2, values + idx);
idx += 3;
}
}
int ref;
int end = crosses.size();
i = 0;
for (; i < end; ++i)
{
for (ref = 1; ref < refSize; ++ref)
{
setJacGVal_cross(ref, i, idx, values, x);
}
}
}
assert(idx <= nele_jac);
return true;
}
bool Optimize::eval_h(Ipopt::Index n, const Ipopt::Number *x,
bool new_x, Ipopt::Number obj_factor,
Ipopt::Index m, const Ipopt::Number *lambda, bool new_lambda,
Ipopt::Index nele_hess, Ipopt::Index *iRow, Ipopt::Index *jCol, Ipopt::Number *values)
{
UNUSED(n);
UNUSED(m);
UNUSED(new_lambda);
UNUSED(new_x);
int idx_pv, idx_nv, idx_cv;
Ipopt::Number e[3];
int idx = 0;
int i, size;
int idx_pc;
if (values == NULL)
{
i = 0, size = edges.size();
for (; i < size; i++)
{
idx_pv = idxVertexEdges[i].first;
idx_nv = idxVertexEdges[i].second;
if (idx_pv >= 0)
{
setHessianPos(idx_pv, idx_pv, iRow, jCol, idx);
if (idx_nv > idx_pv)
{
setHessianPos(idx_nv, idx_pv, iRow, jCol, idx);
}
}
if (idx_nv >= 0)
{
setHessianPos(idx_nv, idx_nv, iRow, jCol, idx);
if (idx_pv > idx_nv)
{
setHessianPos(idx_pv, idx_nv, iRow, jCol, idx);
}
}
}
i = 0;
size = PositionConstraints.size();
for (; i < size; i++)
{
idx_pc = PositionConstraints[i];
setHessianPos(idx_pc, idx_pc, iRow, jCol, idx);
}
//Plane Constraints
i = 0;
size = PlaneConstraints.size();
for (; i < size; ++i)
{
idx_pc = PlaneConstraints[i];
setHessianPos_bending(idx_pc, idx_pc, iRow, jCol, idx);
}
//Bending energy
i = 0;
size = EnergyVertices.size();
for (; i < size; ++i)
{
idx_pv = idxPrevVertex[i];
idx_nv = idxNextVertex[i];
idx_cv = idxTheVertex[i];
if (idx_pv >= 0)
{
setHessianPos_bending(idx_pv, idx_pv, iRow, jCol, idx);
if (idx_cv > idx_pv)
{
setHessianPos_bending(idx_cv, idx_pv, iRow, jCol, idx);
}
if (idx_nv > idx_pv)
{
setHessianPos_bending(idx_nv, idx_pv, iRow, jCol, idx);
}
}
if (idx_nv >= 0)
{
setHessianPos_bending(idx_nv, idx_nv, iRow, jCol, idx);
if (idx_pv > idx_nv)
{
setHessianPos_bending(idx_pv, idx_nv, iRow, jCol, idx);
}
if (idx_cv > idx_nv)
{
setHessianPos_bending(idx_cv, idx_nv, iRow, jCol, idx);
}
}
if (idx_cv >= 0)
{
setHessianPos_bending(idx_cv, idx_cv, iRow, jCol, idx);
if (idx_pv > idx_cv)
{
setHessianPos_bending(idx_pv, idx_cv, iRow, jCol, idx);
}
if (idx_nv > idx_cv)
{
setHessianPos_bending(idx_nv, idx_cv, iRow, jCol, idx);
}
}
}
int ref_E, ref_F, p, q;
i = 0;
size = crosses.size();
for (; i < size; ++i)
{
for (ref_E = 1; ref_E < refSize - 1; ++ref_E)
{
if (crossE[i][ref_E] >= 0 && crossE[i][ref_E + 1] >= 0)
{
setHessianPos_SkewSym(
crossE[i][ref_E],
crossE[i][ref_E + 1],
iRow, jCol, idx);
}
}
for (ref_F = 1; ref_F < refSize - 1; ++ref_F)
{
if (crossF[i][ref_F] >= 0 && crossF[i][ref_F + 1] >= 0)
{
setHessianPos_SkewSym(
crossF[i][ref_F],
crossF[i][ref_F + 1],
iRow, jCol, idx);
}
}
for (ref_E = 1; ref_E < refSize; ++ref_E)
{
p = crossE[i][ref_E];
if (p < 0) continue;
for (ref_F = 1; ref_F < refSize; ++ref_F)
{
q = crossF[i][ref_F];
if (q >= 0)
{
setHessianPos_SkewSym(p, q, iRow, jCol, idx);
}
}
}
}
for (; idx < nele_hess; idx++)
{
iRow[idx] = 0;
jCol[idx] = 0;
}
}
else
{
if (updateCross(x) == false) return false;
double tmp = 0;
setZeros(values, nele_hess);
i = 0, size = edges.size();
for (; i < size; i++)
{
computeEdge(i, x, e);
idx_pv = idxVertexEdges[i].first;
idx_nv = idxVertexEdges[i].second;
if (idx_pv >= 0)
{
setHessianValues(idx, values, 2 * lambda[i]);
if (idx_nv > idx_pv)
{
setHessianValues(idx, values, -2 * lambda[i]);
}
}
if (idx_nv >= 0)
{
setHessianValues(idx, values, 2 * lambda[i]);
if (idx_pv > idx_nv)
{
setHessianValues(idx, values, -2 * lambda[i]);
}
}
}
i = 0;
size = PositionConstraints.size();
tmp = 2 * pOp->PositionConstraintsWeight * obj_factor;
for (; i < size; i++)
{
setHessianValues(idx, values, tmp);
}
//Plane constraints
Point P;
i = 0;
size = PlaneConstraints.size();
for (; i < size; ++i)
{
P = PlaneConstraintsInfo[i].first;
setHessianVal_bending_helper(P * 2 * obj_factor * pOp->PlaneConstraintsCoef, P, values + idx, 0);
idx += 6;
}
//Bending energy
i = 0;
size = EnergyVertices.size();
for (; i < size; i++)
{
tmp = 2 * obj_factor * pOp->BendingEnergyCoef / VerticesWeight[i];
idx_pv = idxPrevVertex[i];
idx_nv = idxNextVertex[i];
idx_cv = idxTheVertex[i];
if (idx_pv >= 0)
{
setHessianValues_bending(i, idx_pv, idx_pv, idx, values, x, tmp);
if (idx_cv > idx_pv)
{
setHessianValues_bending(i, idx_cv, idx_pv, idx, values, x, tmp);
}
if (idx_nv > idx_pv)
{
setHessianValues_bending(i, idx_nv, idx_pv, idx, values, x, tmp);
}
}
if (idx_nv >= 0)
{
setHessianValues_bending(i, idx_nv, idx_nv, idx, values, x, tmp);
if (idx_pv > idx_nv)
{
setHessianValues_bending(i, idx_pv, idx_nv, idx, values, x, tmp);
}
if (idx_cv > idx_nv)
{
setHessianValues_bending(i, idx_cv, idx_nv, idx, values, x, tmp);
}
}
if (idx_cv >= 0)
{
setHessianValues_bending(i, idx_cv, idx_cv, idx, values, x, tmp);
if (idx_pv > idx_cv)
{
setHessianValues_bending(i, idx_pv, idx_cv, idx, values, x, tmp);
}
if (idx_nv > idx_cv)
{
setHessianValues_bending(i, idx_nv, idx_cv, idx, values, x, tmp);
}
}
}
//crossing constraints
int ref_E, ref_F, p, q;
int base = edges.size();
i = 0;
size = crosses.size();
for (; i < size; ++i)
{
for (ref_E = 1; ref_E < refSize - 1; ++ref_E)
{
if (crossE[i][ref_E] >= 0 && crossE[i][ref_E + 1] >= 0)
{
setHessianValues_SkewSym(i, crossE[i][ref_E], crossE[i][ref_E + 1], values, idx, x, lambda[base + i]);
}
}
for (ref_F = 1; ref_F < refSize - 1; ++ref_F)
{
if (crossF[i][ref_F] >= 0 && crossF[i][ref_F + 1] >= 0)
{
setHessianValues_SkewSym(i, crossF[i][ref_F], crossF[i][ref_F + 1], values, idx, x, lambda[base + i]);
}
}
for (ref_E = 1; ref_E < refSize; ++ref_E)
{
p = crossE[i][ref_E];
if (p < 0) continue;
for (ref_F = 1; ref_F < refSize; ++ref_F)
{
q = crossF[i][ref_F];
if (q >= 0)
{
setHessianValues_SkewSym(i, p, q, values, idx, x, lambda[base + i]);
}
}
}
}
}
assert(idx <= nele_hess);
return true;
}
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;
}
}
