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);
