diff --git a/src/processor/processor_odom_icp_3d.cpp b/src/processor/processor_odom_icp_3d.cpp index 20dde002124334b5c64cf8241cd3fbdbba92a3e0..e208e3e27563d18693a4116ea9c6006f34a68ac0 100644 --- a/src/processor/processor_odom_icp_3d.cpp +++ b/src/processor/processor_odom_icp_3d.cpp @@ -88,6 +88,7 @@ unsigned int ProcessorOdomIcp3d::processKnown() { if (origin_ptr_ && (incoming_ptr_ != origin_ptr_)) { + origin_laser_ = std::static_pointer_cast<CaptureLaser3d>(origin_ptr_); pairAlign(origin_laser_->getPointCloud(), incoming_laser_->getPointCloud(), T_origin_last_, @@ -102,11 +103,15 @@ unsigned int ProcessorOdomIcp3d::processKnown() */ unsigned int ProcessorOdomIcp3d::processNew(const int& _max_features) { - pairAlign(last_laser_->getPointCloud(), - incoming_laser_->getPointCloud(), - Eigen::Isometry3d::Identity(), - T_last_incoming_, - registration_solver_); + if (last_ptr_) + { + last_laser_ = std::static_pointer_cast<CaptureLaser3d>(last_ptr_); + pairAlign(last_laser_->getPointCloud(), + incoming_laser_->getPointCloud(), + Eigen::Isometry3d::Identity(), + T_last_incoming_, + registration_solver_); + } return 0; }; @@ -123,6 +128,10 @@ bool ProcessorOdomIcp3d::voteForKeyFrame() const { return true; } + // TODO: + // - vote by distance + // - vote by angle + // - vote by nbr. of captures return false; }; @@ -156,13 +165,13 @@ TimeStamp ProcessorOdomIcp3d::getTimeStamp() const VectorComposite ProcessorOdomIcp3d::getState(const StateStructure& _structure) const { - VectorComposite state_origin = origin_laser_->getFrame()->getState("PO"); + VectorComposite state_origin = origin_ptr_->getFrame()->getState("PO"); Eigen::Isometry3d T_world_origin_robot = Eigen::Translation3d(state_origin.at('P')) * Eigen::Quaterniond(state_origin.at('O').data()); Eigen::Isometry3d T_world_last_robot = T_world_origin_robot * T_robot_sensor_ * T_origin_last_ * T_sensor_robot_; VectorComposite state_last; - state_last.at('P') = T_world_last_robot.translation(); - state_last.at('O') = Eigen::Quaterniond(T_world_last_robot.rotation()).coeffs(); + state_last['P'] = T_world_last_robot.translation(); + state_last['O'] = Eigen::Quaterniond(T_world_last_robot.rotation()).coeffs(); return state_last; } @@ -177,16 +186,19 @@ VectorComposite ProcessorOdomIcp3d::getState(const TimeStamp& _ts, const StateSt */ void ProcessorOdomIcp3d::establishFactors() { - // emplace a feature of type FeatureMotion undet the capture last_ptr_ or last_laser_ - Eigen::Vector7d measurement_of_motion; - measurement_of_motion.head(3) = T_origin_last_.translation(); - measurement_of_motion.tail(4) = Eigen::Quaterniond(T_origin_last_.rotation()).coeffs(); - auto feature = - FeatureBase::emplace<FeatureBase>(last_laser_, "FeatureBase", measurement_of_motion, transform_cov_); - - // emplace a factor of type FactorRelativePose3dWithExtrinsics under the feature above - auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>( - feature, feature, origin_laser_->getFrame(), shared_from_this(), false, TOP_MOTION); + if (last_ptr_ != origin_ptr_) // skip if it's the same frame (it happens the SECOND_TIME) + { + // emplace a feature of type FeatureMotion undet the capture last_ptr_ or last_laser_ + Eigen::Vector7d measurement_of_motion; + measurement_of_motion.head(3) = T_origin_last_.translation(); + measurement_of_motion.tail(4) = Eigen::Quaterniond(T_origin_last_.rotation()).coeffs(); + auto feature = + FeatureBase::emplace<FeatureBase>(last_ptr_, "FeatureBase", measurement_of_motion, transform_cov_); + + // emplace a factor of type FactorRelativePose3dWithExtrinsics under the feature above + auto factor = FactorBase::emplace<FactorRelativePose3dWithExtrinsics>( + feature, feature, origin_ptr_->getFrame(), shared_from_this(), false, TOP_MOTION); + } }; } // namespace wolf