Skip to content

Commit

Permalink
Added convenience for all hands plots.
Browse files Browse the repository at this point in the history
  • Loading branch information
@nistath
nistath committed Nov 27, 2019
1 parent 6ee2023 commit 43adc15
Show file tree
Hide file tree
Showing 2 changed files with 138 additions and 35 deletions.
147 changes: 118 additions & 29 deletions fast_raycast/src/main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,8 +3,13 @@
#include <experimental/filesystem>
#include <iostream>
#include <limits>
#include <memory>
#include <numeric>
#include <optional>
#include <chrono>
#include <thread>

using namespace std::chrono_literals;

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
Expand Down Expand Up @@ -393,7 +398,7 @@ class Grid {

void serialize(std::ostream& stream) {
for (auto const& range : ranges_) {
stream << range.min << "," << range.max << "," << range.step << "\n";
stream << range.low << "," << range.high << "," << range.step << "\n";
}

const static IOFormat CSVFormat(StreamPrecision, DontAlignCols, ", ", "\n");
Expand All @@ -416,15 +421,18 @@ auto computeGrid(DV const& dv, Rays<NRays>& rays, std::array<Range, 2> ranges) {

std::atomic<Index> iters_done = 0;

auto const origin_offset = rays.origin_offset;

#pragma omp parallel for
for (Index x_iter = 0; x_iter < ranges[0].count(); ++x_iter) {
for (Index y_iter = 0; y_iter < ranges[0].count(); ++y_iter) {
rays.origin_offset = {ranges[0].get(x_iter), ranges[1].get(y_iter), 0};
for (Index y_iter = 0; y_iter < ranges[0].count(); ++y_iter) {
for (Index z_iter = 0; z_iter < ranges[1].count(); ++z_iter) {
rays.origin_offset = origin_offset + RowVector3e{0, ranges[0].get(y_iter),
ranges[1].get(z_iter)};

dv.computeSolution(rays, solutions, hit_height, object);
auto idxs_per_cone = dv.computeIdxsPerCone(object);

grid.grid_(x_iter, y_iter) = std::accumulate(
grid.grid_(y_iter, z_iter) = std::accumulate(
idxs_per_cone.begin(), idxs_per_cone.end(), (Index)0,
[](Index prior, std::vector<Index> const& vec) -> Index {
return prior + vec.size();
Expand All @@ -442,16 +450,14 @@ auto computeGrid(DV const& dv, Rays<NRays>& rays, std::array<Range, 2> ranges) {
} // namespace Intersection
} // namespace lcaster

#include <chrono>

int main() {
using namespace lcaster;
using namespace lcaster::Intersection;

World::Lidar vlp32 =
World::Lidar(Vector3e(0.0, 0.0, 0.3), 32, 0.2 * M_PI / 180.0);
vlp32.setRays("fast_raycast/sensor_info/VLP32_LaserInfo.csv");
Rays<Dynamic> rays = vlp32.rays();
Rays<Dynamic>& rays = vlp32.rays();

// constexpr el_t HFOV = M_PI / 8;
// constexpr el_t HBIAS = -HFOV / 2;
Expand All @@ -478,34 +484,116 @@ int main() {
Obstacle::Plane ground({0, 0, 1}, {0, 0, 0});

constexpr el_t sc = 0.75;
World::DV world(
ground,
{
{{-12.75835829 * sc, 18.81545688 * sc, 0.29}, {0, 0, -1}, 0.29, 0.08},
{{-8.27767283 * sc, 17.59812307 * sc, 0.29}, {0, 0, -1}, 0.29, 0.08},
{{-4.26349242 * sc, 16.15671487 * sc, 0.29}, {0, 0, -1}, 0.29, 0.08},
{{-0.99153611 * sc, 14.40308463 * sc, 0.29}, {0, 0, -1}, 0.29, 0.08},
{{1.12454352 * sc, 12.2763364 * sc, 0.29}, {0, 0, -1}, 0.29, 0.08},
{{2.01314521 * sc, 9.33822398 * sc, 0.29}, {0, 0, -1}, 0.29, 0.08},
{{1.98908967 * sc, 5.84530425 * sc, 0.29}, {0, 0, -1}, 0.29, 0.08},
{{1.43369938 * sc, 1.8860828 * sc, 0.29}, {0, 0, -1}, 0.29, 0.08},
{{-13.47161573 * sc, 15.90147955 * sc, 0.29}, {0, 0, -1}, 0.29, 0.08},
{{-9.16834254 * sc, 14.73338793 * sc, 0.29}, {0, 0, -1}, 0.29, 0.08},
{{-5.45778135 * sc, 13.40468399 * sc, 0.29}, {0, 0, -1}, 0.29, 0.08},
{{-2.73894015 * sc, 11.96452376 * sc, 0.29}, {0, 0, -1}, 0.29, 0.08},
{{-1.46294141 * sc, 10.75813953 * sc, 0.29}, {0, 0, -1}, 0.29, 0.08},
{{-0.973512 * sc, 9.05559578 * sc, 0.29}, {0, 0, -1}, 0.29, 0.08},
{{-0.99791948 * sc, 6.12418841 * sc, 0.29}, {0, 0, -1}, 0.29, 0.08},
{{-1.51764477 * sc, 2.42419778 * sc, 0.29}, {0, 0, -1}, 0.29, 0.08},
});

bool earr;
std::cin >> earr;

std::unique_ptr<World::DV> world_ptr;
if (earr) {
world_ptr.reset(new World::DV(
ground,
{
{{-12.75835829 * sc, 18.81545688 * sc, 0.29},
{0, 0, -1},
0.29,
0.08},
{{-8.27767283 * sc, 17.59812307 * sc, 0.29},
{0, 0, -1},
0.29,
0.08},
{{-4.26349242 * sc, 16.15671487 * sc, 0.29},
{0, 0, -1},
0.29,
0.08},
{{-0.99153611 * sc, 14.40308463 * sc, 0.29},
{0, 0, -1},
0.29,
0.08},
{{1.12454352 * sc, 12.2763364 * sc, 0.29}, {0, 0, -1}, 0.29, 0.08},
{{2.01314521 * sc, 9.33822398 * sc, 0.29}, {0, 0, -1}, 0.29, 0.08},
{{1.98908967 * sc, 5.84530425 * sc, 0.29}, {0, 0, -1}, 0.29, 0.08},
{{1.43369938 * sc, 1.8860828 * sc, 0.29}, {0, 0, -1}, 0.29, 0.08},
{{-13.47161573 * sc, 15.90147955 * sc, 0.29},
{0, 0, -1},
0.29,
0.08},
{{-9.16834254 * sc, 14.73338793 * sc, 0.29},
{0, 0, -1},
0.29,
0.08},
{{-5.45778135 * sc, 13.40468399 * sc, 0.29},
{0, 0, -1},
0.29,
0.08},
{{-2.73894015 * sc, 11.96452376 * sc, 0.29},
{0, 0, -1},
0.29,
0.08},
{{-1.46294141 * sc, 10.75813953 * sc, 0.29},
{0, 0, -1},
0.29,
0.08},
{{-0.973512 * sc, 9.05559578 * sc, 0.29}, {0, 0, -1}, 0.29, 0.08},
{{-0.99791948 * sc, 6.12418841 * sc, 0.29}, {0, 0, -1}, 0.29, 0.08},
{{-1.51764477 * sc, 2.42419778 * sc, 0.29}, {0, 0, -1}, 0.29, 0.08},
}));
} else {
world_ptr.reset(new World::DV(
ground, {
{{-5.770087622473654 * sc, 13.960476554628654 * sc, 0.29},
{0, 0, -1},
0.29,
0.08},
{{-3.5531794846979197 * sc, 10.09163739418124 * sc, 0.29},
{0, 0, -1},
0.29,
0.08},
{{-1.8689520677436653 * sc, 6.528338851853511 * sc, 0.29},
{0, 0, -1},
0.29,
0.08},
{{-0.9647880812999942 * sc, 3.5874807367224406 * sc, 0.29},
{0, 0, -1},
0.29,
0.08},
{{-0.9155631877776976 * sc, 1.8321709002122402 * sc, 0.29},
{0, 0, -1},
0.29,
0.08},
{{-3.20524532871408 * sc, 15.516620509454873 * sc, 0.29},
{0, 0, -1},
0.29,
0.08},
{{-0.8977072413626149 * sc, 11.487512447642604 * sc, 0.29},
{0, 0, -1},
0.29,
0.08},
{{0.9215174312005985 * sc, 7.629828740804735 * sc, 0.29},
{0, 0, -1},
0.29,
0.08},
{{1.9951360927293942 * sc, 4.076202420297469 * sc, 0.29},
{0, 0, -1},
0.29,
0.08},
{{1.9875922542497209 * sc, 1.0760700607559186 * sc, 0.29},
{0, 0, -1},
0.29,
0.08},
}));
}
World::DV& world = *world_ptr.get();

el_t xl, xh, xs;
el_t yl, yh, ys;
std::cin >> xl >> xh >> xs >> yl >> yh >> ys;
std::cout << "Computing grid...\n";
auto grid = World::computeGrid(
world, rays, {World::Range{xl, xh, xs}, World::Range{yl, yh, ys}});
grid.serialize("out.csv");

std::string fname;
std::cin >> fname;
grid.serialize(fname);

std::cout << "Computing sample cloud...\n";
Solutions<Dynamic> solutions(rays.rays());
Expand All @@ -520,6 +608,7 @@ int main() {
pcl::visualization::CloudViewer viewer("Simple Cloud Viewer");
viewer.showCloud(cloud);
while (!viewer.wasStopped()) {
std::this_thread::sleep_for(100ms);
}

return 0;
Expand Down
26 changes: 20 additions & 6 deletions process_results/heatmap.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
from matplotlib import pyplot as plt
from PIL import Image


class Range(NamedTuple):
low: float
high: float
Expand All @@ -23,24 +24,37 @@ class Grid(NamedTuple):
def from_csv(cls, fname):
f = open(fname)
ranges = tuple(Range.from_string(f.readline()) for _ in range(2))
grid = np.loadtxt(f, delimiter=",")
grid = np.loadtxt(f, delimiter=",").T

return cls(ranges, grid)

def plot(self):
# plt.xlim(self.ranges[0].low, self.ranges[0].high)
# plt.ylim(self.ranges[1].low, self.ranges[1].high)
plt.imshow(self.grid, cmap='viridis', origin='lower', interpolation='nearest')
plt.imshow(self.grid, cmap='viridis', origin='lower', interpolation='nearest', extent=[
self.ranges[0].low, self.ranges[0].high, self.ranges[1].low, self.ranges[1].high])
plt.ylabel('Vertical Mounting (m)')
plt.xlabel('Longitudinal Mounting (m)')
plt.colorbar()


car = Image.open('/home/nistath/Downloads/car.png')
pix2meter= 752 / 1.530
pix2meter = 752 / 1.530

if True:
grid1 = Grid.from_csv('/home/nistath/Git/rt/depp.csv')
grid2 = Grid.from_csv('/home/nistath/Git/rt/earr.csv')

grid1.plot()
plt.show()
grid2.plot()
plt.show()

grid = Grid.from_csv('/home/nistath/Desktop/fine.csv')
grid = Grid(grid1.ranges, (grid1.grid + grid2.grid)/2)
else:
grid = Grid.from_csv('/home/nistath/Git/rt/earr.csv')

png_write(grid.grid, '/home/nistath/Desktop/grid.png', 'topleft', True)
# grid.plot()
grid.plot()
# plt.imshow(car, alpha=0.1)
# plt.show()
plt.show()

0 comments on commit 43adc15

Please sign in to comment.