Skip to content

Commit 8e18e04

Browse files
jwietrzykowskijwietrzykowski
committed
Work on selecting visible planes - initial working version.
1 parent a41bd3e commit 8e18e04

File tree

6 files changed

+291
-47
lines changed

6 files changed

+291
-47
lines changed

‎include/ConcaveHull.hpp

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -88,6 +88,11 @@ class ConcaveHull {
8888
ConcaveHull intersect(const std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> &otherPolygons3d,
8989
double areaThresh = 0.05) const;
9090

91+
ConcaveHull clipToCameraFrustum(const cv::Mat K,
92+
int rows,
93+
int cols,
94+
double minZ);
95+
9196
ConcaveHull transform(const Vector7d &transform) const;
9297

9398
double minDistance(const ConcaveHull &other) const;

‎include/Map.hpp

Lines changed: 7 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -177,7 +177,13 @@ class Map{
177177

178178
void shiftIds(int startId);
179179

180-
std::vector<int> getVisibleObjs(Vector7d pose, cv::Mat cameraMatrix, int rows, int cols);
180+
std::vector<int> getVisibleObjs(Vector7d pose,
181+
cv::Mat cameraMatrix,
182+
int rows,
183+
int cols,
184+
pcl::visualization::PCLVisualizer::Ptr viewer = nullptr,
185+
int viewPort1 = -1,
186+
int viewPort2 = -1);
181187

182188
inline pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr getOriginalPointCloud(){
183189
return originalPointCloud;

‎src/ConcaveHull.cpp

Lines changed: 63 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -379,6 +379,68 @@ ConcaveHull::intersect(const std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr>
379379
yAxis);
380380
}
381381

382+
ConcaveHull
383+
ConcaveHull::clipToCameraFrustum(const cv::Mat K, int rows, int cols, double minZ)
384+
{
385+
vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clippedPolygons3d;
386+
for(pcl::PointCloud<pcl::PointXYZRGB>::Ptr curPoly3d : polygons3d){
387+
pcl::PointCloud<pcl::PointXYZRGB>::Ptr curClipped(new pcl::PointCloud<pcl::PointXYZRGB>());
388+
389+
for(int pt = 0; pt < curPoly3d->size(); ++pt){
390+
int prevPtIdx = (pt + curPoly3d->size() - 1) % curPoly3d->size();
391+
const pcl::PointXYZRGB &curPt = curPoly3d->at(pt);
392+
const pcl::PointXYZRGB &prevPt = curPoly3d->at(prevPtIdx);
393+
394+
// always add current point if is valid
395+
if(curPt.z >= minZ){
396+
curClipped->push_back(curPt);
397+
}
398+
399+
// if entering or leaving valid region, add point on a border
400+
if(curPt.z >= minZ && prevPt.z < minZ ||
401+
curPt.z < minZ && prevPt.z >= minZ)
402+
{
403+
double dprev = abs(prevPt.z - minZ);
404+
double dcur = abs(curPt.z - minZ);
405+
406+
// both points are close to boundary
407+
if(dprev + dcur < 1e-6){
408+
curClipped->push_back(curPt);
409+
}
410+
else {
411+
double s = dprev / (dprev + dcur);
412+
413+
pcl::PointXYZRGB clipPt = prevPt;
414+
clipPt.x += s*(curPt.x - prevPt.x);
415+
clipPt.y += s*(curPt.y - prevPt.y);
416+
clipPt.z += s*(curPt.z - prevPt.z);
417+
418+
curClipped->push_back(clipPt);
419+
}
420+
}
421+
}
422+
if(curClipped->size() > 2){
423+
clippedPolygons3d.push_back(curClipped);
424+
}
425+
}
426+
427+
vector<Polygon_2> clippedPolygons;
428+
for(pcl::PointCloud<pcl::PointXYZRGB>::Ptr clipPoly3d : clippedPolygons3d){
429+
clippedPolygons.push_back(Polygon_2());
430+
for(int pt = 0; pt < clipPoly3d->size(); ++pt){
431+
clippedPolygons.back().push_back(point3dTo2d(clipPoly3d->at(pt).getVector3fMap().cast<double>()));
432+
}
433+
}
434+
435+
return ConcaveHull(clippedPolygons,
436+
clippedPolygons3d,
437+
plNormal,
438+
plD,
439+
origin,
440+
xAxis,
441+
yAxis);
442+
}
443+
382444
double ConcaveHull::minDistance(const ConcaveHull &other) const {
383445
double minDist = std::numeric_limits<double>::max();
384446

@@ -523,3 +585,4 @@ Eigen::Vector3d ConcaveHull::point2dTo3d(const ConcaveHull::Point_2 &point2d) co
523585

524586

525587

588+

‎src/Map.cpp

Lines changed: 189 additions & 45 deletions
Original file line numberDiff line numberDiff line change
@@ -658,85 +658,229 @@ void Map::shiftIds(int startId) {
658658
clearPending();
659659
}
660660

661-
std::vector<int> Map::getVisibleObjs(Vector7d pose, cv::Mat cameraMatrix, int rows, int cols) {
661+
std::vector<int> Map::getVisibleObjs(Vector7d pose,
662+
cv::Mat cameraMatrix,
663+
int rows,
664+
int cols,
665+
pcl::visualization::PCLVisualizer::Ptr viewer,
666+
int viewPort1,
667+
int viewPort2)
668+
{
669+
static constexpr double shadingLevel = 1.0/8;
670+
662671
g2o::SE3Quat poseSE3Quat(pose);
663672
Eigen::Matrix4d poseMat = poseSE3Quat.to_homogeneous_matrix();
664673
Eigen::Matrix4d poseInvMat = poseSE3Quat.inverse().to_homogeneous_matrix();
674+
Eigen::Matrix4d poseMatt = poseSE3Quat.to_homogeneous_matrix().transpose();
665675
Eigen::Matrix3d R = poseMat.block<3, 3>(0, 0);
666676
Eigen::Vector3d t = poseMat.block<3, 1>(0, 3);
667-
Eigen::Vector3d zAxis = R.col(2);
668677

669-
vectorVector2d imageCorners;
670-
imageCorners.push_back((Eigen::Vector2d() << 0, 0).finished());
671-
imageCorners.push_back((Eigen::Vector2d() << cols - 1, 0).finished());
672-
imageCorners.push_back((Eigen::Vector2d() << cols - 1, rows - 1).finished());
673-
imageCorners.push_back((Eigen::Vector2d() << 0, rows - 1).finished());
678+
// vectorVector2d imageCorners;
679+
// imageCorners.push_back((Eigen::Vector2d() << 0, 0).finished());
680+
// imageCorners.push_back((Eigen::Vector2d() << cols - 1, 0).finished());
681+
// imageCorners.push_back((Eigen::Vector2d() << cols - 1, rows - 1).finished());
682+
// imageCorners.push_back((Eigen::Vector2d() << 0, rows - 1).finished());
683+
684+
if(viewer){
685+
viewer->removeAllPointClouds();
686+
viewer->removeAllShapes();
687+
688+
for (auto it = objInstances.begin(); it != objInstances.end(); ++it) {
689+
it->display(viewer, viewPort1, shadingLevel);
690+
}
691+
viewer->addCoordinateSystem();
692+
Eigen::Affine3f trans = Eigen::Affine3f::Identity();
693+
trans.matrix() = poseMat.cast<float>();
694+
viewer->addCoordinateSystem(0.5, trans, "camera_coord");
695+
}
696+
697+
vector<int> visible;
674698

675699
vector<vector<vector<pair<double, int>>>> projPlanes(rows,
676700
vector<vector<pair<double, int>>>(cols,
677701
vector<pair<double, int>>()));
678702

679703
cv::Mat projPoly(rows, cols, CV_8UC1);
680-
for(auto it = objInstances.begin(); it != objInstances.end(); ++it){
704+
for(auto it = objInstances.begin(); it != objInstances.end(); ++it) {
705+
cout << "id = " << it->getId() << endl;
706+
707+
Eigen::Vector4d planeEqCamera = poseMatt * it->getNormal();
708+
cout << "planeEqCamera = " << planeEqCamera.transpose() << endl;
709+
710+
if (viewer) {
711+
it->cleanDisplay(viewer, viewPort1);
712+
it->display(viewer, viewPort1);
713+
}
714+
681715
// TODO condition for observing the right face of the plane
716+
Eigen::Vector3d normal = planeEqCamera.head<3>();
717+
double d = -planeEqCamera(3);
718+
Eigen::Vector3d zAxis;
719+
zAxis << 0, 0, 1;
682720

683-
projPoly.setTo(0);
684-
vector<cv::Point*> polyCont;
685-
vector<int> polyContNpts;
721+
cout << "normal.dot(zAxis) = " << normal.dot(zAxis) << endl;
722+
cout << "d = " << d << endl;
723+
if (normal.dot(zAxis) < 0 && d < 0) {
724+
// vectorVector3d imageCorners3d;
725+
// bool valid = Misc::projectImagePointsOntoPlane(imageCorners,
726+
// imageCorners3d,
727+
// cameraMatrix,
728+
// planeEqCamera);
686729

687-
vectorVector3d imageCorners3d;
688-
bool valid = Misc::projectImagePointsOntoPlane(imageCorners,
689-
imageCorners3d,
690-
cameraMatrix,
691-
it->getNormal());
730+
projPoly.setTo(0);
731+
vector<cv::Point *> polyCont;
732+
vector<int> polyContNpts;
692733

693-
if(valid) {
694-
695-
ConcaveHull hull = it->getHull();
696-
hull.transform(poseSE3Quat.inverse().toVector());
697-
698-
vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> imagePolygons3d;
699-
imagePolygons3d.emplace_back(new pcl::PointCloud<pcl::PointXYZRGB>());
700-
for(Eigen::Vector3d imCor3d : imageCorners3d) {
701-
pcl::PointXYZRGB pt;
702-
pt.getVector3fMap() = imCor3d.cast<float>();
703-
imagePolygons3d.back()->push_back(pt);
734+
ConcaveHull hull = it->getHull().transform(poseSE3Quat.inverse().toVector());
735+
736+
// if (viewer) {
737+
// hull.display(viewer, viewPort1);
738+
// }
739+
//
740+
// hull.transform(poseSE3Quat.inverse().toVector());
741+
742+
ConcaveHull hullClip = hull.clipToCameraFrustum(cameraMatrix, rows, cols, 0.2);
743+
744+
ConcaveHull hullClipMap = hullClip.transform(poseSE3Quat.toVector());
745+
if (viewer) {
746+
hullClipMap.display(viewer, viewPort1, 1.0, 0.0, 0.0);
704747
}
705-
706-
ConcaveHull hullInt = hull.intersect(imagePolygons3d);
707-
708-
const std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> &polygons3d = hullInt.getPolygons3d();
748+
749+
const std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> &polygons3d = hullClip.getPolygons3d();
750+
cout << "polygons3d.size() = " << polygons3d.size() << endl;
709751
for (pcl::PointCloud<pcl::PointXYZRGB>::Ptr poly3d : polygons3d) {
710752
polyCont.push_back(new cv::Point[poly3d->size()]);
711753
polyContNpts.push_back(poly3d->size());
712-
713-
pcl::PointCloud<pcl::PointXYZRGB>::Ptr poly3dPose(new pcl::PointCloud<pcl::PointXYZRGB>());
714-
// transform to camera frame
715-
pcl::transformPointCloud(*poly3d, *poly3dPose, poseInvMat);
716-
717-
cv::Mat pointsReproj = Misc::reprojectTo2D(poly3dPose, cameraMatrix);
718-
754+
755+
// pcl::PointCloud<pcl::PointXYZRGB>::Ptr poly3dPose(new pcl::PointCloud<pcl::PointXYZRGB>());
756+
// // transform to camera frame
757+
// pcl::transformPointCloud(*poly3d, *poly3dPose, poseInvMat);
758+
759+
cv::Mat pointsReproj = Misc::reprojectTo2D(poly3d, cameraMatrix);
760+
for (int pt = 0; pt < poly3d->size(); ++pt) {
761+
cout << poly3d->at(pt).getVector3fMap().transpose() << endl;
762+
}
763+
cout << "cameraMatrix = " << cameraMatrix << endl;
764+
cout << "pointsReproj = " << pointsReproj << endl;
765+
719766
int corrPointCnt = 0;
720767
for (int pt = 0; pt < pointsReproj.cols; ++pt) {
721768
int u = std::round(pointsReproj.at<cv::Vec3f>(pt)[0]);
722769
int v = std::round(pointsReproj.at<cv::Vec3f>(pt)[1]);
723770
float d = pointsReproj.at<cv::Vec3f>(pt)[2];
724-
771+
725772
if (u >= 0 && u < cols && v >= 0 && v < rows && d > 0) {
726773
++corrPointCnt;
727774
}
728775
polyCont.back()[pt] = cv::Point(u, v);
729776
}
777+
cout << "corrPointCnt = " << corrPointCnt << endl;
778+
if (corrPointCnt == 0) {
779+
delete[] polyCont.back();
780+
polyCont.erase(polyCont.end() - 1);
781+
polyContNpts.erase(polyContNpts.end() - 1);
782+
}
730783
}
731-
cv::fillPoly(projPoly,
732-
(const cv::Point **) polyCont.data(),
733-
polyContNpts.data(),
734-
polyCont.size(),
735-
cv::Scalar(255));
784+
if (polyCont.size() > 0) {
785+
cv::fillPoly(projPoly,
786+
(const cv::Point **) polyCont.data(),
787+
polyContNpts.data(),
788+
polyCont.size(),
789+
cv::Scalar(255));
790+
791+
if (viewer) {
792+
cv::imshow("proj_poly", projPoly);
793+
}
794+
795+
vectorVector2d polyImagePts;
796+
for (int r = 0; r < rows; ++r) {
797+
for (int c = 0; c < cols; ++c) {
798+
if (projPoly.at<uint8_t>(r, c) > 0) {
799+
polyImagePts.push_back((Eigen::Vector2d() << c, r).finished());
800+
}
801+
}
802+
}
803+
vectorVector3d polyPlanePts;
804+
Misc::projectImagePointsOntoPlane(polyImagePts,
805+
polyPlanePts,
806+
cameraMatrix,
807+
planeEqCamera);
808+
for (int pt = 0; pt < polyImagePts.size(); ++pt) {
809+
int x = std::round(polyImagePts[pt](0));
810+
int y = std::round(polyImagePts[pt](1));
811+
// depth is z coordinate
812+
double d = polyPlanePts[pt](2);
813+
814+
projPlanes[y][x].push_back(make_pair(d, it->getId()));
815+
}
816+
}
817+
818+
for (int p = 0; p < polyCont.size(); ++p) {
819+
delete[] polyCont[p];
820+
}
821+
822+
if (viewer) {
823+
static bool cameraInit = false;
824+
825+
if (!cameraInit) {
826+
viewer->initCameraParameters();
827+
viewer->setCameraPosition(0.0, 0.0, -6.0, 0.0, 1.0, 0.0);
828+
cameraInit = true;
829+
}
830+
viewer->resetStoppedFlag();
831+
while (!viewer->wasStopped()) {
832+
viewer->spinOnce(50);
833+
cv::waitKey(50);
834+
std::this_thread::sleep_for(std::chrono::milliseconds(50));
835+
}
836+
837+
hull.cleanDisplay(viewer, viewPort1);
838+
hullClipMap.cleanDisplay(viewer, viewPort1);
839+
}
840+
}
841+
if (viewer) {
842+
it->cleanDisplay(viewer, viewPort1);
843+
it->display(viewer, viewPort1, shadingLevel);
844+
}
845+
}
846+
847+
map<int, int> idToCnt;
848+
for(int r = 0; r < rows; ++r) {
849+
for (int c = 0; c < cols; ++c) {
850+
vector<pair<double, int>> &curPlanes = projPlanes[r][c];
851+
sort(curPlanes.begin(), curPlanes.end());
852+
853+
if(!curPlanes.empty()){
854+
double minD = curPlanes.front().first;
855+
856+
for(const pair<double, int> &curPair : curPlanes){
857+
if(abs(minD - curPair.first) < 0.2){
858+
int id = curPair.second;
859+
if(idToCnt.count(id) > 0){
860+
idToCnt.at(id) += 1;
861+
}
862+
else{
863+
idToCnt[id] = 1;
864+
}
865+
}
866+
}
867+
}
868+
}
869+
}
870+
for(const pair<int, int> &curCnt : idToCnt){
871+
cout << "curCnt " << curCnt.first << " = " << curCnt.second << endl;
872+
if(curCnt.second > 1500){
873+
visible.push_back(curCnt.first);
874+
}
875+
}
876+
877+
if(viewer){
878+
for (auto it = objInstances.begin(); it != objInstances.end(); ++it) {
879+
it->cleanDisplay(viewer, viewPort1);
736880
}
737881
}
738882

739-
return vector<int>();
883+
return visible;
740884
}
741885

742886
pcl::PointCloud<pcl::PointXYZL>::Ptr Map::getLabeledPointCloud()

0 commit comments

Comments
 (0)