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 56 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 182 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 1694 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 214 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 1220 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 317 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 649 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, 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 1256 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 1629 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 575 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 1674 of file PbMapMaker.cpp.
References mPbMap, mtx_pbmap_busy, and mrpt::pbmap::PbMap::savePbMap().
|
protected |
PbMapMaker's exit thread
Definition at line 1683 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 1306 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 356 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 168 of file PbMapMaker.h.
Referenced by PbMapMaker(), saveInfoFiles(), and watchProperties().
|
protected |
Definition at line 168 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 174 of file PbMapMaker.h.
Referenced by PbMapMaker(), saveInfoFiles(), and watchProperties().
|
protected |
Definition at line 174 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 169 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 172 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 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().
pcl::visualization::CloudViewer mrpt::pbmap::PbMapMaker::cloudViewer |
PCL viewer. It runs in a different thread.
Definition at line 80 of file PbMapMaker.h.
Referenced by run().
|
private |
Object to cluster set of planes according to their co-visibility.
Definition at line 131 of file PbMapMaker.h.
Referenced by detectPlanesCloud(), PbMapMaker(), and run().
|
private |
File containing some paramteres and heuristic thresholds"
Definition at line 125 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 74 of file PbMapMaker.h.
Referenced by run().
|
protected |
PbMapMaker's stop var
Definition at line 161 of file PbMapMaker.h.
Referenced by run(), and stop_pbMapMaker().
|
protected |
PbMapMaker's stop controller
Definition at line 158 of file PbMapMaker.h.
Referenced by run(), and stop_pbMapMaker().
|
protected |
The current PbMap.
Definition at line 137 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 128 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 143 of file PbMapMaker.h.
Referenced by detectPlanesCloud(), mergePlanes(), PbMapMaker(), and ~PbMapMaker().
|
private |
Definition at line 133 of file PbMapMaker.h.
Referenced by detectPlanesCloud(), and serializePbMap().
|
protected |
List of planes observed in that last frame introduced.
Definition at line 140 of file PbMapMaker.h.
Referenced by detectPlanesCloud(), and run().
|
protected |
PbMapMaker's thread handle
Definition at line 152 of file PbMapMaker.h.
Referenced by PbMapMaker(), and stop_pbMapMaker().
|
protected |
Definition at line 168 of file PbMapMaker.h.
Referenced by PbMapMaker(), saveInfoFiles(), and watchProperties().
|
protected |
Definition at line 168 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 174 of file PbMapMaker.h.
Referenced by PbMapMaker(), saveInfoFiles(), and watchProperties().
|
protected |
Definition at line 174 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 169 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 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().
|
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().
std::set<unsigned> mrpt::pbmap::PbMapMaker::sQueueObservedPlanes |
observedPlanes is a list containing the current observed planes.
Definition at line 77 of file PbMapMaker.h.
Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at miƩ 12 jul 2023 10:03:34 CEST |