ACloudViewer  3.9.4
A Modern Library for 3D Data Processing
cloudViewer::t::pipelines::slac Namespace Reference

Classes

class  ControlGrid
 
struct  SLACOptimizerParams
 
struct  SLACDebugOption
 

Typedefs

using PoseGraph = cloudViewer::pipelines::registration::PoseGraph
 

Functions

static PointCloud CreateTPCDFromFile (const std::string &fname, const core::Device &device=core::Device("CPU:0"))
 
static void FillInRigidAlignmentTerm (Tensor &AtA, Tensor &Atb, Tensor &residual, PointCloud &tpcd_i, PointCloud &tpcd_j, const Tensor &Ti, const Tensor &Tj, const int i, const int j, const float threshold)
 
void FillInRigidAlignmentTerm (Tensor &AtA, Tensor &Atb, Tensor &residual, const std::vector< std::string > &fnames, const PoseGraph &pose_graph, const SLACOptimizerParams &params, const SLACDebugOption &debug_option)
 
static void FillInSLACAlignmentTerm (Tensor &AtA, Tensor &Atb, Tensor &residual, ControlGrid &ctr_grid, const PointCloud &tpcd_param_i, const PointCloud &tpcd_param_j, const Tensor &Ti, const Tensor &Tj, const int i, const int j, const int n_fragments, const float threshold)
 
void FillInSLACAlignmentTerm (Tensor &AtA, Tensor &Atb, Tensor &residual, ControlGrid &ctr_grid, const std::vector< std::string > &fnames, const PoseGraph &pose_graph, const SLACOptimizerParams &params, const SLACDebugOption &debug_option)
 
void FillInSLACRegularizerTerm (Tensor &AtA, Tensor &Atb, Tensor &residual, ControlGrid &ctr_grid, int n_frags, const SLACOptimizerParams &params, const SLACDebugOption &debug_option)
 
static std::vector< std::string > PreprocessPointClouds (const std::vector< std::string > &fnames, const SLACOptimizerParams &params)
 
static core::Tensor ConvertCorrespondencesTargetIndexedToCx2Form (const core::Tensor &target_correspondences)
 
static core::Tensor GetCorrespondenceSetForPointCloudPair (int i, int j, PointCloud &tpcd_i, PointCloud &tpcd_j, const core::Tensor &T_i, const core::Tensor &T_j, const core::Tensor &T_ij, float distance_threshold, float fitness_threshold, bool debug)
 
void SaveCorrespondencesForPointClouds (const std::vector< std::string > &fnames_processed, const PoseGraph &fragment_pose_graph, const SLACOptimizerParams &params=SLACOptimizerParams(), const SLACDebugOption &debug_option=SLACDebugOption())
 Read pose graph containing loop closures and odometry to compute putative correspondences between pairs of pointclouds. More...
 
static void InitializeControlGrid (ControlGrid &ctr_grid, const std::vector< std::string > &fnames)
 
static void UpdatePoses (PoseGraph &fragment_pose_graph, core::Tensor &delta)
 
static void UpdateControlGrid (ControlGrid &ctr_grid, core::Tensor &delta)
 
std::pair< PoseGraph, ControlGridRunSLACOptimizerForFragments (const std::vector< std::string > &fragment_filenames, const PoseGraph &fragment_pose_graph, const SLACOptimizerParams &params=SLACOptimizerParams(), const SLACDebugOption &debug_option=SLACDebugOption())
 Simultaneous Localization and Calibration: Self-Calibration of Consumer Depth Cameras, CVPR 2014 Qian-Yi Zhou and Vladlen Koltun Estimate a shared control grid for all fragments for scene reconstruction, implemented in https://github.com/qianyizh/ElasticReconstruction. More...
 
PoseGraph RunRigidOptimizerForFragments (const std::vector< std::string > &fragment_filenames, const PoseGraph &fragment_pose_graph, const SLACOptimizerParams &params=SLACOptimizerParams(), const SLACDebugOption &debug_option=SLACDebugOption())
 Extended ICP to simultaneously align multiple point clouds with dense pairwise point-to-plane distances. More...
 
static Eigen::Vector3d Jet (double v, double vmin, double vmax)
 
void VisualizePointCloudCorrespondences (const t::geometry::PointCloud &tpcd_i, const t::geometry::PointCloud &tpcd_j, const core::Tensor correspondences, const core::Tensor &T_ij)
 Visualize pairs with correspondences. More...
 
void VisualizePointCloudEmbedding (t::geometry::PointCloud &tpcd_param, ControlGrid &ctr_grid, bool show_lines)
 
void VisualizePointCloudDeformation (const geometry::PointCloud &tpcd_param, ControlGrid &ctr_grid)
 
void VisualizeGridDeformation (ControlGrid &cgrid)
 
void pybind_slac (py::module &m)
 

Variables

static const Eigen::Vector3d kSourceColor = Eigen::Vector3d(0, 1, 0)
 
