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; } }