double SignedDistanceField::getDistanceAt(const Position3& position) const
{
double xCenter = size_.x() / 2.0;
double yCenter = size_.y() / 2.0;
int i = std::round(xCenter - (position.x() - position_.x()) / resolution_);
int j = std::round(yCenter - (position.y() - position_.y()) / resolution_);
int k = std::round((position.z() - zIndexStartHeight_) / resolution_);
i = std::max(i, 0);
i = std::min(i, size_.x() - 1);
j = std::max(j, 0);
j = std::min(j, size_.y() - 1);
k = std::max(k, 0);
k = std::min(k, (int)data_.size() - 1);
return data_[k](i, j);
}
Vector3 SignedDistanceField::getDistanceGradientAt(const Position3& position) const
{
double xCenter = size_.x() / 2.0;
double yCenter = size_.y() / 2.0;
int i = std::round(xCenter - (position.x() - position_.x()) / resolution_);
int j = std::round(yCenter - (position.y() - position_.y()) / resolution_);
int k = std::round((position.z() - zIndexStartHeight_) / resolution_);
i = std::max(i, 1);
i = std::min(i, size_.x() - 2);
j = std::max(j, 1);
j = std::min(j, size_.y() - 2);
k = std::max(k, 1);
k = std::min(k, (int)data_.size() - 2);
double dx = (data_[k](i - 1, j) - data_[k](i + 1, j)) / (2 * resolution_);
double dy = (data_[k](i, j - 1) - data_[k](i, j + 1)) / (2 * resolution_);
double dz = (data_[k + 1](i, j) - data_[k - 1](i, j)) / (2 * resolution_);
return Vector3(dx, dy, dz);
}
double SignedDistanceField::getInterpolatedDistanceAt(const Position3& position) const
{
double xCenter = size_.x() / 2.0;
double yCenter = size_.y() / 2.0;
int i = std::round(xCenter - (position.x() - position_.x()) / resolution_);
int j = std::round(yCenter - (position.y() - position_.y()) / resolution_);
int k = std::round((position.z() - zIndexStartHeight_) / resolution_);
i = std::max(i, 0);
i = std::min(i, size_.x() - 1);
j = std::max(j, 0);
j = std::min(j, size_.y() - 1);
k = std::max(k, 0);
k = std::min(k, (int)data_.size() - 1);
Vector3 gradient = getDistanceGradientAt(position);
double xp = position_.x() + ((size_.x() - i) - xCenter) * resolution_;
double yp = position_.y() + ((size_.y() - j) - yCenter) * resolution_;
double zp = zIndexStartHeight_ + k * resolution_;
Vector3 error = position - Vector3(xp, yp, zp);
return data_[k](i, j) + gradient.dot(error);
}
void GridMapRosConverter::toPointCloud(const grid_map::GridMap& gridMap,
const std::vector<std::string>& layers,
const std::string& pointLayer,
sensor_msgs::PointCloud2& pointCloud)
{
// Header.
pointCloud.header.frame_id = gridMap.getFrameId();
pointCloud.header.stamp.fromNSec(gridMap.getTimestamp());
pointCloud.is_dense = false;
// Fields.
std::vector <std::string> fieldNames;
for (const auto& layer : layers) {
if (layer == pointLayer) {
fieldNames.push_back("x");
fieldNames.push_back("y");
fieldNames.push_back("z");
} else if (layer == "color") {
fieldNames.push_back("rgb");
} else {
fieldNames.push_back(layer);
}
}
pointCloud.fields.clear();
pointCloud.fields.reserve(fieldNames.size());
int offset = 0;
for (auto& name : fieldNames) {
sensor_msgs::PointField point_field;
point_field.name = name;
point_field.count = 1;
point_field.datatype = sensor_msgs::PointField::FLOAT32;
point_field.offset = offset;
pointCloud.fields.push_back(point_field);
offset = offset + point_field.count * 4; // 4 for sensor_msgs::PointField::FLOAT32
}
// Resize.
size_t nPoints = gridMap.getSize().prod();
pointCloud.height = 1;
pointCloud.width = nPoints;
pointCloud.point_step = offset;
pointCloud.row_step = pointCloud.width * pointCloud.point_step;
pointCloud.data.resize(pointCloud.height * pointCloud.row_step);
// Points.
std::unordered_map<std::string, sensor_msgs::PointCloud2Iterator<float>> fieldIterators;
for (auto& name : fieldNames) {
fieldIterators.insert(
std::pair<std::string, sensor_msgs::PointCloud2Iterator<float>>(
name, sensor_msgs::PointCloud2Iterator<float>(pointCloud, name)));
}
GridMapIterator mapIterator(gridMap);
for (size_t i = 0; i < nPoints; ++i) {
Position3 position;
position.setConstant(NAN);
gridMap.getPosition3(pointLayer, *mapIterator, position);
for (auto& iterator : fieldIterators) {
if (iterator.first == "x") {
*iterator.second = (float) position.x();
} else if (iterator.first == "y") {
*iterator.second = (float) position.y();
} else if (iterator.first == "z") {
*iterator.second = (float) position.z();
} else if (iterator.first == "rgb") {
*iterator.second = gridMap.at("color", *mapIterator);
} else {
*iterator.second = gridMap.at(iterator.first, *mapIterator);
}
}
++mapIterator;
for (auto& iterator : fieldIterators) {
++iterator.second;
}
}
}
bool ElevationMap::add(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointCloud, Eigen::VectorXf& pointCloudVariances, const ros::Time& timestamp, const Eigen::Affine3d& transformationSensorToMap)
{
if (pointCloud->size() != pointCloudVariances.size()) {
ROS_ERROR("ElevationMap::add: Size of point cloud (%i) and variances (%i) do not agree.",
(int) pointCloud->size(), (int) pointCloudVariances.size());
return false;
}
// Initialization for time calculation.
const ros::WallTime methodStartTime(ros::WallTime::now());
boost::recursive_mutex::scoped_lock scopedLockForRawData(rawMapMutex_);
// Update initial time if it is not initialized.
if (initialTime_.toSec() == 0) {
initialTime_ = timestamp;
}
const double scanTimeSinceInitialization = (timestamp - initialTime_).toSec();
for (unsigned int i = 0; i < pointCloud->size(); ++i) {
auto& point = pointCloud->points[i];
Index index;
Position position(point.x, point.y);
if (!rawMap_.getIndex(position, index)) continue; // Skip this point if it does not lie within the elevation map.
auto& elevation = rawMap_.at("elevation", index);
auto& variance = rawMap_.at("variance", index);
auto& horizontalVarianceX = rawMap_.at("horizontal_variance_x", index);
auto& horizontalVarianceY = rawMap_.at("horizontal_variance_y", index);
auto& horizontalVarianceXY = rawMap_.at("horizontal_variance_xy", index);
auto& color = rawMap_.at("color", index);
auto& time = rawMap_.at("time", index);
auto& lowestScanPoint = rawMap_.at("lowest_scan_point", index);
auto& sensorXatLowestScan = rawMap_.at("sensor_x_at_lowest_scan", index);
auto& sensorYatLowestScan = rawMap_.at("sensor_y_at_lowest_scan", index);
auto& sensorZatLowestScan = rawMap_.at("sensor_z_at_lowest_scan", index);
const float& pointVariance = pointCloudVariances(i);
const float scanTimeSinceInitialization = (timestamp - initialTime_).toSec();
if (!rawMap_.isValid(index)) {
// No prior information in elevation map, use measurement.
elevation = point.z;
variance = pointVariance;
horizontalVarianceX = minHorizontalVariance_;
horizontalVarianceY = minHorizontalVariance_;
horizontalVarianceXY = 0.0;
colorVectorToValue(point.getRGBVector3i(), color);
continue;
}
// Deal with multiple heights in one cell.
const double mahalanobisDistance = fabs(point.z - elevation) / sqrt(variance);
if (mahalanobisDistance > mahalanobisDistanceThreshold_) {
if (scanTimeSinceInitialization - time <= scanningDuration_ && elevation > point.z) {
// Ignore point if measurement is from the same point cloud (time comparison) and
// if measurement is lower then the elevation in the map.
} else if (scanTimeSinceInitialization - time <= scanningDuration_) {
// If point is higher.
elevation = point.z;
variance = pointVariance;
} else {
variance += multiHeightNoise_;
}
continue;
}
// Store lowest points from scan for visibility checking.
const float pointHeightPlusUncertainty = point.z + 3.0 * sqrt(pointVariance); // 3 sigma.
if (std::isnan(lowestScanPoint) || pointHeightPlusUncertainty < lowestScanPoint){
lowestScanPoint = pointHeightPlusUncertainty;
const Position3 sensorTranslation(transformationSensorToMap.translation());
sensorXatLowestScan = sensorTranslation.x();
sensorYatLowestScan = sensorTranslation.y();
sensorZatLowestScan = sensorTranslation.z();
}
// Fuse measurement with elevation map data.
elevation = (variance * point.z + pointVariance * elevation) / (variance + pointVariance);
variance = (pointVariance * variance) / (pointVariance + variance);
// TODO Add color fusion.
colorVectorToValue(point.getRGBVector3i(), color);
time = scanTimeSinceInitialization;
// Horizontal variances are reset.
horizontalVarianceX = minHorizontalVariance_;
horizontalVarianceY = minHorizontalVariance_;
horizontalVarianceXY = 0.0;
}
clean();
rawMap_.setTimestamp(timestamp.toNSec()); // Point cloud stores time in microseconds.
const ros::WallDuration duration = ros::WallTime::now() - methodStartTime;
ROS_INFO("Raw map has been updated with a new point cloud in %f s.", duration.toSec());
return true;
}