transformCommand.cpp
Go to the documentation of this file.
00001 
00002 
00003 
00004 
00005 
00006 
00007 
00008 
00009 
00010 
00011 
00012 
00013 
00014 
00015 
00016 
00017 
00018 
00019 
00020 
00021 
00022 
00023 
00024 
00025 
00026 
00027 
00028 
00029 
00030 
00031 
00032 
00033 
00034 
00035 
00039 
00040 #include <algorithm>
00041 #include <pcl/apps/point_cloud_editor/transformCommand.h>
00042 #include <pcl/apps/point_cloud_editor/selection.h>
00043 #include <pcl/apps/point_cloud_editor/common.h>
00044 
00045 TransformCommand::TransformCommand(ConstSelectionPtr selection_ptr,
00046                                    CloudPtr cloud_ptr,
00047                                    const float *matrix,
00048                                    float translate_x,
00049                                    float translate_y,
00050                                    float translate_z)
00051   : selection_ptr_(selection_ptr), cloud_ptr_(cloud_ptr),
00052     translate_x_(translate_x), translate_y_(translate_y),
00053     translate_z_(translate_z)
00054 {
00055   internal_selection_ptr_ = SelectionPtr(new Selection(*selection_ptr));
00056   std::copy(matrix, matrix+MATRIX_SIZE, transform_matrix_);
00057   const float *cloud_matrix = cloud_ptr_->getMatrix();
00058   std::copy(cloud_matrix, cloud_matrix+MATRIX_SIZE, cloud_matrix_);
00059   invertMatrix(cloud_matrix, cloud_matrix_inv_);
00060   cloud_ptr_->getCenter(cloud_center_[X], cloud_center_[Y], cloud_center_[Z]);
00061 }
00062 
00063 void
00064 TransformCommand::execute()
00065 {
00066   if (!cloud_ptr_)
00067     return;
00068   applyTransform(selection_ptr_);
00069 }
00070 
00071 
00072 void
00073 TransformCommand::undo()
00074 { 
00075   if (!cloud_ptr_)
00076     return;
00077   float transform_matrix_inv[MATRIX_SIZE];
00078   invertMatrix(transform_matrix_, transform_matrix_inv);
00079   float x,y,z;
00080   unsigned int index = 0;
00081   Selection::const_iterator it;
00082   for(it = internal_selection_ptr_ -> begin();
00083       it != internal_selection_ptr_-> end(); ++it)
00084   {
00085     Point3D pt;
00086     index = *it;
00087     pt.x = (*cloud_ptr_)[index].x - cloud_center_[X];
00088     pt.y = (*cloud_ptr_)[index].y - cloud_center_[Y];
00089     pt.z = (*cloud_ptr_)[index].z - cloud_center_[Z];
00090 
00091     x = pt.x * cloud_matrix_[0] +
00092         pt.y * cloud_matrix_[4] +
00093         pt.z * cloud_matrix_[8] + cloud_matrix_[12];
00094     y = pt.x * cloud_matrix_[1] +
00095         pt.y * cloud_matrix_[5] +
00096         pt.z * cloud_matrix_[9] + cloud_matrix_[13];
00097     z = pt.x * cloud_matrix_[2] +
00098         pt.y * cloud_matrix_[6] +
00099         pt.z * cloud_matrix_[10] + cloud_matrix_[14];
00100 
00101     pt.x = x - translate_x_;
00102     pt.y = y - translate_y_;
00103     pt.z = z - translate_z_;
00104 
00105     x = pt.x * transform_matrix_inv[0] +
00106         pt.y * transform_matrix_inv[4] +
00107         pt.z * transform_matrix_inv[8] + transform_matrix_inv[12];
00108     y = pt.x * transform_matrix_inv[1] +
00109         pt.y * transform_matrix_inv[5] +
00110         pt.z * transform_matrix_inv[9] + transform_matrix_inv[13];
00111     z = pt.x * transform_matrix_inv[2] +
00112         pt.y * transform_matrix_inv[6] +
00113         pt.z * transform_matrix_inv[10] + transform_matrix_inv[14];
00114 
00115     pt.x = x * cloud_matrix_inv_[0] +
00116            y * cloud_matrix_inv_[4] +
00117            z * cloud_matrix_inv_[8] + cloud_matrix_inv_[12];
00118     pt.y = x * cloud_matrix_inv_[1] +
00119            y * cloud_matrix_inv_[5] +
00120            z * cloud_matrix_inv_[9] + cloud_matrix_inv_[13];
00121     pt.z = x * cloud_matrix_inv_[2] +
00122            y * cloud_matrix_inv_[6] +
00123            z * cloud_matrix_inv_[10] + cloud_matrix_inv_[14];
00124 
00125     (*cloud_ptr_)[index].x = pt.x + cloud_center_[X];
00126     (*cloud_ptr_)[index].y = pt.y + cloud_center_[Y];
00127     (*cloud_ptr_)[index].z = pt.z + cloud_center_[Z];
00128   }
00129 }
00130 
00131 void
00132 TransformCommand::applyTransform(ConstSelectionPtr sel_ptr)
00133 {
00134   float x,y,z;
00135   unsigned int index = 0;
00136   // now modify the selected points' coordinates
00137   Selection::const_iterator it;
00138   for(it = sel_ptr -> begin(); it != sel_ptr-> end(); ++it)
00139   {
00140     index = *it;
00141     Point3D pt = cloud_ptr_->getObjectSpacePoint(index);
00142     x = pt.x * transform_matrix_[0] +
00143         pt.y * transform_matrix_[4] +
00144         pt.z * transform_matrix_[8] + transform_matrix_[12];
00145     y = pt.x * transform_matrix_[1] +
00146         pt.y * transform_matrix_[5] +
00147         pt.z * transform_matrix_[9] + transform_matrix_[13];
00148     z = pt.x * transform_matrix_[2] +
00149         pt.y * transform_matrix_[6] +
00150         pt.z * transform_matrix_[10] + transform_matrix_[14];
00151 
00152     pt.x = x + translate_x_;
00153     pt.y = y + translate_y_;
00154     pt.z = z + translate_z_;
00155 
00156     x = pt.x * cloud_matrix_inv_[0] +
00157         pt.y * cloud_matrix_inv_[4] +
00158         pt.z * cloud_matrix_inv_[8] + cloud_matrix_inv_[12];
00159     y = pt.x * cloud_matrix_inv_[1] +
00160         pt.y * cloud_matrix_inv_[5] +
00161         pt.z * cloud_matrix_inv_[9] + cloud_matrix_inv_[13];
00162     z = pt.x * cloud_matrix_inv_[2] +
00163         pt.y * cloud_matrix_inv_[6] +
00164         pt.z * cloud_matrix_inv_[10] + cloud_matrix_inv_[14];
00165 
00166     (*cloud_ptr_)[index].x = x + cloud_center_[X];
00167     (*cloud_ptr_)[index].y = y + cloud_center_[Y];
00168     (*cloud_ptr_)[index].z = z + cloud_center_[Z];
00169   }
00170 }
00171 
00172 
00173 
00174 
00175 
00176 
00177 
00178 
00179 
00180 


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:37:05