<commit_before>#include <clams/trajectory_visualizer.h>

using namespace std;
using namespace Eigen;

namespace clams
{

  TrajectoryVisualizer::TrajectoryVisualizer(StreamSequenceBase::ConstPtr sseq, Trajectory traj) :
    dddm_(NULL),
    max_range_(MAX_RANGE_MAP),
    vgsize_(DEFAULT_VGSIZE),
    sseq_(sseq),
    traj_(traj),
    map_(new Cloud),
    quitting_(false),
    needs_update_(false),
    frame_idx_(0),
    show_frame_(false),
    use_distortion_model_(false),
    color_frame_(false)
  {
    vis_.registerKeyboardCallback(&TrajectoryVisualizer::keyboardCallback, *this);
    vis_.registerPointPickingCallback(&TrajectoryVisualizer::pointPickingCallback, *this);
    vis_.setBackgroundColor(1, 1, 1);
    vis_.addText("", 10, 10, 16, 0, 0, 0, "title");

    Cloud::Ptr pcd(new Cloud);
    Point tmp(0, 0, 0);
    pcd->push_back(tmp);  // PCLVis / VTK can spit out horrible garbage if you give it an empty pcd.
    vis_.addPointCloud(pcd, "map");
    vis_.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "map");
  }

  void TrajectoryVisualizer::run()
  {
    // -- Build the map.
    cout << "Building map..." << endl;
    map_ = SlamCalibrator::buildMap(sseq_, traj_, max_range_, vgsize_);
    cout << "Done building map." << endl;
    cout << map_->size() << " points." << endl;
    needs_update_ = true;

    // -- Run the main visualization loop.
    incrementFrameIdx(1);
    while(true) {
      { scopeLockRead; if(quitting_) break; }
      usleep(5e3);
    
      lockWrite();
      if(needs_update_) {
        Cloud::Ptr pcd(new Cloud);
        *pcd = *map_;
        
        // -- Add the raw sensor data from the current frame.
        if(show_frame_) { 
          if(traj_.exists(frame_idx_)) {
            Frame pose_frame;
            sseq_->readFrame(frame_idx_, &pose_frame);
            if(dddm_ && use_distortion_model_) {
              ScopedTimer st("Undistorting");
              dddm_->undistort(pose_frame.depth_.get());
            }
            Cloud pose_pcd;
            sseq_->proj_.frameToCloud(pose_frame, &pose_pcd);
            if(!color_frame_) {
              for(size_t i = 0; i < pose_pcd.size(); ++i) {
                pose_pcd[i].r = 255;
                pose_pcd[i].g = 0;
                pose_pcd[i].b = 0;
              }
            }
          
            Affine3f transform = traj_.get(frame_idx_).cast<float>();
            pcl::transformPointCloud(pose_pcd, pose_pcd, transform);
            *pcd += pose_pcd;
          }
        }

        vis_.removeShape("title");
        vis_.addText(title_, 10, 10, 16, 0, 0, 0, "title");
      
        vis_.updatePointCloud(pcd, "map");
        needs_update_ = false;
      }

      vis_.removeCoordinateSystem();
      vis_.addCoordinateSystem(0.3, traj_.get(frame_idx_).cast<float>());
    
      unlockWrite();
      vis_.spinOnce(3);
    }
  }

  void TrajectoryVisualizer::pointPickingCallback(const pcl::visualization::PointPickingEvent& event, void* cookie)
  {
    scopeLockRead;

    if(event.getPointIndex() == -1)
      return;
    if(!traj_.exists(frame_idx_))
      return;
    
    Point pt;
    event.getPoint(pt.x, pt.y, pt.z);
    cout << "Selected point: " << pt.x << ", " << pt.y << ", " << pt.z << endl;
    vis_.removeAllShapes();
  
    Point origin;
    Affine3f transform = traj_.get(frame_idx_).cast<float>();
    origin.getVector3fMap() = transform.translation();
    vis_.addArrow<Point, Point>(origin, pt, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2, "line");<commit_msg>Trying to fix oddly persistent PCLVisualizer window.  Adding comment about how this is a known bug in PCL.<commit_after>#include <clams/trajectory_visualizer.h>

using namespace std;
using namespace Eigen;

namespace clams
{

  TrajectoryVisualizer::TrajectoryVisualizer(StreamSequenceBase::ConstPtr sseq, Trajectory traj) :
    dddm_(NULL),
    max_range_(MAX_RANGE_MAP),
    vgsize_(DEFAULT_VGSIZE),
    sseq_(sseq),
    traj_(traj),
    map_(new Cloud),
    quitting_(false),
    needs_update_(false),
    frame_idx_(0),
    show_frame_(false),
    use_distortion_model_(false),
    color_frame_(false)
  {
    vis_.registerKeyboardCallback(&TrajectoryVisualizer::keyboardCallback, *this);
    vis_.registerPointPickingCallback(&TrajectoryVisualizer::pointPickingCallback, *this);
    vis_.setBackgroundColor(1, 1, 1);
    vis_.addText("", 10, 10, 16, 0, 0, 0, "title");

    Cloud::Ptr pcd(new Cloud);
    Point tmp(0, 0, 0);
    pcd->push_back(tmp);  // PCLVis / VTK can spit out horrible garbage if you give it an empty pcd.
    vis_.addPointCloud(pcd, "map");
    vis_.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "map");
  }

  void TrajectoryVisualizer::run()
  {
    // -- Build the map.
    cout << "Building map..." << endl;
    map_ = SlamCalibrator::buildMap(sseq_, traj_, max_range_, vgsize_);
    cout << "Done building map." << endl;
    cout << map_->size() << " points." << endl;
    needs_update_ = true;

    // -- Run the main visualization loop.
    incrementFrameIdx(1);
    while(true) {
      { scopeLockRead; if(quitting_) break; }
      usleep(5e3);
    
      lockWrite();
      if(needs_update_) {
        Cloud::Ptr pcd(new Cloud);
        *pcd = *map_;
        
        // -- Add the raw sensor data from the current frame.
        if(show_frame_) { 
          if(traj_.exists(frame_idx_)) {
            Frame pose_frame;
            sseq_->readFrame(frame_idx_, &pose_frame);
            if(dddm_ && use_distortion_model_) {
              ScopedTimer st("Undistorting");
              dddm_->undistort(pose_frame.depth_.get());
            }
            Cloud pose_pcd;
            sseq_->proj_.frameToCloud(pose_frame, &pose_pcd);
            if(!color_frame_) {
              for(size_t i = 0; i < pose_pcd.size(); ++i) {
                pose_pcd[i].r = 255;
                pose_pcd[i].g = 0;
                pose_pcd[i].b = 0;
              }
            }
          
            Affine3f transform = traj_.get(frame_idx_).cast<float>();
            pcl::transformPointCloud(pose_pcd, pose_pcd, transform);
            *pcd += pose_pcd;
          }
        }

        vis_.removeShape("title");
        vis_.addText(title_, 10, 10, 16, 0, 0, 0, "title");
      
        vis_.updatePointCloud(pcd, "map");
        needs_update_ = false;
      }

      vis_.removeCoordinateSystem();
      vis_.addCoordinateSystem(0.3, traj_.get(frame_idx_).cast<float>());
    
      unlockWrite();
      vis_.spinOnce(3);
    }

    // This PCL call doesn't appear to do anything.
    // The result is that the visualizer persists when it shouldn't.
    // This is a known bug. See
    // https://github.com/PointCloudLibrary/pcl/pull/85
    vis_.close();  
  }

  void TrajectoryVisualizer::pointPickingCallback(const pcl::visualization::PointPickingEvent& event, void* cookie)
  {
    scopeLockRead;

    if(event.getPointIndex() == -1)
      return;
    if(!traj_.exists(frame_idx_))
      return;
    
    Point pt;
    event.getPoint(pt.x, pt.y, pt.z);
    cout << "Selected point: " << pt.x << ", " << pt.y << ", " << pt.z << endl;
    vis_.removeAllShapes();
  
    Point origin;
    Affine3f transform = traj_.get(frame_idx_).cast<float>();
    origin.getVector3fMap() = transform.translation();
    vis_.addArrow<Point, Point>(origin, pt, 0.2, 0.2, 0.2, 0.2, 0.2, 0.2, "line");
{'fraction_non_alphanumeric': 0.13831953305280303, 'fraction_numerical': 0.014931451065562644, 'mean_word_length': 2.537205952952472, 'pattern_counts': {'":': 0, '<': 71, '<?xml version=': 0, '>': 23, 'https://': 1, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '3950245', 'n_tokens_mistral': 2566, 'n_tokens_neox': 2448, 'n_words': 476}