WriterPcd.cs

Go to the documentation of this file.

using System.Collections.Generic;
using System.IO;
using System.Text;
using SharedObjects;

namespace PointWriter
{
    // example
    //# .PCD v.7 - Point Cloud Data file format
    //VERSION .7
    //FIELDS x y z rgb
    //SIZE 4 4 4 4
    //TYPE F F F F
    //COUNT 1 1 1 1
    //WIDTH  163247
    //HEIGHT 1
    //VIEWPOINT 0 0 0 1 0 0 0
    //POINTS 163247
    //DATA ascii
    //178 56 456 45645


    public class WriterPcd : IPointWriter
    {
        private readonly List<ColorPoint3D> points = new List<ColorPoint3D>();

        [LogException]
        public void AddPoint(ColorPoint3D point)
        {
            this.points.Add(point);
        }

        [Log]
        [LogException]
        public void Write(string fileName)
        {
            StreamWriter streamWriter = new StreamWriter(fileName);
            streamWriter.Write(this.CreateHeader());
            foreach (ColorPoint3D point in this.points)
            {
                //todo color?
                streamWriter.WriteLine("{0} {1} {2} {3}", (float)point.X, (float)point.Y, (float)point.Z, (int)point.Color.R << 16|(int)point.Color.G<<8|(int)point.Color.B);
            }
            streamWriter.Close();
        }

        [LogException]
        private string CreateHeader()
        {
            StringBuilder builder = new StringBuilder();
            builder.AppendLine("VERSION .7");
            builder.AppendLine("FIELDS x y z rgb");
            builder.AppendLine("SIZE 4 4 4 4");
            builder.AppendLine("TYPE F F F F");
            builder.AppendLine("COUNT 1 1 1 1");
            builder.AppendLine($"WIDTH   {this.points.Count}");
            builder.AppendLine("HEIGHT 1");
            builder.AppendLine("VIEWPOINT 0 0 0 1 0 0 0");
            builder.AppendLine($"POINTS   {this.points.Count}");
            builder.AppendLine("DATA ascii");
            return builder.ToString();
        }
    }
}