/** * <p> * Each angle should always be increasing or decreasing. If this order is broken * at any point then the point is not visible from the current point of view. * </p> * <p/> * <p> * See section 3.2 of: Feng Lu and Evangelos Milios, "Robot Pose Estimation in Unknown Environments by Matching 2D Range Scans" * Journal of Intelligent and Robotics Systems, 18: 249-275, 1997. * </p> */ protected void checkVisibleByDeltaAngle(ScanInfo info) { final int N = param.getNumberOfScans(); boolean increasing = param.getAngleIncrement() > 0; double ang[] = info.theta; for (int i = 1; i < N; i++) { if (!info.vis[i]) continue; double deltaAng = UtilAngle.minus(ang[i], ang[i - 1]); if (increasing) { if (deltaAng < 0) info.vis[i] = false; } else { if (deltaAng > 0) info.vis[i] = false; } } }
public void process(double[] parameters, double[] functions) { updateState(parameters); int index = 0; for( ; index < parameters.length; index++) { if(index <3) functions[index] = parameterChangePenalty*Math.abs(parameters[index]-originalParam[index]); else functions[index] = parameterChangePenalty*Math.abs(UtilAngle.minus(parameters[index],originalParam[index])); } endEffectorFrame.getTransformToDesiredFrame(actualTransform, baseFrame); extractTandR(actualTransform, actualT, actualR); functions[index+0] = actualT.getX() - desiredT.getX(); functions[index+1] = actualT.getY() - desiredT.getY(); functions[index+2] = actualT.getZ() - desiredT.getZ(); if (solveOrientation) { functions[index+3] = orientationDiscount * UtilAngle.minus(actualR.getX(), desiredR.getX()); functions[index+4] = orientationDiscount * UtilAngle.minus(actualR.getY(), desiredR.getY()); functions[index+5] = orientationDiscount * UtilAngle.minus(actualR.getZ(), desiredR.getZ()); } } }
public void process(double[] parameters, double[] functions) { updateState(parameters); int index = 0; for( ; index < parameters.length; index++) { if(index <3) functions[index] = parameterChangePenalty*Math.abs(parameters[index]-originalParam[index]); else functions[index] = parameterChangePenalty*Math.abs(UtilAngle.minus(parameters[index],originalParam[index])); } endEffectorFrame.getTransformToDesiredFrame(actualTransform, baseFrame); extractTandR(actualTransform, actualT, actualR); functions[index+0] = actualT.getX() - desiredT.getX(); functions[index+1] = actualT.getY() - desiredT.getY(); functions[index+2] = actualT.getZ() - desiredT.getZ(); if (solveOrientation) { functions[index+3] = orientationDiscount * UtilAngle.minus(actualR.getX(), desiredR.getX()); functions[index+4] = orientationDiscount * UtilAngle.minus(actualR.getY(), desiredR.getY()); functions[index+5] = orientationDiscount * UtilAngle.minus(actualR.getZ(), desiredR.getZ()); } } }