List of usage examples for org.opencv.calib3d Calib3d solvePnPRansac
public static boolean solvePnPRansac(MatOfPoint3f objectPoints, MatOfPoint2f imagePoints, Mat cameraMatrix, MatOfDouble distCoeffs, Mat rvec, Mat tvec, boolean useExtrinsicGuess, int iterationsCount, float reprojectionError, double confidence, Mat inliers, int flags)
From source file:arlocros.ComputePose.java
License:Apache License
public boolean computePose(Mat rvec, Mat tvec, Mat image2) throws NyARException, FileNotFoundException { // convert image to NyAR style for processing final INyARRgbRaster imageRaster = NyARImageHelper.createFromMat(image2); // create new marker system configuration i_config = new NyARMarkerSystemConfig(i_param); markerSystemState = new NyARMarkerSystem(i_config); // Create wrapper that passes cam pictures to marker system cameraSensorWrapper = new NyARSensor(i_screen_size); ids = new int[markerPatterns.size()]; patternmap = new HashMap<>(); for (int i = 0; i < markerPatterns.size(); i++) { // create marker description from pattern file and add to marker // system ids[i] = markerSystemState.addARMarker(arCodes.get(i), 25, markerConfig.getMarkerSize()); patternmap.put(ids[i], markerPatterns.get(i)); }// w w w . j a v a2 s . c o m cameraSensorWrapper.update(imageRaster); markerSystemState.update(cameraSensorWrapper); // init 3D point list final List<Point3> points3dlist = new ArrayList<>(); final List<Point> points2dlist = new ArrayList<>(); for (final int id : ids) { // process only if this marker has been detected if (markerSystemState.isExistMarker(id) && markerSystemState.getConfidence(id) > 0.7) { // read and add 2D points final NyARIntPoint2d[] vertex2d = markerSystemState.getMarkerVertex2D(id); Point p = new Point(vertex2d[0].x, vertex2d[0].y); points2dlist.add(p); p = new Point(vertex2d[1].x, vertex2d[2].y); points2dlist.add(p); p = new Point(vertex2d[2].x, vertex2d[2].y); points2dlist.add(p); p = new Point(vertex2d[3].x, vertex2d[3].y); points2dlist.add(p); // final MatOfPoint mop = new MatOfPoint(); // mop.fromList(points2dlist); // final List<MatOfPoint> pts = new ArrayList<>(); // pts.add(mop); // read and add corresponding 3D points points3dlist.addAll(markerConfig.create3dpointlist(patternmap.get(id))); if (visualization) { // draw red rectangle around detected marker Core.rectangle(image2, new Point(vertex2d[0].x, vertex2d[0].y), new Point(vertex2d[2].x, vertex2d[2].y), new Scalar(0, 0, 255)); final String markerFile = patternmap.get(id).replaceAll(".*4x4_", "").replace(".patt", ""); Core.putText(image2, markerFile, new Point((vertex2d[2].x + vertex2d[0].x) / 2.0, vertex2d[0].y - 5), 4, 1, new Scalar(250, 0, 0)); } } } // load 2D and 3D points to Mats for solvePNP final MatOfPoint3f objectPoints = new MatOfPoint3f(); objectPoints.fromList(points3dlist); final MatOfPoint2f imagePoints = new MatOfPoint2f(); imagePoints.fromList(points2dlist); if (visualization) { // show image with markers detected Imshow.show(image2); } // do not call solvePNP with empty intput data (no markers detected) if (points2dlist.size() == 0) { objectPoints.release(); imagePoints.release(); return false; } // uncomment these lines if using RANSAC-based pose estimation (more // shaking) Mat inliers = new Mat(); Calib3d.solvePnPRansac(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec, false, 300, 5, 16, inliers, Calib3d.CV_P3P); ArMarkerPoseEstimator.getLog() .info("Points detected: " + points2dlist.size() + " inliers: " + inliers.size()); objectPoints.release(); imagePoints.release(); // avoid publish zero pose if localization failed if (inliers.rows() == 0) { inliers.release(); return false; } inliers.release(); return true; }
From source file:com.github.rosjava_catkin_package_a.ARLocROS.ComputePose.java
License:Apache License
public boolean computePose(Mat rvec, Mat tvec, Mat image2) throws NyARException, FileNotFoundException { // convert image to NyAR style for processing final INyARRgbRaster imageRaster = NyARImageHelper.createFromMat(image2); // create new marker system configuration i_config = new NyARMarkerSystemConfig(i_param); markerSystemState = new NyARMarkerSystem(i_config); // Create wrapper that passes cam pictures to marker system cameraSensorWrapper = new NyARSensor(i_screen_size); ids = new int[markerPatterns.size()]; patternmap = new HashMap<>(); for (int i = 0; i < markerPatterns.size(); i++) { // create marker description from pattern file and add to marker // system ids[i] = markerSystemState.addARMarker(arCodes.get(i), 25, markerConfig.getMarkerSize()); patternmap.put(ids[i], markerPatterns.get(i)); }/* w w w.j a va 2 s . c o m*/ cameraSensorWrapper.update(imageRaster); markerSystemState.update(cameraSensorWrapper); // init 3D point list final List<Point3> points3dlist = new ArrayList<>(); final List<Point> points2dlist = new ArrayList<>(); for (final int id : ids) { // process only if this marker has been detected if (markerSystemState.isExistMarker(id) && markerSystemState.getConfidence(id) > 0.7) { // read and add 2D points final NyARIntPoint2d[] vertex2d = markerSystemState.getMarkerVertex2D(id); Point p = new Point(vertex2d[0].x, vertex2d[0].y); points2dlist.add(p); p = new Point(vertex2d[1].x, vertex2d[2].y); points2dlist.add(p); p = new Point(vertex2d[2].x, vertex2d[2].y); points2dlist.add(p); p = new Point(vertex2d[3].x, vertex2d[3].y); points2dlist.add(p); final MatOfPoint mop = new MatOfPoint(); mop.fromList(points2dlist); final List<MatOfPoint> pts = new ArrayList<>(); pts.add(mop); // read and add corresponding 3D points points3dlist.addAll(markerConfig.create3dpointlist(patternmap.get(id))); // draw red rectangle around detected marker Core.rectangle(image2, new Point(vertex2d[0].x, vertex2d[0].y), new Point(vertex2d[2].x, vertex2d[2].y), new Scalar(0, 0, 255)); } } // load 2D and 3D points to Mats for solvePNP final MatOfPoint3f objectPoints = new MatOfPoint3f(); objectPoints.fromList(points3dlist); final MatOfPoint2f imagePoints = new MatOfPoint2f(); imagePoints.fromList(points2dlist); if (visualization) { // show image with markers detected Imshow.show(image2); } // do not call solvePNP with empty intput data (no markers detected) if (points2dlist.size() == 0) { return false; } // uncomment these lines if using RANSAC-based pose estimation (more // shaking) Mat inliers = new Mat(); Calib3d.solvePnPRansac(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec, false, 300, 5, 16, inliers, Calib3d.CV_P3P); ARLoc.getLog().info("Points detected: " + points2dlist.size() + " inliers: " + inliers.size()); // avoid publish zero pose if localization failed if (inliers.rows() == 0) { return false; } return true; }