static const Eigen::Vector3d kTargetColor = Eigen::Vector3d(1, 0, 0)
 
static const Eigen::Vector3d kCorresColor = Eigen::Vector3d(0, 0, 1)
 
static const std::unordered_map< std::string, std::string > map_shared_argument_docstrings
 

Typedef Documentation

◆ PoseGraph

Function Documentation

◆ ConvertCorrespondencesTargetIndexedToCx2Form()

◆ CreateTPCDFromFile()

static PointCloud cloudViewer::t::pipelines::slac::CreateTPCDFromFile ( const std::string &  fname,
const core::Device device = core::Device("CPU:0") 
)
static

◆ FillInRigidAlignmentTerm() [1/2]

◆ FillInRigidAlignmentTerm() [2/2]

static void cloudViewer::t::pipelines::slac::FillInRigidAlignmentTerm ( Tensor AtA,
Tensor Atb,
Tensor residual,
PointCloud tpcd_i,
PointCloud tpcd_j,
const Tensor Ti,
const Tensor Tj,
const int  i,
const int  j,
const float  threshold 
)
static

◆ FillInSLACAlignmentTerm() [1/2]

◆ FillInSLACAlignmentTerm() [2/2]

◆ FillInSLACRegularizerTerm()

◆ GetCorrespondenceSetForPointCloudPair()

◆ InitializeControlGrid()

static void cloudViewer::t::pipelines::slac::InitializeControlGrid ( ControlGrid ctr_grid,
const std::vector< std::string > &  fnames 
)
static

◆ Jet()

static Eigen::Vector3d cloudViewer::t::pipelines::slac::Jet ( double  v,
double  vmin,
double  vmax 
)
static

Definition at line 21 of file Visualization.cpp.

◆ PreprocessPointClouds()

static std::vector<std::string> cloudViewer::t::pipelines::slac::PreprocessPointClouds ( const std::vector< std::string > &  fnames,
const SLACOptimizerParams params 
)
static

◆ pybind_slac()

◆ RunRigidOptimizerForFragments()

PoseGraph cloudViewer::t::pipelines::slac::RunRigidOptimizerForFragments ( const std::vector< std::string > &  fragment_filenames,
const PoseGraph fragment_pose_graph,
const SLACOptimizerParams params = SLACOptimizerParams(),
const SLACDebugOption debug_option = SLACDebugOption() 
)

Extended ICP to simultaneously align multiple point clouds with dense pairwise point-to-plane distances.

Parameters
fragment_fnamesVector of filenames for pointcloud fragments.
fragment_pose_graphLegacy PoseGraph for pointcloud fragments.
paramsParameters to tune in rigid optimization.
debug_optionDebug options.
Returns
Updated pose graph.

Definition at line 370 of file SLACOptimizer.cpp.

References cloudViewer::core::Tensor::Arange(), FillInRigidAlignmentTerm(), cloudViewer::core::Float32, cloudViewer::core::Tensor::IndexSet(), LogInfo, cloudViewer::utility::filesystem::MakeDirectory(), cloudViewer::core::Tensor::Ones(), params, PreprocessPointClouds(), SaveCorrespondencesForPointClouds(), cloudViewer::core::Tensor::Solve(), UpdatePoses(), and cloudViewer::core::Tensor::Zeros().

Referenced by pybind_slac().

◆ RunSLACOptimizerForFragments()

std::pair< PoseGraph, ControlGrid > cloudViewer::t::pipelines::slac::RunSLACOptimizerForFragments ( const std::vector< std::string > &  fragment_filenames,
const PoseGraph fragment_pose_graph,
const SLACOptimizerParams params = SLACOptimizerParams(),
const SLACDebugOption debug_option = SLACDebugOption() 
)

Simultaneous Localization and Calibration: Self-Calibration of Consumer Depth Cameras, CVPR 2014 Qian-Yi Zhou and Vladlen Koltun Estimate a shared control grid for all fragments for scene reconstruction, implemented in https://github.com/qianyizh/ElasticReconstruction.

Parameters
fragment_filenamesVector of filenames for pointcloud fragments.
fragment_pose_graphLegacy PoseGraph for pointcloud fragments.
paramsParameters to tune in SLAC.
debug_optionDebug options.
Returns
pair of optimized registration::PoseGraph and slac::ControlGrid.

Definition at line 298 of file SLACOptimizer.cpp.

References cloudViewer::core::Tensor::Arange(), cloudViewer::t::pipelines::slac::ControlGrid::Compactify(), FillInSLACAlignmentTerm(), FillInSLACRegularizerTerm(), cloudViewer::core::Float32, cloudViewer::core::Tensor::GetLength(), cloudViewer::core::Tensor::IndexSet(), InitializeControlGrid(), cloudViewer::core::Int64, LogInfo, cloudViewer::core::make_pair(), cloudViewer::utility::filesystem::MakeDirectory(), cloudViewer::pipelines::registration::PoseGraph::nodes_, cloudViewer::core::Tensor::Ones(), params, PreprocessPointClouds(), SaveCorrespondencesForPointClouds(), cloudViewer::t::pipelines::slac::ControlGrid::Size(), cloudViewer::core::Tensor::Slice(), cloudViewer::core::Tensor::Solve(), UpdateControlGrid(), UpdatePoses(), and cloudViewer::core::Tensor::Zeros().

