void homography(
            const std::vector<cv::KeyPoint>& keypoints_0,
            const std::vector<cv::KeyPoint>& keypoints_1,
            const std::vector<cv::DMatch>& matches,
            cv::Mat& H)
    {
        const size_t N = matches.size();
        std::vector<int> queryIdxs(N), trainIdxs(N);
        for (size_t i = 0; i < N; ++i)
        {
            queryIdxs[i] = matches[i].queryIdx;
            trainIdxs[i] = matches[i].trainIdx;
        }

        if (threshold_ >= 0)
        {
            std::vector<cv::Point2f> points1;
            cv::KeyPoint::convert(keypoints_0, points1, queryIdxs);

            std::vector<cv::Point2f> points2;
            cv::KeyPoint::convert(keypoints_1, points2, trainIdxs);

            H = cv::findHomography(
                    cv::Mat(points1), cv::Mat(points2),
                    cv::RANSAC, threshold_);
        }
    }
void CameraPoseOptimization::computeCorrespondenceBetweenTwoFrames(
	vector<cv::KeyPoint>& keypointsInCurrFrame,
	cv::Mat& descriptorsInCurrFrame,
	vector<DMatch>& filteredMatches,
	vector<char>& matchesMask)
{
	FastFeatureDetector detector;
	BriefDescriptorExtractor extractor;

	// Match the descriptors of the two images through cross-checking
	BFMatcher matcher(NORM_L2);
	crossCheckMatching(matcher, m_descriptorsInLastFrame, descriptorsInCurrFrame, filteredMatches);

	// Given the corresponding keypoint pairs of two images, compute the homography matrix,
	// which can be treated as a 3D transformation matrix between two images.
	vector<int> queryIdxs(filteredMatches.size()), trainIdxs(filteredMatches.size());
	for (size_t i = 0; i < filteredMatches.size(); i++)
	{
		queryIdxs[i] = filteredMatches[i].queryIdx;
		trainIdxs[i] = filteredMatches[i].trainIdx;
	}
	vector<Point2f> points1, points2;
	KeyPoint::convert(m_keypointsInLastFrame, points1, queryIdxs);
	KeyPoint::convert(keypointsInCurrFrame, points2, trainIdxs);
	Mat H12 = findHomography(Mat(points1), Mat(points2), CV_RANSAC, g_thresholdRansacReproj);

	//std::cout << H12.cols << "," << H12.rows << std::endl;
	//for (int i = 0; i != H12.rows; ++i)
	//{
	//	for (int j = 0; j != H12.cols; ++j)
	//	{
	//		std::cout << H12.at<double>(i,j) << " ";
	//	}
	//	std::cout << std::endl;
	//}
	//

	// For each keypoint A in the first image, perform the homography matrix on it to get its corresponding
	// transformed point B in the second image. If B is equal to A's corresponding keypoint computed by 
	// the cross-checking method, then treat this pair of points as reasonable by putting its flag in 
	// the variable "matchesMask" as 1. Otherwise, treat the pair as unreasonable by putting its flag in 
	// the variable "matchesMask" as 0.
	matchesMask.clear();
	matchesMask.resize(filteredMatches.size(), 1);

	if (!H12.empty())
	{
		Mat points1t;
		perspectiveTransform(Mat(points1), points1t, H12);
		for (size_t i1 = 0; i1 < points1.size(); i1++)
		{
			// If the distance between A's transformed point A1 and its corresponding point B is too large, 
			// then the correspondence pair (A,B) is unreasonable.
			if (norm(points2[i1] - points1t.at<Point2f>((int)i1, 0)) > g_thresholdRansacReproj)
				matchesMask[i1] = 0; // mask 0 for unreasonable pair
		}
	}
}
void pkmDetector::update()
{
	if (bSetImageSearch && bSetImageTemplate) 
	{
		if (!bInitialized) 
		{
			
			
			// do matching between the template and image search
			// without tracking previous features since none initialized
			filteredMatches.clear();
			switch( matcherFilterType )
			{
				case CROSS_CHECK_FILTER :
					crossCheckMatching( descriptorMatcher, img_template_descriptors, img_search_descriptors, filteredMatches, 1 );
					break;
				default :
					simpleMatching( descriptorMatcher, img_template_descriptors, img_search_descriptors, filteredMatches );
			}
			
			// reindex based on found matches
			vector<int> queryIdxs( filteredMatches.size() ), trainIdxs( filteredMatches.size() );
			for( size_t i = 0; i < filteredMatches.size(); i++ )
			{
				queryIdxs[i] = filteredMatches[i].queryIdx;
				trainIdxs[i] = filteredMatches[i].trainIdx;
			}
			
			// build homograhpy w/ ransac
			vector<cv::Point2f> points1; cv::KeyPoint::convert(img_template_keypoints, points1, queryIdxs);
			vector<cv::Point2f> points2; cv::KeyPoint::convert(img_search_keypoints, points2, trainIdxs);
			H12 = findHomography( cv::Mat(points1), cv::Mat(points2), CV_RANSAC, ransacReprojThreshold );
			
			// create a mask of the current inliers based on transform distance
			vector<char> matchesMask( filteredMatches.size(), 0 );
			
			printf("Matched %d features.\n", filteredMatches.size());
			
			// convert previous image points to current image points via homography 
			// although this is a transformation from image to world coordinates
			// it should estimate the current image points
			cv::Mat points1t; perspectiveTransform(cv::Mat(points1), points1t, H12);
			for( size_t i1 = 0; i1 < points1.size(); i1++ )
			{
				if( norm(points2[i1] - points1t.at<cv::Point2f>((int)i1,0)) < 4 ) // inlier
				{
					matchesMask[i1] = 1;
					img_search_points_inliers[1].push_back(points2[i1]);
				}
				else {
					img_search_points_outliers[1].push_back(points2[i1]);
				}
			}
			
			
			// update bounding box
			cv::Mat bb;
			perspectiveTransform(cv::Mat(img_template_boundingbox), bb, H12);
			for( int i = 0; i < 4; i++ )
			{
				dst_corners[i] = bb.at<cv::Point2f>(i,0);
				//img_template_boundingbox[i] = bb.at<cv::Point2f>(i,0);
			}
			
			/*
			// draw inliers
			drawMatches( img_search, img_template_keypoints, 
						 img_template, img_search_keypoints, 
						filteredMatches, drawImg, 
						CV_RGB(0, 255, 0), CV_RGB(0, 0, 255), matchesMask
	#if DRAW_RICH_KEYPOINTS_MODE
						, DrawMatchesFlags::DRAW_RICH_KEYPOINTS
	#endif
						);
			
	#if DRAW_OUTLIERS_MODE
			// draw outliers
			for( size_t i1 = 0; i1 < matchesMask.size(); i1++ )
				matchesMask[i1] = !matchesMask[i1];
			drawMatches( img_template, img_template_keypoints, 
						img_search, img_search_keypoints, filteredMatches, drawImg, CV_RGB(0, 0, 255), CV_RGB(255, 0, 0), matchesMask,
						DrawMatchesFlags::DRAW_OVER_OUTIMG | DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS );
			
	#endif
			
			imshow( winName, drawImg );
			*/
			
		}
		else {
			// track features from previous frame into the current frame and see which 
			// features are inliers, and which are outliers.  among the features that
			// are outliers, see if any were marked as inliers in the previous frame and
			// remark then as outliers
			
			// mark decsriptors on new features marked as outliers once the number of 
			// inliers drops to a certain threshold and perform matching on the template.
			
			// patch based seems powerful as well.  creating a manifold or detecting planes 
			// in the set of inliers and tracking them as a whole may be more powerful.
			//<#statements#>
		}
		
		//std::swap(current_template_points, previous_template_points);
		std::swap(img_search_points_inliers[1], img_search_points_inliers[0]);
		std::swap(img_search_points_outliers[1], img_search_points_outliers[0]);
		swap(prev_img_search, img_search);
	} // end bSetImageSearch && bSetImageTemplate
	
}
    void callback(const sensor_msgs::ImageConstPtr& msg)
    {
        if (image_0_ == NULL)
        {
            // Take first image:
            try
            {
                image_0_ = cv_bridge::toCvCopy(msg,
                        sensor_msgs::image_encodings::isColor(msg->encoding) ?
                        sensor_msgs::image_encodings::BGR8 :
                        sensor_msgs::image_encodings::MONO8);
            }
            catch (cv_bridge::Exception& e)
            {
                ROS_ERROR_STREAM("Failed to take first image: " << e.what());
                return;
            }

            ROS_INFO("First image taken");

            // Detect keypoints:
            detector_->detect(image_0_->image, keypoints_0_);
            ROS_INFO_STREAM(keypoints_0_.size() << " points found.");

            // Extract keypoints' descriptors:
            extractor_->compute(image_0_->image, keypoints_0_, descriptors_0_);
        }
        else
        {
            // Take second image:
            try
            {
                image_1_ = cv_bridge::toCvShare(msg,
                        sensor_msgs::image_encodings::isColor(msg->encoding) ?
                        sensor_msgs::image_encodings::BGR8 :
                        sensor_msgs::image_encodings::MONO8);
            }
            catch (cv_bridge::Exception& e)
            {
                ROS_ERROR_STREAM("Failed to take image: " << e.what());
                return;
            }

            // Detect keypoints:
            std::vector<cv::KeyPoint> keypoints_1;
            detector_->detect(image_1_->image, keypoints_1);
            ROS_INFO_STREAM(keypoints_1.size() << " points found on the new image.");

            // Extract keypoints' descriptors:
            cv::Mat descriptors_1;
            extractor_->compute(image_1_->image, keypoints_1, descriptors_1);

            // Compute matches:
            std::vector<cv::DMatch> matches;
            match(descriptors_0_, descriptors_1, matches);

            // Compute homography:
            cv::Mat H;
            homography(keypoints_0_, keypoints_1, matches, H);

            // Draw matches:
            const int s = std::max(image_0_->image.rows, image_0_->image.cols);
            cv::Size size(s, s);
            cv::Mat draw_image;
            warped_image_ = boost::make_shared<cv_bridge::CvImage>(
                    image_0_->header, image_0_->encoding,
                    cv::Mat(size, image_0_->image.type()));
            if (!H.empty()) // filter outliers
            {
                std::vector<char> matchesMask(matches.size(), 0);

                const size_t N = matches.size();
                std::vector<int> queryIdxs(N), trainIdxs(N);
                for (size_t i = 0; i < N; ++i)
                {
                    queryIdxs[i] = matches[i].queryIdx;
                    trainIdxs[i] = matches[i].trainIdx;
                }

                std::vector<cv::Point2f> points1, points2;
                cv::KeyPoint::convert(keypoints_0_, points1, queryIdxs);
                cv::KeyPoint::convert(keypoints_1, points2, trainIdxs);

                cv::Mat points1t;
                cv::perspectiveTransform(cv::Mat(points1), points1t, H);

                double maxInlierDist = threshold_ < 0 ? 3 : threshold_;
                for (size_t i1 = 0; i1 < points1.size(); ++i1)
                {
                    if (cv::norm(points2[i1] - points1t.at<cv::Point2f>((int)i1,0)) <= maxInlierDist ) // inlier
                        matchesMask[i1] = 1;
                }
                // draw inliers
                cv::drawMatches(
                        image_0_->image, keypoints_0_,
                        image_1_->image, keypoints_1, matches,
                        draw_image, cv::Scalar(0, 255, 0), cv::Scalar(0, 0, 255),
                        matchesMask,
                        cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS);

                // draw outliers
                for (size_t i1 = 0; i1 < matchesMask.size(); ++i1)
                    matchesMask[i1] = !matchesMask[i1];
                cv::drawMatches(
                        image_0_->image, keypoints_0_,
                        image_1_->image, keypoints_1, matches,
                        draw_image, cv::Scalar(0, 0, 255), cv::Scalar(255, 0, 0),
                        matchesMask,
                        cv::DrawMatchesFlags::DRAW_OVER_OUTIMG | cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS);

                ROS_INFO_STREAM("Number of inliers: " << cv::countNonZero(matchesMask));

                // Wrap the new image using the homography,
                // so we should have something similar to the original image:
                warpPerspective(
                        image_1_->image, warped_image_->image, H, size,
                        cv::INTER_LINEAR | cv::WARP_INVERSE_MAP);

                // Print the homography found:
                ROS_INFO_STREAM("Homography = " << H);
            }
            else
            {
                cv::drawMatches(
                        image_0_->image, keypoints_0_,
                        image_1_->image, keypoints_1, matches,
                        draw_image);

                image_1_->image.copyTo(warped_image_->image);

                ROS_WARN_STREAM("No homography transformation found!");
            }

            // Publish warped image (using homography):
            warped_image_->header = image_1_->header;
            pub_.publish(warped_image_->toImageMsg());

            // Show images:
            cv::imshow("correspondences", draw_image);
            cv::imshow("homography", warped_image_->image);
            cv::waitKey(3);
        }
    }