title: ICP produces alignments with high Fitness values
body: <p>I am learning how to use ICP with PCL. As such I wrote a function that reads a series of .pcd files from a given folder.</p>
  
  <pre><code>//Reads all PCD files in a folder. Specify the path without the last '/' and the number on the last and first file: xxx.pcb.
  //Returns null if something goes wrong. Otherwise returns a single global pointcloud with each subcloud colour coded.
  pcl::PointCloud&lt;pcl::PointXYZRGB&gt;::Ptr LoadPCDFiles(std::string FolderPath, int FinalFile, int FirstFile)
  {
      //Initialize some variables we need.
      pcl::PointCloud&lt;pcl::PointXYZRGB&gt;::Ptr cloud(new pcl::PointCloud&lt;pcl::PointXYZRGB&gt;());
      pcl::PointCloud&lt;pcl::PointXYZRGB&gt;::Ptr Global(new pcl::PointCloud&lt;pcl::PointXYZRGB&gt;());
      srand(time(NULL));
      int FileIter = FirstFile;
      std::string BasePathString = FolderPath + "/%.3d.pcd";
      Eigen::Matrix4f AccumulatedTransformationMatrix = Eigen::Matrix4f::Identity();
  
      while (FileIter &lt;= FinalFile) {
          char buffer[260];
          std::sprintf(buffer, BasePathString.c_str(), FileIter);
          std::string path(buffer);
          cloud = ReadPCDFile(path, std::rand() % 256, std::rand() % 256, std::rand() % 256);
          if (cloud == NULL) return NULL;//Error
          //pcl::transformPointCloud&lt;pcl::PointXYZRGB&gt;(*cloud, *cloud, Eigen::Vector3f(cloud-&gt;sensor_origin_.x(), cloud-&gt;sensor_origin_.y(), cloud-&gt;sensor_origin_.z()), cloud-&gt;sensor_orientation_);
          //pcl::transformPointCloud&lt;pcl::PointXYZRGB&gt;(*cloud, *cloud, AccumulatedTransformationMatrix);
  
          int AlignmentIter = 0; //The number of alignment attemps we have made.
          int MaxAlignment = 25; //Maax number of attempts allowed.
          float LastFitness = VTK_FLOAT_MAX;
          bool hasConverged = false;
          std::cout &lt;&lt; "Loaded PCD: " &lt;&lt; path &lt;&lt; std::endl;
          if (FileIter != FirstFile) {
  
              while (AlignmentIter &lt; MaxAlignment &amp;&amp; LastFitness &gt; 0.001) {
                  pcl::PointCloud&lt;pcl::PointXYZRGB&gt;::Ptr Temp(new pcl::PointCloud&lt;pcl::PointXYZRGB&gt;());
                  pcl::IterativeClosestPoint&lt;pcl::PointXYZRGB, pcl::PointXYZRGB&gt; icp;
                  icp.setInputSource(cloud);
                  icp.setInputTarget(Global);
  
                  icp.align(*Temp);
  
                  float CurrentFitness = icp.getFitnessScore();
                  hasConverged = icp.hasConverged();
                  std::cout &lt;&lt; "Alignment value: " &lt;&lt; CurrentFitness &lt;&lt; std::endl;
  
                  if (abs(LastFitness - CurrentFitness) &lt; 0.0001) { break; }
  
                  cloud = Temp;
                  LastFitness = CurrentFitness;
                  AlignmentIter++;
              }
  
  
              //AccumulatedTransformationMatrix = AccumulatedTransformationMatrix * icp.getFinalTransformation();
              if (hasConverged) {
                  *Global = *Global + *cloud;
  
                  std::cout &lt;&lt; "Updated global cloud: " &lt;&lt; (*Global).size() &lt;&lt; std::endl;
              } else {
                  std::cout &lt;&lt; "Unable to align the clouds." &lt;&lt; std::endl;
              }
          } else {
              *Global = *cloud;
              std::cout &lt;&lt; "First subcloud added." &lt;&lt; std::endl;
          }
  
  
  
          FileIter++;
      }
  
      std::cout &lt;&lt; "Merged clouds." &lt;&lt; std::endl;
      return Global;
  }
  </code></pre>
  
  <p>However, this produces some very messy clouds. </p>
  
  <p><img src="https://puu.sh/jXBTL/b90d3f1c24.png" alt="Two simple PCs of a corner"></p>
  
  <p>This had a fitness value of just over 0.052.
  <img src="https://puu.sh/jXw78/7b623c98c5.png" alt="An entire room of PCs"></p>
  
  <p>This had fitness values in the range 0.0030 - 0.0856. <a href="http://puu.sh/jXEGm/34b27f0c0d.png" rel="nofollow noreferrer">All values.</a></p>
  
  <p>Each cloud I have given a random colour to highlight what cloud ended up where. </p>
  
  <p>The <code>ReadPCDFile()</code> function simply reads the file specified and adds the random colour. The commented out lines of code is simply my desperate messing with some ideas, but ultimately they all failed. </p>
  
  <p>This is an example of one of the PCD files.</p>
  
  <pre><code># .PCD v.7 - Point Cloud Data file format
  VERSION .7
  FIELDS x y z
  SIZE 4 4 4
  TYPE F F F
  COUNT 1 1 1
  WIDTH 13509
  HEIGHT 1
  VIEWPOINT 7.9950223 0.60561943 0.4050533 - 0.3022367 0.019049812 0.95157933 0.052790195
  POINTS 13509
  DATA ascii
  - 1.0610341 - 0.55464476 1.933659
  - 1.0921015 - 0.5514014 1.9721884
  - 1.0254035 - 0.55889213 1.922963
  - 1.0736096 - 0.54884547 1.9840248
  - 1.1002594 - 0.5510374 1.9640691
  - 1.05391 - 0.556308 1.9436798
  - 1.0801263 - 0.5577761 1.9309663
  - 1.1306747 - 0.54748887 1.9968926
  - 0.98958766 - 0.55052584 1.926852
  .
  .
  .
  </code></pre>
  
  <p>I hope you can help. Do not hesitate to ask for any info that I can provide.</p>
  
  <p>Thank you very much for your help.</p>