Skip to content

Commit

Permalink
Merge pull request #88 from saarnold/angular_interpolation_fix
Browse files Browse the repository at this point in the history 
DepthMap: fixed an issue when computing the step resolution in the polar case
  • Loading branch information
@jmachowinski
jmachowinski authored Sep 27, 2016
2 parents fecaa9d + 93e5aa9 commit f40e861
Show file tree
Hide file tree
Showing 2 changed files with 88 additions and 9 deletions.
19 changes: 10 additions & 9 deletions src/samples/DepthMap.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -71,13 +71,13 @@ struct DepthMap
std::vector<base::Time> timestamps;

/** Defines the vertical projection type of the depth map.
* If polar, the vertical intervals are angles in the range of (-PI, PI].
* If polar, the vertical intervals are angular rotations around the Y-unit axis.
* If planar, the vertical intervals are positions on an image plane coordinate frame.
*/
PROJECTION_TYPE vertical_projection;

/** Defines the horizontal projection type of the depth map.
* If polar, the horizontal intervals are angles in the range of (-PI, PI].
* If polar, the horizontal intervals are angular rotations around the Z-unit axis.
* If planar, the horizontal intervals are positions on an image plane coordinate frame.
*/
PROJECTION_TYPE horizontal_projection;
Expand All @@ -87,7 +87,9 @@ struct DepthMap
* In planar projection mode the vertical intervals are y coordinates on an
* depth-image plane, with zero in the middle of the plane.
* In polar projection mode the vertical intervals are angular rotations
* around the Y-unit axis.
* around the Y-unit axis. Vertical angles must always be ordered from a smaller
* to a higher value, since the rows of the data matrices are interpreted from the upper
* to the lower row.
* The field has either two or |vertical_size| entries. In the case of two
* entries the intervals are interpreted as upper and under boundaries. The
* transformation for each measurement will be interpolated.
Expand All @@ -99,7 +101,9 @@ struct DepthMap
* In planar projection mode the horizontal intervals are x coordinates on an
* depth-image plane, with zero in the middle of the plane.
* In polar projection mode the horizontal intervals are angular rotations
* around the Z-unit axis.
* around the Z-unit axis. Horizontal angles must always be ordered from a higher
* to a smaller value, since the columns of the data matrices are interpreted
* from the left to the right.
* The field has either two or |horizontal_size| entries. In the case of two
* entries the intervals are interpreted as left and right boundaries. The
* transformation for each measurement will be interpolated.
Expand Down Expand Up @@ -518,7 +522,7 @@ struct DepthMap
else if(horizontal_projection == POLAR)
{
for(unsigned h = 0; h < horizontal_size; h++)
horizontal_angles[h] = base::Angle::fromRad(horizontal_interval.front() - ((double)h * step_resolution));
horizontal_angles[h] = base::Angle::fromRad(horizontal_interval.front() + ((double)h * step_resolution));
}
else
throw std::invalid_argument("Invalid argument for horizontal projection type.");
Expand Down Expand Up @@ -597,10 +601,7 @@ struct DepthMap
if(interval.back() == interval.front())
step_resolution = (2.0 * M_PI) / (double)(elements-1);
else
{
double diff = std::abs(base::Angle::normalizeRad(interval.back() - interval.front()));
step_resolution = diff / (double)(elements-1);
}
step_resolution = (interval.back() - interval.front()) / (double)(elements-1);
}
else
throw std::invalid_argument("Invalid argument for projection type.");
Expand Down
78 changes: 78 additions & 0 deletions test/test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -765,6 +765,84 @@ BOOST_AUTO_TEST_CASE(depth_map_test)
BOOST_CHECK_THROW(scan.getMeasurementState(0,1), std::out_of_range);
scan.convertDepthMapToPointCloud(scan_points, transform);
BOOST_CHECK(scan_points.size() == 1);

// check horizontal angular interpolation
scan.reset();
scan.horizontal_size = 3;
scan.vertical_size = 1;
scan.vertical_projection = base::samples::DepthMap::POLAR;
scan.horizontal_projection = base::samples::DepthMap::POLAR;
scan.distances.push_back(sqrt(2.0));
scan.distances.push_back(1.0);
scan.distances.push_back(sqrt(2.0));
scan.vertical_interval.push_back(0.0);
scan.horizontal_interval.push_back(M_PI - M_PI_4);
scan.horizontal_interval.push_back(-M_PI + M_PI_4);
scan_points.clear();
scan.convertDepthMapToPointCloud(scan_points);
BOOST_CHECK(scan_points.size() == scan.vertical_size * scan.horizontal_size);
BOOST_CHECK(scan_points[0].isApprox(Eigen::Vector3d(-1.0,1.0,0.0), 1e-6));
BOOST_CHECK(scan_points[1].isApprox(Eigen::Vector3d(1.0,0.0,0.0), 1e-6));
BOOST_CHECK(scan_points[2].isApprox(Eigen::Vector3d(-1.0,-1.0,0.0), 1e-6));

