transformation_estimation_wdf.hpp
Go to the documentation of this file.
00001 /*
00002  * TransformationEstimationWDF.cpp
00003  *
00004  *  Created on: Mar 15, 2012
00005  *      Author: darko490
00006  */
00007 
00008 //#include "transformation_estimation_wdf.h"
00009 
00011 //template <typename PointSource, typename PointTarget>
00012 //TransformationEstimationWDF<PointSource, PointTarget>::TransformationEstimationWDF() {
00013 //      // Set default alpha weight
00014 //      alpha_ = 0.3;
00015 //      indices_src_dfp_set_ = false;
00016 //      indices_tgt_dfp_set_ = false;
00017 //      weights_dfp_set_ = false;
00018 //}
00019 
00021 //template <typename PointSource, typename PointTarget>
00022 //TransformationEstimationWDF<PointSource, PointTarget>::~TransformationEstimationWDF() {
00023 //      // TODO Auto-generated destructor stub
00024 //}
00025 
00027 template <typename PointSource, typename PointTarget> inline void
00028 TransformationEstimationWDF<PointSource, PointTarget>::setAlpha(float alpha_arg) {
00029         // Limit alpha_ to range [0,1]
00030         if (alpha_arg>1) alpha_ = 1;
00031         else if (alpha_arg<0) alpha_ = 0;
00032         else alpha_ = alpha_arg;
00033 }
00034 
00035 template <typename PointSource, typename PointTarget> inline float
00036 TransformationEstimationWDF<PointSource, PointTarget>::getAlpha(void) {
00037         return alpha_;
00038 }
00039 
00040 template <typename PointSource, typename PointTarget> inline void
00041 TransformationEstimationWDF<PointSource, PointTarget>::setBeta(float beta_arg) {
00042         beta_ = beta_arg;
00043 }
00044 
00045 template <typename PointSource, typename PointTarget> inline float
00046 TransformationEstimationWDF<PointSource, PointTarget>::getBeta(void) {
00047         return beta_;
00048 }
00049 
00051 template <typename PointSource, typename PointTarget> inline void
00052 TransformationEstimationWDF<PointSource, PointTarget>::setCorrespondecesDFP ( pcl::Correspondences correspondences_dfp ) {
00053         const int nr_correspondences_dfp = (int)correspondences_dfp.size();
00054         indices_src_dfp_.resize(nr_correspondences_dfp);
00055         indices_tgt_dfp_.resize(nr_correspondences_dfp);
00056         for (int i = 0; i < nr_correspondences_dfp; ++i)
00057         {
00058                 indices_src_dfp_[i] = correspondences_dfp[i].index_query;
00059                 indices_tgt_dfp_[i] = correspondences_dfp[i].index_match;
00060         }
00061         // Update flags
00062         indices_src_dfp_set_ = true;
00063         indices_tgt_dfp_set_ = true;
00064 }
00065 
00067 template <typename PointSource, typename PointTarget> inline void
00068 TransformationEstimationWDF<PointSource, PointTarget>::setCorrespondecesDFP( std::vector<int> &indices_src_dfp_arg,
00069                                                                                                                                                                 std::vector<int> &indices_tgt_dfp_arg ) {
00070         // Copy data
00071         indices_src_dfp_ = indices_src_dfp_arg;
00072         indices_tgt_dfp_ = indices_tgt_dfp_arg;
00073         // Update flags
00074         indices_src_dfp_set_ = true;
00075         indices_tgt_dfp_set_ = true;
00076 }
00077 
00079 template <typename PointSource, typename PointTarget> inline void
00080 TransformationEstimationWDF<PointSource, PointTarget>::setWeightsDFP (std::vector<float> weights_dfp_arg) {
00081         weights_dfp_ = weights_dfp_arg;
00082         weights_dfp_set_ = true;
00083 }
00084 
00086 template <typename PointSource, typename PointTarget> inline void
00087 TransformationEstimationWDF<PointSource, PointTarget>::estimateRigidTransformation (
00088             const pcl::PointCloud<PointSource> &cloud_src,
00089             const pcl::PointCloud<PointTarget> &cloud_tgt,
00090             Eigen::Matrix4f &transformation_matrix) {
00091         // TODO
00092 }
00093 
00094 template <typename PointSource, typename PointTarget> inline void
00095 TransformationEstimationWDF<PointSource, PointTarget>::estimateRigidTransformation (
00096     const pcl::PointCloud<PointSource> &cloud_src,
00097     const pcl::PointCloud<PointTarget> &cloud_tgt,
00098     const pcl::Correspondences &correspondences,
00099     Eigen::Matrix4f &transformation_matrix) {
00100         // TODO
00101 }
00102 
00103 template <typename PointSource, typename PointTarget> inline void
00104 TransformationEstimationWDF<PointSource, PointTarget>::estimateRigidTransformation (
00105     const pcl::PointCloud<PointSource> &cloud_src,
00106     const std::vector<int> &indices_src,
00107     const pcl::PointCloud<PointTarget> &cloud_tgt,
00108     Eigen::Matrix4f &transformation_matrix) {
00109         // TODO
00110 }
00111 
00113 template <typename PointSource, typename PointTarget> inline void
00114 TransformationEstimationWDF<PointSource, PointTarget>::estimateRigidTransformation (
00115                 const pcl::PointCloud<PointSource> &cloud_src,
00116                 const std::vector<int> &indices_src,
00117                 const std::vector<int> &indices_src_dfp,
00118                 const pcl::PointCloud<PointTarget> &cloud_tgt,
00119                 const std::vector<int> &indices_tgt,
00120                 const std::vector<int> &indices_tgt_dfp,
00121                 float alpha_arg,
00122                 Eigen::Matrix4f &transformation_matrix)
00123 {
00124         if (indices_src.size () != indices_tgt.size ())
00125         {
00126                 PCL_ERROR ("[pcl::registration::TransformationEstimationWDF::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", (unsigned long)indices_src.size (), (unsigned long)indices_tgt.size ());
00127                 return;
00128         }
00129 
00130         if (indices_src_dfp.size () != indices_tgt_dfp.size ())
00131         {
00132                 PCL_ERROR ("[pcl::registration::TransformationEstimationWDF::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", (unsigned long)indices_src_dfp.size (), (unsigned long)indices_tgt_dfp.size ());
00133                 return;
00134         }
00135 
00136         if ( (indices_src.size () + indices_tgt_dfp.size () )< 4)     // need at least 4 samples
00137         {
00138                 PCL_ERROR ("[pcl::registration::TransformationEstimationWDF] ");
00139                 PCL_ERROR ("Need at least 4 points to estimate a transform! Source and target have %lu points!",
00140                                 (unsigned long)indices_src.size ());
00141                 return;
00142         }
00143 
00144         // If no warp function has been set, use the default (WarpPointRigid6D)
00145         if (!warp_point_)
00146                 warp_point_.reset (new pcl::WarpPointRigid6D<PointSource, PointTarget>);
00147 
00148         int n_unknowns = warp_point_->getDimension ();  // get dimension of unknown space
00149         int num_p = indices_src.size ();
00150         int num_dfp = indices_src_dfp.size ();
00151         Eigen::VectorXd x(n_unknowns);
00152         x.setConstant (n_unknowns, 0);
00153 
00154         // Set temporary pointers
00155         tmp_src_ = &cloud_src;
00156         tmp_tgt_ = &cloud_tgt;
00157         tmp_idx_src_ = &indices_src;
00158         tmp_idx_tgt_ = &indices_tgt;
00159         tmp_idx_src_dfp_ = &indices_src_dfp;
00160         tmp_idx_tgt_dfp_ = &indices_tgt_dfp;
00161 
00162 
00163         // TODO: CHANGE NUMBER OF VALUES TO NUM_P + NUM_DFP
00164         OptimizationFunctor functor (n_unknowns, 1, num_p, num_dfp, this); // Initialize functor
00165         Eigen::NumericalDiff<OptimizationFunctor> num_diff (functor); // Add derivative functionality
00166         Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor> > lm (num_diff);
00167         int info = lm.minimize (x); // Minimize cost
00168 
00169         // Compute the norm of the residuals
00170 //      PCL_DEBUG ("[pcl::registration::TransformationEstimationLM::estimateRigidTransformation]");
00171 //      PCL_DEBUG ("LM solver finished with exit code %i, having a residual norm of %g. \n", info, lm.fvec.norm ());
00172 //      PCL_DEBUG ("Final solution: [%f", x[0]);
00173         //std::cout << "[pcl::registration::TransformationEstimationWDF::estimateRigidTransformation]" << std::endl;
00174         //std::cout << "LM solver finished with exit code " << info <<", having a residual norm of " << lm.fvec.norm () << std::endl;
00175         //std::cout << "Final solution: " << x[0] << std::endl;
00176         for (int i = 1; i < n_unknowns; ++i)
00177                 PCL_DEBUG (" %f", x[i]);
00178         PCL_DEBUG ("]\n");
00179 
00180         // Return the correct transformation
00181         Eigen::VectorXf params = x.cast<float> ();
00182         warp_point_->setParam (params);
00183         transformation_matrix = warp_point_->getTransform ();
00184 
00185         tmp_src_ = NULL;
00186         tmp_tgt_ = NULL;
00187         tmp_idx_src_ = tmp_idx_tgt_ = NULL;
00188 }
00189 
00191 template <typename PointSource, typename PointTarget> inline void
00192 TransformationEstimationWDF<PointSource, PointTarget>::estimateRigidTransformation (
00193     const pcl::PointCloud<PointSource> &cloud_src,
00194     const std::vector<int> &indices_src,
00195     const pcl::PointCloud<PointTarget> &cloud_tgt,
00196     const std::vector<int> &indices_tgt,
00197     Eigen::Matrix4f &transformation_matrix )
00198 {
00199         if (indices_src.size () != indices_tgt.size ())
00200         {
00201                 PCL_ERROR ("[pcl::registration::TransformationEstimationWDF::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", (unsigned long)indices_src.size (), (unsigned long)indices_tgt.size ());
00202                 return;
00203         }
00204 
00205         if (!((indices_src_dfp_set_)&&(indices_tgt_dfp_set_)))
00206         {
00207                 PCL_ERROR ("[pcl::registration::TransformationEstimationWDF::estimateRigidTransformation] Correspondences of distinctive feature points in source and target clouds are not set");
00208                 return;
00209         }
00210 
00211         if (indices_src_dfp_.size () != indices_tgt_dfp_.size ())
00212         {
00213                 PCL_ERROR ("[pcl::registration::TransformationEstimationWDF::estimateRigidTransformation] Number or distinctive feature points in source (%lu) differs than target (%lu)!\n", (unsigned long)indices_src_dfp_.size (), (unsigned long)indices_tgt_dfp_.size ());
00214                 return;
00215         }
00216 
00217         if (weights_dfp_set_ && ((weights_dfp_.size () != indices_src_dfp_.size ())))
00218         {
00219                 PCL_ERROR ("[pcl::registration::TransformationEstimationWDF::estimateRigidTransformation] Number or distinctive feature point pairs weights of  (%lu) differs than number of distinctive feature points (%lu)!\n", (unsigned long)weights_dfp_.size (), (unsigned long)indices_tgt_dfp_.size ());
00220                 return;
00221         }
00222 
00223         if ( (indices_src.size () + indices_src_dfp_.size()) < 4)     // need at least 4 samples
00224         {
00225                 PCL_ERROR ("[pcl::registration::TransformationEstimationWDF] ");
00226                 PCL_ERROR ("Need at least 4 points to estimate a transform! Source and target have %lu points!",
00227                                 (unsigned long)indices_src.size ());
00228                 return;
00229         }
00230 
00231         // If no warp function has been set, use the default (WarpPointRigid6D)
00232         if (!warp_point_)
00233                 warp_point_.reset (new pcl::WarpPointRigid6D<PointSource, PointTarget>);
00234 
00235         int n_unknowns = warp_point_->getDimension ();  // get dimension of unknown space
00236         int num_p = indices_src.size ();
00237         int num_dfp = indices_src_dfp_.size ();
00238         Eigen::VectorXd x(n_unknowns);
00239         x.setConstant (n_unknowns, 0);
00240 
00241         // Set temporary pointers to clouds
00242         tmp_src_ = &cloud_src;
00243         tmp_tgt_ = &cloud_tgt;
00244         // ... common points
00245         tmp_idx_src_ = &indices_src;
00246         tmp_idx_tgt_ = &indices_tgt;
00247         // ... DF points
00248         tmp_idx_src_dfp_ = &indices_src_dfp_;
00249         tmp_idx_tgt_dfp_ = &indices_tgt_dfp_;
00250 
00251         //std::cerr << "indices_src_dfp_ size " << indices_src_dfp_.size() << " \n ";
00252         //std::cerr << "indices_tgt_dfp_ size " << indices_tgt_dfp_.size() << " \n ";
00253 
00254         int info;
00255         double lm_norm;
00256         //int iter;
00257         if (weights_dfp_set_) {
00258                 // DF Weights are set
00259                 tmp_dfp_weights_ = &weights_dfp_;
00260                 OptimizationFunctorWithWeights functor (n_unknowns, num_p+num_dfp, num_p, num_dfp, this); // Initialize functor
00261                 Eigen::NumericalDiff<OptimizationFunctorWithWeights> num_diff (functor); // Add derivative functionality
00262                 Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctorWithWeights> > lm (num_diff);
00263                 /* From Eigen::  enum   Status {
00264                   NotStarted = -2, Running = -1, ImproperInputParameters = 0, RelativeReductionTooSmall = 1,
00265                   RelativeErrorTooSmall = 2, RelativeErrorAndReductionTooSmall = 3, CosinusTooSmall = 4, TooManyFunctionEvaluation = 5,
00266                   FtolTooSmall = 6, XtolTooSmall = 7, GtolTooSmall = 8, UserAsked = 9
00267                 } */
00268                 info = lm.minimize (x); // Minimize cost
00269                 lm_norm = lm.fvec.norm ();
00270                 //iter = lm.iter;
00271         } else {
00272                 // DF Weights are not set
00273                 OptimizationFunctor functor (n_unknowns, num_p+num_dfp, num_p, num_dfp, this); // Initialize functor
00274                 Eigen::NumericalDiff<OptimizationFunctor> num_diff (functor); // Add derivative functionality
00275 
00276                 Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor> > lm (num_diff);
00277                 info = lm.minimize (x); // Minimize cost
00278                 lm_norm = lm.fvec.norm ();
00279                 //iter = lm.iter;
00280         }
00281 
00282   // Compute the norm of the residuals
00283   PCL_DEBUG ("[pcl::registration::TransformationEstimationLM::estimateRigidTransformation]");
00284   PCL_DEBUG ("LM solver finished with exit code %i, having a residual norm of %g. \n", info, lm_norm);
00285   PCL_DEBUG ("Final solution: [%f", x[0]);
00286   for (int i = 1; i < n_unknowns; ++i)
00287     PCL_DEBUG (" %f", x[i]);
00288   PCL_DEBUG ("]\n");
00289 
00290         // Return the correct transformation
00291         Eigen::VectorXf params = x.cast<float> ();
00292         warp_point_->setParam (params);
00293         transformation_matrix = warp_point_->getTransform ();
00294         //std::cout << "        Obtained transform = " << std::endl << transformation_matrix << std::endl;
00295 
00296         tmp_src_ = NULL;
00297         tmp_tgt_ = NULL;
00298         tmp_idx_src_ = tmp_idx_tgt_ = NULL;
00299         tmp_idx_src_dfp_ = tmp_idx_tgt_dfp_ = NULL;
00300         tmp_weights_ = NULL;
00301 };
00302 
00304 template <typename PointSource, typename PointTarget> inline void
00305 TransformationEstimationWDF<PointSource, PointTarget>::estimateRigidTransformation (
00306                 const pcl::PointCloud<PointSource> &cloud_src,
00307                 const pcl::PointCloud<PointTarget> &cloud_tgt,
00308                 const pcl::Correspondences &correspondences,
00309                 const pcl::Correspondences &correspondences_dfp,
00310                 float alpha_arg,
00311                 Eigen::Matrix4f &transformation_matrix)
00312 {
00313         const int nr_correspondences = (int)correspondences.size();
00314         std::vector<int> indices_src(nr_correspondences);
00315         std::vector<int> indices_tgt(nr_correspondences);
00316         for (int i = 0; i < nr_correspondences; ++i)
00317         {
00318                 indices_src[i] = correspondences[i].index_query;
00319                 indices_tgt[i] = correspondences[i].index_match;
00320         }
00321 
00322         const int nr_correspondences_dfp = (int)correspondences_dfp.size();
00323         std::vector<int> indices_src_dfp(nr_correspondences_dfp);
00324         std::vector<int> indices_tgt_dfp(nr_correspondences_dfp);
00325         for (int i = 0; i < nr_correspondences_dfp; ++i)
00326         {
00327                 indices_src_dfp[i] = correspondences_dfp[i].index_query;
00328                 indices_tgt_dfp[i] = correspondences_dfp[i].index_match;
00329         }
00330         estimateRigidTransformation(cloud_src, indices_src, indices_src_dfp, cloud_tgt, indices_tgt, indices_tgt_dfp,
00331                         alpha_arg, transformation_matrix);
00332 }
00333 
00335 template <typename PointSource, typename PointTarget> int
00336 TransformationEstimationWDF<PointSource, PointTarget>::OptimizationFunctor::operator() (const Eigen::VectorXd &x, Eigen::VectorXd &fvec) const
00337 {
00338         const pcl::PointCloud<PointSource> & src_points = *estimator_->tmp_src_;
00339         const pcl::PointCloud<PointTarget> & tgt_points = *estimator_->tmp_tgt_;
00340         const std::vector<int> & src_indices = *estimator_->tmp_idx_src_;
00341         const std::vector<int> & tgt_indices = *estimator_->tmp_idx_tgt_;
00342         const std::vector<int> & src_indices_dfp = *estimator_->tmp_idx_src_dfp_;
00343         const std::vector<int> & tgt_indices_dfp = *estimator_->tmp_idx_tgt_dfp_;
00344         const float alpha = estimator_->alpha_;
00345 
00346         // Initialize the warp function with the given parameters
00347         Eigen::VectorXf params = x.cast<float> ();
00348         estimator_->warp_point_->setParam (params);
00349 
00350         //Eigen::Matrix4f curr_transformation_matrix = estimator_->warp_point_->getTransform ();
00351         //std::cout << "[OptimizationFunctor::operator()] current transform = " << std::endl << curr_transformation_matrix << std::endl;
00352 
00353         // Sum of squared distances of distinctive feature points
00354 //      double diff_value_dfp = 0;
00355         const double dfp_factor = alpha/number_dfp;
00356         for (int i = 0; i < number_dfp; ++i)
00357         {
00358                 const PointSource & p_src = src_points.points[src_indices_dfp[i]];
00359                 const PointTarget & p_tgt = tgt_points.points[tgt_indices_dfp[i]];
00360 
00361                 // Transform the source point based on the current warp parameters
00362                 PointSource p_src_warped;
00363                 estimator_->warp_point_->warpPoint (p_src, p_src_warped);
00364 
00365                 // Estimate the distance (cost function)
00366 //              diff_value_dfp += estimator_->computeDistance (p_src_warped, p_tgt);
00367                 fvec[i] = dfp_factor * estimator_->computeDistance (p_src_warped, p_tgt);
00368         }
00369 
00370         const double p_factor = (1-alpha)/number_p;
00371         for (int i = 0; i < number_p; ++i)
00372         {
00373                 const PointSource & p_src = src_points.points[src_indices[i]];
00374                 const PointTarget & p_tgt = tgt_points.points[tgt_indices[i]];
00375 
00376                 // Transform the source point based on the current warp parameters
00377                 PointSource p_src_warped;
00378                 estimator_->warp_point_->warpPoint (p_src, p_src_warped);
00379 
00380                 // Estimate the distance (cost function)
00381 //              diff_value_p += estimator_->computeDistance (p_src_warped, p_tgt);
00382                 fvec[i+number_dfp] = p_factor * estimator_->computeDistancePointToPlane (p_src_warped, p_tgt);
00383         }
00384         // Divide by number of points
00385 //      diff_value_p = diff_value_p/number_p;
00386 
00387         // Update function value
00388 //      fvec[0] = ((alpha * diff_value_dfp) + ((1-alpha) * diff_value_p))*1000;
00389 
00390 //      std::cout << "[OptimizationFunctor::operator()] fvec.blueNorm = " << fvec.blueNorm() << std::endl;
00391 
00392         return (0);
00393 }
00394 
00396 template <typename PointSource, typename PointTarget> int
00397 TransformationEstimationWDF<PointSource, PointTarget>::OptimizationFunctorWithWeights::operator() (const Eigen::VectorXd &x, Eigen::VectorXd &fvec) const
00398 {
00399         const pcl::PointCloud<PointSource> & src_points = *estimator_->tmp_src_;
00400         const pcl::PointCloud<PointTarget> & tgt_points = *estimator_->tmp_tgt_;
00401         const std::vector<int> & src_indices = *estimator_->tmp_idx_src_;
00402         const std::vector<int> & tgt_indices = *estimator_->tmp_idx_tgt_;
00403         const std::vector<int> & src_indices_dfp = *estimator_->tmp_idx_src_dfp_;
00404         const std::vector<int> & tgt_indices_dfp = *estimator_->tmp_idx_tgt_dfp_;
00405         const std::vector<float> & weights_dfp = *estimator_->tmp_dfp_weights_;
00406         const float alpha = estimator_->alpha_;
00407 
00408 
00409 //        const PointCloud<PointSource> & src_points = *estimator_->tmp_src_;
00410 //        const PointCloud<PointTarget> & tgt_points = *estimator_->tmp_tgt_;
00411 //        const std::vector<int> & src_indices = *estimator_->tmp_idx_src_;
00412 //        const std::vector<int> & tgt_indices = *estimator_->tmp_idx_tgt_;
00413 //
00414 //        // Initialize the warp function with the given parameters
00415 //        Eigen::VectorXf params = x.cast<float> ();
00416 //        estimator_->warp_point_->setParam (params);
00417 //
00418 //        // Transform each source point and compute its distance to the corresponding target point
00419 //        for (int i = 0; i < m_values; ++i)
00420 //        {
00421 //          const PointSource & p_src = src_points.points[src_indices[i]];
00422 //          const PointTarget & p_tgt = tgt_points.points[tgt_indices[i]];
00423 //
00424 //          // Transform the source point based on the current warp parameters
00425 //          PointSource p_src_warped;
00426 //          estimator_->warp_point_->warpPoint (p_src, p_src_warped);
00427 //
00428 //          // Estimate the distance (cost function)
00429 //          fvec[i] = estimator_->computeDistance (p_src_warped, p_tgt);
00430 //        }
00431 //        return (0);
00432 
00433         // Initialize the warp function with the given parameters
00434         Eigen::VectorXf params = x.cast<float> ();
00435         estimator_->warp_point_->setParam (params);
00436 
00437         // Sum of squared distances of distinctive feature points
00438 //      double diff_value_dfp = 0;
00439         const double dfp_factor = alpha/number_dfp;
00440         for (int i = 0; i < number_dfp; ++i)
00441         {
00442                 const PointSource & p_src = src_points.points[src_indices_dfp[i]];
00443                 const PointTarget & p_tgt = tgt_points.points[tgt_indices_dfp[i]];
00444 
00445                 // Transform the source point based on the current warp parameters
00446                 PointSource p_src_warped;
00447                 estimator_->warp_point_->warpPoint (p_src, p_src_warped);
00448 
00449                 // Estimate the distance (cost function)
00450 //              diff_value_dfp += weights_dfp[i] * estimator_->computeDistance (p_src_warped, p_tgt);
00451                 fvec[i] = dfp_factor * weights_dfp[i] * estimator_->computeDistance (p_src_warped, p_tgt);
00452         }
00453         // Divide by number of points
00454 //      diff_value_dfp = diff_value_dfp/number_dfp;
00455 
00456         // Sum of squared distances of common points
00457 //      double diff_value_p = 0;
00458         const double p_factor = (1-alpha)/number_p;
00459         for (int i = 0; i < number_p; ++i)
00460         {
00461                 const PointSource & p_src = src_points.points[src_indices[i]];
00462                 const PointTarget & p_tgt = tgt_points.points[tgt_indices[i]];
00463 
00464                 // Transform the source point based on the current warp parameters
00465                 PointSource p_src_warped;
00466                 estimator_->warp_point_->warpPoint (p_src, p_src_warped);
00467 
00468                 // Estimate the distance (cost function)
00469 //              diff_value_p += estimator_->computeDistance (p_src_warped, p_tgt);
00470 
00471                 // TODO: ADD WEIGHTS FOR COMMON POINTS, TOO
00472                 fvec[i+number_dfp] = p_factor * estimator_->computeDistance (p_src_warped, p_tgt);
00473         }
00474         // Divide by number of points
00475 //      diff_value_p = diff_value_p/number_p;
00476 
00477         // Update function value
00478 //      fvec[0] = ((alpha * diff_value_dfp) + ((1-alpha) * diff_value_p))*1000;
00479         return (0);
00480 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


rgbd_registration
Author(s): Ross Kidson
autogenerated on Thu May 23 2013 15:36:54