bool GridMapRosConverter::toCvImage(const grid_map::GridMap& gridMap, const std::string& layer,
cv::Mat& cvImage, const float dataMin, const float dataMax)
{
if (gridMap.getSize()(0) > 0 && gridMap.getSize()(1) > 0) {
// Initialize blank image.
cvImage = cv::Mat::zeros(gridMap.getSize()(0), gridMap.getSize()(1), CV_8UC4);
} else {
ROS_ERROR("Invalid grid map?");
return false;
}
// Clamp outliers.
grid_map::GridMap map = gridMap;
map.get(layer) = map.get(layer).unaryExpr(grid_map::Clamp<float>(dataMin, dataMax));
// Find upper and lower values.
float lowerValue = map.get(layer).minCoeffOfFinites();
float upperValue = map.get(layer).maxCoeffOfFinites();
uchar imageMax = std::numeric_limits<unsigned char>::max();
for (GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator) {
if (map.isValid(*iterator, layer)) {
float value = map.at(layer, *iterator);
uchar imageValue = (uchar)(((value - lowerValue) / (upperValue - lowerValue)) * (float)imageMax);
grid_map::Index imageIndex(iterator.getUnwrappedIndex());
cvImage.at<cv::Vec<uchar, 4>>(imageIndex(1), imageIndex(0))[0] = imageValue;
cvImage.at<cv::Vec<uchar, 4>>(imageIndex(1), imageIndex(0))[1] = imageValue;
cvImage.at<cv::Vec<uchar, 4>>(imageIndex(1), imageIndex(0))[2] = imageValue;
cvImage.at<cv::Vec<uchar, 4>>(imageIndex(1), imageIndex(0))[3] = imageMax;
}
}
return true;
}
// ---------------------------------------------------------------------------
// CSmileyModel::ReplaceTextWithCodes
// ---------------------------------------------------------------------------
//
void CSmileyModel::ReplaceTextWithCodes( TDes& aText, TInt aDocPos,
TInt aNodeIndex )
{
TInt linkIndex( iStrArray[aNodeIndex].iChild & ~KLinkIndexMark );
TInt len( iLinkArray[linkIndex].iStrLength );
TInt imageIndex( iLinkArray[linkIndex].iImageIndex );
aText[aDocPos] = iImageInfoArray[imageIndex].iCode;
aText[aDocPos + 1] = CSmileyManager::KCompensateChar;
const TInt KThirdIndex = 2;
for ( TInt i( KThirdIndex ); i < len; i++ )
{
aText[i + aDocPos] = CSmileyManager::KPlaceHolder;
}
}
// ---------------------------------------------------------------------------
// CSmileyModel::SmileyCode
// ---------------------------------------------------------------------------
//
TText CSmileyModel::SmileyCode( TInt aNodeIndex )
{
TInt linkIndex( iStrArray[aNodeIndex].iChild & ~KLinkIndexMark );
TInt imageIndex( iLinkArray[linkIndex].iImageIndex );
return iImageInfoArray[imageIndex].iCode;
}