gtsam_points
Loading...
Searching...
No Matches
Public Types | Public Member Functions | Protected Member Functions | Protected Attributes | List of all members
gtsam_points::IntegratedCT_ICPFactor_< TargetFrame, SourceFrame > Class Template Reference

Continuous Time ICP Factor Bellenbach et al., "CT-ICP: Real-time Elastic LiDAR Odometry with Loop Closure", 2021. More...

#include <integrated_ct_icp_factor.hpp>

Inheritance diagram for gtsam_points::IntegratedCT_ICPFactor_< TargetFrame, SourceFrame >:
Inheritance graph
[legend]
Collaboration diagram for gtsam_points::IntegratedCT_ICPFactor_< TargetFrame, SourceFrame >:
Collaboration graph
[legend]

Public Types

using shared_ptr = gtsam_points::shared_ptr< IntegratedCT_ICPFactor_< TargetFrame, SourceFrame > >
 

Public Member Functions

 IntegratedCT_ICPFactor_ (gtsam::Key source_t0_key, gtsam::Key source_t1_key, const std::shared_ptr< const TargetFrame > &target, const std::shared_ptr< const SourceFrame > &source, const std::shared_ptr< const NearestNeighborSearch > &target_tree)
 Constructor.
 
 IntegratedCT_ICPFactor_ (gtsam::Key source_t0_key, gtsam::Key source_t1_key, const std::shared_ptr< const TargetFrame > &target, const std::shared_ptr< const SourceFrame > &source)
 Constructor.
 
virtual void print (const std::string &s="", const gtsam::KeyFormatter &keyFormatter=gtsam::DefaultKeyFormatter) const override
 Print the factor information.
 
virtual size_t memory_usage () const
 
virtual size_t dim () const override
 
virtual double error (const gtsam::Values &values) const override
 
virtual gtsam::GaussianFactor::shared_ptr linearize (const gtsam::Values &values) const override
 
void set_num_threads (int n)
 
void set_max_correspondence_distance (double dist)
 
const std::vector< double > & get_time_table () const
 
const std::vector< int > & get_time_indices () const
 
const std::vector< gtsam::Pose3 > & get_source_poses () const
 
std::vector< Eigen::Vector4d > deskewed_source_points (const gtsam::Values &values, bool local=false)
 
virtual void update_poses (const gtsam::Values &values) const
 

Protected Member Functions

virtual void update_correspondences () const
 

Protected Attributes

int num_threads
 
double max_correspondence_distance_sq
 
std::shared_ptr< const NearestNeighborSearchtarget_tree
 
std::vector< double > time_table
 
std::vector< gtsam::Pose3 > source_poses
 
std::vector< gtsam::Matrix6 > pose_derivatives_t0
 
std::vector< gtsam::Matrix6 > pose_derivatives_t1
 
std::vector< int > time_indices
 
std::vector< long > correspondences
 
std::shared_ptr< const TargetFrame > target
 
std::shared_ptr< const SourceFrame > source
 

Detailed Description

template<typename TargetFrame = gtsam_points::PointCloud, typename SourceFrame = gtsam_points::PointCloud>
class gtsam_points::IntegratedCT_ICPFactor_< TargetFrame, SourceFrame >

Continuous Time ICP Factor Bellenbach et al., "CT-ICP: Real-time Elastic LiDAR Odometry with Loop Closure", 2021.

Constructor & Destructor Documentation

◆ IntegratedCT_ICPFactor_() [1/2]

template<typename TargetFrame , typename SourceFrame >
gtsam_points::IntegratedCT_ICPFactor_< TargetFrame, SourceFrame >::IntegratedCT_ICPFactor_ ( gtsam::Key  source_t0_key,
gtsam::Key  source_t1_key,
const std::shared_ptr< const TargetFrame > &  target,
const std::shared_ptr< const SourceFrame > &  source,
const std::shared_ptr< const NearestNeighborSearch > &  target_tree 
)

Constructor.

Parameters
source_t0_keyKey of the source pose at the beginning of the scan
source_t1_keyKey of the source pose at the end of the scan
targetTarget point cloud
sourceSource point cloud
target_treeNN search for the target point cloud

◆ IntegratedCT_ICPFactor_() [2/2]

template<typename TargetFrame , typename SourceFrame >
gtsam_points::IntegratedCT_ICPFactor_< TargetFrame, SourceFrame >::IntegratedCT_ICPFactor_ ( gtsam::Key  source_t0_key,
gtsam::Key  source_t1_key,
const std::shared_ptr< const TargetFrame > &  target,
const std::shared_ptr< const SourceFrame > &  source 
)

Constructor.

Parameters
source_t0_keyKey of the source pose at the beginning of the scan
source_t1_keyKey of the source pose at the end of the scan
targetTarget point cloud
sourceSource point cloud

Member Function Documentation

◆ print()

template<typename TargetFrame , typename SourceFrame >
void gtsam_points::IntegratedCT_ICPFactor_< TargetFrame, SourceFrame >::print ( const std::string &  s = "",
const gtsam::KeyFormatter &  keyFormatter = gtsam::DefaultKeyFormatter 
) const
overridevirtual

Print the factor information.

Reimplemented in gtsam_points::IntegratedCT_GICPFactor_< TargetFrame, SourceFrame >.


The documentation for this class was generated from the following files: