Skip to content

Commit

Permalink
depth filter of consistency
Browse files Browse the repository at this point in the history
  • Loading branch information
@hlx1996
hlx1996 committed Mar 14, 2019
1 parent 2cf41cc commit 333164e
Show file tree
Hide file tree
Showing 3 changed files with 152 additions and 46 deletions.
3 changes: 1 addition & 2 deletions README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,10 +47,9 @@ _For more examples and usage, please refer to the [Wiki][wiki]._

## Release History
Prediction: 1.0.0 will be our first elegant version.

* 0.5.2
* ADD: Support local map
* ADD: Support raw depth image as input
* ADD: Support raw depth image as input, and depth filter of consistency
* 0.5.1
* CHANGE: Improve performance of raycasting a lot
* 0.5.0
Expand Down
48 changes: 36 additions & 12 deletions launch/demo.launch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,42 +3,66 @@
required="true">

<param name="resolution" value="0.1"/>
<param name="update_esdf_every_n_sec" value="0.1"/>

<!--Hash Table only-->
<param name="reservedSize" value="1000000"/>

<!--Array only-->
<param name="lx" value="-20.0"/>
<param name="ly" value="-20.0"/>
<param name="lz" value="-1.6"/>
<param name="rx" value="+20.0"/>
<param name="ry" value="+20.0"/>
<param name="rz" value="+2"/>

<!-- raycasting parameters -->
<param name="min_ray_length_m" value="0.5"/>
<param name="max_ray_length_m" value="5.0"/>
<param name="update_mesh_every_n_sec" value="0.1"/>
<!--If hash table is used, make sure the value is 0. In array implementation, 0 means no other thread.-->
<param name="num_thread" value="0"/>



<!-- needed when using depth image-->
<param name="cx" value="323.120483"/>
<param name="cy" value="236.743210"/>
<param name="fx" value="385.754486"/>
<param name="fy" value="385.754486"/>

<!-- probabilistic grid map -->
<param name="pHit" value="0.70"/>
<param name="pMiss" value="0.35"/>
<param name="pMin" value="0.12"/>
<param name="pMax" value="0.97"/>
<param name="pOcc" value="0.80"/>
<!--Hash Table only-->
<param name="reservedSize" value="1000000"/>

<!--Array only-->
<param name="lx" value="-20.0"/>
<param name="ly" value="-20.0"/>
<param name="lz" value="-1.6"/>
<param name="rx" value="+20.0"/>
<param name="ry" value="+20.0"/>
<param name="rz" value="+2"/>
<!-- global / local -->
<param name="globalMap" value="false"/>
<param name="globalUpdate" value="false"/>
<param name="globalVis" value="false"/>
<param name="radius_x" value="3.0"/>
<param name="radius_y" value="3.0"/>
<param name="radius_z" value="1.5"/>

<!--depth_filter -->
<param name="use_depth_filter" value="true"/>
<param name="depth_filter_tolerance" value="0.1"/>
<param name="depth_filter_maxdist" value="10.0"/>
<param name="depth_filter_mindist" value="0.1"/>
<!--unit: pixel-->
<param name="depth_filter_margin" value="0"/>

<!-- visulization -->
<!--0 for no visulize-->
<param name="visulize_every_n_updates" value="10"/>
<param name="slice_vis" value="0.7"/>
<param name="max_dist" value="2.0"/>

<param name="vis_lower_bound" value="-10"/>
<!-- relative to the lz if array is used, relative to the origin if hash table is used -->
<param name="vis_lower_bound" value="0"/>
<param name="vis_upper_bound" value="+10"/>

<!-- subsribe source -->
<!--<remap from="~pointcloud" to="/camera/depth/color/points"/>-->
<remap from="~depth" to="/camera/depth/image_rect_raw"/>
<remap from="~transform" to="/vins_estimator/camera_pose"/>
Expand Down
147 changes: 115 additions & 32 deletions test/test_pointcloud_raycasting.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,6 +45,9 @@ double max_ray_length_m;
int visulize_every_n_updates, param_num_thread;
double slice_vis, max_dist, vis_lower_bound, vis_upper_bound, center_x, center_y, focal_x, focal_y;
double pHit, pMiss, pMin, pMax, pOcc;
double depth_filter_maxdist, depth_filter_mindist, depth_filter_tolerance;
int depth_filter_margin;
bool use_depth_filter;
#ifdef SIGNED_NEEDED
ESDF_Map *inv_esdf_map;
#endif
Expand All @@ -66,9 +69,9 @@ sensor_msgs::PointCloud2::ConstPtr s_pc;
#endif

