int main() { /* the basic rig config */ RigConfig rc; rc.loadFromFile("rigconfig_core_pullup.xml"); /* camera parameter data */ CameraParameters::Ptr cameraParameters(new CameraParameters(rc)); cameraParameters->print(); /* pose tracking for the camera*/ PoseTrackerKinfu::Ptr poseTracker(new PoseTrackerKinfu(rc)); poseTracker->start(); /* range finder with kinfu cloud provider */ CloudProvider<pcl::PointXYZ>::Ptr rangeimageProvider = poseTracker->getKinfu(); RangeFinder<pcl::PointXYZ>::Ptr rangeFinder(new RangeFinder<pcl::PointXYZ>(rc)); rangeFinder->setCloudSource(rangeimageProvider); /* viewfinder to extract pois from the scene */ ViewFinderRangeImage<pcl::PointXYZ> viewFinder(0.5); viewFinder.setRangeFinder(rangeFinder); viewFinder.setCameraParameters(cameraParameters); /* the focus motor */ FocusMotor::Ptr focusMotor(new FocusMotorTwoPolys(rc)); /* create a focus controller */ FocusControllerSinglePoint::Ptr fCtrlSingle(new FocusControllerSinglePoint( cameraParameters, poseTracker)); fCtrlSingle->setIdentifier("SingeTracker0"); fCtrlSingle->setPriority(5); fCtrlSingle->start(); /* focus control */ CentralFocusControl fControl; fControl.setFocusMotor(focusMotor); fControl.addFocusController(fCtrlSingle); fControl.start(); /* simulate a main loop */ for (int i=0; i<50; i++) { poseTracker->getPose(); fControl.focusDistance(); fControl.getFocusedPoint(); fControl.activlyControlled(); sleep(0.55); } /* save cloud from kinfu */ pcl::PCDWriter writer; CloudProvider<pcl::PointXYZ>::CloudPtr outCloud( new CloudProvider<pcl::PointXYZ>::Cloud( *rangeimageProvider->getLastCloud())); writer.writeASCII("kinfu_cloud_pullup.pcd", *outCloud ); viewFinder.compute(); pcl::RangeImagePlanar::Ptr rangeImage = viewFinder.getRangeImage(); RangeImageWriter riWriter(rangeImage); riWriter.save("rangeimage_pullup.png"); viewFinder.getMiddlePoint(); }
void crossCheckMatching( cv::Ptr<cv::DescriptorMatcher>& descriptorMatcher, const cv::Mat& descriptors1, const cv::Mat& descriptors2, vector<cv::DMatch>& filteredMatches12, int knn ); void filterTrackingsByRadialProportion(vector<cv::Point2f>& pts1, vector<cv::Point2f>& pts2, cv::Mat& K, cv::Mat& newCamMat, cv::Mat& distCoeffs, cv::Size& imSize, double prop = 1.00); double generateVirtualPointset(const vector<cv::Point2f>& pts1, const vector<cv::Point2f>& pts2, vector<cv::Point2f>& virtual_pts, double bias_fraction = 0.5); /// \brief Transforms points using homography void transformPoints(vector<cv::Point2f>& pts1, cv::Mat& H); /// \brief Remove features beyond a desired quantity. Removes by strength, but if many of equal strength, randomness is introduced. void reduceFeaturesToMaximum(vector<cv::KeyPoint>& keypoints, int maximumToKeep); /// \brief Implement optical flow algorithm and filtering void trackPoints(const cv::Mat& im1, const cv::Mat& im2, vector<cv::Point2f>& pts1, vector<cv::Point2f>& pts2, int distanceConstraint, double thresh, vector<unsigned int>& lostIndices, cv::Mat H12 = cv::Mat(), cameraParameters camData = cameraParameters()); /// \brief Implement optical flow algorithm and filtering for calibrating stereo cameras void trackPoints2(const cv::Mat& im1, const cv::Mat& im2, vector<cv::Point2f>& pts1, vector<cv::Point2f>& pts2, int distanceConstraint, double thresh, vector<unsigned int>& lostIndices, const cv::Size patternSize, double& errorThreshold); void checkDistances(vector<cv::Point2f>& pts1, vector<cv::Point2f>& pts2, vector<uchar>& statusVec, double distanceConstraint); /// \brief Prints KeyPoints onto a copy of an image void displayKeyPoints(const cv::Mat& image, const vector<cv::KeyPoint>& KeyPoints, cv::Mat& outImg, const cv::Scalar& color, int thickness = 1, bool pointsOnly = false); /// \brief Prints KeyPoints onto a copy of an image void displayKeyPoints(const cv::Mat& image, const vector<cv::Point2f>& pts, cv::Mat& outImg, cv::Scalar color, int thickness = 1, bool pointsOnly = false); /// \brief Prints cv::Match paths onto a copy of an image, given unfiltered points and cv::Matches structure void drawMatchPaths(cv::Mat& src, cv::Mat& dst, vector<cv::KeyPoint>& kp1, vector<cv::KeyPoint>& kp2, vector<vector<cv::DMatch> >& matches1to2);