Referenced by pybind_slac().

◆ SaveCorrespondencesForPointClouds()

void cloudViewer::t::pipelines::slac::SaveCorrespondencesForPointClouds ( const std::vector< std::string > &  fnames_processed,
const PoseGraph fragment_pose_graph,
const SLACOptimizerParams params = SLACOptimizerParams(),
const SLACDebugOption debug_option = SLACDebugOption() 
)

Read pose graph containing loop closures and odometry to compute putative correspondences between pairs of pointclouds.

Parameters
fnames_processedVector of filenames for processed pointcloud fragments.
fragment_pose_graphLegacy PoseGraph for pointcloud fragments.
paramsParameters to tune in finding correspondences.
debug_optionSLACDebugOption containing the debug options.

Definition at line 209 of file SLACOptimizer.cpp.

References CreateTPCDFromFile(), cloudViewer::t::pipelines::slac::SLACDebugOption::debug_, cloudViewer::pipelines::registration::PoseGraph::edges_, cloudViewer::core::eigen_converter::EigenMatrixToTensor(), cloudViewer::utility::filesystem::FileExists(), format, GetCorrespondenceSetForPointCloudPair(), cloudViewer::core::Tensor::GetLength(), LogInfo, cloudViewer::pipelines::registration::PoseGraph::nodes_, params, and cloudViewer::core::Tensor::Save().

Referenced by pybind_slac(), RunRigidOptimizerForFragments(), and RunSLACOptimizerForFragments().

◆ UpdateControlGrid()

◆ UpdatePoses()

◆ VisualizeGridDeformation()

◆ VisualizePointCloudCorrespondences()

void cloudViewer::t::pipelines::slac::VisualizePointCloudCorrespondences ( const t::geometry::PointCloud tpcd_i,
const t::geometry::PointCloud tpcd_j,
const core::Tensor  correspondences,
const core::Tensor T_ij 
)

Visualize pairs with correspondences.

Parameters
tpcd_i,sourcepoint cloud.
tpcd_j,targetpoint cloud.
correspondencesPutative correspondence between tcpd_i and tpcd_j.
T_ijTransformation from tpcd_i to tpcd_j. Use T_j.Inverse() @ T_i (node transformation in a pose graph) to check global correspondences , and T_ij (edge transformation) to check pairwise correspondences.

Definition at line 46 of file Visualization.cpp.

References cloudViewer::t::geometry::PointCloud::Clone(), cloudViewer::geometry::LineSet::CreateFromPointCloudCorrespondences(), cloudViewer::visualization::DrawGeometries(), cloudViewer::core::Tensor::GetLength(), kCorresColor, kSourceColor, kTargetColor, cloudViewer::core::make_pair(), cloudViewer::core::Tensor::To(), cloudViewer::t::geometry::PointCloud::ToLegacy(), and cloudViewer::t::geometry::PointCloud::Transform().

Referenced by FillInRigidAlignmentTerm(), FillInSLACAlignmentTerm(), and GetCorrespondenceSetForPointCloudPair().

◆ VisualizePointCloudDeformation()

◆ VisualizePointCloudEmbedding()

Variable Documentation

◆ kCorresColor

const Eigen::Vector3d cloudViewer::t::pipelines::slac::kCorresColor = Eigen::Vector3d(0, 0, 1)
static

Definition at line 19 of file Visualization.cpp.

Referenced by VisualizePointCloudCorrespondences().

◆ kSourceColor

const Eigen::Vector3d cloudViewer::t::pipelines::slac::kSourceColor = Eigen::Vector3d(0, 1, 0)
static

Definition at line 17 of file Visualization.cpp.

Referenced by VisualizePointCloudCorrespondences().

◆ kTargetColor

const Eigen::Vector3d cloudViewer::t::pipelines::slac::kTargetColor = Eigen::Vector3d(1, 0, 0)
static

Definition at line 18 of file Visualization.cpp.

Referenced by VisualizePointCloudCorrespondences().

◆ map_shared_argument_docstrings

const std::unordered_map<std::string, std::string> cloudViewer::t::pipelines::slac::map_shared_argument_docstrings
static
Initial value:
= {
{"fnames_processed",
"List of filenames (str) for pre-processed pointcloud "
"fragments."},
{"fragment_filenames",
"List of filenames (str) for pointcloud fragments."},
{"fragment_pose_graph", "PoseGraph for pointcloud fragments"},
{"params",
"slac_optimizer_params Parameters to tune in optimization."},
{"debug_option", "debug options."}}

Definition at line 24 of file slac.cpp.

Referenced by pybind_slac().