text: #include "PhotometricStero.h"
  
  #include <cmath>
  #include <cstdlib>
  #include <iostream>
  #include <climits>
  #include <cstdio>
  #include<Eigen/SparseCholesky>
  
  namespace phoSte {
  
  namespace {
  // use iniRSize points to get the initial r
  const int iniRSize = 10;
  // use 20 points to get the circle information
  const int adjustSize = 20;
  // iterate iterTimes to get the circle
  const int iterTime = 10;
  // threshold of stopping the iteration
  const double normThresh = 0.01;
  
  phoSte::circle getCircle(std::vector<cv::Point>& contour) {
      double circle_x = 0;
      double circle_y = 0;
      double r = 0;
      int pointSize = contour.size();
      if(pointSize < iniRSize) {
          std::cout << "the counter is error" << std::endl;
          exit(-1);
      }
      for(int i = 0; i < pointSize; i++) {
          circle_x += contour[i].x;
          circle_y += contour[i].y;
      }
      circle_x /= pointSize;
      circle_y /= pointSize;
      for(int i = 0; i < iniRSize; i++) {
          r += sqrt((contour[i].x - circle_x) * (contour[i].x - circle_x) +
                    (contour[i].y - circle_y) * (contour[i].y - circle_y));
      }
      r /= iniRSize;
  
      // use adjustment calculation to get the circle
      if(pointSize < adjustSize) {
          std::cout << "the counter is error" << std::endl;
          exit(-1);
      }
      std::vector<int> pointIndex;
      pointIndex.reserve(adjustSize);
      int distanceOfIndex = pointSize / adjustSize;
      pointIndex.push_back(0);
      for(int i = 1; i < adjustSize; i++) {
          pointIndex.push_back(pointIndex[i - 1] + distanceOfIndex);
      }
  
      cv::Mat B(adjustSize, 3, CV_64F);
      cv::Mat L(adjustSize, 1, CV_64F);
      double v_norm = INT_MAX;
      for(int i = 0; i < iterTime; i++) {
          double *pB = (double *)B.data;
          double *pL = (double *)L.data;
          for(int j = 0; j < adjustSize; j++) {
              *pB = contour[pointIndex[j] + i].x - circle_x;
              *(pB + 1) = contour[pointIndex[j] + i].y - circle_y;
              *(pB + 2) = r;
              pB += 3;
              *pL = ((contour[pointIndex[j] + i].x - circle_x) * (contour[pointIndex[j] + i].x - circle_x)
                     + (contour[pointIndex[j] + i].y - circle_y) * (contour[pointIndex[j] + i].y - circle_y)
                     - r * r) / 2;
              pL++;
          }
          cv::Mat pseudoInverse;
          cv::invert(B, pseudoInverse, cv::DECOMP_SVD);
          cv::Mat result = pseudoInverse * L;
          circle_x += result.at<double>(0, 0);
          circle_y += result.at<double>(1, 0);
          r += result.at<double>(2, 0);
      }
      return phoSte::circle(circle_x, circle_y, r);
  }
  
  // get the direction of light according to the metalCircle
  phoSte::light getLightDirection(const phoSte::circle& metalCircle,
                                  const cv::Point& maxPoint) {
      int maxX = maxPoint.x;
      int maxY = maxPoint.y;
      double nx = maxX - metalCircle.mCenterX;
      double ny = maxY - metalCircle.mCenterY;
      double nz = sqrt(metalCircle.mR * metalCircle.mR - nx * nx - ny * ny);
      double rootSquareSum = sqrt(nx * nx + ny * ny + nz * nz);
      nx /= rootSquareSum;
      ny /= rootSquareSum;
      nz /= rootSquareSum;
      double nDotR = nz;
      double lx = 2 * nDotR * nx;
      double ly = 2 * nDotR * ny;
      double lz = 2 * nDotR * nz - 1;
  
      return phoSte::light(lx, ly, lz);
  }
  
  void swapNum(double& a, double& b) {
      double tmp = a;
      a = b;
      b = tmp;
  
  }
  
  // find the num at the ratio of the nums
  // always use the endI as the pivot
  // use the random integer because there might be some order of the pixel value
  double quickSelect(std::vector<double>& nums, int startI, int endI, int selectI) {
      if(startI == endI)
          return nums[startI];
      int pivotIndex = rand() % (endI - startI + 1) + startI;
      swapNum(nums[endI], nums[pivotIndex]);
      double pivot = nums[endI];
      int i = startI - 1;
      int j = startI;
      while(j != endI) {
          if(nums[j] < pivot) {
              i++;
              swapNum(nums[j], nums[i]);
          }
          j++;
      }
      i++;
      swapNum(nums[endI], nums[i]);
      if(i == selectI)
          return nums[i];
      else if(i < selectI) {
          return quickSelect(nums, i + 1, endI, selectI);
      } else
          return quickSelect(nums, startI, i - 1, selectI);
  }
  
  // do the transform to image to output
  // maxValue to 255, minValue to 0
  int linearTransform(double max, double min, double value) {
      return 255 / (max - min) * (value - min);
  }
  }
  
  photometryStero::~photometryStero() {
      if(!mImageStorage.empty()) {
          for(int i = 0; i < mImageStorage.size(); i++) {
              delete mImageStorage[i];
          }
      }
  }
  
  photometryStero::photometryStero(int n, int startI, int endI, std::string path,
                                   std::string metal1Phere1Name, std::string metal2Phere1Name,
                                   std::string lambertPhereName, std::string objectName,
                                   double discardRatio): mDiscardRatio(discardRatio), imageNum(n) {
      int distance = (endI - startI + 1) / n;
      for (int i = 0; i < n; i++) {
          int index = i * distance + startI;
          char imageName[100];
          sprintf(imageName, "image%03d.pbm", index);
          std::string imageNameStr = imageName;
          imageNameStr = path + imageNameStr;
          mImageNames.push_back(imageNameStr);
      }
      std::string mask1Name = metal1Phere1Name;
      std::string mask2Name = metal2Phere1Name;
      std::string mask3Name = lambertPhereName;
  
      mMaskNames.push_back(path + mask1Name);
      mMaskNames.push_back(path + mask2Name);
      mMaskNames.push_back(path + mask3Name);
      mMaskNames.push_back(path + objectName);
  }
  
  // read image and mask image according to the name
  // return true if read successfully
  // return false if read fail
  bool photometryStero::readImage() {
      if(mImageNames.empty() || mMaskNames.empty())
          return false;
  
      // read the image
      for(int i = 0; i < mImageNames.size(); i++) {
          CPFMAccess* PFMAccessI = new CPFMAccess();
          char *tpChar = new char[mImageNames[i].size() + 1];
          std::strcpy(tpChar, mImageNames[i].c_str());
          if(! (PFMAccessI -> LoadFromFile(tpChar)) )
              return false;
          delete[]tpChar;
          float *imageData = PFMAccessI -> GetData();
          mImageStorage.push_back(PFMAccessI);
          cv::Mat image((PFMAccessI->GetWidth()) * (PFMAccessI->GetHeight()), 3,
                        CV_32F, imageData);
          cv::Mat p2Image (0.2989 * image.col(0) + 0.5870 * image.col(1)
                           + 0.1140 * image.col(2));
          p2Image = p2Image.reshape(0, PFMAccessI->GetHeight());
          mp2Images.push_back(p2Image);
      }
      // read the maskImage
      for(int i = 0; i < mMaskNames.size(); i++) {
          cv::Mat p2Mask(cv::imread(mMaskNames[i], CV_LOAD_IMAGE_GRAYSCALE));
          if(! p2Mask.data )
              return false;
          mp2Mask.push_back(p2Mask);
      }
      return true;
  }
  
  void photometryStero::getLightInformation(const int metalIndex, const int lambIndex) {
      // get the metal circle
      cv::threshold(mp2Mask[metalIndex], mp2Mask[metalIndex], 255 / 2, 255, cv::THRESH_BINARY);
      std::vector<std::vector<cv::Point>> metalContour;
      cv::findContours(mp2Mask[metalIndex], metalContour, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
      // use the contour with the max size to calculate the circle
      int mostPoint = 0;
      int mostPointIdx = 0;
      for(int i = 0; i < metalContour.size(); i++) {
          if(metalContour.size() > mostPoint) {
              mostPoint = metalContour.size();
              mostPointIdx = i;
          }
      }
      m_metalSphere = getCircle(metalContour[mostPointIdx]);
  
      // get the lambertian circle
      cv::threshold(mp2Mask[lambIndex], mp2Mask[lambIndex], 255 / 2, 255, cv::THRESH_BINARY);
      std::vector<std::vector<cv::Point>> lambContour;
      cv::findContours(mp2Mask[lambIndex], lambContour, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
      // use the contour with the max size to calculate the circle
      mostPoint = 0;
      mostPointIdx = 0;
      for(int i = 0; i < lambContour.size(); i++) {
          if(lambContour.size() > mostPoint) {
              mostPoint = lambContour.size();
              mostPointIdx = i;
          }
      }
      m_lambSpere = getCircle(lambContour[mostPointIdx]);
  
      // get the direction and the intensity of every image
      for(int i = 0; i < mp2Images.size(); i++) {
          cv::Point metalMaxPoint;
          cv::minMaxLoc(mp2Images[i], NULL, NULL, NULL, &metalMaxPoint, mp2Mask[metalIndex]);
          m_light.push_back(getLightDirection(m_metalSphere, metalMaxPoint));
          double lightIntensity;
          cv::minMaxLoc(mp2Images[i], NULL, &lightIntensity, NULL, NULL, mp2Mask[lambIndex]);
          m_light[i].mIntensity = lightIntensity;
      }
  }
  
  // calculate the norm and albedo for every pixel
  // here, threshold is for the dark discard
  void photometryStero::getPixelNormAndAlbedo(const int objectIndex) {
      // calculate the pixel num
      std::vector<double> pixelThreshold;
      for(int i = 0; i < mp2Mask[objectIndex].rows; i++) {
          for(int j = 0; j < mp2Mask[objectIndex].cols; j++) {
              if(mp2Mask[objectIndex].at<unsigned char>(i, j) >= 255) {
                  mObjectX.push_back(j);
                  mObjectY.push_back(i);
              }
          }
      }
      std::vector<std::vector<double>> allPixelValue;
      allPixelValue.reserve(imageNum);
      int objectPixelNum = mObjectX.size();
      for(int i = 0; i < imageNum; i++) {
          std::vector<double> pixelValue;
          allPixelValue.push_back(pixelValue);
          allPixelValue[i].reserve(objectPixelNum);
          for(int j = 0; j < objectPixelNum; j++) {
              allPixelValue[i].push_back(mp2Images[i].at<float>(mObjectY[j], mObjectX[j]));
          }
      }
      // threshold for every image;
      std::vector<double> allThreshold;
      allThreshold.reserve(imageNum);
      int thresholdIndex = mDiscardRatio * objectPixelNum;
      for(int i = 0; i < imageNum; i++) {
          std::vector<double> tmpPixelValue = allPixelValue[i];
          allThreshold.push_back(
              quickSelect(tmpPixelValue, 0, objectPixelNum - 1, thresholdIndex));
      }
  
      cv::Mat I(imageNum, objectPixelNum, CV_64F);
      cv::Mat L(imageNum, 3, CV_64F);
      mN.create(3, objectPixelNum, CV_64F);
      mAlbedo.create(1, objectPixelNum, CV_64F);
      double *pL = (double *)L.data;
      for(int i = 0; i < imageNum; i++) {
          *pL = m_light[i].mx;
          *(pL + 1) = m_light[i].my;
          *(pL + 2) = m_light[i].mz;
          pL += 3;
      }
      cv::Mat LPseudoInvert;
      cv::invert(L, LPseudoInvert, cv::DECOMP_SVD);
      double *pI = (double *)I.data;
      double *pN = (double *)mN.data;
  
      for(int i = 0; i < objectPixelNum; i++) {
          int inValidNum = 0;
          cv::Mat specificL;
          for(int j = 0; j < imageNum; j++) {
              if(allPixelValue[j][i] < allThreshold[j]) {
                  inValidNum ++;
                  if(inValidNum == 1) {
                      L.copyTo(specificL);
                  }
                  double *p2LRow = specificL.ptr<double>(j);
                  *(p2LRow) = 0;
                  *(p2LRow + 1) = 0;
                  *(p2LRow + 2) = 0;
                  *(pI + j * objectPixelNum + i) = 0;
              } else {
                  *(pI + j * objectPixelNum + i) = allPixelValue[j][i] / m_light[j].mIntensity;
              }
              if(imageNum - inValidNum < 3) {
                  break;
              }
          }
          if(inValidNum == 0) {
              mN.col(i) = LPseudoInvert * I.col(i);
              double nx = mN.col(i).at<double>(0, 0);
              double ny = mN.col(i).at<double>(1, 0);
              double nz = mN.col(i).at<double>(2, 0);
              mAlbedo.at<double>(0, i) = sqrt(nx * nx + ny * ny + nz * nz);
          } else if(imageNum - inValidNum >= 3) {
              cv::Mat specificLPseudoInvert;
              cv::invert(specificL, specificLPseudoInvert, cv::DECOMP_SVD);
              mN.col(i) = specificLPseudoInvert * I.col(i);
              double nx = mN.col(i).at<double>(0, 0);
              double ny = mN.col(i).at<double>(1, 0);
              double nz = mN.col(i).at<double>(2, 0);
              mAlbedo.at<double>(0, i) = sqrt(nx * nx + ny * ny + nz * nz);
          } else {
              mN.at<double>(0, i) = 0;
              mN.at<double>(1, i) = 0;
              mN.at<double>(2, i) = 0;
              mAlbedo.at<double>(0, i) = 0;
              mInvalidIndex.push_back(i);
          }
      }
  }
  
  void photometryStero::outputImage() {
      double max = INT_MIN, min = INT_MAX;
      if (mp2Images.empty()) {
          std::cout << "no image to output" << std::endl;
      }
      cv::Mat out(mp2Images[0].rows, mp2Images[0].cols, CV_8U);
      for (int i = 0; i < mp2Images[0].rows; i++) {
          for (int j = 0; j < mp2Images[0].cols; j++) {
              double pixelValue = mp2Images[0].at<float>(i, j);
              max = pixelValue > max ? pixelValue : max;
              min = pixelValue < min ? pixelValue : min;
          }
      }
      for (int i = 0; i < mp2Images[0].rows; i++) {
          for (int j = 0; j < mp2Images[0].cols; j++) {
              double pixelValue = mp2Images[0].at<float>(i, j);
              out.at<unsigned char>(i, j) = linearTransform(max, min, pixelValue);
          }
      }
      cv::imshow("outputImage", out);
      cv::waitKey(0);
  }
  
  cv::Mat photometryStero::outputNormalImage(int objectIndex) {
      cv::Mat result = cv::Mat::zeros(mp2Images[0].rows, mp2Images[0].cols, CV_32FC3);
      int nextInvalid = mInvalidIndex.empty() ? INT_MAX : mInvalidIndex[0];
      int nextInvalidIndex = 0;
      int invalidPixelNum = 0;
      for (int i = 0; i < mObjectX.size(); i++) {
          double nx = mN.at<double>(0, i);
          double ny = mN.at<double>(1, i);
          double nz = mN.at<double>(2, i);
          if (i == nextInvalid) {
              invalidPixelNum++;
              result.at<cv::Vec3f>(mObjectY[i], mObjectX[i])[0] = 0;
              result.at<cv::Vec3f>(mObjectY[i], mObjectX[i])[1] = 0;
              result.at<cv::Vec3f>(mObjectY[i], mObjectX[i])[2] = 0;
              if (nextInvalidIndex + 1 == mInvalidIndex.size()) {
                  nextInvalid = INT_MAX;
              } else {
                  nextInvalidIndex++;
                  nextInvalid = mInvalidIndex[nextInvalidIndex];
              }
          } else {
              double rootsquareSum = sqrt(nx * nx + ny * ny + nz * nz);
              float nxf = (1 + nx / rootsquareSum) / 2;
              float nyf = (1 + ny / rootsquareSum) / 2;
              float nzf = (1 + nz / rootsquareSum) / 2;
              result.at<cv::Vec3f>(mObjectY[i], mObjectX[i])[0] = nxf;
              result.at<cv::Vec3f>(mObjectY[i], mObjectX[i])[1] = nyf;
              result.at<cv::Vec3f>(mObjectY[i], mObjectX[i])[2] = nzf;
          }
      }
      std::cout << invalidPixelNum << " pixels are not drawn" << std::endl;
      return result;
  }
  
  // this function is only for debug
  void photometryStero::addSmallMaskForObject(int size, int midX, int midY) {
      cv::Mat smallMask = cv::Mat::zeros(mp2Images[0].rows, mp2Images[0].cols, CV_8U);
      int num = 0;
      for (int i = midY - size / 2; i <= midY + size / 2; i++) {
          for (int j = midX - size / 2; j <= midX + size / 2; j++) {
              smallMask.at<unsigned char>(i, j) = 255;
              num++;
          }
      }
      mp2Mask.push_back(smallMask);
  }
  
  cv::Mat photometryStero::outputAlbedoImage(int objectIndex) {
      cv::Mat result = cv::Mat::zeros(mp2Images[0].rows, mp2Images[0].cols, CV_32F);
      int nextInvalid = mInvalidIndex.empty() ? INT_MAX : mInvalidIndex[0];
      int nextInvalidIndex = 0;
      int invalidPixelNum = 0;
      for (int i = 0; i < mObjectX.size(); i++) {
          float albedo = mAlbedo.at<double>(0, i);
          if (i == nextInvalid) {
              invalidPixelNum++;
              result.at<float>(mObjectY[i], mObjectX[i]) = 0;
              if (nextInvalidIndex + 1 == mInvalidIndex.size()) {
                  nextInvalid = INT_MAX;
              } else {
                  nextInvalidIndex++;
                  nextInvalid = mInvalidIndex[nextInvalidIndex];
              }
          } else {
              result.at<float>(mObjectY[i], mObjectX[i]) = albedo;
          }
      }
      std::cout << invalidPixelNum << " pixels are not drawn" << std::endl;
      return result;
  }
  
  // use the normal direction dot the (0, 0, 1) and then times the albedo
  cv::Mat photometryStero::outputNormalWithAlbedo(int objectIndex) {
      cv::Mat result = cv::Mat::zeros(mp2Images[0].rows, mp2Images[0].cols, CV_32F);
      int nextInvalid = mInvalidIndex.empty() ? INT_MAX : mInvalidIndex[0];
      int nextInvalidIndex = 0;
      int invalidPixelNum = 0;
      for (int i = 0; i < mObjectX.size(); i++) {
          float albedo = mAlbedo.at<double>(0, i);
          double nx = mN.at<double>(0, i);
          double ny = mN.at<double>(1, i);
          double nz = mN.at<double>(2, i);
          float intensity = nz / sqrt(nx * nx + ny * ny + nz * nz) * albedo;
          if (i == nextInvalid) {
              invalidPixelNum++;
              result.at<float>(mObjectY[i], mObjectX[i]) = 0;
              if (nextInvalidIndex + 1 == mInvalidIndex.size()) {
                  nextInvalid = INT_MAX;
              } else {
                  nextInvalidIndex++;
                  nextInvalid = mInvalidIndex[nextInvalidIndex];
              }
          } else {
              result.at<float>(mObjectY[i], mObjectX[i]) = intensity;
          }
      }
      std::cout << invalidPixelNum << " pixels are not drawn" << std::endl;
      return result;
  }
  
  // use the normal map to get the height map
  // mode 1: use the discard ratio to discard some low lengths
  // mode 2: use sigmoid function to map to the depth
  // in order to avoid the mistake due to the small nz, I plus 0.05 when nz is too small
  cv::Mat photometryStero::getHeightMap(int mode, double parameter) {
      // make the map from object pixel to object index
      int nextInvalid = mInvalidIndex.empty() ? INT_MAX : mInvalidIndex[0];
      int nextInvalidIndex = 0;
      cv::Mat pixel2ObjectIndex = cv::Mat::zeros(mp2Images[0].rows,
                                  mp2Images[0].cols, CV_32S);
      for (int i = 0; i < mObjectX.size(); i++) {
          if (i == nextInvalid) {
              pixel2ObjectIndex.at<int>(mObjectY[i], mObjectX[i]) = -1;
              if (nextInvalidIndex + 1 == mInvalidIndex.size()) {
                  nextInvalid = INT_MAX;
              } else {
                  nextInvalidIndex++;
                  nextInvalid = mInvalidIndex[nextInvalidIndex];
              }
          } else {
              // use i + 1 to prevent the collision with 0
              pixel2ObjectIndex.at<int>(mObjectY[i], mObjectX[i]) = i + 1;
  
          }
      }
      // first of all, iterate until all object points have normal value
      // I use the 3*3 window to get average normal
      cv::Mat NForHeight;
      mN.copyTo(NForHeight);
      cv::Mat heightMap = cv::Mat::zeros(mp2Images[0].rows, mp2Images[0].rows, CV_32F);
      int nInvalid = mInvalidIndex.size();
      int iterateTime = 0;
      while (nInvalid > 0) {
          if (iterateTime > 10) {
              std::cout << "too many iterations during getting values to the invalid pixel"
                        << std::endl;
              return heightMap;
          }
          iterateTime++;
          for (int i = 0; i < mInvalidIndex.size(); i++) {
              int index = mInvalidIndex[i];
              if (pixel2ObjectIndex.at<int>(mObjectY[index], mObjectX[index]) == -1) {
                  int x = mObjectX[index];
                  int y = mObjectY[index];
                  double nxSum = 0;
                  double nySum = 0;
                  double nzSum = 0;
                  int nValid = 0;
                  for (int dx = -1; dx <= 1; dx++) {
                      for (int dy = -1; dy <= 1; dy++) {
                          if (pixel2ObjectIndex.at<int>(y + dy, x + dx) > 0) {
                              nValid++;
                              nxSum += NForHeight.at<double>(0, pixel2ObjectIndex.at<int>(y + dy, x + dx));
                              nySum += NForHeight.at<double>(1, pixel2ObjectIndex.at<int>(y + dy, x + dx));
                              nzSum += NForHeight.at<double>(2, pixel2ObjectIndex.at<int>(y + dy, x + dx));
                          }
                      }
                  }
                  if (nValid > 0) {
                      pixel2ObjectIndex.at<int>(y, x) = index + 1;
                      nInvalid--;
                      nxSum /= nValid;
                      nySum /= nValid;
                      nzSum /= nValid;
                      double squareSum = sqrt(nxSum * nxSum + nySum * nySum + nzSum * nzSum);
                      NForHeight.at<double>(0, index) = nxSum / squareSum;
                      NForHeight.at<double>(1, index) = nySum / squareSum;
                      NForHeight.at<double>(2, index) = nzSum / squareSum;
                  }
              }
          }
      }
      std::cout << "the iteration time is " << iterateTime;
  
      // form the sparse matrix and solve the linear equation
      // first of all, scan all the points to find the number of equations
      int nEquation = 0;
      for (int i = 0; i < mObjectX.size(); i++) {
          int x = mObjectX[i];
          int y = mObjectY[i];
          if (pixel2ObjectIndex.at<int>(y + 1, x) > 0) {
              nEquation++;
          }
          if (pixel2ObjectIndex.at<int>(y, x + 1) > 0) {
              nEquation++;
          }
      }
      // then use the sparse matrix to solve the linear equation
      // I set z(allIndex / 2)=0 as the coordinate origin
      int midIndex = mObjectX.size() / 2;
      int midX = mObjectX[midIndex];
      int midY = mObjectY[midIndex];
      double midNx_midNz = -(NForHeight.at<double>(0, midIndex)) / (NForHeight.at<double>(2, midIndex));
      double midNy_midNz = -(NForHeight.at<double>(1, midIndex)) / (NForHeight.at<double>(2, midIndex));
      Eigen::VectorXd b(nEquation);
      typedef Eigen::SparseMatrix<double> SpMat;
      // since the sparse matrix of eigen is col main, so use the transpose to store the data
      SpMat A(mObjectX.size() - 1, nEquation);
      typedef Eigen::Triplet<double> T;
      std::vector<T> tripletList;
      tripletList.reserve(nEquation * 3);
      int currentEquation = 0;
      for (int i = 0; i < mObjectX.size(); i++) {
          int x = mObjectX[i];
          int y = mObjectY[i];
          if (i != midIndex) {
              int thisZIndex = i < midIndex ? i : i - 1;
              if (pixel2ObjectIndex.at<int>(y + 1, x) > 0) {
                  if (y + 1 != midY || x != midX) {
                      int index = pixel2ObjectIndex.at<int>(y + 1, x) - 1;
                      int upZIndex = index < midIndex ? index : index - 1;
                      tripletList.push_back(T(upZIndex, currentEquation, 1));
                      tripletList.push_back(T(thisZIndex, currentEquation, -1));
                      if (NForHeight.at<double>(2, index) < 0.001) {
                          b(currentEquation) = -(NForHeight.at<double>(1, index) + 0.05) / (NForHeight.at<double>(2, index) + 0.05);
                      } else {
                          b(currentEquation) = -(NForHeight.at<double>(1, index)) / (NForHeight.at<double>(2, index));
                      }
                      currentEquation++;
                  } else {
                      int index = pixel2ObjectIndex.at<int>(y + 1, x) - 1;
                      tripletList.push_back(T(thisZIndex, currentEquation, -1));
                      if (NForHeight.at<double>(2, index) < 0.001) {
                          b(currentEquation) = -(NForHeight.at<double>(1, index) + 0.05) / (NForHeight.at<double>(2, index) + 0.05);
                      } else {
                          b(currentEquation) = -(NForHeight.at<double>(1, index)) / (NForHeight.at<double>(2, index));
                      }
                      currentEquation++;
                  }
              }
              if (pixel2ObjectIndex.at<int>(y, x + 1) > 0) {
                  if (y != midY || x + 1 != midX) {
                      int index = pixel2ObjectIndex.at<int>(y, x + 1) - 1;
                      int rightZIndex = index < midIndex ? index : index - 1;
                      tripletList.push_back(T(rightZIndex, currentEquation, 1));
                      tripletList.push_back(T(thisZIndex, currentEquation, -1));
                      if (NForHeight.at<double>(2, index) < 0.001) {
                          b(currentEquation) = -(NForHeight.at<double>(0, index) + 0.05) / (NForHeight.at<double>(2, index) + 0.05);
                      } else {
                          b(currentEquation) = -(NForHeight.at<double>(0, index)) / (NForHeight.at<double>(2, index));
                      }
                      currentEquation++;
                  } else {
                      int index = pixel2ObjectIndex.at<int>(y, x + 1) - 1;
                      tripletList.push_back(T(thisZIndex, currentEquation, -1));
                      if (NForHeight.at<double>(2, index) < 0.001) {
                          b(currentEquation) = -(NForHeight.at<double>(0, index) + 0.05) / (NForHeight.at<double>(2, index) + 0.05);
                      } else {
                          b(currentEquation) = -(NForHeight.at<double>(0, index)) / (NForHeight.at<double>(2, index));
                      }
                      currentEquation++;
                  }
              }
          } else {
              int thisIndex = midIndex;
              if (pixel2ObjectIndex.at<int>(y + 1, x) > 0) {
                  int index = pixel2ObjectIndex.at<int>(y + 1, x) - 1;
                  int upZIndex = index < midIndex ? index : index - 1;
                  tripletList.push_back(T(upZIndex, currentEquation, 1));
                  b(currentEquation) = -midNy_midNz;
                  currentEquation++;
              }
              if (pixel2ObjectIndex.at<int>(y, x + 1) > 0) {
                  int index = pixel2ObjectIndex.at<int>(y, x + 1) - 1;
                  int rightZIndex = index < midIndex ? index : index - 1;
                  tripletList.push_back(T(rightZIndex, currentEquation, 1));
                  b(currentEquation) = -midNx_midNz;
                  currentEquation++;
              }
          }
      }
      A.setFromTriplets(tripletList.begin(), tripletList.end());
      SpMat Atranspose = A.transpose();
      A = A * Atranspose;
      A.makeCompressed();
      Eigen::SimplicialLLT <Eigen::SparseMatrix<double>> solver;
      solver.compute(A);
      if (solver.info() != Eigen::Success) {
          std::cout << "decomposition fails" << std::endl;
          return heightMap;
      }
      b = Atranspose.transpose() * b;
      Eigen::VectorXd z = solver.solve(b);
      if (solver.info() != Eigen::Success) {
          std::cout << "solving failed" << std::endl;
          return heightMap;
      }
  
      // try to discard ratio to show;
      switch (mode) {
      case 1: {
          std::vector<double> zVec;
          zVec.reserve(mObjectX.size());
          for (int i = 0; i < mObjectX.size() - 1; i++) {
              zVec.push_back(z(i));
          }
          zVec.push_back(0);
          std::vector<double> zVec2(zVec.begin(), zVec.end());
          double minThreshold = quickSelect(zVec, 0, zVec.size() - 1, zVec.size() * parameter);
          double maxZ = z.maxCoeff();
          for (int i = 0; i < mObjectX.size(); i++) {
              int x = mObjectX[i];
              int y = mObjectY[i];
              double originz;
              if (i < midIndex) {
                  originz = z(i);
              } else if (i > midIndex) {
                  originz = z(i - 1);
              } else {
                  originz = 0;
              }
  
              if (originz < minThreshold) {
                  heightMap.at<float>(y, x) = 0;
              } else {
                  float newz = (originz - minThreshold) / (maxZ - minThreshold);
                  heightMap.at<float>(y, x) = newz;
              }
          }
          break;
      }
      case 2: {
          std::vector<double> zVec;
          zVec.reserve(mObjectX.size());
          for (int i = 0; i < mObjectX.size() - 1; i++) {
              zVec.push_back(z(i));
          }
          zVec.push_back(0);
          std::vector<double> zVec2(zVec.begin(), zVec.end());
          for (int i = 0; i < mObjectX.size(); i++) {
              int x = mObjectX[i];
              int y = mObjectY[i];
              double originz;
              if (i < midIndex) {
                  originz = z(i);
              } else if (i > midIndex) {
                  originz = z(i - 1);
              } else {
                  originz = 0;
              }
              heightMap.at<float>(y, x) = 1 / (1 + exp(parameter * originz));
          }
          break;
      }
      default:
          break;
      }
      // output the height map to matrix;
      return heightMap;
  }
  
  }
  