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; }
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; }
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"; }