scan.horizontal_interval.clear();
scan.horizontal_interval.push_back(-M_PI_4);
scan.horizontal_interval.push_back(-2.0*M_PI+M_PI_4);
scan_points.clear();
scan.convertDepthMapToPointCloud(scan_points);
BOOST_CHECK(scan_points.size() == scan.vertical_size * scan.horizontal_size);
BOOST_CHECK(scan_points[0].isApprox(Eigen::Vector3d(1.0,-1.0,0.0), 1e-6));
BOOST_CHECK(scan_points[1].isApprox(Eigen::Vector3d(-1.0,0.0,0.0), 1e-6));
BOOST_CHECK(scan_points[2].isApprox(Eigen::Vector3d(1.0,1.0,0.0), 1e-6));

scan.horizontal_interval.clear();
scan.horizontal_interval.push_back(M_PI_4);
scan.horizontal_interval.push_back(-M_PI_4);
scan_points.clear();
scan.convertDepthMapToPointCloud(scan_points);
BOOST_CHECK(scan_points.size() == scan.vertical_size * scan.horizontal_size);
BOOST_CHECK(scan_points[0].isApprox(Eigen::Vector3d(1.0,1.0,0.0), 1e-6));
BOOST_CHECK(scan_points[1].isApprox(Eigen::Vector3d(1.0,0.0,0.0), 1e-6));
BOOST_CHECK(scan_points[2].isApprox(Eigen::Vector3d(1.0,-1.0,0.0), 1e-6));

// check vertical angular interpolation
scan.reset();
scan.horizontal_size = 1;
scan.vertical_size = 3;
scan.vertical_projection = base::samples::DepthMap::POLAR;
scan.horizontal_projection = base::samples::DepthMap::POLAR;
scan.distances.push_back(sqrt(2.0));
scan.distances.push_back(1.0);
scan.distances.push_back(sqrt(2.0));
scan.horizontal_interval.push_back(0.0);
scan.vertical_interval.push_back(-M_PI + M_PI_4);
scan.vertical_interval.push_back(M_PI - M_PI_4);
scan_points.clear();
scan.convertDepthMapToPointCloud(scan_points);
BOOST_CHECK(scan_points.size() == scan.vertical_size * scan.horizontal_size);
BOOST_CHECK(scan_points[0].isApprox(Eigen::Vector3d(-1.0,0.0,1.0), 1e-6));
BOOST_CHECK(scan_points[1].isApprox(Eigen::Vector3d(1.0,0.0,0.0), 1e-6));
BOOST_CHECK(scan_points[2].isApprox(Eigen::Vector3d(-1.0,0.0,-1.0), 1e-6));

scan.vertical_interval.clear();
scan.vertical_interval.push_back(M_PI_4);
scan.vertical_interval.push_back(2.0*M_PI-M_PI_4);
scan_points.clear();
scan.convertDepthMapToPointCloud(scan_points);
BOOST_CHECK(scan_points.size() == scan.vertical_size * scan.horizontal_size);
BOOST_CHECK(scan_points[0].isApprox(Eigen::Vector3d(1.0,0.0,-1.0), 1e-6));
BOOST_CHECK(scan_points[1].isApprox(Eigen::Vector3d(-1.0,0.0,0.0), 1e-6));
BOOST_CHECK(scan_points[2].isApprox(Eigen::Vector3d(1.0,0.0,1.0), 1e-6));

scan.vertical_interval.clear();
scan.vertical_interval.push_back(-M_PI_4);
scan.vertical_interval.push_back(M_PI_4);
scan_points.clear();
scan.convertDepthMapToPointCloud(scan_points);
BOOST_CHECK(scan_points.size() == scan.vertical_size * scan.horizontal_size);
BOOST_CHECK(scan_points[0].isApprox(Eigen::Vector3d(1.0,0.0,1.0), 1e-6));
BOOST_CHECK(scan_points[1].isApprox(Eigen::Vector3d(1.0,0.0,0.0), 1e-6));
BOOST_CHECK(scan_points[2].isApprox(Eigen::Vector3d(1.0,0.0,-1.0), 1e-6));
}

BOOST_AUTO_TEST_CASE( pose_test )
Expand Down

0 comments on commit f40e861

Please sign in to comment.