float PoseWithCovarianceStampedToGaussianPointCloud::gaussian(const Eigen::Vector2f& input,
const Eigen::Vector2f& mean,
const Eigen::Matrix2f& S,
const Eigen::Matrix2f& S_inv)
{
Eigen::Vector2f diff = input - mean;
if (normalize_method_ == "normalize_area") {
return normalize_value_ * 1 / (2 * M_PI * sqrt(S.determinant())) * exp(- 0.5 * (diff.transpose() * S_inv * diff)[0]);
}
else if (normalize_method_ == "normalize_height") {
return normalize_value_ * exp(- 0.5 * (diff.transpose() * S_inv * diff)[0]);
}
}
/** Compute optimal translation scale.
* @param std::vector<Eigen::Vector4f> vector with 3D points
* @param double median distance of all points
* @param camera pitch angle
* @param camera height
* @return double optimal scale */
double MonoOdometer5::getTranslationScale(std::vector<Eigen::Vector4f> points3D, double median, double pitch, double height)
{
double sigma = median/50.0;
double weight = 1.0/(2.0*sigma*sigma);
// ds stores the height of all points from the ground (?)
std::vector<Eigen::Vector2f> pointsPlane;
std::vector<double> ds;
Eigen::Vector2f inclination;
inclination(0) = cos(-pitch);
inclination(1) = sin(-pitch);
for(int i=0; i<points3D.size(); i++)
{
double d = inclination.transpose() * points3D[i].segment<2>(1);
ds.push_back(d);
}
int bestIndex = 0;
double maxSum = 0.0;
for(int i=0; i<points3D.size(); i++)
{
if(ds[i] > median / param_odometerMotionThreshold_)
{
double sum = 0.0;
for(int j=0; j<points3D.size(); j++)
{
double dist = ds[j] - ds[i];
sum += exp(-dist*dist*weight);
}
if(sum > maxSum)
{
maxSum = sum;
bestIndex = i;
}
}
}
double scale = height / ds[bestIndex];
return scale;
}