Skip to content

Commit

Permalink
Updated ICP Demos. (#3463)
Browse files Browse the repository at this point in the history 
* RGBD odometry compiles, debugging

* minor fixes

* hybrid model works

* enhance example TOdometryRGBD

* better naming

* adjust include tree

* switch to natural source target for point to plane

* switch to natural source target for hybrid

* introduce direct intensity odometry

* wrap duplicate reduction/solve code and fix memory leak

* squeeze depth preprocessing in one pass

* add dispatch pattern for unprojection and depth processing

* fix tests

* add benchmark

* expose depth_max to multiscale odometry interface

* add truncated pyrdown for depth

* address comments

* address major comments

* split implementations

* fix doxygen

* raycasting with target map selection mask

* fix minor bugs and upgrade vh skeleton

* minor

* minor update

* test t point cloud in gui

* add frame interface

* simplify frame constructors

* self-included refactored system

* initial integration of the scene

* draft

* minor

* visualize frustum

* multi-thread surface extraction with unsafe pcd extraction

* minor

* fall to tgeometry (not working yet)

* buffer pcd and update geometry

* temporarily disable normal

* rough realsense demo (fix tsdf scale factor)

* update raycasting interface for depth_scale

* add optional nullopt for surface extraction

* colored depth map and temprarily remove multithread for surface reconstruction

* apply turbo depth colormap

* add enable color selector

* add button interaction and better initialization

* expose point cloud size estimation for surface extraction

* add preset point cloud buffer estimate and format transformation

* add clear to hashmap

* support buffered hashmap in integration

* fix pybind and examples for updated tsdf_voxelgrid

* fix tests

* init minmax map generation, debugging

* minmax map works, next upgrade raycasting

* clone pose and enable rangemap visualization

* add hashmap cache and profile

* fix minor bug in caching

* move initialization into raycaster

* change to extrinsic-first interface in raycasting

* optimized pose inversion and transform indexer

* reenable voxelhashing

* reorganize shape dtype device check for transformations

* simplfy raycasting interface

* fix ci

* fix pytest for tsdfvoxelgrid

* speed up raycast by reorganizing control flow

* squeeze commits

* add pybind

* reimplement pyrdown depth to avoid licence issue

* fix unit test

* merge timage update

* upgrade odometry with timage interface

* sum reduction in __shared__ working for point to plane

* fix rgbd odom tests

* implement and reorganize sum-reduction in rgbd odometry

* fix raycasting

* address review comments

* hide pyrdown_depth

* adapt pyrdown depth inside rgbd odometry

* upgrade bilateral filtering and create vtx map

* apply style

* replace residual as float and expose inlier count

* wrap up odometry result with a class

* minor

* add convergence criteria and early exit

* Scale UInt8 colors to 0-1.0 range before conversion to Legacy PCD

* Yixing comments

Clarify documentation

* Non-visual unit tests

ClipTransform
CreateVertexMap
CreateNormalMap

* more fix related to merge

* added icp modified kernel to use shared reduction

* wrap odometry loss params

* huber with problems

* minor

* update huber

* fix bilateral in point2plane

* fix hybrid issue

* Explicitly check for and support the 4 possible color Dtypes

* added kitti scripts. replaced update with material update

* address review comments and change const params

* Remove explicit copy in conversion to Float64

* rename functions

* fix bug in intensity

* fix style

* revert renaming in registration

* revert renaming in registration

* add singular case handling

* merged with updated reg.

* lyft working, added more param for config file

* apply-style

* updated

* minor changes

* licence added

* licence added to VoxelHashingGUI

* documentation update

* more documentaion with example

* review comments, and added start_index parameter for convinience of user

* review comments, detailed warnings for user

* instruction to download dataset updated

* changed window dim

* review comments

* apply-style

* updated reconstruction example

* camera follows current scan

* Config file modified

* more cleanups, and removed depreciated scripts

* changed window resolution back

* revert parameters for reconstruction demo

* review changes

* bug fixes

* support UInt8, UInt16, Float32, Float64 colors

* removed un-used codes

Co-authored-by: Wei Dong <[email protected]>
Co-authored-by: Rene Sepulveda <[email protected]>
Co-authored-by: Sameer Sheorey <[email protected]>
  • Loading branch information
@reyanshsolis @theNded
@errissa @ssheorey
4 people authored May 22, 2021
1 parent 4923938 commit ff11ad2
Show file tree
Hide file tree
Showing 5 changed files with 367 additions and 194 deletions.
80 changes: 46 additions & 34 deletions examples/cpp/TICPOdometry.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -160,36 +160,38 @@ class ExampleWindow : public ReconstructionWindow {
// Initialize visualizer.
if (visualize_output_) {
{
// lock to protect `pcd_.curren_scan_` before modifying
// lock to protect `curren_scan_` and `bbox_` before modifying
// the value, ensuring the visualizer thread doesn't read the
// data, while we are modifying it.
std::lock_guard<std::mutex> lock(pcd_.lock_);
std::lock_guard<std::mutex> lock(pcd_and_bbox_.lock_);

// Copying the pointcloud to pcd_.current_scan_ on the `main
// thread` on CPU, which is later passed to the visualizer for
// rendering.
pcd_.current_scan_ = pointclouds_device_[0].CPU();
// Copying the pointcloud to pcd_and_bbox_.current_scan_ on the
// `main thread` on CPU, which is later passed to the visualizer
// for rendering.
pcd_and_bbox_.current_scan_ = pointclouds_device_[0].CPU();

// Removing `normal` attribute before passing it to
// the visualizer might give us some performance benifits.
pcd_.current_scan_.RemovePointAttr("normals");
pcd_and_bbox_.current_scan_.RemovePointAttr("normals");
}

gui::Application::GetInstance().PostToMainThread(this, [&]() {
std::lock_guard<std::mutex> lock(pcd_.lock_);
std::lock_guard<std::mutex> lock(pcd_and_bbox_.lock_);

// Setting background for the visualizer. [In this case: Black].
this->widget3d_->GetScene()->SetBackground({0, 0, 0, 1.0});

// Adding the first frame of the sequence to the visualizer,
// and rendering it using the material set for `current scan`.
this->widget3d_->GetScene()->AddGeometry(
filenames_[0], &pcd_.current_scan_, mat_);
filenames_[0], &pcd_and_bbox_.current_scan_, mat_);

// Getting bounding box and center to setup camera view.
auto bbox = this->widget3d_->GetScene()->GetBoundingBox();
auto center = bbox.GetCenter().cast<float>();
this->widget3d_->SetupCamera(verticalFoV, bbox, center);
pcd_and_bbox_.bbox_ =
this->widget3d_->GetScene()->GetBoundingBox();
auto center = pcd_and_bbox_.bbox_.GetCenter().cast<float>();
this->widget3d_->SetupCamera(verticalFoV, pcd_and_bbox_.bbox_,
center);
});
}
// ------------------------------------------------------------------
Expand Down Expand Up @@ -263,26 +265,35 @@ class ExampleWindow : public ReconstructionWindow {
std::stringstream out_;

{
// lock `pcd_.current_scan_` before modifying the value, to
// protect the case, when visualizer is accessing it
// at the same time we are modifying it.
std::lock_guard<std::mutex> lock(pcd_.lock_);
// lock `current_scan_` and `bbox_` before modifying the
// value, to protect the case, when visualizer is accessing
// it at the same time we are modifying it.
std::lock_guard<std::mutex> lock(pcd_and_bbox_.lock_);

// For visualization it is required that the pointcloud
// must be on CPU device.
// The `target` pointcloud is transformed to it's global
// position in the model by it's `frame to model transform`.
pcd_.current_scan_ =
pcd_and_bbox_.current_scan_ =
target.Transform(cumulative_transform.To(device_,
dtype_))
.CPU();

// Translate bounding box to current scan frame to model
// transform.
pcd_and_bbox_.bbox_ = pcd_and_bbox_.bbox_.Translate(
core::eigen_converter::TensorToEigenMatrixXd(
cumulative_transform.Clone()
.Slice(0, 0, 3)
.Slice(1, 3, 4)),
/*relative = */ false);

total_points_in_frame +=
pcd_.current_scan_.GetPoints().GetLength();
pcd_and_bbox_.current_scan_.GetPoints().GetLength();

// Removing `normal` attribute before passing it to
// the visualizer might give us some performance benifits.
pcd_.current_scan_.RemovePointAttr("normals");
pcd_and_bbox_.current_scan_.RemovePointAttr("normals");
}

if (i != 0) {
Expand All @@ -305,7 +316,8 @@ class ExampleWindow : public ReconstructionWindow {
// so, we don't need to use locks for such cases.
this->SetOutput(out_);

std::lock_guard<std::mutex> lock(pcd_.lock_);
std::lock_guard<std::mutex> lock(
pcd_and_bbox_.lock_);

// We render the `source` or the previous
// "current scan" pointcloud by using the material
Expand All @@ -317,15 +329,14 @@ class ExampleWindow : public ReconstructionWindow {
// a different material. In next iteration we will
// change the material to the `model` material.
this->widget3d_->GetScene()->AddGeometry(
filenames_[i + 1], &pcd_.current_scan_,
mat_);

// Bounding box and camera setup.
auto bbox = this->widget3d_->GetScene()
->GetBoundingBox();
auto center = bbox.GetCenter().cast<float>();
this->widget3d_->SetupCamera(verticalFoV, bbox,
center);
filenames_[i + 1],
&pcd_and_bbox_.current_scan_, mat_);

// Setup camera.
auto center = pcd_and_bbox_.bbox_.GetCenter()
.cast<float>();
this->widget3d_->SetupCamera(
verticalFoV, pcd_and_bbox_.bbox_, center);
});
}
// --------------------------------------------------------------
Expand Down Expand Up @@ -533,8 +544,6 @@ class ExampleWindow : public ReconstructionWindow {
t::io::ReadPointCloud(path, pointcloud_local,
{"auto", false, false, true});

// Cpnverting attributes to Floar32 and currently only
// Float32 pointcloud is supported by the tensor
// registration module.
for (std::string attr : {"points", "colors", "normals"}) {
if (pointcloud_local.HasPointAttr(attr)) {
Expand All @@ -555,7 +564,7 @@ class ExampleWindow : public ReconstructionWindow {
pointcloud_local.GetPoints()
.Slice(0, 0, -1)
.Slice(1, 2, 3)
.To(dtype_, true));
.To(dtype_, false));

// Normals are required by `PointToPlane` registration method.
// Currenly Normal Estimation is not supported by
Expand Down Expand Up @@ -640,15 +649,18 @@ class ExampleWindow : public ReconstructionWindow {
}

private:
// lock to protect `pcd_.current_scan_` before modifying
// lock to protect `current_scan_` and `bbox_` before modifying
// the value, ensuring the visualizer thread doesn't read the
// data, while we are modifying it.
struct {
// Mutex lock to protect data memeber current_scan_.
std::mutex lock_;
// Pointcloud to store the "current scan", used for visualization.
t::geometry::PointCloud current_scan_;
} pcd_;
// Bounding box. It is translated by the translation component of the
// cumulative transformation.
geometry::AxisAlignedBoundingBox bbox_;
} pcd_and_bbox_;

// Checks if the GUI is closed, and if so, stop the code.
std::atomic<bool> is_done_;
Expand Down
Loading

0 comments on commit ff11ad2

Please sign in to comment.