//------------------------------------------------------------------------ void phobos::system::agg::curve3_div::bezier(double x1, double y1, double x2, double y2, double x3, double y3) { m_points.add(point_d(x1, y1)); recursive_bezier(x1, y1, x2, y2, x3, y3, 0); m_points.add(point_d(x3, y3)); }
//------------------------------------------------------------------------ void curve3_div::bezier(double x1, double y1, double x2, double y2, double x3, double y3) { m_points.add(point_d(x1, y1)); recursive_bezier(x1, y1, x2, y2, x3, y3, 0); m_points.add(point_d(x3, y3)); }
//------------------------------------------------------------------------ void curve4_div::bezier(double x1, double y1, double x2, double y2, double x3, double y3, double x4, double y4) { m_points.add(point_d(x1, y1)); recursive_bezier(x1, y1, x2, y2, x3, y3, x4, y4, 0); m_points.add(point_d(x4, y4)); }
//------------------------------------------------------------------------ void vcgen_bspline::add_vertex(double x, double y, unsigned cmd) { m_status = initial; if(is_move_to(cmd)) { m_src_vertices.modify_last(point_d(x, y)); } else { if(is_vertex(cmd)) { m_src_vertices.add(point_d(x, y)); } else { m_closed = get_close_flag(cmd); } } }
//------------------------------------------------------------------------ void curve4_div::recursive_bezier(double x1, double y1, double x2, double y2, double x3, double y3, double x4, double y4, unsigned level) { if(level > curve_recursion_limit) { return; } // Calculate all the mid-points of the line segments //---------------------- double x12 = (x1 + x2) / 2; double y12 = (y1 + y2) / 2; double x23 = (x2 + x3) / 2; double y23 = (y2 + y3) / 2; double x34 = (x3 + x4) / 2; double y34 = (y3 + y4) / 2; double x123 = (x12 + x23) / 2; double y123 = (y12 + y23) / 2; double x234 = (x23 + x34) / 2; double y234 = (y23 + y34) / 2; double x1234 = (x123 + x234) / 2; double y1234 = (y123 + y234) / 2; // Try to approximate the full cubic curve by a single straight line //------------------ double dx = x4-x1; double dy = y4-y1; double d2 = fabs(((x2 - x4) * dy - (y2 - y4) * dx)); double d3 = fabs(((x3 - x4) * dy - (y3 - y4) * dx)); double da1, da2, k; switch((int(d2 > curve_collinearity_epsilon) << 1) + int(d3 > curve_collinearity_epsilon)) { case 0: // All collinear OR p1==p4 //---------------------- k = dx*dx + dy*dy; if(k == 0) { d2 = calc_sq_distance(x1, y1, x2, y2); d3 = calc_sq_distance(x4, y4, x3, y3); } else { k = 1 / k; da1 = x2 - x1; da2 = y2 - y1; d2 = k * (da1*dx + da2*dy); da1 = x3 - x1; da2 = y3 - y1; d3 = k * (da1*dx + da2*dy); if(d2 > 0 && d2 < 1 && d3 > 0 && d3 < 1) { // Simple collinear case, 1---2---3---4 // We can leave just two endpoints return; } if(d2 <= 0) d2 = calc_sq_distance(x2, y2, x1, y1); else if(d2 >= 1) d2 = calc_sq_distance(x2, y2, x4, y4); else d2 = calc_sq_distance(x2, y2, x1 + d2*dx, y1 + d2*dy); if(d3 <= 0) d3 = calc_sq_distance(x3, y3, x1, y1); else if(d3 >= 1) d3 = calc_sq_distance(x3, y3, x4, y4); else d3 = calc_sq_distance(x3, y3, x1 + d3*dx, y1 + d3*dy); } if(d2 > d3) { if(d2 < m_distance_tolerance_square) { m_points.add(point_d(x2, y2)); return; } } else { if(d3 < m_distance_tolerance_square) { m_points.add(point_d(x3, y3)); return; } } break; case 1: // p1,p2,p4 are collinear, p3 is significant //---------------------- if(d3 * d3 <= m_distance_tolerance_square * (dx*dx + dy*dy)) { if(m_angle_tolerance < curve_angle_tolerance_epsilon) { m_points.add(point_d(x23, y23)); return; } // Angle Condition //---------------------- da1 = fabs(atan2(y4 - y3, x4 - x3) - atan2(y3 - y2, x3 - x2)); if(da1 >= pi) da1 = 2*pi - da1; if(da1 < m_angle_tolerance) { m_points.add(point_d(x2, y2)); m_points.add(point_d(x3, y3)); return; } if(m_cusp_limit != 0.0) { if(da1 > m_cusp_limit) { m_points.add(point_d(x3, y3)); return; } } } break; case 2: // p1,p3,p4 are collinear, p2 is significant //---------------------- if(d2 * d2 <= m_distance_tolerance_square * (dx*dx + dy*dy)) { if(m_angle_tolerance < curve_angle_tolerance_epsilon) { m_points.add(point_d(x23, y23)); return; } // Angle Condition //---------------------- da1 = fabs(atan2(y3 - y2, x3 - x2) - atan2(y2 - y1, x2 - x1)); if(da1 >= pi) da1 = 2*pi - da1; if(da1 < m_angle_tolerance) { m_points.add(point_d(x2, y2)); m_points.add(point_d(x3, y3)); return; } if(m_cusp_limit != 0.0) { if(da1 > m_cusp_limit) { m_points.add(point_d(x2, y2)); return; } } } break; case 3: // Regular case //----------------- if((d2 + d3)*(d2 + d3) <= m_distance_tolerance_square * (dx*dx + dy*dy)) { // If the curvature doesn't exceed the distance_tolerance value // we tend to finish subdivisions. //---------------------- if(m_angle_tolerance < curve_angle_tolerance_epsilon) { m_points.add(point_d(x23, y23)); return; } // Angle & Cusp Condition //---------------------- k = atan2(y3 - y2, x3 - x2); da1 = fabs(k - atan2(y2 - y1, x2 - x1)); da2 = fabs(atan2(y4 - y3, x4 - x3) - k); if(da1 >= pi) da1 = 2*pi - da1; if(da2 >= pi) da2 = 2*pi - da2; if(da1 + da2 < m_angle_tolerance) { // Finally we can stop the recursion //---------------------- m_points.add(point_d(x23, y23)); return; } if(m_cusp_limit != 0.0) { if(da1 > m_cusp_limit) { m_points.add(point_d(x2, y2)); return; } if(da2 > m_cusp_limit) { m_points.add(point_d(x3, y3)); return; } } } break; } // Continue subdivision //---------------------- recursive_bezier(x1, y1, x12, y12, x123, y123, x1234, y1234, level + 1); recursive_bezier(x1234, y1234, x234, y234, x34, y34, x4, y4, level + 1); }
//------------------------------------------------------------------------ void curve3_div::recursive_bezier(double x1, double y1, double x2, double y2, double x3, double y3, unsigned level) { if(level > curve_recursion_limit) { return; } // Calculate all the mid-points of the line segments //---------------------- double x12 = (x1 + x2) / 2; double y12 = (y1 + y2) / 2; double x23 = (x2 + x3) / 2; double y23 = (y2 + y3) / 2; double x123 = (x12 + x23) / 2; double y123 = (y12 + y23) / 2; double dx = x3-x1; double dy = y3-y1; double d = fabs(((x2 - x3) * dy - (y2 - y3) * dx)); double da; if(d > curve_collinearity_epsilon) { // Regular case //----------------- if(d * d <= m_distance_tolerance_square * (dx*dx + dy*dy)) { // If the curvature doesn't exceed the distance_tolerance value // we tend to finish subdivisions. //---------------------- if(m_angle_tolerance < curve_angle_tolerance_epsilon) { m_points.add(point_d(x123, y123)); return; } // Angle & Cusp Condition //---------------------- da = fabs(atan2(y3 - y2, x3 - x2) - atan2(y2 - y1, x2 - x1)); if(da >= pi) da = 2*pi - da; if(da < m_angle_tolerance) { // Finally we can stop the recursion //---------------------- m_points.add(point_d(x123, y123)); return; } } } else { // Collinear case //------------------ da = dx*dx + dy*dy; if(da == 0) { d = calc_sq_distance(x1, y1, x2, y2); } else { d = ((x2 - x1)*dx + (y2 - y1)*dy) / da; if(d > 0 && d < 1) { // Simple collinear case, 1---2---3 // We can leave just two endpoints return; } if(d <= 0) d = calc_sq_distance(x2, y2, x1, y1); else if(d >= 1) d = calc_sq_distance(x2, y2, x3, y3); else d = calc_sq_distance(x2, y2, x1 + d*dx, y1 + d*dy); } if(d < m_distance_tolerance_square) { m_points.add(point_d(x2, y2)); return; } } // Continue subdivision //---------------------- recursive_bezier(x1, y1, x12, y12, x123, y123, level + 1); recursive_bezier(x123, y123, x23, y23, x3, y3, level + 1); }
//------------------------------------------------------------------------ void curve4_div::recursive_bezier(double x1, double y1, double x2, double y2, double x3, double y3, double x4, double y4, unsigned level) { if(level > curve_recursion_limit) { return; } // Calculate all the mid-points of the line segments //---------------------- double x12 = (x1 + x2) / 2; double y12 = (y1 + y2) / 2; double x23 = (x2 + x3) / 2; double y23 = (y2 + y3) / 2; double x34 = (x3 + x4) / 2; double y34 = (y3 + y4) / 2; double x123 = (x12 + x23) / 2; double y123 = (y12 + y23) / 2; double x234 = (x23 + x34) / 2; double y234 = (y23 + y34) / 2; double x1234 = (x123 + x234) / 2; double y1234 = (y123 + y234) / 2; // Try to approximate the full cubic curve by a single straight line //------------------ double dx = x4-x1; double dy = y4-y1; double d2 = fabs(((x2 - x4) * dy - (y2 - y4) * dx)); double d3 = fabs(((x3 - x4) * dy - (y3 - y4) * dx)); double da1, da2; switch((int(d2 > curve_collinearity_epsilon) << 1) + int(d3 > curve_collinearity_epsilon)) { case 0: // All collinear OR p1==p4 //---------------------- if(fabs(x1 + x3 - x2 - x2) + fabs(y1 + y3 - y2 - y2) + fabs(x2 + x4 - x3 - x3) + fabs(y2 + y4 - y3 - y3) <= m_distance_tolerance_manhattan) { m_points.add(point_d(x1234, y1234)); return; } break; case 1: // p1,p2,p4 are collinear, p3 is considerable //---------------------- if(d3 * d3 <= m_distance_tolerance_square * (dx*dx + dy*dy)) { if(m_angle_tolerance < curve_angle_tolerance_epsilon) { m_points.add(point_d(x23, y23)); return; } // Angle Condition //---------------------- da1 = fabs(atan2(y4 - y3, x4 - x3) - atan2(y3 - y2, x3 - x2)); if(da1 >= pi) da1 = 2*pi - da1; if(da1 < m_angle_tolerance) { m_points.add(point_d(x2, y2)); m_points.add(point_d(x3, y3)); return; } if(m_cusp_limit != 0.0) { if(da1 > m_cusp_limit) { m_points.add(point_d(x3, y3)); return; } } } break; case 2: // p1,p3,p4 are collinear, p2 is considerable //---------------------- if(d2 * d2 <= m_distance_tolerance_square * (dx*dx + dy*dy)) { if(m_angle_tolerance < curve_angle_tolerance_epsilon) { m_points.add(point_d(x23, y23)); return; } // Angle Condition //---------------------- da1 = fabs(atan2(y3 - y2, x3 - x2) - atan2(y2 - y1, x2 - x1)); if(da1 >= pi) da1 = 2*pi - da1; if(da1 < m_angle_tolerance) { m_points.add(point_d(x2, y2)); m_points.add(point_d(x3, y3)); return; } if(m_cusp_limit != 0.0) { if(da1 > m_cusp_limit) { m_points.add(point_d(x2, y2)); return; } } } break; case 3: // Regular care //----------------- if((d2 + d3)*(d2 + d3) <= m_distance_tolerance_square * (dx*dx + dy*dy)) { // If the curvature doesn't exceed the distance_tolerance value // we tend to finish subdivisions. //---------------------- if(m_angle_tolerance < curve_angle_tolerance_epsilon) { m_points.add(point_d(x23, y23)); return; } // Angle & Cusp Condition //---------------------- double a23 = atan2(y3 - y2, x3 - x2); da1 = fabs(a23 - atan2(y2 - y1, x2 - x1)); da2 = fabs(atan2(y4 - y3, x4 - x3) - a23); if(da1 >= pi) da1 = 2*pi - da1; if(da2 >= pi) da2 = 2*pi - da2; if(da1 + da2 < m_angle_tolerance) { // Finally we can stop the recursion //---------------------- m_points.add(point_d(x23, y23)); return; } if(m_cusp_limit != 0.0) { if(da1 > m_cusp_limit) { m_points.add(point_d(x2, y2)); return; } if(da2 > m_cusp_limit) { m_points.add(point_d(x3, y3)); return; } } } break; } // Continue subdivision //---------------------- recursive_bezier(x1, y1, x12, y12, x123, y123, x1234, y1234, level + 1); recursive_bezier(x1234, y1234, x234, y234, x34, y34, x4, y4, level + 1); }
//------------------------------------------------------------------------ void curve3_div::recursive_bezier(double x1, double y1, double x2, double y2, double x3, double y3, unsigned level) { if(level > curve_recursion_limit) { return; } // Calculate all the mid-points of the line segments //---------------------- double x12 = (x1 + x2) / 2; double y12 = (y1 + y2) / 2; double x23 = (x2 + x3) / 2; double y23 = (y2 + y3) / 2; double x123 = (x12 + x23) / 2; double y123 = (y12 + y23) / 2; double dx = x3-x1; double dy = y3-y1; double d = fabs(((x2 - x3) * dy - (y2 - y3) * dx)); if(d > curve_collinearity_epsilon) { // Regular care //----------------- if(d * d <= m_distance_tolerance_square * (dx*dx + dy*dy)) { // If the curvature doesn't exceed the distance_tolerance value // we tend to finish subdivisions. //---------------------- if(m_angle_tolerance < curve_angle_tolerance_epsilon) { m_points.add(point_d(x123, y123)); return; } // Angle & Cusp Condition //---------------------- double da = fabs(atan2(y3 - y2, x3 - x2) - atan2(y2 - y1, x2 - x1)); if(da >= pi) da = 2*pi - da; if(da < m_angle_tolerance) { // Finally we can stop the recursion //---------------------- m_points.add(point_d(x123, y123)); return; } } } else { if(fabs(x1 + x3 - x2 - x2) + fabs(y1 + y3 - y2 - y2) <= m_distance_tolerance_manhattan) { m_points.add(point_d(x123, y123)); return; } } // Continue subdivision //---------------------- recursive_bezier(x1, y1, x12, y12, x123, y123, level + 1); recursive_bezier(x123, y123, x23, y23, x3, y3, level + 1); }
inline void ICP::UpdateAndReject(Pair& init_f){ double sigma=0.0; double mean=0.0; unsigned int N = data_indices.size() + init_f.size(); Eigen::Vector4d point_s(0.0,0.0,0.0,1.0); Eigen::Vector4d point_d(0.0,0.0,0.0,1.0); std::vector<double> dists(N); //compute mean unsigned int k=0; for (unsigned int i=0 ; i < N; i++){ if(i<data_indices.size()){ point_s(0) = cloud_m->points[ model_indices[i] ].x; point_s(1) = cloud_m->points[ model_indices[i] ].y; point_s(2) = cloud_m->points[ model_indices[i] ].z; point_d(0) = cloud_d->points[ data_indices[i] ].x; point_d(1) = cloud_d->points[ data_indices[i] ].y; point_d(2) = cloud_d->points[ data_indices[i] ].z; }else{ point_s(0) = cloud_m->points[ init_f[k].first ].x; point_s(1) = cloud_m->points[ init_f[k].first ].y; point_s(2) = cloud_m->points[ init_f[k].first ].z; point_d(0) = cloud_d->points[ init_f[k].second ].x; point_d(1) = cloud_d->points[ init_f[k].second ].y; point_d(2) = cloud_d->points[ init_f[k].second ].z; k++; } point_d = T*point_d; dists[i]= (point_d - point_s).norm(); mean = mean + dists[i]; } mean = mean/N; //compute standart diviation for (unsigned int i=0; i < N; i++){ sigma = sigma + (dists[i]-mean)*(dists[i]-mean); } sigma = sigma/N; sigma = sqrt(sigma); //How good is the registration if (mean<D) //very good Dmax = mean + 3*sigma; else if (mean<3*D) //good Dmax = mean + 2*sigma; else if (mean<6*D) //bad Dmax = mean + sigma; else { //very bad std::vector<double> dists2 = dists; sort (dists2.begin(), dists2.end()); if (dists2.size() % 2 == 0) { Dmax = (dists2[dists2.size()/2-1] + dists2[dists2.size()/2]) / 2.0; }else { Dmax = dists2[dists2.size()/2]; } } //Update the maching k=0; unsigned int i=0; for (i=0 ; i <data_indices.size() ; i++){ if (dists[i] < Dmax){ model_indices[k] = model_indices[i]; data_indices[k] = data_indices[i]; k++; } } model_indices.resize(k); data_indices.resize(k); k=0; unsigned int j,l; for (j=i, l=0; j<N ; j++,l++){ if (dists[j] < Dmax){ init_f[k]= init_f[l]; k++; } } if(k!=init_f.size()) init_f.resize(k); }