This class construct the PbMap extracting planar segments from Range images, which pose must be also provided. The range images and their poses are communicated with the object frameQueue. PbMapMaker run its own thread, which is created at initialization.
Definition at line 57 of file PbMapMaker.h.
#include <mrpt/pbmap/PbMapMaker.h>
Public Member Functions | |
PbMapMaker (const std::string &config_file) | |
~PbMapMaker () | |
PbMap | getPbMap () |
void | serializePbMap (std::string path) |
Public Attributes | |
std::vector< frameRGBDandPose > | frameQueue |
std::set< unsigned > | sQueueObservedPlanes |
pcl::visualization::CloudViewer | cloudViewer |
Protected Member Functions | |
void | viz_cb (pcl::visualization::PCLVisualizer &viz) |
void | run () |
bool | stop_pbMapMaker () |
void | watchProperties (std::set< unsigned > &observedPlanes, Plane &observedPlane) |
double dist_THRESHOLDFull = 1.2; double dist_THRESHOLDFull_inv = 1/dist_THRESHOLDFull; More... | |
void | saveInfoFiles () |
Protected Attributes | |
PbMap | mPbMap |
std::set< unsigned > | observedPlanes |
PlaneInferredInfo * | mpPlaneInferInfo |
std::thread | pbmaker_hd |
bool | m_pbmaker_must_stop |
bool | m_pbmaker_finished |
float | rejectAreaF |
float | acceptAreaF |
float | rejectAreaT |
float | acceptAreaT |
float | rejectElongF |
float | acceptElongF |
float | rejectElongT |
float | acceptElongT |
float | rejectC1C2C3_F |
float | acceptC1C2C3_F |
float | rejectC1C2C3_T |
float | acceptC1C2C3_T |
float | rejectNrgb_F |
float | acceptNrgb_F |
float | rejectNrgb_T |
float | acceptNrgb_T |
float | rejectIntensity_F |
float | acceptIntensity_F |
float | rejectIntensity_T |
float | acceptIntensity_T |
float | rejectColor_F |
float | acceptColor_F |
float | rejectColor_T |
float | acceptColor_T |
float | rejectHistH_F |
float | acceptHistH_F |
float | rejectHistH_T |
float | acceptHistH_T |
Private Member Functions | |
void | detectPlanesCloud (pcl::PointCloud< PointT >::Ptr &pointCloudPtr_arg, Eigen::Matrix4f &poseKF, double distThreshold, double angleThreshold, double minInliersF) |
bool | arePlanesNearby (Plane &plane1, Plane &plane2, const float distThreshold) |
void | checkProximity (Plane &plane, float proximity) |
bool | areSamePlane (Plane &plane1, Plane &plane2, const float &cosAngleThreshold, const float &distThreshold, const float &proxThreshold) |
void | mergePlanes (Plane &updatePlane, Plane &discardPlane) |
Private Attributes | |
FILE * | config_Param |
PbMapLocaliser * | mpPbMapLocaliser |
SemanticClustering * | clusterize |
boost::mutex | mtx_pbmap_busy |
PbMapMaker::PbMapMaker | ( | const std::string & | config_file | ) |
PbMapMaker's constructor sets some threshold for plane segmentation and map growing from a configuration file (or default). This constructor also starts PbMapMaker's own thread.
Definition at line 181 of file PbMapMaker.cpp.
References acceptAreaF, acceptAreaT, acceptC1C2C3_F, acceptC1C2C3_T, acceptColor_F, acceptColor_T, acceptElongF, acceptElongT, acceptHistH_F, acceptHistH_T, acceptIntensity_F, acceptIntensity_T, acceptNrgb_F, acceptNrgb_T, ASSERT_FILE_EXISTS_, clusterize, config_pbmap::config_localiser, configPbMap, config_pbmap::detect_loopClosure, config_pbmap::makeClusters, mPbMap, mpPbMapLocaliser, mpPlaneInferInfo, pbmaker_hd, readConfigFile(), rejectAreaF, rejectAreaT, rejectC1C2C3_F, rejectC1C2C3_T, rejectColor_F, rejectColor_T, rejectElongF, rejectElongT, rejectHistH_F, rejectHistH_T, rejectIntensity_F, rejectIntensity_T, rejectNrgb_F, rejectNrgb_T, and run().
PbMapMaker::~PbMapMaker | ( | ) |
PbMapMaker's destructor is used to save some debugging info to file.
Definition at line 1691 of file PbMapMaker.cpp.
References mpPbMapLocaliser, mpPlaneInferInfo, saveInfoFiles(), and stop_pbMapMaker().
|
private |
Returns true when the closest distance between the patches "plane1" and "plane2" is under distThreshold.
cout << "Elements\n" << plane2.v3normal << "\n xyz " << plane1.polygonContourPtr->points[i].x << " " << plane1.polygonContourPtr->points[i].y << " " << plane1.polygonContourPtr->points[i].z
<< " xyz2 " << plane1.polygonContourPtr->points[i-1].x << " "
< plane1.polygonContourPtr->points[i-1].y << " " << plane1.polygonContourPtr->points[i-1].z << endl;
assert( plane2.v3normal.dot(diffPoints(plane1.polygonContourPtr->points[i], plane1.polygonContourPtr->points[i-1]) ) != 0 );
Definition at line 213 of file PbMapMaker.cpp.
References mrpt::pbmap::diffPoints(), mrpt::pbmap::dist3D_Segment_to_Segment2(), mrpt::pbmap::getVector3fromPointXYZ(), mrpt::pbmap::isInHull(), mrpt::pbmap::Plane::polygonContourPtr, mrpt::pbmap::Plane::v3center, and mrpt::pbmap::Plane::v3normal.
Referenced by areSamePlane(), and checkProximity().
|
private |
Returns true if the two input planes represent the same physical surface for some given angle and distance thresholds. If the planes are the same they are merged in this and the function returns true. Otherwise it returns false.
Check if the the input plane is the same than this plane for some given angle and distance thresholds. If the planes are the same they are merged in this and the function returns true. Otherwise it returns false.
Definition at line 1217 of file PbMapMaker.cpp.
References arePlanesNearby(), mrpt::pbmap::Plane::v3center, and mrpt::pbmap::Plane::v3normal.
Referenced by detectPlanesCloud().
Check for new graph connections of the input plane. These connections are stablished when the minimum distance between two patches is under the input threshold "proximity"
Definition at line 316 of file PbMapMaker.cpp.
References arePlanesNearby(), mrpt::pbmap::Plane::id, mPbMap, mrpt::pbmap::Plane::nearbyPlanes, and mrpt::pbmap::PbMap::vPlanes.
Referenced by detectPlanesCloud().
|
private |
Find planar patches in the input organised point cloud "pointCloudPtr_arg", and update the PbMap with them (it update previous planes and initialize new ones when they are first observed), the input pose "poseInv" is used to place the current observations into a common frame of reference. Different thresholds are used to control the plane segmentation:
cout << " ID " << detectedPlanes[i].id << "
obs " << detectedPlanes[i].numObservations;
cout << " areaVoxels " <<
detectedPlanes[i].areaVoxels << " areaVoxels " << detectedPlanes[i].areaHull;
cout << " ratioXY " <<
detectedPlanes[i].elongation << " structure " << detectedPlanes[i].bFromStructure << " label " << detectedPlanes[i].label;
cout << "\n PpalComp\n" <<
detectedPlanes[i].v3PpalDir << "\n RGB\n" << detectedPlanes[i].v3colorNrgb;
cout << "\n Neighbors (" <<
detectedPlanes[i].neighborPlanes.size() << "): ";
for(map<unsigned,unsigned>::iterator
it=detectedPlanes[i].neighborPlanes.begin(); it != detectedPlanes[i].neighborPlanes.end(); it++)
for(map<unsigned,unsigned>::iterator
it=detectedPlanes[i].neighborPlanes.begin(); it != detectedPlanes[i].neighborPlanes.end(); it++)
Definition at line 646 of file PbMapMaker.cpp.
References mrpt::pbmap::Plane::areaHull, mrpt::pbmap::Plane::areaVoxels, areSamePlane(), mrpt::pbmap::Plane::calcConvexHull(), mrpt::pbmap::Plane::calcElongationAndPpalDir(), mrpt::pbmap::Plane::calcMainColor(), checkProximity(), clusterize, mrpt::pbmap::compose(), mrpt::pbmap::Plane::compute2DPolygonalArea(), mrpt::pbmap::Plane::computeMassCenterAndArea(), configPbMap, CS_visualize, mrpt::pbmap::SemanticClustering::currentSemanticGroup, config_pbmap::detect_loopClosure, mrpt::pbmap::PbMap::FloorPlane, for(), mrpt::pbmap::PbMap::globalMapPtr, mrpt::pbmap::SemanticClustering::groups, mrpt::pbmap::Plane::id, config_pbmap::inferStructure, config_pbmap::makeClusters, config_pbmap::max_cos_normal, config_pbmap::max_dist_center_plane, mergePlanes(), mPbMap, mpPbMapLocaliser, mpPlaneInferInfo, mtx_pbmap_busy, observedPlanes, mrpt::pbmap::Plane::planePointCloudPtr, mrpt::pbmap::Plane::polygonContourPtr, config_pbmap::proximity_neighbor_planes, config_pbmap::proximity_threshold, mrpt::pbmap::PlaneInferredInfo::searchTheFloor(), mrpt::pbmap::Plane::v3center, mrpt::pbmap::Plane::v3colorNrgb, mrpt::pbmap::Plane::v3normal, mrpt::pbmap::PbMap::vPlanes, and mrpt::pbmap::PbMapLocaliser::vQueueObservedPlanes.
Referenced by run().
|
inline |
Merge the two input patches into "updatePlane". Recalculate center, normal vector, area, inlier points (filtered), convex hull, etc.
Definition at line 1253 of file PbMapMaker.cpp.
References mrpt::pbmap::Plane::areaVoxels, mrpt::pbmap::Plane::calcConvexHull(), mrpt::pbmap::Plane::computeMassCenterAndArea(), mrpt::pbmap::Plane::forcePtsLayOnPlane(), mrpt::pbmap::PlaneInferredInfo::isFullExtent(), mpPlaneInferInfo, mrpt::math::norm(), mrpt::pbmap::Plane::planePointCloudPtr, mrpt::pbmap::Plane::polygonContourPtr, and mrpt::pbmap::Plane::v3normal.
Referenced by detectPlanesCloud().
|
protected |
This executes the PbMapMaker's thread
Definition at line 1626 of file PbMapMaker.cpp.
References config_pbmap::angle_threshold, cloudViewer, clusterize, configPbMap, detectPlanesCloud(), config_pbmap::dist_threshold, mrpt::pbmap::SemanticClustering::evalPartition(), frameQueue, keyboardEventOccurred(), m_pbmaker_finished, m_pbmaker_must_stop, config_pbmap::makeClusters, config_pbmap::minInliersRate, mPbMap, observedPlanes, viz_cb(), and mrpt::pbmap::PbMap::vPlanes.
Referenced by PbMapMaker().
|
protected |
Definition at line 572 of file PbMapMaker.cpp.
References acceptAreaF, acceptAreaT, acceptC1C2C3_F, acceptC1C2C3_T, acceptColor_F, acceptColor_T, acceptElongF, acceptElongT, acceptHistH_F, acceptHistH_T, acceptIntensity_F, acceptIntensity_T, acceptNrgb_F, acceptNrgb_T, config_pbmap::color_threshold, configPbMap, config_pbmap::hue_threshold, config_pbmap::intensity_threshold, rejectAreaF, rejectAreaT, rejectC1C2C3_F, rejectC1C2C3_T, rejectColor_F, rejectColor_T, rejectElongF, rejectElongT, rejectHistH_F, rejectHistH_T, rejectIntensity_F, rejectIntensity_T, rejectNrgb_F, and rejectNrgb_T.
Referenced by ~PbMapMaker().
void PbMapMaker::serializePbMap | ( | std::string | path | ) |
Serialize the PbMap.
Definition at line 1671 of file PbMapMaker.cpp.
References mPbMap, mtx_pbmap_busy, and mrpt::pbmap::PbMap::savePbMap().
|
protected |
PbMapMaker's exit thread
Definition at line 1680 of file PbMapMaker.cpp.
References m_pbmaker_finished, m_pbmaker_must_stop, and pbmaker_hd.
Referenced by ~PbMapMaker().
|
protected |
PCL visualizer callback
pcl::visualization::PointCloudColorHandlerCustom PointT> color (plane_i.planePointCloudPtr, red[plane_i.semanticGroup%10], grn[plane_i.semanticGroup%10], blu[plane_i.semanticGroup%10]);
Definition at line 1303 of file PbMapMaker.cpp.
References ablu, agrn, mrpt::pbmap::PbMapLocaliser::alignedModelPtr, ared, blu, CS_visualize, mrpt::pbmap::Plane::dominantIntensity, mrpt::pbmap::PbMapLocaliser::foundPlaces, mrpt::pbmap::PbMap::globalMapPtr, graphRepresentation, grn, mPbMap, mpPbMapLocaliser, mrpt::pbmap::Plane::planePointCloudPtr, mrpt::pbmap::Plane::polygonContourPtr, red, mrpt::system::os::sprintf(), mrpt::pbmap::Plane::v3center, mrpt::pbmap::Plane::v3colorNrgb, mrpt::pbmap::Plane::v3normal, and mrpt::pbmap::PbMap::vPlanes.
Referenced by run().
|
protected |
double dist_THRESHOLDFull = 1.2; double dist_THRESHOLDFull_inv = 1/dist_THRESHOLDFull;
Definition at line 355 of file PbMapMaker.cpp.
References acceptAreaF, acceptAreaT, acceptC1C2C3_F, acceptC1C2C3_T, acceptColor_F, acceptColor_T, acceptElongF, acceptElongT, acceptHistH_F, acceptHistH_T, acceptIntensity_F, acceptIntensity_T, acceptNrgb_F, acceptNrgb_T, BhattacharyyaDist(), config_pbmap::color_threshold, configPbMap, mrpt::pbmap::Plane::dominantIntensity, mrpt::pbmap::Plane::elongation, mrpt::pbmap::Plane::hist_H, config_pbmap::hue_threshold, mrpt::pbmap::Plane::id, config_pbmap::intensity_threshold, mPbMap, mrpt::math::norm(), mrpt::pbmap::Plane::numObservations, mrpt::pbmap::Plane::prog_area, mrpt::pbmap::Plane::prog_C1C2C3, mrpt::pbmap::Plane::prog_elongation, mrpt::pbmap::Plane::prog_hist_H, mrpt::pbmap::Plane::prog_intensity, mrpt::pbmap::Plane::prog_Nrgb, rejectAreaF, rejectAreaT, rejectC1C2C3_F, rejectC1C2C3_T, rejectColor_F, rejectColor_T, rejectElongF, rejectElongT, rejectHistH_F, rejectHistH_T, rejectIntensity_F, rejectIntensity_T, rejectNrgb_F, rejectNrgb_T, mrpt::pbmap::Plane::v3colorC1C2C3, mrpt::pbmap::Plane::v3colorNrgb, and mrpt::pbmap::PbMap::vPlanes.
|
protected |
Definition at line 169 of file PbMapMaker.h.
Referenced by PbMapMaker(), saveInfoFiles(), and watchProperties().
|
protected |
Definition at line 169 of file PbMapMaker.h.
Referenced by PbMapMaker(), saveInfoFiles(), and watchProperties().
|
protected |
Definition at line 171 of file PbMapMaker.h.
Referenced by PbMapMaker(), saveInfoFiles(), and watchProperties().
|
protected |
Definition at line 171 of file PbMapMaker.h.
Referenced by PbMapMaker(), saveInfoFiles(), and watchProperties().
|
protected |
Definition at line 175 of file PbMapMaker.h.
Referenced by PbMapMaker(), saveInfoFiles(), and watchProperties().
|
protected |
Definition at line 175 of file PbMapMaker.h.
Referenced by PbMapMaker(), saveInfoFiles(), and watchProperties().
|
protected |
Definition at line 170 of file PbMapMaker.h.
Referenced by PbMapMaker(), saveInfoFiles(), and watchProperties().
|
protected |
Definition at line 170 of file PbMapMaker.h.
Referenced by PbMapMaker(), saveInfoFiles(), and watchProperties().
|
protected |
Definition at line 176 of file PbMapMaker.h.
Referenced by PbMapMaker(), saveInfoFiles(), and watchProperties().
|
protected |
Definition at line 176 of file PbMapMaker.h.
Referenced by PbMapMaker(), saveInfoFiles(), and watchProperties().
|
protected |
Definition at line 173 of file PbMapMaker.h.
Referenced by PbMapMaker(), saveInfoFiles(), and watchProperties().
|
protected |
Definition at line 173 of file PbMapMaker.h.
Referenced by PbMapMaker(), saveInfoFiles(), and watchProperties().
|
protected |
Definition at line 172 of file PbMapMaker.h.
Referenced by PbMapMaker(), saveInfoFiles(), and watchProperties().
|
protected |
Definition at line 172 of file PbMapMaker.h.
Referenced by PbMapMaker(), saveInfoFiles(), and watchProperties().
pcl::visualization::CloudViewer mrpt::pbmap::PbMapMaker::cloudViewer |
PCL viewer. It runs in a different thread.
Definition at line 81 of file PbMapMaker.h.
Referenced by run().
|
private |
Object to cluster set of planes according to their co-visibility.
Definition at line 132 of file PbMapMaker.h.
Referenced by detectPlanesCloud(), PbMapMaker(), and run().
|
private |
File containing some paramteres and heuristic thresholds"
Definition at line 126 of file PbMapMaker.h.
std::vector<frameRGBDandPose> mrpt::pbmap::PbMapMaker::frameQueue |
frameQueue is a vector containing the frameRGBDandPose (range image + pose) to be processed.
Definition at line 75 of file PbMapMaker.h.
Referenced by run().
|
protected |
PbMapMaker's stop var
Definition at line 162 of file PbMapMaker.h.
Referenced by run(), and stop_pbMapMaker().
|
protected |
PbMapMaker's stop controller
Definition at line 159 of file PbMapMaker.h.
Referenced by run(), and stop_pbMapMaker().
|
protected |
The current PbMap.
Definition at line 138 of file PbMapMaker.h.
Referenced by checkProximity(), detectPlanesCloud(), getPbMap(), PbMapMaker(), run(), serializePbMap(), viz_cb(), and watchProperties().
|
private |
Object to detect previous places.
Definition at line 129 of file PbMapMaker.h.
Referenced by detectPlanesCloud(), PbMapMaker(), viz_cb(), and ~PbMapMaker().
|
protected |
Object to infer some knowledge in the map planes.
Definition at line 144 of file PbMapMaker.h.
Referenced by detectPlanesCloud(), mergePlanes(), PbMapMaker(), and ~PbMapMaker().
|
private |
Definition at line 134 of file PbMapMaker.h.
Referenced by detectPlanesCloud(), and serializePbMap().
|
protected |
List of planes observed in that last frame introduced.
Definition at line 141 of file PbMapMaker.h.
Referenced by detectPlanesCloud(), and run().
|
protected |
PbMapMaker's thread handle
Definition at line 153 of file PbMapMaker.h.
Referenced by PbMapMaker(), and stop_pbMapMaker().
|
protected |
Definition at line 169 of file PbMapMaker.h.
Referenced by PbMapMaker(), saveInfoFiles(), and watchProperties().
|
protected |
Definition at line 169 of file PbMapMaker.h.
Referenced by PbMapMaker(), saveInfoFiles(), and watchProperties().
|
protected |
Definition at line 171 of file PbMapMaker.h.
Referenced by PbMapMaker(), saveInfoFiles(), and watchProperties().
|
protected |
Definition at line 171 of file PbMapMaker.h.
Referenced by PbMapMaker(), saveInfoFiles(), and watchProperties().
|
protected |
Definition at line 175 of file PbMapMaker.h.
Referenced by PbMapMaker(), saveInfoFiles(), and watchProperties().
|
protected |
Definition at line 175 of file PbMapMaker.h.
Referenced by PbMapMaker(), saveInfoFiles(), and watchProperties().
|
protected |
Definition at line 170 of file PbMapMaker.h.
Referenced by PbMapMaker(), saveInfoFiles(), and watchProperties().
|
protected |
Definition at line 170 of file PbMapMaker.h.
Referenced by PbMapMaker(), saveInfoFiles(), and watchProperties().
|
protected |
Definition at line 176 of file PbMapMaker.h.
Referenced by PbMapMaker(), saveInfoFiles(), and watchProperties().
|
protected |
Definition at line 176 of file PbMapMaker.h.
Referenced by PbMapMaker(), saveInfoFiles(), and watchProperties().
|
protected |
Definition at line 173 of file PbMapMaker.h.
Referenced by PbMapMaker(), saveInfoFiles(), and watchProperties().
|
protected |
Definition at line 173 of file PbMapMaker.h.
Referenced by PbMapMaker(), saveInfoFiles(), and watchProperties().
|
protected |
Definition at line 172 of file PbMapMaker.h.
Referenced by PbMapMaker(), saveInfoFiles(), and watchProperties().
|
protected |
Definition at line 172 of file PbMapMaker.h.
Referenced by PbMapMaker(), saveInfoFiles(), and watchProperties().
std::set<unsigned> mrpt::pbmap::PbMapMaker::sQueueObservedPlanes |
observedPlanes is a list containing the current observed planes.
Definition at line 78 of file PbMapMaker.h.
Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ae4571287 Thu Nov 23 00:06:53 2017 +0100 at dom oct 27 23:51:55 CET 2019 |