Esempio n. 1
0
void
HoughTransform::findMaxima (Float32 threshold, std::vector <Float32>& maxima_values, std::vector <Indices>& maxima_voter_ids)
{
  Float32 value;
  Indices voter_ids;
  Accumulator::ptr accumulator = m_accumulator->copy ();
  ExtractIndices extractor;
  PointCloud3D inliers, old_cloud, new_cloud;

//  Accumulator::ptr accumulator_backup = m_accumulator;
//  PointCloud3D::const_ptr cloud_backup = m_cloud;

  HoughTransform::ptr hough = this->copy ();

  maxima_values.clear ();
  maxima_voter_ids.clear ();

  UInt8 i = 0;

  old_cloud = *m_cloud;

  while (true) {
    Indices inliers_indices;

    Indices max_indices = accumulator->getMaxValue ();
    PRINT (max_indices[0]);
    PRINT (max_indices[1]);
    getInliers (old_cloud, max_indices, inliers_indices);
    extractor.setInputCloud (old_cloud);
    extractor.setExtractIndices (inliers_indices);
    extractor.setNegative (false);
    extractor.compute (inliers);
    extractor.setNegative (true);
    extractor.compute (new_cloud);
    PRINT (old_cloud.size ());
    old_cloud = new_cloud;
    PRINT (inliers.size ());
    PRINT (new_cloud.size ());
    hough->m_cloud = &inliers;
    hough->m_accumulator->reset ();
    hough->run ();
    *accumulator -= *hough->m_accumulator;
    if (++i == 3)
      break;
  }

  *m_accumulator = *accumulator;
}
Esempio n. 2
0
void PnPRefiner::refinePose(const std::vector<FeatureMatch> &matches,
		cv::Matx33f &rotation,
		cv::Vec3f &translation,
		int &inlierCount,
		std::vector<MatchReprojectionErrors> &errors)
{
	const int matchCount=matches.size();

	////////////////////////////////////////////////////
	//Convert input to double

	//Rotation
	const cv::Matx33d rotation_d = rotation;
	cv::Vec3d rparams_d;
	ceres::RotationMatrixToAngleAxis(ceres::RowMajorAdapter3x3(rotation_d.val), rparams_d.val);

	//Transation
	cv::Vec3d translation_d = translation;

	////////////////////////////////////////////////////
	//Check inliers before
	//int inlierCountBefore;
	//getInliers(matches, rparams_d, translation_d, inlierCountBefore, errors);
	//DTSLAM_LOG << "PnPRefine: inliers before: " << inlierCountBefore << "\n";

	int totalMatchCount=0;
	//int refineMatchCount=0;

	////////////////////////////////////////////////////
	// Prepare ceres problem
	//Non-linear minimization with distortion
	ceres::Solver::Options options;
	options.linear_solver_type = ceres::ITERATIVE_SCHUR;
	options.preconditioner_type = ceres::SCHUR_JACOBI;
	options.dense_linear_algebra_library_type = ceres::LAPACK;

	options.num_threads = 1; //Multi-threading here adds too much overhead
	options.num_linear_solver_threads = 1;

	options.logging_type = ceres::SILENT;
	options.minimizer_progress_to_stdout = false;

        // in some environments ceres uses std::tr1::shared_ptr, in others
        // it uses std::shared_ptr. Let's keep it simple by not using
        // make_shared.
	options.linear_solver_ordering.reset(new ceres::ParameterBlockOrdering());

	ceres::Problem problem;

	problem.AddParameterBlock(rparams_d.val, 3);
	problem.AddParameterBlock(translation_d.val, 3);
	options.linear_solver_ordering->AddElementToGroup(rparams_d.val, 1);
	options.linear_solver_ordering->AddElementToGroup(translation_d.val, 1);

	for(int i=0; i<matchCount; i++)
	{
		const FeatureMatch &match = matches[i];
		auto &feature = match.measurement.getFeature();

		totalMatchCount++;

		if (feature.is3D())
		{
			//const double costScale = 1;//std::min(10,match.trackLength);
			const int costScale = (feature.getMeasurements().size() > 3) ? 10 : 1;
			//if(costScale <= 1)
			//	continue;

			ceres::LossFunction *lossFuncA = new ceres::CauchyLoss(mOutlierPixelThreshold);
			ceres::LossFunction *lossFunc = new ceres::ScaledLoss(lossFuncA,costScale,ceres::TAKE_OWNERSHIP);

			problem.AddResidualBlock(
					new ceres::AutoDiffCostFunction<PoseReprojectionError3D,2,3,3>(
							new PoseReprojectionError3D(match)),
							lossFunc, rparams_d.val, translation_d.val);
		}
		else if (FLAGS_PoseUse2D)
		{
			//This uses only a single measurement to constrain the 2D feature, not optimal but faster
			auto &refM = *match.measurement.getFeature().getMeasurements()[0];
			
			ceres::LossFunction *lossFunc = new ceres::CauchyLoss(mOutlierPixelThreshold);
			
			problem.AddResidualBlock(
					new ceres::AutoDiffCostFunction<EpipolarSegmentErrorForPose,2,3,3>(
					new EpipolarSegmentErrorForPose(refM, match.measurement, (float)FLAGS_MinDepth)),
					lossFunc, rparams_d.val, translation_d.val);

			////This uses all measurements to constrain the 2D feature. This might give too much weight to the 2D features.
			//for (auto &mPtr : match.measurement.getFeature().getMeasurements())
			//{
			//	auto &m = *mPtr;
			//	if (m.getPositionCount() > 1)
			//		continue;

			//	ceres::LossFunction *lossFunc = new ceres::CauchyLoss(mOutlierPixelThreshold);

			//	problem.AddResidualBlock(
			//		new ceres::AutoDiffCostFunction<EpipolarSegmentErrorForPose, 2, 3, 3>(
			//		new EpipolarSegmentErrorForPose(m, match.measurement, (float)FLAGS_MinDepth)),
			//		lossFunc, rparams_d.val, translation_d.val);
			//	totalMatchCount++;
			//}
		}
	}

	
	////////////////////////////////////////////////////
	// Solve
	ceres::Solver::Summary summary;

	ceres::Solve(options, &problem, &summary);

	//DTSLAM_LOG << summary.FullReport();

	////////////////////////////////////////////////////
	// Again with inliers only
//	ceres::Problem problemInliers;
//
//    options.linear_solver_ordering->Clear();
//
//	problemInliers.AddParameterBlock(rparams_d.val, 3);
//	problemInliers.AddParameterBlock(translation_d.val, 3);
//	options.linear_solver_ordering->AddElementToGroup(rparams_d.val, 1);
//	options.linear_solver_ordering->AddElementToGroup(translation_d.val, 1);
//
//	for(int i=0; i<match3DCount; i++)
//	{
//		const FeatureMatch &match = matches3D[i];
//		//double costScale = std::min(10,match.trackLength);
//		//double costScale = match.trackLength;
//		double costScale = 1;
//		//if(costScale <= 1)
//		//	continue;
//
//		ceres::LossFunction *lossFunc = new ceres::ScaledLoss(NULL,costScale,ceres::TAKE_OWNERSHIP);
//
//		MatchReprojectionErrors errors;
//		if(!getReprojectionErrors3D(match, rparams_d, translation_d, errors))
//			continue;
//
//		problemInliers.AddResidualBlock(
//				new ceres::AutoDiffCostFunction<PoseReprojectionError3D,2,3,3>(
//						new PoseReprojectionError3D(match)),
//				lossFunc, rparams_d.val, translation_d.val);
//		refineMatchCount++;
//	}
//	if(FLAGS_PoseUse2D)
//	{
//		for(int i=0; i<match2DCount; i++)
//		{
//			const FeatureMatch &match = matches2D[i];
//
//			MatchReprojectionErrors errors;
//			if(!getReprojectionErrors2D(match, rparams_d, translation_d, errors))
//				continue;
//
//			for(auto &mPtr : match.measurement.getFeature().getMeasurements())
//			{
//				auto &m = *mPtr;
//				if(m.getPositionCount() > 1)
//					continue;
//
//				problem.AddResidualBlock(
//						new ceres::AutoDiffCostFunction<PoseReprojectionError2D,2,3,3>(
//								new PoseReprojectionError2D(m, match.measurement)),
//						NULL, rparams_d.val, translation_d.val);
//			}
//		}
//		refineMatchCount++;
//	}
//
//	ceres::Solver::Summary summaryInliers;
//	ceres::Solve(options, &problemInliers, &summaryInliers);
//	//DTSLAM_LOG << summaryInliers.FullReport();
//
//	DTSLAM_LOG << "PnPRefine: total matches=" << totalMatchCount << ", inliers used for refinement=" << refineMatchCount << "\n";

	////////////////////////////////////////////////////
	//Extract result
	cv::Matx33d finalRotation_d;
	ceres::AngleAxisToRotationMatrix(rparams_d.val, ceres::RowMajorAdapter3x3(finalRotation_d.val));
	rotation = finalRotation_d;

	translation = translation_d;

	getInliers(matches, rotation, translation, inlierCount, errors);
	DTSLAM_LOG << "PnPRefine: final inlier count=" << inlierCount << "\n";
}
/** Estimate monocular visual odometry.
 * @param std::vector<Match> vector with matches
 * @param Eigen::Matrix3f& (output) estimated rotation matrix
 * @param Eigen::Vector3f& (output) estimated translation vector
 * @param bool show optical flow (true), don't show otherwise
 * @param std::vector<Match> output vector with all inlier matches
 * @param std::vector<Eigen::Vector3f> output vector with 3D points, triangulated from all inlier matches
 * @return bool true is motion successfully estimated, false otherwise */
