private void updateIntrinsic() { cameraModel.width = camWidth; cameraModel.height = camHeight; cameraModel.cx = camWidth/2; cameraModel.cy = camHeight/2; double f = (camWidth/2.0)/Math.tan(UtilAngle.degreeToRadian(hfov)/2.0); cameraModel.fx = cameraModel.fy = f; cameraModel.skew = 0; }
public void setVector( Vector3D_F64 v ) { yaw = Math.atan2( v.x , v.z ); tilt = UtilAngle.atanSafe(-v.y,Math.abs(v.z)); }
public void setHorizontalFieldOfView( double radians ) { camera.setFieldOfView(UtilAngle.degree(radians)); }
/** * Given the interpolated index, compute the angle from the 3 indexes. The angle for each index * is computed from the weighted gradients. * @param offset Interpolated index offset relative to index0. range -1 to 1 * @return Interpolated angle. */ double interpolateAngle(int index0, int index1, int index2, double offset) { double angle1 = Math.atan2(histogramY[index1],histogramX[index1]); double deltaAngle; if( offset < 0 ) { double angle0 = Math.atan2(histogramY[index0],histogramX[index0]); deltaAngle = UtilAngle.dist(angle0,angle1); } else { double angle2 = Math.atan2(histogramY[index2], histogramX[index2]); deltaAngle = UtilAngle.dist(angle2,angle1); } return UtilAngle.bound(angle1 + deltaAngle*offset); }
@Override public boolean generate(List<Edgel> dataSet, LinePolar2D_F32 model ) { if( dataSet.size() == 2 ) { Edgel a = dataSet.get(0); Edgel b = dataSet.get(1); float dx = b.x - a.x; float dy = b.y - a.y; // the gradient's orientation is perpendicular to the line's slope double lineAngle = UtilAngle.atanSafe(-dx, dy); // see if their orientations are aligned with the line's angle if(UtilAngle.distHalf(lineAngle, a.theta) > angleTol || UtilAngle.distHalf(lineAngle, b.theta) > angleTol) return false; } // edgel extends Point2D_F32 so this should be legal FitLine_F32.polar((List)dataSet,model); return true; }
public static double distanceHsv( Gaussian2D_F64 model , double hue , double saturation ) { double dx = UtilAngle.dist(hue,model.x); double dy = saturation-model.y; // chi-square = d^T*S*d double tmp0 = dx*model.sxx + dy*model.sxy; double tmp1 = dx*model.sxy + dy*model.syy; return tmp0*dx + tmp1*dy; } }
/** * If there is a nearly perfect line a node farther down the line can come before. This just selects the closest */ void pruneNearlyIdenticalAngles() { for (int i = 0; i < listInfo.size(); i++) { NodeInfo infoN = listInfo.get(i); for (int j = 0; j < infoN.edges.size(); ) { int k = (j+1)%infoN.edges.size; double angularDiff = UtilAngle.dist(infoN.edges.get(j).angle,infoN.edges.get(k).angle); if( angularDiff < UtilAngle.radian(5)) { NodeInfo infoJ = infoN.edges.get(j).target; NodeInfo infoK = infoN.edges.get(k).target; double distJ = infoN.ellipse.center.distance(infoJ.ellipse.center); double distK = infoN.ellipse.center.distance(infoK.ellipse.center); if( distJ < distK ) { infoN.edges.remove(k); } else { infoN.edges.remove(j); } } else { j++; } } } }
private void renderCircleHexagonal(Canvas canvas, int cols, int rows, int centerDistance , int circleDiameter ) { double spaceX = centerDistance/2.0; double spaceY = centerDistance*Math.sin(UtilAngle.radian(60)); for (int row = 0; row < rows; row++) { int y = (int)((rows-row-1)*spaceY); for (int col = row%2; col < cols; col += 2) { int x = (int)(col*spaceX); canvas.drawOval(new RectF(x,y,x+circleDiameter,y+circleDiameter),paintBlack); } } }
private void updateState(double[] parameters) { for (int i = 0; i < parameters.length; i++) { OneDoFJoint oneDoFJoint = oneDoFJoints[i]; // apply constraints to the input parameters double newQ = UtilAngle.bound(parameters[i]); if (Double.isNaN(newQ)) continue; newQ = parameters[i] = MathTools.clipToMinMax(newQ, oneDoFJoint.getJointLimitLower(), oneDoFJoint.getJointLimitUpper()); oneDoFJoint.setQ(newQ); oneDoFJoint.getFrameAfterJoint().update(); } }
/** * <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; } } }
float phi = (float) UtilAngle.radianToDegree(e.phi);
/** * Finds the two edges with the greatest angular distance between them. */ void findLargestAnglesForAllNodes() { for (int i = 0; i < listInfo.size(); i++) { NodeInfo info = listInfo.get(i); if( info.edges.size < 2 ) continue; for (int k = 0, j = info.edges.size-1; k < info.edges.size; j=k,k++) { double angleA = info.edges.get(j).angle; double angleB = info.edges.get(k).angle; double distance = UtilAngle.distanceCCW(angleA,angleB); if( distance > info.angleBetween ) { info.angleBetween = distance; info.left = info.edges.get(j).target; info.right = info.edges.get(k).target; } } } }
/** * Applies trilinear interpolation across the descriptor */ protected void trilinearInterpolation( float weight , float sampleX , float sampleY , double angle , TupleDesc_F64 descriptor ) { for (int i = 0; i < widthGrid; i++) { double weightGridY = 1.0 - Math.abs(sampleY-i); if( weightGridY <= 0) continue; for (int j = 0; j < widthGrid; j++) { double weightGridX = 1.0 - Math.abs(sampleX-j); if( weightGridX <= 0 ) continue; for (int k = 0; k < numHistogramBins; k++) { double angleBin = k*histogramBinWidth; double weightHistogram = 1.0 - UtilAngle.dist(angle,angleBin)/histogramBinWidth; if( weightHistogram <= 0 ) continue; int descriptorIndex = (i*widthGrid + j)*numHistogramBins + k; descriptor.value[descriptorIndex] += weight*weightGridX*weightGridY*weightHistogram; } } } }
int bestFarthest = 0; float targetAngle = UtilAngle.atanSafe(target.slopeY(),target.slopeX()); float cos = (float)Math.cos(targetAngle); float sin = (float)Math.sin(targetAngle); LineSegment2D_F32 c = candidates.get(i); float angle = UtilAngle.atanSafe(c.slopeY(),c.slopeX()); if( UtilAngle.distHalf(targetAngle,angle) > lineSlopeAngleTol ) continue; pt1 = (farthestIndex %2) == 0 ? c.a : c.b; float angleCombined = UtilAngle.atanSafe(pt1.y-pt0.y,pt1.x-pt0.x); if( UtilAngle.distHalf(targetAngle,angleCombined) <= lineSlopeAngleTol ) { bestDistance = distX; bestIndex = i;
@Override public void updateCylinder(int width, int height, double vfov) { synchronized (distorter) { distorter.configure(width,height,(float)UtilAngle.radian(vfov)); distortImage.setModel(distorter); // let it know the transform has changed if( distorted.width != width || distorted.height != height ) { rendered = new BufferedImage(width, height, BufferedImage.TYPE_INT_BGR); distorted.reshape(width, height); } if (inputMethod == InputMethod.IMAGE) { renderOutput(inputCopy); } } }
private void updateState(double[] parameters) { for (int i = 0; i < parameters.length; i++) { OneDoFJointBasics oneDoFJoint = oneDoFJoints[i]; // apply constraints to the input parameters double newQ = UtilAngle.bound(parameters[i]); if (Double.isNaN(newQ)) continue; newQ = parameters[i] = MathTools.clamp(newQ, oneDoFJoint.getJointLimitLower(), oneDoFJoint.getJointLimitUpper()); oneDoFJoint.setQ(newQ); oneDoFJoint.getFrameAfterJoint().update(); } }
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()); } } }
@Override public void onDraw(Canvas canvas, Matrix imageToView) { drawBitmap(canvas,imageToView); canvas.concat(imageToView); synchronized (lockGui) { for(int i = 0; i < ellipsesVis.size; i++ ) { EllipseRotated_F64 ellipse = ellipsesVis.get(i); float phi = (float) UtilAngle.radianToDegree(ellipse.phi); float cx = (float) ellipse.center.x; float cy = (float) ellipse.center.y; float w = (float) ellipse.a; float h = (float) ellipse.b; // really skinny ones are probably just a line and not what the user wants if (w <= 2 || h <= 2) return; canvas.save(); canvas.rotate(phi, cx, cy); r.set(cx - w, cy - h, cx + w + 1, cy + h + 1); canvas.drawOval(r, paint); canvas.restore(); } } } }