search

laurentkneip / opengv

OpenGV is a collection of computer vision methods for solving geometric vision problems. It is hosted and maintained by the Mobile Perception Lab of ShanghaiTech.
Other
1.02k stars 353 forks source link

Ransac Distance Model in pure rotation motion for Eigensolver Sac problem #106

Open ckchng opened 4 years ago

ckchng commented 4 years ago

void opengv::sac_problems:: relative_pose::EigensolverSacProblem::getSelectedDistancesToModel( const model_t & model, const std::vector & indices, std::vector & scores) const { /double referenceNorm = model.translation.norm(); double pureRotation_threshold = 0.0001; double tanAlpha_threshold = tan(0.002); double norm_threshold = 0.001;//0.000625 for(size_t i = 0; i < indices.size(); i++) { bearingVector_t f1 = _adapter.getBearingVector1(indices[i]); bearingVector_t f2 = _adapter.getBearingVector2(indices[i]); Eigen::Vector3d n = f1.cross(model.rotationf2); if( referenceNorm > pureRotation_threshold ) { double np_norm = n.transpose() model.eigenvectors.col(0); Eigen::Vector3d np = np_norm model.eigenvectors.col(0); Eigen::Vector3d no = n - np; double maxDistance = norm_threshold + tanAlpha_threshold no.norm(); scores.push_back(np.norm()/maxDistance); } else scores.push_back(n.norm()/norm_threshold); }/

translation_t tempTranslation = _adapter.gett12(); rotation_t tempRotation = _adapter.getR12();

translation_t translation = model.translation; rotation_t rotation = model.rotation; _adapter.sett12(translation); _adapter.setR12(rotation);

transformation_t inverseSolution; inverseSolution.block<3,3>(0,0) = rotation.transpose(); inverseSolution.col(3) = -inverseSolution.block<3,3>(0,0)*translation;

Eigen::Matrix<double,4,1> p_hom; p_hom[3] = 1.0;

for( size_t i = 0; i < indices.size(); i++ ) { p_hom.block<3,1>(0,0) = opengv::triangulation::triangulate2(_adapter,indices[i]); bearingVector_t reprojection1 = p_hom.block<3,1>(0,0); bearingVector_t reprojection2 = inverseSolution * p_hom; reprojection1 = reprojection1 / reprojection1.norm(); reprojection2 = reprojection2 / reprojection2.norm(); bearingVector_t f1 = _adapter.getBearingVector1(indices[i]); bearingVector_t f2 = _adapter.getBearingVector2(indices[i]);

//bearing-vector based outlier criterium (select threshold accordingly):
//1-(f1'*f2) = 1-cos(alpha) \in [0:2]
double reprojError1 = 1.0 - (f1.transpose() * reprojection1);
double reprojError2 = 1.0 - (f2.transpose() * reprojection2);
scores.push_back(reprojError1 + reprojError2);

}

_adapter.sett12(tempTranslation); _adapter.setR12(tempRotation); }

I realize that the first part of the code was commented out. It seems to be dealing with pure rotation motion. If I understand correctly, the 'triangulate2' is not going to project the points properly under pure rotation motion right?

philipzimmermann commented 1 year ago

I noticed that the outlier threshold for the RotationOnlySacProblem doesn't have any influence on the outliers. Could this have to do with this?