// Convert from CameraPose to tf::Pose (position and orientation)
inline void CameraPose2TFPose(const CameraPose& cameraPose, tf::Pose& pose)
{
cv::Vec4d orientation = cameraPose.GetOrientationQuaternion();
cv::Vec3d position = cameraPose.GetPosition();
pose.setOrigin(tf::Vector3(position[0], position[1], position[2]));
pose.setRotation(tf::Quaternion(orientation[1], orientation[2], orientation[3], orientation[0] )); // q = (x,y,z,w)
}
sptam::sptam_node::~sptam_node()
{
ROS_INFO_STREAM("starting sptam node cleanup...");
// transform_thread_ is null if odometry is disabled
if ( transform_thread_ ) {
ROS_INFO_STREAM("wait for transform publisher thread to join...");
transform_thread_->join();
delete transform_thread_;
}
ROS_INFO_STREAM("stopping sptam threads...");
sptam_->stop();
#ifdef SHOW_PROFILING
for ( const auto& mapPoint : map_.GetMapPoints() ) {
WriteToLog( " tk MeasurementCount: ", mapPoint->GetMeasurementCount() );
}
#endif
#ifdef SHOW_PROFILING
for ( const auto& keyFrame : map_.GetKeyFrames() ) {
CameraPose keyFramePose = keyFrame->GetCameraPose();
WriteToLog("BASE_LINK_KF:", keyFrame->GetId(), keyFramePose.GetPosition(), keyFramePose.GetOrientationMatrix());
}
#endif
// create map file
{
std::ofstream out("map cloud.dat");
for ( const auto& point : map_.GetMapPoints() )
out << point->GetPosition().x << " " << point->GetPosition().y << " " << point->GetPosition().z << std::endl;
out.close();
}
ROS_INFO_STREAM("done!");
// sleep for one second
ros::Duration( 1.0 ).sleep();
}
CameraPose StereoFrame::ComputeRightCameraPose(const CameraPose& leftCameraPose, const double stereo_baseline)
{
// The position is the baseline converted to world coordinates.
// The orientation is the same as the left camera.
return CameraPose(leftCameraPose.ToWorld( Eigen::Vector3d(stereo_baseline, 0, 0) ), leftCameraPose.GetOrientationQuaternion(), leftCameraPose.covariance());
}
inline double distance(CameraPose& pose)
{ return (this->get_position() - pose.get_position()).norm(); }