Add option to manually set projection matrix in OrganizedNeighbor

Author: mvieth

common/include/pcl/common/impl/projection_matrix.hpp

@@ -214,6 +214,10 @@ estimateProjectionMatrix (
   if (projection_matrix.coeff (0) < 0)
     projection_matrix *= -1.0;
 
+  // if the bottom row is all zeros, except for the second-to-last element, then scale the whole projection matrix (convention)
+  if(std::abs(eigen_vectors.coeff (96))<1e-7 && std::abs(eigen_vectors.coeff (108))<1e-7 && std::abs(eigen_vectors.coeff (120))>1e-5 && std::abs(eigen_vectors.coeff (132))<1e-7)
+    projection_matrix /= projection_matrix.coeffRef (10);
+
   return (residual);
 }
 

features/include/pcl/features/impl/feature.hpp

@@ -119,14 +119,22 @@ Feature<PointInT, PointOutT>::initCompute ()
   // Check if a space search locator was given
   if (!tree_)
   {
-    if (surface_->isOrganized () && input_->isOrganized ())
+    if (surface_->isOrganized () && input_->isOrganized ()) {
       tree_.reset (new pcl::search::OrganizedNeighbor<PointInT> ());
-    else
+      if(!tree_->setInputCloud (surface_)) { // may return false if OrganizedNeighbor cannot work with the cloud, then use KdTree instead
+        tree_.reset (new pcl::search::KdTree<PointInT> (false));
+      }
+    } else {
       tree_.reset (new pcl::search::KdTree<PointInT> (false));
+    }
   }
 
-  if (tree_->getInputCloud () != surface_) // Make sure the tree searches the surface
-    tree_->setInputCloud (surface_);
+  if (tree_->getInputCloud () != surface_) { // Make sure the tree searches the surface
+    if(!tree_->setInputCloud (surface_)) {
+      PCL_ERROR ("[pcl::%s::compute] The given search method cannot work with the given input cloud/search surface.\n", getClassName ().c_str ());
+      return (false);
+    }
+  }
 
 
   // Do a fast check to see if the search parameters are well defined

search/include/pcl/search/impl/organized.hpp

@@ -372,16 +372,21 @@ pcl::search::OrganizedNeighbor<PointT>::estimateProjectionMatrix ()
 
   // precalculate KR * KR^T needed by calculations during nn-search
   KR_KRT_ = KR_ * KR_.transpose ();
+  return true;
+}
 
-  // final test: project a few points at known image coordinates and test if the projected coordinates are close
+template<typename PointT> bool
+pcl::search::OrganizedNeighbor<PointT>::testProjectionMatrix () const
+{
+  // test: project a few points at known image coordinates and test if the projected coordinates are close
   for(std::size_t i=0; i<11; ++i) {
     const std::size_t test_index = input_->size()*i/11u;
     if (!mask_[test_index])
       continue;
     const auto& test_point = (*input_)[test_index];
     pcl::PointXY q;
     if (!projectPoint(test_point, q) || std::abs(q.x-test_index%input_->width)>1 || std::abs(q.y-test_index/input_->width)>1) {
-      PCL_WARN ("[pcl::%s::estimateProjectionMatrix] Input dataset does not seem to be from a projective device! (point %zu (%g,%g,%g) projected to pixel coordinates (%g,%g), but actual pixel coordinates are (%zu,%zu))\n",
+      PCL_WARN ("[pcl::%s::testProjectionMatrix] Input dataset does not seem to be from a projective device! (point %zu (%g,%g,%g) projected to pixel coordinates (%g,%g), but actual pixel coordinates are (%zu,%zu))\n",
                 this->getName ().c_str (), test_index, test_point.x, test_point.y, test_point.z, q.x, q.y, static_cast<std::size_t>(test_index%input_->width), static_cast<std::size_t>(test_index/input_->width));
       return false;
     }

search/include/pcl/search/organized.h

@@ -150,7 +150,7 @@ namespace pcl
                 mask_[idx] = 1;
           }
 
-          return estimateProjectionMatrix () && isValid ();
+          return (eps_ < 0 || estimateProjectionMatrix ()) && testProjectionMatrix() && isValid ();
         }
 
         /** \brief Search for all neighbors of query point that are within a given radius.
@@ -170,10 +170,33 @@ namespace pcl
                       std::vector<float> &k_sqr_distances,
                       unsigned int max_nn = 0) const override;
 
-        /** \brief estimated the projection matrix from the input cloud. */
+        /** \brief Estimate the projection matrix from the input cloud.
+          * \return True if it was possible to estimate the matrix, false otherwise
+          */
         bool
         estimateProjectionMatrix ();
 
+        /** \brief Quick test if projection matrix and input cloud work together.
+          */
+        bool
+        testProjectionMatrix () const;
+
+        /** \brief Set projection matrix manually. Projection matrix will _not_ be estimated automatically any more. If you want to use this, call it _before_ setInputCloud.
+          */
+        void
+        setProjectionMatrix (const Eigen::Matrix<float, 3, 4>& projection_matrix)
+        {
+          eps_ = -1.0f; // signal to not call estimateProjectionMatrix
+          projection_matrix_ = projection_matrix;
+          // rest is the same as in estimateProjectionMatrix():
+          // get left 3x3 sub matrix, which contains K * R, with K = camera matrix = [[fx s cx] [0 fy cy] [0 0 1]]
+          // and R being the rotation matrix
+          KR_ = projection_matrix_.topLeftCorner <3, 3> ();
+
+          // precalculate KR * KR^T needed by calculations during nn-search
+          KR_KRT_ = KR_ * KR_.transpose ();
+        }
+
          /** \brief Search for the k-nearest neighbors for a given query point.
            * \note limiting the maximum search radius (with setMaxDistance) can lead to a significant improvement in search speed
            * \param[in] p_q the given query point (\ref setInputCloud must be given a-priori!)
@@ -280,7 +303,7 @@ namespace pcl
         Eigen::Matrix<float, 3, 3, Eigen::RowMajor> KR_KRT_;
 
         /** \brief epsilon value for the MSE of the projection matrix estimation*/
-        const float eps_;
+        float eps_;
 
         /** \brief using only a subsample of points to calculate the projection matrix. pyramid_level_ = use down sampled cloud given by pyramid_level_*/
         const unsigned pyramid_level_;