// *************** params ****************
double range_sensor = 5.0;
Eigen::Vector3d radius(range_sensor, range_sensor, range_sensor / 2);
bool globalVis = true, globalUpdate = true, globalMap = false;
double radius_x, radius_y, radius_z;
Eigen::Vector3d radius;
bool globalVis, globalUpdate, globalMap;
// *************** params ****************

bool newMsg = false;
Expand Down Expand Up @@ -227,7 +230,7 @@ void raycastProcess(int i, int part, int tt) {
}
}
ros::Time t2 = ros::Time::now();
cout << "raycasting \t" << tot1 << "\t" << tot2 << "\t" << (t2 - t1).toSec() << endl;
// cout << "raycasting \t" << tot1 << "\t" << tot2 << "\t" << (t2 - t1).toSec() << endl;

}

Expand All @@ -236,7 +239,7 @@ void raycastProcess(int i, int part, int tt) {
int receivedCount = 0, dmCount = 0, pcCount = 0, posCount = 0;

void pointcloudCallBack(const sensor_msgs::PointCloud2::ConstPtr &pointcloud_map) {
cout << ++pcCount << '\t' << "POINTCLOUD: " << pointcloud_map->header.stamp << endl;
// cout << ++pcCount << '\t' << "POINTCLOUD: " << pointcloud_map->header.stamp << endl;
/*
T.block<3, 3>(0, 0) = q.normalized().toRotationMatrix();
T.block<3, 1>(0, 3) = pos;
Expand All @@ -259,9 +262,9 @@ void pointcloudCallBack(const sensor_msgs::PointCloud2::ConstPtr &pointcloud_map
*/
int tt = ++tot;
// cout << "PC: " << tt << endl;
cout << "size : " << sizeof(*pointcloud_map) << endl;
// cout << "size : " << sizeof(*pointcloud_map) << endl;
imageQueue.push(pointcloud_map);
cout << "Queue Size\t" << imageQueue.size() << ' ' << transformQueue.size() << endl;
// cout << "Queue Size\t" << imageQueue.size() << ' ' << transformQueue.size() << endl;
//if (cnt == 1500) std::thread(save).join();
// std::cout << "Image timestamp\t" << msg->header.stamp << "\t" << std::endl;

Expand All @@ -286,7 +289,7 @@ void pointcloudCallBack(const sensor_msgs::PointCloud2::ConstPtr &pointcloud_map
continue;
}

cout << ++receivedCount << '\t' << fabs((_poseTime - _imageTime).toSec()) << endl;
// cout << ++receivedCount << '\t' << fabs((_poseTime - _imageTime).toSec()) << endl;
newMsg = true;
#ifndef PROBABILISTIC
s_pc = imageQueue.front();
Expand Down Expand Up @@ -360,14 +363,32 @@ void pointcloudCallBack(const sensor_msgs::PointCloud2::ConstPtr &pointcloud_map

}

cv::Mat img, lastImg;
Eigen::Matrix4d lastT;
bool firstImg = true;

double getInterpolation(const cv::Mat &img, double u, double v) {

int vu = img.at<uint16_t>(v, u);
int v1u = img.at<uint16_t>(v + 1, u);
int vu1 = img.at<uint16_t>(v, u + 1);
int v1u1 = img.at<uint16_t>(v + 1, u + 1);

float a = u - (float) u;
float c = v - (float) v;

return (vu * (1.f - a) + vu1 * a) * (1.f - c) + (v1u * (1.f - a) + v1u1 * a) * c;
}

int sum_kkp = 0, cnt_kkp = 0, t1 = 0, t2 = 0;

void depthCallBack(const sensor_msgs::Image::ConstPtr &depth_map) {
cout << ++dmCount << '\t' << "DEPTH_MAP: " << depth_map->header.stamp << endl;
// cout << ++dmCount << '\t' << "DEPTH_MAP: " << depth_map->header.stamp << endl;
int tt = ++tot;
// cout << "PC: " << tt << endl;
cout << "size : " << sizeof(*depth_map) << endl;
// cout << "size : " << sizeof(*depth_map) << endl;
depthMapQueue.push(depth_map);
cout << "Queue Size\t" << depthMapQueue.size() << ' ' << transformQueue.size() << endl;
// cout << "Queue Size\t" << depthMapQueue.size() << ' ' << transformQueue.size() << endl;
//if (cnt == 1500) std::thread(save).join();
// std::cout << "Image timestamp\t" << msg->header.stamp << "\t" << std::endl;

Expand All @@ -392,21 +413,25 @@ void depthCallBack(const sensor_msgs::Image::ConstPtr &depth_map) {
continue;
}

cout << ++receivedCount << '\t' << fabs((_poseTime - _imageTime).toSec()) << endl;
// cout << ++receivedCount << '\t' << fabs((_poseTime - _imageTime).toSec()) << endl;
newMsg = true;
#ifndef PROBABILISTIC
// TODO: Modify this, this will cause Compile Error
s_pc = depthMapQueue.front();
return;
#else
if (use_depth_filter) {
lastT = T;
lastImg = img.clone();
}
T.block<3, 3>(0, 0) = s_q.toRotationMatrix();
T.block<3, 1>(0, 3) = s_pos;
T(3, 0) = T(3, 1) = T(3, 2) = 0;
T(3, 3) = 1;
T = T * T_D_B * T_B_C;
origin = Eigen::Vector3d(T(0, 3), T(1, 3), T(2, 3)) / T(3, 3);

cv::Mat img;

cv_bridge::CvImagePtr cv_ptr;
cv_ptr = cv_bridge::toCvCopy(depthMapQueue.front(), depthMapQueue.front()->encoding);
constexpr double kDepthScalingFactor = 1000.0;
Expand All @@ -417,23 +442,66 @@ void depthCallBack(const sensor_msgs::Image::ConstPtr &depth_map) {

double depth;
cloud.clear();
for (int v = 10; v < img.rows - 10; v++)
for (int u = 10; u < img.cols - 10; u++) {
depth = img.at<uint16_t>(v, u) / kDepthScalingFactor;
pcl::PointXYZ point;
point.x = (u - center_x) * depth / focal_x;
point.y = (v - center_y) * depth / focal_y;
point.z = depth;

cloud.push_back(point);
}

cout << cloud.points.size() << endl;

double uu, vv;

t1 = t2 = 0;
if (!use_depth_filter) {
for (int v = 0; v < img.rows; v++)
for (int u = 0; u < img.cols; u++) {
depth = img.at<uint16_t>(v, u) / kDepthScalingFactor;
pcl::PointXYZ point;
point.x = (u - center_x) * depth / focal_x;
point.y = (v - center_y) * depth / focal_y;
point.z = depth;
cloud.push_back(point);
}
} else {
Eigen::Vector4d tmpp;
Eigen::Vector3d tmp;
for (int v = depth_filter_margin; v < img.rows - depth_filter_margin; v++)
for (int u = depth_filter_margin; u < img.cols - depth_filter_margin; u++) {
depth = img.at<uint16_t>(v, u) / kDepthScalingFactor;
pcl::PointXYZ point;
point.x = (u - center_x) * depth / focal_x;
point.y = (v - center_y) * depth / focal_y;
point.z = depth;
if (!firstImg) {
// if (depth > depth_filter_maxdist || depth < depth_filter_mindist) continue;
tmpp = lastT.inverse() * T * Eigen::Vector4d(point.x, point.y, point.z, 1);
tmp = Eigen::Vector3d(tmpp(0), tmpp(1), tmpp(2)) / tmpp(3);
uu = tmp.x() * focal_x / tmp.z() + center_x;
vv = tmp.y() * focal_y / tmp.z() + center_y;
if (uu >= 0 && uu < img.cols - 1 && vv >= 0 && vv < img.rows - 1) {
if (fabs(getInterpolation(lastImg, uu, vv) / kDepthScalingFactor - tmp.z())
< depth_filter_tolerance) {
cloud.push_back(point);
// ++t1;
}
// ++t2;
} //else cloud.push_back(point);
} //else cloud.push_back(point);
}
firstImg = false;
}
// cout << t1 << "\t" << t2 << "\t" << (double) t1 / t2 << '\t' << (double) t2 / (img.rows * img.cols) << endl;
// cout << "pointcloud_size:\t" << cloud.points.size() << "\t"
// << (sum_kkp += cloud.points.size()) / (double) (++cnt_kkp) << endl;

cout << "pointcloud_size:\t" << cloud.points.size() << endl;
if ((int) cloud.points.size() == 0)
return;

// sensor_msgs::PointCloud m;
// m.header.frame_id = "world";
// m.points.clear();
// for (int i = 0; i < cloud.points.size(); i++) {
// Eigen::Vector4d tmp = T * Eigen::Vector4d(cloud[i].x, cloud[i].y, cloud[i].z, 1);
// geometry_msgs::Point32 p;
// p.x = tmp(0) / tmp(3);
// p.y = tmp(1) / tmp(3);
// p.z = tmp(2) / tmp(3);
// m.points.push_back(p);
// }
// recpc_pub.publish(m);
// TODO: when using vector, this is not needed
#ifdef HASH_TABLE
uset.clear();
Expand Down Expand Up @@ -547,7 +615,7 @@ void publishPose(Eigen::Vector3d pos, Eigen::Quaterniond q) {

// }
void transformCallback(const geometry_msgs::PoseStamped::ConstPtr &msg) {
cout << ++posCount << '\t' << "TRANSFORM: " << msg->header.stamp << endl;
// cout << ++posCount << '\t' << "TRANSFORM: " << msg->header.stamp << endl;
pos = Eigen::Vector3d(msg->pose.position.x,
msg->pose.position.y,
msg->pose.position.z);
Expand Down Expand Up @@ -586,7 +654,7 @@ void updateESDFEvent(const ros::TimerEvent & /*event*/) {
handlePCTimer.Stop();
#endif
esdf_cnt++;
// cout << "Running " << esdf_cnt << " updates."<< endl;
cout << "Running " << esdf_cnt << " updates." << endl;
ros::Time t1 = ros::Time::now();
if (esdf_map->checkUpdate()) {

Expand Down Expand Up @@ -670,6 +738,21 @@ int main(int argc, char **argv) {
node.param<double>("rx", rx, 20.f);
node.param<double>("ry", ry, 20.f);
node.param<double>("rz", rz, 5.f);
node.param<double>("radius_x", radius_x, 3.f);
node.param<double>("radius_y", radius_y, 3.f);
node.param<double>("radius_z", radius_z, 1.5f);
radius = Eigen::Vector3d(radius_x, radius_y, radius_z);

node.param<bool>("globalMap", globalMap, false);
node.param<bool>("globalUpdate", globalUpdate, false);
node.param<bool>("globalVis", globalVis, false);

node.param<bool>("use_depth_filter", use_depth_filter, true);
node.param<double>("depth_filter_tolerance", depth_filter_tolerance, 0.1f);
node.param<double>("depth_filter_maxdist", depth_filter_maxdist, 10.f);
node.param<double>("depth_filter_mindist", depth_filter_mindist, 0.1f);
node.param<int>("depth_filter_margin", depth_filter_margin, 0);

lCornor << lx, ly, lz;
rCornor << rx, ry, rz;
map_size = rCornor - lCornor;
Expand Down Expand Up @@ -761,10 +844,10 @@ int main(int argc, char **argv) {
meshPub = node.advertise<visualization_msgs::Marker>("ESDF_Map/robot", 100, true);
recpc_pub = node.advertise<sensor_msgs::PointCloud>("ESDF_Map/rectified_pointcloud", 1, true);

double update_mesh_every_n_sec;
node.param<double>("update_mesh_every_n_sec", update_mesh_every_n_sec, 0.1);
double update_esdf_every_n_sec;
node.param<double>("update_esdf_every_n_sec", update_esdf_every_n_sec, 0.1);
ros::Timer update_mesh_timer_ =
node.createTimer(ros::Duration(update_mesh_every_n_sec),
node.createTimer(ros::Duration(update_esdf_every_n_sec),
&updateESDFEvent);
// double update_mesh_every_n_sec;
// node.param<double>( "update_mesh_every_n_sec", update_mesh_every_n_sec, 0.5);
Expand Down

0 comments on commit 333164e

Please sign in to comment.