diff --git a/robosherlock/descriptors/annotators/detection/PlaneAnnotator.yaml b/robosherlock/descriptors/annotators/detection/PlaneAnnotator.yaml index 5ad72cbb..a426731c 100644 --- a/robosherlock/descriptors/annotators/detection/PlaneAnnotator.yaml +++ b/robosherlock/descriptors/annotators/detection/PlaneAnnotator.yaml @@ -10,6 +10,7 @@ parameters: max_curvature: 0.1 distance_threshold: 0.02 angular_threshold_deg: 5.0 + plane_direction: "all" capabilities: inputs: ['rs.pcl.FilteredPointCloud'] outputs: ['rs.annotation.Plane'] diff --git a/robosherlock/src/detection/src/PlaneAnnotator.cpp b/robosherlock/src/detection/src/PlaneAnnotator.cpp index 6e630319..a888afac 100644 --- a/robosherlock/src/detection/src/PlaneAnnotator.cpp +++ b/robosherlock/src/detection/src/PlaneAnnotator.cpp @@ -50,569 +50,587 @@ using namespace uima; class PlaneAnnotator : public DrawingAnnotator { private: - enum - { - PCL, - BOARD, - MPS, - FILE - } mode; - - // BOARD - cv::Mat cameraMatrix; - cv::Mat distortionCoefficients; - cv::Mat rotation; - cv::Mat translation; - cv::Mat planeNormal; - double planeDistance; - - // PCL - pcl::PointIndices::Ptr plane_inliers; - pcl::PointCloud::Ptr cloud; - pcl::PointCloud::Ptr display; - std::vector mapping_indices; - - bool useNonNANCloud; - - // MPS - std::vector, Eigen::aligned_allocator>> - regions; - std::vector modelCoefficients; - std::vector inlierIndices; - - // Drawing - bool foundPlane, saveToFile; - cv::Mat image_; - double pointSize; - - // params - int min_plane_inliers, max_iterations; - float distance_threshold, max_curvature, angular_threshold_deg; - - std::string pathToModelFile; + enum + { + PCL, + BOARD, + MPS, + FILE + } mode; + + // BOARD + cv::Mat cameraMatrix; + cv::Mat distortionCoefficients; + cv::Mat rotation; + cv::Mat translation; + cv::Mat planeNormal; + double planeDistance; + + // PCL + pcl::PointIndices::Ptr plane_inliers; + pcl::PointCloud::Ptr cloud; + pcl::PointCloud::Ptr display; + std::vector mapping_indices; + + bool useNonNANCloud; + + // MPS + std::vector, Eigen::aligned_allocator>> + regions; + std::vector modelCoefficients; + std::vector inlierIndices; + + // Drawing + bool foundPlane, saveToFile; + cv::Mat image_; + double pointSize; + + // params + int min_plane_inliers, max_iterations; + float distance_threshold, max_curvature, angular_threshold_deg; + std::string planeDirection; + + std::string pathToModelFile; public: - PlaneAnnotator() - : DrawingAnnotator(__func__) - , mode(BOARD) - , display(new pcl::PointCloud()) - , saveToFile(false) - , pointSize(1) - { - pathToModelFile = ros::package::getPath("robosherlock") + "/config/plane_model.xml"; - } - - TyErrorId initialize(AnnotatorContext& ctx) - { - outInfo("initialize"); - - if (ctx.isParameterDefined("plane_estimation_mode")) + PlaneAnnotator() + : DrawingAnnotator(__func__) + , mode(BOARD) + , display(new pcl::PointCloud()) + , saveToFile(false) + , pointSize(1) { - std::string sMode; - ctx.extractValue("plane_estimation_mode", sMode); - outInfo("mode set to: " << sMode); - if (sMode == "BOARD") - { - mode = BOARD; - } - else if (sMode == "PCL") - { - mode = PCL; - } - else if (sMode == "MPS") - { - mode = MPS; - } - else if (sMode == "FILE") - { - mode = FILE; - } + pathToModelFile = ros::package::getPath("robosherlock") + "/config/plane_model.xml"; } - if (ctx.isParameterDefined("use_non_nan_cloud")) - { - ctx.extractValue("use_non_nan_cloud", useNonNANCloud); - } - if (ctx.isParameterDefined("min_plane_inliers")) - { - ctx.extractValue("min_plane_inliers", min_plane_inliers); - } - if (ctx.isParameterDefined("max_iterations")) - { - ctx.extractValue("max_iterations", max_iterations); - } - if (ctx.isParameterDefined("distance_threshold")) - { - ctx.extractValue("distance_threshold", distance_threshold); - } - if (ctx.isParameterDefined("max_curvature")) - { - ctx.extractValue("max_curvature", max_curvature); - } - if (ctx.isParameterDefined("angular_threshold_deg")) + + TyErrorId initialize(AnnotatorContext& ctx) { - ctx.extractValue("angular_threshold_deg", angular_threshold_deg); + outInfo("initialize"); + + if (ctx.isParameterDefined("plane_estimation_mode")) + { + std::string sMode; + ctx.extractValue("plane_estimation_mode", sMode); + outInfo("mode set to: " << sMode); + if (sMode == "BOARD") + { + mode = BOARD; + } + else if (sMode == "PCL") + { + mode = PCL; + } + else if (sMode == "MPS") + { + mode = MPS; + } + else if (sMode == "FILE") + { + mode = FILE; + } + } + if (ctx.isParameterDefined("use_non_nan_cloud")) + { + ctx.extractValue("use_non_nan_cloud", useNonNANCloud); + } + if (ctx.isParameterDefined("min_plane_inliers")) + { + ctx.extractValue("min_plane_inliers", min_plane_inliers); + } + if (ctx.isParameterDefined("max_iterations")) + { + ctx.extractValue("max_iterations", max_iterations); + } + if (ctx.isParameterDefined("distance_threshold")) + { + ctx.extractValue("distance_threshold", distance_threshold); + } + if (ctx.isParameterDefined("max_curvature")) + { + ctx.extractValue("max_curvature", max_curvature); + } + if (ctx.isParameterDefined("angular_threshold_deg")) + { + ctx.extractValue("angular_threshold_deg", angular_threshold_deg); + } + if (ctx.isParameterDefined("save_to_file")) + { + ctx.extractValue("save_to_file", saveToFile); + } + if (ctx.isParameterDefined("plane_direction")) + { + ctx.extractValue("plane_direction", planeDirection); + } + + return UIMA_ERR_NONE; } - if (ctx.isParameterDefined("save_to_file")) + + TyErrorId reconfigure() { - ctx.extractValue("save_to_file", saveToFile); + outDebug("Reconfiguring"); + AnnotatorContext& ctx = getAnnotatorContext(); + initialize(ctx); + if (ctx.isParameterDefined("plane_estimation_mode")) + { + std::string sMode; + ctx.extractValue("plane_estimation_mode", sMode); + outInfo("mode set to: " << sMode); + } + return UIMA_ERR_NONE; } - return UIMA_ERR_NONE; - } - - TyErrorId reconfigure() - { - outDebug("Reconfiguring"); - AnnotatorContext& ctx = getAnnotatorContext(); - initialize(ctx); - if (ctx.isParameterDefined("plane_estimation_mode")) + TyErrorId destroy() { - std::string sMode; - ctx.extractValue("plane_estimation_mode", sMode); - outInfo("mode set to: " << sMode); + outDebug("destroy"); + return UIMA_ERR_NONE; } - return UIMA_ERR_NONE; - } - - TyErrorId destroy() - { - outDebug("destroy"); - return UIMA_ERR_NONE; - } private: - TyErrorId processWithLock(CAS& tcas, ResultSpecification const& res_spec) - { - MEASURE_TIME; - outDebug("process begins"); + TyErrorId processWithLock(CAS& tcas, ResultSpecification const& res_spec) + { + MEASURE_TIME; + outDebug("process begins"); - rs::SceneCas cas(tcas); - rs::Scene scene = cas.getScene(); + rs::SceneCas cas(tcas); + rs::Scene scene = cas.getScene(); - cas.get(VIEW_COLOR_IMAGE, image_); + cas.get(VIEW_COLOR_IMAGE, image_); - foundPlane = false; + foundPlane = false; - std::vector planes; - scene.annotations.filter(planes); - if (!planes.empty()) - { - loadPlaneModel(tcas, scene, true); - foundPlane = true; + std::vector planes; + scene.annotations.filter(planes); + if (!planes.empty()) + { + loadPlaneModel(tcas, scene, true); + foundPlane = true; + } + else + { + switch (mode) + { + case BOARD: + outInfo("Estimating form board"); + estimateFromBoard(tcas, scene); + break; + case PCL: + outInfo("Estimating form Point Cloud"); + estimateFromPCL(tcas, scene); + break; + case MPS: + outInfo("Estimating form MPS"); + estimateFromMPS(tcas, scene); + break; + case FILE: + outInfo("Loading from File"); + loadPlaneModel(tcas, scene, false); + break; + } + } + + return UIMA_ERR_NONE; } - else + + void estimateFromBoard(CAS& tcas, rs::Scene& scene) { - switch (mode) - { - case BOARD: - outInfo("Estimating form board"); - estimateFromBoard(tcas, scene); - break; - case PCL: - outInfo("Estimating form Point Cloud"); - estimateFromPCL(tcas, scene); - break; - case MPS: - outInfo("Estimating form MPS"); - estimateFromMPS(tcas, scene); - break; - case FILE: - outInfo("Loading from File"); - loadPlaneModel(tcas, scene, false); - break; - } - } + rs::SceneCas cas(tcas); - return UIMA_ERR_NONE; - } + sensor_msgs::CameraInfo camInfo; + cas.get(VIEW_CAMERA_INFO, camInfo); + readCameraInfo(camInfo); - void estimateFromBoard(CAS& tcas, rs::Scene& scene) - { - rs::SceneCas cas(tcas); + std::vector boards; + scene.annotations.filter(boards); - sensor_msgs::CameraInfo camInfo; - cas.get(VIEW_CAMERA_INFO, camInfo); - readCameraInfo(camInfo); + if (boards.empty()) + { + foundPlane = false; + outInfo("no board found!"); + return; + } - std::vector boards; - scene.annotations.filter(boards); + foundPlane = true; + rs::Board& board = boards[0]; + cv::Mat pointsWorld, pointsImage, rotW2C; + rs::conversion::from(board.pointsImage(), pointsImage); + rs::conversion::from(board.pointsWorld(), pointsWorld); + + cv::solvePnPRansac(pointsWorld, pointsImage, cameraMatrix, distortionCoefficients, rotation, translation, + !rotation.empty() && !translation.empty(), 100, 1.0); + cv::Rodrigues(rotation, rotW2C); + + planeNormal = cv::Mat(3, 1, CV_64F); + planeNormal.at(0) = 0; + planeNormal.at(1) = 0; + planeNormal.at(2) = 1; + planeNormal = rotW2C * planeNormal; + planeDistance = planeNormal.dot(translation); + + std::vector planeModel(4); + planeModel[0] = -planeNormal.at(0); + planeModel[1] = -planeNormal.at(1); + planeModel[2] = -planeNormal.at(2); + planeModel[3] = -planeDistance; + + rs::Plane plane = rs::create(tcas); + plane.model(planeModel); + plane.source("CheckerBoard"); + // TODO: add empty? + // plane.inliers(plane_inliers->indices); + // plane.roi(rs::conversion::to(tcas, roi)); + // plane.mask(rs::conversion::to(tcas, mask)); + scene.annotations.append(plane); + } + + void getMask(const pcl::PointIndices& inliers, const cv::Size& size, cv::Mat& mask, cv::Rect& roi) + { + cv::Mat tmp = cv::Mat::zeros(size.height, size.width, CV_8U); + int minX = size.width, maxX = 0; + int minY = size.height, maxY = 0; + + //#pragma omp parallel for + for (size_t i = 0; i < inliers.indices.size(); ++i) + { + const int index = inliers.indices[i]; + const int x = index % size.width; + const int y = index / size.width; + tmp.at(y, x) = 255; + + minX = std::min(minX, x); + maxX = std::max(maxX, x); + minY = std::min(minY, y); + maxY = std::max(maxY, y); + } - if (boards.empty()) - { - foundPlane = false; - outInfo("no board found!"); - return; + roi = cv::Rect(minX, minY, (maxX - minX) + 1, (maxY - minY) + 1); + tmp(roi).copyTo(mask); } - foundPlane = true; - rs::Board& board = boards[0]; - cv::Mat pointsWorld, pointsImage, rotW2C; - rs::conversion::from(board.pointsImage(), pointsImage); - rs::conversion::from(board.pointsWorld(), pointsWorld); - - cv::solvePnPRansac(pointsWorld, pointsImage, cameraMatrix, distortionCoefficients, rotation, translation, - !rotation.empty() && !translation.empty(), 100, 1.0); - cv::Rodrigues(rotation, rotW2C); - - planeNormal = cv::Mat(3, 1, CV_64F); - planeNormal.at(0) = 0; - planeNormal.at(1) = 0; - planeNormal.at(2) = 1; - planeNormal = rotW2C * planeNormal; - planeDistance = planeNormal.dot(translation); - - std::vector planeModel(4); - planeModel[0] = -planeNormal.at(0); - planeModel[1] = -planeNormal.at(1); - planeModel[2] = -planeNormal.at(2); - planeModel[3] = -planeDistance; - - rs::Plane plane = rs::create(tcas); - plane.model(planeModel); - plane.source("CheckerBoard"); - // TODO: add empty? - // plane.inliers(plane_inliers->indices); - // plane.roi(rs::conversion::to(tcas, roi)); - // plane.mask(rs::conversion::to(tcas, mask)); - scene.annotations.append(plane); - } - - void getMask(const pcl::PointIndices& inliers, const cv::Size& size, cv::Mat& mask, cv::Rect& roi) - { - cv::Mat tmp = cv::Mat::zeros(size.height, size.width, CV_8U); - int minX = size.width, maxX = 0; - int minY = size.height, maxY = 0; - - //#pragma omp parallel for - for (size_t i = 0; i < inliers.indices.size(); ++i) + void estimateFromMPS(CAS& tcas, rs::Scene& scene) { - const int index = inliers.indices[i]; - const int x = index % size.width; - const int y = index / size.width; - tmp.at(y, x) = 255; - - minX = std::min(minX, x); - maxX = std::max(maxX, x); - minY = std::min(minY, y); - maxY = std::max(maxY, y); - } - - roi = cv::Rect(minX, minY, (maxX - minX) + 1, (maxY - minY) + 1); - tmp(roi).copyTo(mask); - } + outInfo("estimating plane form Point Cloud data"); + rs::SceneCas cas(tcas); - void estimateFromMPS(CAS& tcas, rs::Scene& scene) - { - outInfo("estimating plane form Point Cloud data"); - rs::SceneCas cas(tcas); + cloud = pcl::PointCloud::Ptr(new pcl::PointCloud); + plane_inliers = pcl::PointIndices::Ptr(new pcl::PointIndices); - cloud = pcl::PointCloud::Ptr(new pcl::PointCloud); - plane_inliers = pcl::PointIndices::Ptr(new pcl::PointIndices); + pcl::PointCloud::Ptr normals(new pcl::PointCloud); + pcl::OrganizedMultiPlaneSegmentation mps; + pcl::PointCloud::Ptr labels(new pcl::PointCloud()); + std::vector labelIndices; + std::vector boundaryIndices; - pcl::PointCloud::Ptr normals(new pcl::PointCloud); - pcl::OrganizedMultiPlaneSegmentation mps; - pcl::PointCloud::Ptr labels(new pcl::PointCloud()); - std::vector labelIndices; - std::vector boundaryIndices; + regions.clear(); + modelCoefficients.clear(); + inlierIndices.clear(); - regions.clear(); - modelCoefficients.clear(); - inlierIndices.clear(); + cas.get(VIEW_CLOUD, *cloud); + cas.get(VIEW_NORMALS, *normals); - cas.get(VIEW_CLOUD, *cloud); - cas.get(VIEW_NORMALS, *normals); + mps.setMinInliers(min_plane_inliers); + mps.setMaximumCurvature(max_curvature); + mps.setAngularThreshold(angular_threshold_deg * M_PI / 180); + mps.setDistanceThreshold(distance_threshold); + mps.setProjectPoints(false); + mps.setInputNormals(normals); + mps.setInputCloud(cloud); - mps.setMinInliers(min_plane_inliers); - mps.setMaximumCurvature(max_curvature); - mps.setAngularThreshold(angular_threshold_deg * M_PI / 180); - mps.setDistanceThreshold(distance_threshold); - mps.setProjectPoints(false); - mps.setInputNormals(normals); - mps.setInputCloud(cloud); + mps.segmentAndRefine(regions, modelCoefficients, inlierIndices, labels, labelIndices, boundaryIndices); - mps.segmentAndRefine(regions, modelCoefficients, inlierIndices, labels, labelIndices, boundaryIndices); + size_t biggest = 0; + std::vector biggest_planeModel(4); - size_t biggest = 0; - std::vector biggest_planeModel(4); + for (size_t i = 0; i < regions.size(); ++i) + { + const pcl::PlanarRegion& region = regions[i]; + const Eigen::Vector4f model = region.getCoefficients(); + std::vector planeModel(4); + + if (model[3] < 0) + { + planeModel[0] = model[0]; + planeModel[1] = model[1]; + planeModel[2] = model[2]; + planeModel[3] = model[3]; + } + else + { + planeModel[0] = -model[0]; + planeModel[1] = -model[1]; + planeModel[2] = -model[2]; + planeModel[3] = -model[3]; + } + + cv::Mat mask; + cv::Rect roi; + getMask(inlierIndices[i], cv::Size(cloud->width, cloud->height), mask, roi); + + rs::Plane plane = rs::create(tcas); + plane.model(planeModel); + plane.inliers(inlierIndices[i].indices); + plane.roi(rs::conversion::to(tcas, roi)); + plane.mask(rs::conversion::to(tcas, mask)); + plane.source("MPS"); + scene.annotations.append(plane); + + if (region.getCount() > regions[biggest].getCount()) + { + biggest = i; + biggest_planeModel = planeModel; + } + + plane_inliers->indices.insert(plane_inliers->indices.end(), inlierIndices[i].indices.begin(), + inlierIndices[i].indices.end()); + } - for (size_t i = 0; i < regions.size(); ++i) - { - const pcl::PlanarRegion& region = regions[i]; - const Eigen::Vector4f model = region.getCoefficients(); - std::vector planeModel(4); - - if (model[3] < 0) - { - planeModel[0] = model[0]; - planeModel[1] = model[1]; - planeModel[2] = model[2]; - planeModel[3] = model[3]; - } - else - { - planeModel[0] = -model[0]; - planeModel[1] = -model[1]; - planeModel[2] = -model[2]; - planeModel[3] = -model[3]; - } - - cv::Mat mask; - cv::Rect roi; - getMask(inlierIndices[i], cv::Size(cloud->width, cloud->height), mask, roi); - - rs::Plane plane = rs::create(tcas); - plane.model(planeModel); - plane.inliers(inlierIndices[i].indices); - plane.roi(rs::conversion::to(tcas, roi)); - plane.mask(rs::conversion::to(tcas, mask)); - plane.source("MPS"); - scene.annotations.append(plane); - - if (region.getCount() > regions[biggest].getCount()) - { - biggest = i; - biggest_planeModel = planeModel; - } - - plane_inliers->indices.insert(plane_inliers->indices.end(), inlierIndices[i].indices.begin(), - inlierIndices[i].indices.end()); + if (!regions.empty()) + { + foundPlane = true; + } + else + { + outInfo("No plane found in the cloud"); + } } - if (!regions.empty()) - { - foundPlane = true; - } - else + void estimateFromPCL(CAS& tcas, rs::Scene& scene) { - outInfo("No plane found in the cloud"); - } - } + outInfo("Estimating plane form Point Cloud data"); + rs::SceneCas cas(tcas); - void estimateFromPCL(CAS& tcas, rs::Scene& scene) - { - outInfo("Estimating plane form Point Cloud data"); - rs::SceneCas cas(tcas); + cloud = pcl::PointCloud::Ptr(new pcl::PointCloud()); + pcl::ModelCoefficients::Ptr plane_coefficients(new pcl::ModelCoefficients); - cloud = pcl::PointCloud::Ptr(new pcl::PointCloud()); - pcl::ModelCoefficients::Ptr plane_coefficients(new pcl::ModelCoefficients); - - if (useNonNANCloud) - { - rs::ReferenceClusterPoints rcp = rs::create(tcas); - cas.get(VIEW_CLOUD_NON_NAN, rcp); - rs::conversion::from(rcp.cloud(), *cloud); - } - else - { - cas.get(VIEW_CLOUD, *cloud); - } + if (useNonNANCloud) + { + rs::ReferenceClusterPoints rcp = rs::create(tcas); + cas.get(VIEW_CLOUD_NON_NAN, rcp); + rs::conversion::from(rcp.cloud(), *cloud); + } + else + { + cas.get(VIEW_CLOUD, *cloud); + } - if (cloud->size() == 0) - { - outError("No PointCloud present;"); - } + if (cloud->size() == 0) + { + outError("No PointCloud present;"); + } - std::vector planeModel(4); - if (process_cloud(plane_coefficients)) - { - foundPlane = true; - - if (plane_coefficients->values[3] < 0) - { - planeModel[0] = plane_coefficients->values[0]; - planeModel[1] = plane_coefficients->values[1]; - planeModel[2] = plane_coefficients->values[2]; - planeModel[3] = plane_coefficients->values[3]; - } - else - { - planeModel[0] = -plane_coefficients->values[0]; - planeModel[1] = -plane_coefficients->values[1]; - planeModel[2] = -plane_coefficients->values[2]; - planeModel[3] = -plane_coefficients->values[3]; - } - - if (saveToFile) - { - outInfo("Saving Plane to file: " << pathToModelFile); - cv::Mat coeffs = cv::Mat_(4, 1); - for (size_t i = 0; i < planeModel.size(); ++i) - { - coeffs.at(i) = planeModel[i]; - } - cv::FileStorage fs; - fs.open(pathToModelFile, cv::FileStorage::WRITE); - fs << "PlaneModel" << coeffs; - fs.release(); - } - - pcl::PointIndices::Ptr temp(new pcl::PointIndices()); - temp->indices.resize(plane_inliers->indices.size()); - for (size_t i = 0; i < plane_inliers->indices.size(); ++i) - { - temp->indices[i] = mapping_indices[plane_inliers->indices[i]]; - } - plane_inliers.swap(temp); - - cv::Mat mask; - cv::Rect roi; - getMask(*plane_inliers, cv::Size(cloud->width, cloud->height), mask, roi); - - rs::Plane plane = rs::create(tcas); - plane.model(planeModel); - plane.inliers(plane_inliers->indices); - plane.roi(rs::conversion::to(tcas, roi)); - plane.mask(rs::conversion::to(tcas, mask)); - plane.source("RANSAC"); - scene.annotations.append(plane); - } - else - { - outInfo("No plane found in the cloud"); + std::vector planeModel(4); + if (process_cloud(plane_coefficients)) + { + foundPlane = true; + + if (plane_coefficients->values[3] < 0) + { + planeModel[0] = plane_coefficients->values[0]; + planeModel[1] = plane_coefficients->values[1]; + planeModel[2] = plane_coefficients->values[2]; + planeModel[3] = plane_coefficients->values[3]; + } + else + { + planeModel[0] = -plane_coefficients->values[0]; + planeModel[1] = -plane_coefficients->values[1]; + planeModel[2] = -plane_coefficients->values[2]; + planeModel[3] = -plane_coefficients->values[3]; + } + + if (saveToFile) + { + outInfo("Saving Plane to file: " << pathToModelFile); + cv::Mat coeffs = cv::Mat_(4, 1); + for (size_t i = 0; i < planeModel.size(); ++i) + { + coeffs.at(i) = planeModel[i]; + } + cv::FileStorage fs; + fs.open(pathToModelFile, cv::FileStorage::WRITE); + fs << "PlaneModel" << coeffs; + fs.release(); + } + + pcl::PointIndices::Ptr temp(new pcl::PointIndices()); + temp->indices.resize(plane_inliers->indices.size()); + for (size_t i = 0; i < plane_inliers->indices.size(); ++i) + { + temp->indices[i] = mapping_indices[plane_inliers->indices[i]]; + } + plane_inliers.swap(temp); + + cv::Mat mask; + cv::Rect roi; + getMask(*plane_inliers, cv::Size(cloud->width, cloud->height), mask, roi); + + rs::Plane plane = rs::create(tcas); + plane.model(planeModel); + plane.inliers(plane_inliers->indices); + plane.roi(rs::conversion::to(tcas, roi)); + plane.mask(rs::conversion::to(tcas, mask)); + plane.source("RANSAC"); + scene.annotations.append(plane); + } + else + { + outInfo("No plane found in the cloud"); + } } - } - - void loadPlaneModel(CAS& tcas, rs::Scene& scene, bool from_annotation = false) - { - outInfo("loading plane from model file"); - rs::SceneCas cas(tcas); - plane_inliers = pcl::PointIndices::Ptr(new pcl::PointIndices); - cloud = pcl::PointCloud::Ptr(new pcl::PointCloud()); - pcl::PointCloud::Ptr cloudFiltered(new pcl::PointCloud()); - cas.get(VIEW_CLOUD, *cloud); - std::vector planeModel(4); - - if (!from_annotation) - { - cv::Mat planeCoeffs; - cv::FileStorage fs; - - if (fs.open(pathToModelFile, cv::FileStorage::READ)) - { - outInfo("plane model file found"); - } - else - { - outWarn("Could not load plane model. Are you sure you want to load a plane from model file?"); - outWarn("Have You saved a plane_model file in the config folder?"); - exit(1); - } - fs["PlaneModel"] >> planeCoeffs; - - foundPlane = true; - planeModel[0] = planeCoeffs.at(0); - planeModel[1] = planeCoeffs.at(1); - planeModel[2] = planeCoeffs.at(2); - planeModel[3] = planeCoeffs.at(3); - // TODO find inliers for plane; - std::vector mappingIndices; - pcl::removeNaNFromPointCloud(*cloud, *cloudFiltered, mappingIndices); - - for (size_t i = 0; i < cloudFiltered->points.size(); ++i) - { - float num = fabs(planeModel[0] * cloudFiltered->points[i].x + planeModel[1] * cloudFiltered->points[i].y + - planeModel[2] * cloudFiltered->points[i].z + planeModel[3]); - float denum = std::sqrt(std::pow(planeModel[0], 2) + std::pow(planeModel[1], 2) + std::pow(planeModel[2], 2)); - - float dist = num / denum; - if (fabs(dist) < 0.015) - { - plane_inliers->indices.push_back(mappingIndices[i]); - } - } - outInfo("No. of inliers found: " << plane_inliers->indices.size()); - - cv::Mat mask; - cv::Rect roi; - getMask(*plane_inliers, cv::Size(cloud->width, cloud->height), mask, roi); - - rs::Plane plane = rs::create(tcas); - plane.model(planeModel); - plane.inliers(plane_inliers->indices); - plane.roi(rs::conversion::to(tcas, roi)); - plane.mask(rs::conversion::to(tcas, mask)); - plane.source("offline estimation"); - scene.annotations.append(plane); - } - else + void loadPlaneModel(CAS& tcas, rs::Scene& scene, bool from_annotation = false) { - std::vector planes; - scene.annotations.filter(planes); - if (!planes.empty()) - { - planeModel[0] = planes[0].model.get()[0]; - planeModel[1] = planes[0].model.get()[1]; - planeModel[2] = planes[0].model.get()[2]; - planeModel[3] = planes[0].model.get()[3]; + outInfo("loading plane from model file"); + rs::SceneCas cas(tcas); + plane_inliers = pcl::PointIndices::Ptr(new pcl::PointIndices); + cloud = pcl::PointCloud::Ptr(new pcl::PointCloud()); + pcl::PointCloud::Ptr cloudFiltered(new pcl::PointCloud()); + cas.get(VIEW_CLOUD, *cloud); + std::vector planeModel(4); - - // TODO find inliers for plane; - std::vector mappingIndices; - pcl::removeNaNFromPointCloud(*cloud, *cloudFiltered, mappingIndices); - - for (size_t i = 0; i < cloudFiltered->points.size(); ++i) + if (!from_annotation) { - float num = fabs(planeModel[0] * cloudFiltered->points[i].x + planeModel[1] * cloudFiltered->points[i].y + - planeModel[2] * cloudFiltered->points[i].z + planeModel[3]); - float denum = std::sqrt(std::pow(planeModel[0], 2) + std::pow(planeModel[1], 2) + std::pow(planeModel[2], 2)); + cv::Mat planeCoeffs; + cv::FileStorage fs; + + if (fs.open(pathToModelFile, cv::FileStorage::READ)) + { + outInfo("plane model file found"); + } + else + { + outWarn("Could not load plane model. Are you sure you want to load a plane from model file?"); + outWarn("Have You saved a plane_model file in the config folder?"); + exit(1); + } + fs["PlaneModel"] >> planeCoeffs; + + foundPlane = true; + planeModel[0] = planeCoeffs.at(0); + planeModel[1] = planeCoeffs.at(1); + planeModel[2] = planeCoeffs.at(2); + planeModel[3] = planeCoeffs.at(3); + // TODO find inliers for plane; + std::vector mappingIndices; + pcl::removeNaNFromPointCloud(*cloud, *cloudFiltered, mappingIndices); + + for (size_t i = 0; i < cloudFiltered->points.size(); ++i) + { + float num = fabs(planeModel[0] * cloudFiltered->points[i].x + planeModel[1] * cloudFiltered->points[i].y + + planeModel[2] * cloudFiltered->points[i].z + planeModel[3]); + float denum = std::sqrt(std::pow(planeModel[0], 2) + std::pow(planeModel[1], 2) + std::pow(planeModel[2], 2)); + + float dist = num / denum; + if (fabs(dist) < 0.015) + { + plane_inliers->indices.push_back(mappingIndices[i]); + } + } + outInfo("No. of inliers found: " << plane_inliers->indices.size()); + + cv::Mat mask; + cv::Rect roi; + getMask(*plane_inliers, cv::Size(cloud->width, cloud->height), mask, roi); + + rs::Plane plane = rs::create(tcas); + plane.model(planeModel); + plane.inliers(plane_inliers->indices); + plane.roi(rs::conversion::to(tcas, roi)); + plane.mask(rs::conversion::to(tcas, mask)); + plane.source("offline estimation"); + scene.annotations.append(plane); - float dist = num / denum; - if (fabs(dist) < 0.015) - { - plane_inliers->indices.push_back(mappingIndices[i]); - } } - outInfo("No. of inliers found: " << plane_inliers->indices.size()); - - cv::Mat mask; - cv::Rect roi; - getMask(*plane_inliers, cv::Size(cloud->width, cloud->height), mask, roi); - - planes[0].inliers(plane_inliers->indices); - planes[0].roi(rs::conversion::to(tcas, roi)); - planes[0].mask(rs::conversion::to(tcas, mask)); - - } + else + { + std::vector planes; + scene.annotations.filter(planes); + if (!planes.empty()) + { + planeModel[0] = planes[0].model.get()[0]; + planeModel[1] = planes[0].model.get()[1]; + planeModel[2] = planes[0].model.get()[2]; + planeModel[3] = planes[0].model.get()[3]; + + + // TODO find inliers for plane; + std::vector mappingIndices; + pcl::removeNaNFromPointCloud(*cloud, *cloudFiltered, mappingIndices); + + for (size_t i = 0; i < cloudFiltered->points.size(); ++i) + { + float num = fabs(planeModel[0] * cloudFiltered->points[i].x + planeModel[1] * cloudFiltered->points[i].y + + planeModel[2] * cloudFiltered->points[i].z + planeModel[3]); + float denum = std::sqrt(std::pow(planeModel[0], 2) + std::pow(planeModel[1], 2) + std::pow(planeModel[2], 2)); + + float dist = num / denum; + if (fabs(dist) < 0.015) + { + plane_inliers->indices.push_back(mappingIndices[i]); + } + } + outInfo("No. of inliers found: " << plane_inliers->indices.size()); + + cv::Mat mask; + cv::Rect roi; + getMask(*plane_inliers, cv::Size(cloud->width, cloud->height), mask, roi); + + planes[0].inliers(plane_inliers->indices); + planes[0].roi(rs::conversion::to(tcas, roi)); + planes[0].mask(rs::conversion::to(tcas, mask)); + + } + } } - } - bool process_cloud(pcl::ModelCoefficients::Ptr& plane_coefficients) - { - plane_inliers = pcl::PointIndices::Ptr(new pcl::PointIndices); - pcl::PointCloud::Ptr cloud_filtered(cloud); - pcl::PointCloud::Ptr cloud_filtered_no_nan(new pcl::PointCloud); + bool process_cloud(pcl::ModelCoefficients::Ptr& plane_coefficients) + { + plane_inliers = pcl::PointIndices::Ptr(new pcl::PointIndices); + pcl::PointCloud::Ptr cloud_filtered(cloud); + pcl::PointCloud::Ptr cloud_filtered_no_nan(new pcl::PointCloud); - mapping_indices.clear(); + mapping_indices.clear(); - pcl::removeNaNFromPointCloud(*cloud_filtered, *cloud_filtered_no_nan, mapping_indices); + pcl::removeNaNFromPointCloud(*cloud_filtered, *cloud_filtered_no_nan, mapping_indices); - // find the major plane - pcl::SACSegmentation plane_segmentation; + // find the major plane + pcl::SACSegmentation plane_segmentation; - plane_segmentation.setOptimizeCoefficients(true); - plane_segmentation.setModelType(pcl::SACMODEL_PLANE); - plane_segmentation.setMethodType(pcl::SAC_RANSAC); - plane_segmentation.setDistanceThreshold(distance_threshold); - plane_segmentation.setMaxIterations(max_iterations); - plane_segmentation.setInputCloud(cloud_filtered_no_nan); - plane_segmentation.segment(*plane_inliers, *plane_coefficients); + if (planeDirection == "vertical" || planeDirection == "horizontal") { + Eigen::Vector3f axis; + if (planeDirection == "vertical") { + axis = Eigen::Vector3f(0.0, 0.0, 1.0); + } else { + axis = Eigen::Vector3f(0.0, 1.0, 0.0); + } + plane_segmentation.setAxis(axis); + plane_segmentation.setEpsAngle(55.0f * (M_PI/180.0f)); + plane_segmentation.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE); + } else { + plane_segmentation.setModelType(pcl::SACMODEL_PLANE); + } - if (plane_inliers->indices.size() < min_plane_inliers) - { - outWarn("not enough inliers!"); - return false; - } + plane_segmentation.setOptimizeCoefficients(true); + plane_segmentation.setMethodType(pcl::SAC_RANSAC); + plane_segmentation.setDistanceThreshold(distance_threshold); + plane_segmentation.setMaxIterations(max_iterations); + plane_segmentation.setInputCloud(cloud_filtered_no_nan); + plane_segmentation.segment(*plane_inliers, *plane_coefficients); - std::sort(plane_inliers->indices.begin(), plane_inliers->indices.end()); - if (plane_inliers->indices.size() == 0) - { - return false; - } - outDebug("Number of inliers in plane:" << plane_inliers->indices.size()); + if (plane_inliers->indices.size() < min_plane_inliers) + { + outWarn("not enough inliers!"); + return false; + } + + std::sort(plane_inliers->indices.begin(), plane_inliers->indices.end()); + if (plane_inliers->indices.size() == 0) + { + return false; + } + outDebug("Number of inliers in plane:" << plane_inliers->indices.size()); #if DEBUG_OUTPUT - pcl::PCDWriter writer; + pcl::PCDWriter writer; outInfo("Size of input cloud: " << cloud->points.size()); outInfo("Filtered cloud size: " << cloud_filtered_no_nan->points.size()); outInfo("Downsampled cloud size: " << cloud_downsampled->points.size()); @@ -626,135 +644,135 @@ class PlaneAnnotator : public DrawingAnnotator } #endif - return true; - } - - void readCameraInfo(const sensor_msgs::CameraInfo& camInfo) - { - cameraMatrix = cv::Mat(3, 3, CV_64F); - double* it = cameraMatrix.ptr(0); - *it++ = camInfo.K[0]; - *it++ = camInfo.K[1]; - *it++ = camInfo.K[2]; - *it++ = camInfo.K[3]; - *it++ = camInfo.K[4]; - *it++ = camInfo.K[5]; - *it++ = camInfo.K[6]; - *it++ = camInfo.K[7]; - *it++ = camInfo.K[8]; - - distortionCoefficients = cv::Mat(1, camInfo.D.size(), CV_64F); - it = distortionCoefficients.ptr(0); - for (size_t i = 0; i < camInfo.D.size(); ++i, ++it) - { - *it = camInfo.D[i]; + return true; + } + + void readCameraInfo(const sensor_msgs::CameraInfo& camInfo) + { + cameraMatrix = cv::Mat(3, 3, CV_64F); + double* it = cameraMatrix.ptr(0); + *it++ = camInfo.K[0]; + *it++ = camInfo.K[1]; + *it++ = camInfo.K[2]; + *it++ = camInfo.K[3]; + *it++ = camInfo.K[4]; + *it++ = camInfo.K[5]; + *it++ = camInfo.K[6]; + *it++ = camInfo.K[7]; + *it++ = camInfo.K[8]; + + distortionCoefficients = cv::Mat(1, camInfo.D.size(), CV_64F); + it = distortionCoefficients.ptr(0); + for (size_t i = 0; i < camInfo.D.size(); ++i, ++it) + { + *it = camInfo.D[i]; + } } - } - void drawImageWithLock(cv::Mat& disp) - { - if (!foundPlane) - { - disp = cv::Mat::zeros(image_.rows, image_.cols, CV_8UC3); - return; - } - else if (foundPlane && image_.size().area() != cloud->size()) + void drawImageWithLock(cv::Mat& disp) { - disp = cv::Mat::zeros(480, 640, CV_8UC3); - return; - } - std::vector pointsImage; - std::vector axis(4); + if (!foundPlane) + { + disp = cv::Mat::zeros(image_.rows, image_.cols, CV_8UC3); + return; + } + else if (foundPlane && image_.size().area() != cloud->size()) + { + disp = cv::Mat::zeros(480, 640, CV_8UC3); + return; + } + std::vector pointsImage; + std::vector axis(4); - switch (mode) - { - case BOARD: - disp = image_.clone(); - - axis[0] = cv::Point3f(0, 0, 0); - axis[1] = cv::Point3f(0.02, 0, 0); - axis[2] = cv::Point3f(0, 0.02, 0); - axis[3] = cv::Point3f(0, 0, 0.02); - - cv::projectPoints(axis, rotation, translation, cameraMatrix, distortionCoefficients, pointsImage); - - // draw the axes on the colored image - cv::line(disp, pointsImage[0], pointsImage[1], CV_RGB(255, 0, 0), 2, cv::LINE_AA); - cv::line(disp, pointsImage[0], pointsImage[2], CV_RGB(0, 255, 0), 2, cv::LINE_AA); - cv::line(disp, pointsImage[0], pointsImage[3], CV_RGB(0, 0, 255), 2, cv::LINE_AA); - break; - case PCL: - case FILE: - case MPS: - disp = cv::Mat::zeros(cloud->height, cloud->width, CV_8UC3); -#pragma omp parallel for - for (size_t i = 0; i < plane_inliers->indices.size(); ++i) + switch (mode) { - const size_t index = plane_inliers->indices[i]; - const size_t r = index / disp.cols; - const size_t c = index % disp.cols; - const pcl::PointXYZRGBA& point = cloud->at(index); - disp.at(r, c) = cv::Vec3b(point.b, point.g, point.r); + case BOARD: + disp = image_.clone(); + + axis[0] = cv::Point3f(0, 0, 0); + axis[1] = cv::Point3f(0.02, 0, 0); + axis[2] = cv::Point3f(0, 0.02, 0); + axis[3] = cv::Point3f(0, 0, 0.02); + + cv::projectPoints(axis, rotation, translation, cameraMatrix, distortionCoefficients, pointsImage); + + // draw the axes on the colored image + cv::line(disp, pointsImage[0], pointsImage[1], CV_RGB(255, 0, 0), 2, cv::LINE_AA); + cv::line(disp, pointsImage[0], pointsImage[2], CV_RGB(0, 255, 0), 2, cv::LINE_AA); + cv::line(disp, pointsImage[0], pointsImage[3], CV_RGB(0, 0, 255), 2, cv::LINE_AA); + break; + case PCL: + case FILE: + case MPS: + disp = cv::Mat::zeros(cloud->height, cloud->width, CV_8UC3); +#pragma omp parallel for + for (size_t i = 0; i < plane_inliers->indices.size(); ++i) + { + const size_t index = plane_inliers->indices[i]; + const size_t r = index / disp.cols; + const size_t c = index % disp.cols; + const pcl::PointXYZRGBA& point = cloud->at(index); + disp.at(r, c) = cv::Vec3b(point.b, point.g, point.r); + } + break; } - break; } - } - - void fillVisualizerWithLock(pcl::visualization::PCLVisualizer& visualizer, const bool firstRun) - { - const std::string& cloudname = this->name; - pcl::ExtractIndices ei; - // uint32_t colors[6] = {0xFFFF0000, 0xFF00FF00, 0xFF0000FF, 0xFFFFFF00, 0xFFFF00FF, 0xFF00FFFF}; - const pcl::PointCloud::VectorType& origPoints = this->cloud->points; - pcl::PointCloud::Ptr output; - switch (mode) + void fillVisualizerWithLock(pcl::visualization::PCLVisualizer& visualizer, const bool firstRun) { - case BOARD: - output = cloud; - break; - case FILE: - case PCL: - output.reset(new pcl::PointCloud); - ei.setInputCloud(cloud); - ei.setIndices(plane_inliers); - // ei.setKeepOrganized(true); - ei.filter(*output); - break; - case MPS: - output.reset(new pcl::PointCloud); - for (size_t i = 0; i < inlierIndices.size(); ++i) - { - const pcl::PointIndices& indices = this->inlierIndices[i]; - const size_t outIndex = output->points.size(); - output->points.resize(outIndex + indices.indices.size()); - uint32_t rgba = rs::common::colors[i % rs::common::numberOfColors]; + const std::string& cloudname = this->name; + pcl::ExtractIndices ei; + // uint32_t colors[6] = {0xFFFF0000, 0xFF00FF00, 0xFF0000FF, 0xFFFFFF00, 0xFFFF00FF, 0xFF00FFFF}; + const pcl::PointCloud::VectorType& origPoints = this->cloud->points; + + pcl::PointCloud::Ptr output; + switch (mode) + { + case BOARD: + output = cloud; + break; + case FILE: + case PCL: + output.reset(new pcl::PointCloud); + ei.setInputCloud(cloud); + ei.setIndices(plane_inliers); + // ei.setKeepOrganized(true); + ei.filter(*output); + break; + case MPS: + output.reset(new pcl::PointCloud); + for (size_t i = 0; i < inlierIndices.size(); ++i) + { + const pcl::PointIndices& indices = this->inlierIndices[i]; + const size_t outIndex = output->points.size(); + output->points.resize(outIndex + indices.indices.size()); + uint32_t rgba = rs::common::colors[i % rs::common::numberOfColors]; #pragma omp parallel for - for (size_t j = 0; j < indices.indices.size(); ++j) - { - const size_t index = indices.indices[j]; - output->points[outIndex + j] = origPoints[index]; - output->points[outIndex + j].rgba = rgba; - } - } - output->width = output->points.size(); - output->height = 1; - output->is_dense = 1; - break; - } + for (size_t j = 0; j < indices.indices.size(); ++j) + { + const size_t index = indices.indices[j]; + output->points[outIndex + j] = origPoints[index]; + output->points[outIndex + j].rgba = rgba; + } + } + output->width = output->points.size(); + output->height = 1; + output->is_dense = 1; + break; + } - if (firstRun) - { - visualizer.addPointCloud(output, cloudname); - visualizer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pointSize, cloudname); - } - else - { - visualizer.updatePointCloud(output, cloudname); - visualizer.getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pointSize, cloudname); + if (firstRun) + { + visualizer.addPointCloud(output, cloudname); + visualizer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pointSize, cloudname); + } + else + { + visualizer.updatePointCloud(output, cloudname); + visualizer.getPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, pointSize, cloudname); + } } - } }; MAKE_AE(PlaneAnnotator)