void
publishTfTransform(const Sophus::SE3& T, const ros::Time& stamp,
const string& frame_id, const string& child_frame_id,
tf::TransformBroadcaster& br)
{
tf::Transform transform_msg;
Eigen::Quaterniond q(T.rotationMatrix());
transform_msg.setOrigin(tf::Vector3(T.translation().x(), T.translation().y(), T.translation().z()));
tf::Quaternion tf_q; tf_q.setX(q.x()); tf_q.setY(q.y()); tf_q.setZ(q.z()); tf_q.setW(q.w());
transform_msg.setRotation(tf_q);
br.sendTransform(tf::StampedTransform(transform_msg, stamp, frame_id, child_frame_id));
}
void Homography::
findBestDecomposition()
{
assert(decompositions.size() == 8);
for(size_t i=0; i<decompositions.size(); i++)
{
HomographyDecomposition &decom = decompositions[i];
size_t nPositive = 0;
for(size_t m=0; m<fts_c1.size(); m++)
{
if(!inliers[m])
continue;
const Vector2d& v2 = fts_c1[m];
double dVisibilityTest = (H_c2_from_c1(2,0) * v2[0] + H_c2_from_c1(2,1) * v2[1] + H_c2_from_c1(2,2)) / decom.d;
if(dVisibilityTest > 0.0)
nPositive++;
}
decom.score = -nPositive;
}
sort(decompositions.begin(), decompositions.end());
decompositions.resize(4);
for(size_t i=0; i<decompositions.size(); i++)
{
HomographyDecomposition &decom = decompositions[i];
int nPositive = 0;
for(size_t m=0; m<fts_c1.size(); m++)
{
if(!inliers[m])
continue;
Vector3d v3 = unproject2d(fts_c1[m]);
double dVisibilityTest = v3.dot(decom.n) / decom.d;
if(dVisibilityTest > 0.0)
nPositive++;
};
decom.score = -nPositive;
}
sort(decompositions.begin(), decompositions.end());
decompositions.resize(2);
// According to Faugeras and Lustman, ambiguity exists if the two scores are equal
// but in practive, better to look at the ratio!
double dRatio = (double) decompositions[1].score / (double) decompositions[0].score;
if(dRatio < 0.9) // no ambiguity!
decompositions.erase(decompositions.begin() + 1);
else // two-way ambiguity. Resolve by sampsonus score of all points.
{
double dErrorSquaredLimit = thresh * thresh * 4;
double adSampsonusScores[2];
for(size_t i=0; i<2; i++)
{
Sophus::SE3 T = decompositions[i].T;
Matrix3d Essential = T.rotationMatrix() * sqew(T.translation());
double dSumError = 0;
for(size_t m=0; m < fts_c1.size(); m++ )
{
double d = sampsonusError(fts_c1[m], Essential, fts_c2[m]);
if(d > dErrorSquaredLimit)
d = dErrorSquaredLimit;
dSumError += d;
}
adSampsonusScores[i] = dSumError;
}
if(adSampsonusScores[0] <= adSampsonusScores[1])
decompositions.erase(decompositions.begin() + 1);
else
decompositions.erase(decompositions.begin());
}
}