bool MonoOdometer8::estimateMotion(std::vector<Match> matches, Eigen::Matrix3f &R, Eigen::Vector3f &t, bool showOpticalFlow, std::vector<Match> &inlierMatches, std::vector<Eigen::Vector3f> &points3D)
{
    // check number of correspondences
    int N = matches.size();
    if(N < param_odometerMinNumberMatches_)
    {
        // too few matches to compute F
        R = Eigen::Matrix3f::Identity();
        t << 0.0, 0.0, 0.0;
        return false;
    }

    // normalize 2D features
    Eigen::Matrix3f NormTPrev, NormTCurr;
    std::vector<Match> matchesNorm = normalize2DPoints(matches, NormTPrev, NormTCurr);

    Eigen::Matrix3f F, E;
    std::vector<int> inlierIndices;
		
    // RANSAC loop
    for(int j=0; j<param_odometerRansacIters_; j++)
    {
        // get random sample
        std::vector<int> chosenIndices = getRandomSample(matchesNorm.size(), 8);
            
        // compute fundamental matrix
        F = getF(matchesNorm, chosenIndices);
            
        // get inliers
        std::vector<int> inlierIndicesCurr = getInliers(matchesNorm, F);
            
        if(inlierIndicesCurr.size() > inlierIndices.size())
        {
                inlierIndices = inlierIndicesCurr;
        }
    }

    // check number of inliers
    if(inlierIndices.size() < param_odometerMinNumberMatches_)
    {
        R = Eigen::Matrix3f::Identity();
        t << 0.0, 0.0, 0.0;
        return false;
    }

    // compute fundamental matrix out of all inliers
    F = getF(matchesNorm, inlierIndices);

    // save inlier and outlier matches
    std::vector<Match> outlierMatches;
    for(int i=0; i<matches.size(); i++)
    {
        if(elemInVec(inlierIndices, i))
        {
            inlierMatches.push_back(matches[i]);
        }
        else
        {
            outlierMatches.push_back(matches[i]);
        }
    }

    // plot optical flow and print #inliers (for debugging)
    if(showOpticalFlow)
    {
        cv::Mat image(1024, 768, CV_8UC1, cv::Scalar(0));
        cv::Mat of1 = highlightOpticalFlow(image, inlierMatches, cv::Scalar(0, 255, 0));
        cv::Mat of2 = highlightOpticalFlow(of1, outlierMatches, cv::Scalar(0, 0, 255));
        cv::namedWindow("Optical flow", CV_WINDOW_AUTOSIZE);
        cv::imshow("Optical flow", of2);
        cv::waitKey(10);
    }
    
    // denormalize F
    F = NormTCurr.transpose() * F * NormTPrev;
    
    // compute essential matrix E
    E = F2E(F);

    // get rotation and translation and triangulate points
    Eigen::Matrix<float, 4, Eigen::Dynamic> points3DMat;
    E2Rt(E, inlierMatches, R, t, points3DMat);

    // normalize 3D points (force last coordinate to 0)
    for(int j=0; j<points3DMat.cols(); j++)
    {
        Eigen::Vector3f pt = points3DMat.block<3, 1>(0, j);
        double lastCoord = points3DMat(3, j);
        pt = pt / lastCoord;
        if(pt(2) > 0)
        {
            points3D.push_back(pt);
        }
        else
        {
            // remove match if not a valid point
            inlierMatches.erase(inlierMatches.begin() + j);
        }
    }

    // check number of valid points
    if(points3D.size() < param_odometerMinNumberMatches_)
    {
        R = Eigen::Matrix3f::Identity();
        t << 0.0, 0.0, 0.0;
        return false;
    }

    // inforce translation norm to 1
    t = t / sqrt(t.dot(t));

    return true;
}
Esempio n. 4
0
void EssentialRefiner::refineEssential(const std::vector<FeatureMatch> &matches,
		cv::Matx33f &rotation, cv::Vec3f &translation, int &inlierCount, std::vector<MatchReprojectionErrors> &errors)
{
	const int matchCount=matches.size();

	//Convert input to double
	const cv::Matx33d rotation_d = rotation;
	cv::Vec3d rparams_d;
	ceres::RotationMatrixToAngleAxis(ceres::RowMajorAdapter3x3(rotation_d.val), rparams_d.val);

	cv::Vec3d translation_d = translation;

	//Check inliers before
	int inlierCountBefore;
	getInliers(matches, rparams_d, translation_d, inlierCountBefore, errors);

	//Non-linear minimization with distortion
	ceres::Solver::Options options;
	options.linear_solver_type = ceres::SPARSE_SCHUR;
	options.minimizer_progress_to_stdout = false;
	//options.function_tolerance = mOutlierPixelThreshold/100;
	//options.parameter_tolerance = 1e-5;
        
        // in some environments ceres uses std::tr1::shared_ptr, in others
        // it uses std::shared_ptr. Let's keep it simple by not using
        // make_shared.
	options.linear_solver_ordering.reset(new ceres::ParameterBlockOrdering());

	ceres::Problem problem;

	problem.AddParameterBlock(rparams_d.val, 3);
	problem.AddParameterBlock(translation_d.val, 3, new Fixed3DNormParametrization(1.0));
	problem.AddParameterBlock(translation_d.val, 3);
	options.linear_solver_ordering->AddElementToGroup(rparams_d.val, 1);
	options.linear_solver_ordering->AddElementToGroup(translation_d.val, 1);

	ceres::CauchyLoss robustLoss(mOutlierPixelThreshold);

	for(int i=0; i<matchCount; i++)
	{
		auto &match = matches[i];
		auto &refM = *match.measurement.getFeature().getMeasurements()[0];
		
		//		bool isInlier = inlierMask[i]!=-1;
//		if(!isInlier && mRefineOnlyInliers)
//			continue;

		const int scale = 1<<match.measurement.getOctave();
		//const int scale = 1;

		ceres::LossFunction *lossFunc_i = &robustLoss;
		ceres::ScaledLoss *scaledLoss = new ceres::ScaledLoss(lossFunc_i, scale, ceres::DO_NOT_TAKE_OWNERSHIP);

		//problem.AddResidualBlock(
		//		new ceres::AutoDiffCostFunction<PoseReprojectionError2D,2,3,3>(
		//				new PoseReprojectionError2D(match)),
		//				scaledLoss, rparams_d.val, translation_d.val);
		problem.AddResidualBlock(
			new ceres::AutoDiffCostFunction<EpipolarSegmentErrorForPose, 2, 3, 3>(
			new EpipolarSegmentErrorForPose(refM, match.measurement, (float)FLAGS_MinDepth)),
			scaledLoss, rparams_d.val, translation_d.val);
	}

	ceres::Solver::Summary summary;

	ceres::Solve(options, &problem, &summary);

	//DTSLAM_LOG << summary.FullReport();

	//Extract result
	cv::Matx33d finalRotation_d;
	ceres::AngleAxisToRotationMatrix(rparams_d.val, ceres::RowMajorAdapter3x3(finalRotation_d.val));
	rotation = finalRotation_d;

	translation = translation_d;

	getInliers(matches, rparams_d, translation_d, inlierCount, errors);

	const int inlierPercentage = (int)(100*inlierCount / (float)matches.size());
	DTSLAM_LOG << "Essential estimation refinement: inliersBefore=" << inlierCountBefore << ", inliers=" << inlierCount << "/" << matches.size() <<  "(" << inlierPercentage << "%)\n";
}