Eigen::Matrix<double,2,2> FeatureCoordinates::get_J() const{
assert(mpCamera_ != nullptr);
if(!mpCamera_->bearingToPixel(get_nor(),c_,matrix2dTemp_)){
matrix2dTemp_.setZero();
std::cout << " \033[31mERROR: No valid coordinate data!\033[0m" << std::endl;
}
return matrix2dTemp_;
}
FeatureCoordinates FeatureCoordinates::get_patchCorner(const double x, const double y) const{
FeatureCoordinates temp; // TODO: avoid temp
get_nor().boxPlus(get_warp_nor()*Eigen::Vector2d(x,y),norTemp_);
temp.set_nor(norTemp_);
temp.mpCamera_ = mpCamera_;
temp.camID_ = camID_;
return temp;
}
bool FeatureCoordinates::getDepthFromTriangulation(const FeatureCoordinates& other, const V3D& C2rC2C1, const QPD& qC2C1, FeatureDistance& d){
const V3D C2v1 = qC2C1.rotate(get_nor().getVec());
const V3D C2v2 = other.get_nor().getVec();
const double a = 1.0-pow(C2v1.dot(C2v2),2.0);
if(a < 1e-3){
return false;
}
const double distance = C2v1.dot((M3D::Identity()-C2v2*C2v2.transpose())*C2rC2C1) / a;
d.setParameter(fabs(distance));
return true;
}
bool FeatureCoordinates::getDepthFromTriangulation(const FeatureCoordinates& other, const V3D& C2rC2C1, const QPD& qC2C1, FeatureDistance& d) {
Eigen::Matrix<double,3,2> B;
B << qC2C1.rotate(get_nor().getVec()), other.get_nor().getVec();
const Eigen::Matrix2d BtB = B.transpose() * B;
if(BtB.determinant() < 0.000001) { // Projection rays almost parallel.
return false;
}
const Eigen::Vector2d dv = - BtB.inverse() * B.transpose() * C2rC2C1;
d.setParameter(fabs(dv[0]));
return true;
}
float FeatureCoordinates::getDepthUncertaintyTau(const V3D& C1rC1C2, const float d, const float px_error_angle){
const V3D C1fP = get_nor().getVec();
float t_0 = C1rC1C2(0);
float t_1 = C1rC1C2(1);
float t_2 = C1rC1C2(2);
float a_0 = C1fP(0) * d - t_0;
float a_1 = C1fP(1) * d - t_1;
float a_2 = C1fP(2) * d - t_2;
float t_norm = std::sqrt(t_0 * t_0 + t_1 * t_1 + t_2 * t_2);
float a_norm = std::sqrt(a_0 * a_0 + a_1 * a_1 + a_2 * a_2);
float alpha = std::acos((C1fP(0) * t_0 + C1fP(1) * t_1 + C1fP(2) * t_2) / t_norm);
float beta = std::acos(( a_0 * (-t_0) + a_1 * (-t_1) + a_2 * (-t_2) ) / (t_norm * a_norm));
float beta_plus = beta + px_error_angle;
float gamma_plus = M_PI - alpha - beta_plus; // Triangle angles sum to PI.
float d_plus = t_norm * std::sin(beta_plus) / std::sin(gamma_plus); // Law of sines.
return (d_plus - d); // Tau.
}