void CloudTransformer::transform_xyzrgb(Types::HomogMatrix hm_) {
CLOG(LTRACE) << "CloudTransformer::transform_xyzrgb()";
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = in_cloud_xyzrgb.read();
Eigen::Matrix4f trans = hm_.getElements();
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud2(new pcl::PointCloud<pcl::PointXYZRGB>());
pcl::transformPointCloud(*cloud, *cloud2, trans) ;
out_cloud_xyzrgb.write(cloud2);
}
void CalcObjectLocation::calculate() {
CLOG(LDEBUG)<<"in calculate()";
Types::HomogMatrix homogMatrix;
vector<cv::Mat_<double> > rvec;
vector<cv::Mat_<double> > tvec;
vector<cv::Mat_<double> > axis;
vector<double> fi;
cv::Mat_<double> tvectemp;
cv::Mat_<double> rotMatrix;
rotMatrix = cv::Mat_<double>::zeros(3,3);
tvectemp = cv::Mat_<double>::zeros(3,1);
while (!in_homogMatrix.empty()) {
cv::Mat_<double> rvectemp;
homogMatrix=in_homogMatrix.read();
if (homogMatrix.getElements() == Eigen::Matrix4f::Identity()) {
continue;
}
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j) {
rotMatrix(i,j)=homogMatrix.getElement(i, j);
}
tvectemp(i, 0) = homogMatrix.getElement(i, 3);
}
Rodrigues(rotMatrix, rvectemp);
CLOG(LINFO) << rvectemp << "\n";
rvec.push_back(rvectemp);
tvec.push_back(tvectemp);
}
if (rvec.size() == 1) {
out_homogMatrix.write(homogMatrix);
return;
}
float fi_sum=0, fi_avg;
cv::Mat_<double> axis_sum, axis_avg;
cv::Mat_<double> rvec_avg;
cv::Mat_<double> tvec_avg, tvec_sum;
axis_sum = cv::Mat_<double>::zeros(3,1);
tvec_sum = cv::Mat_<double>::zeros(3,1);
for(int i = 0; i < rvec.size(); i++) {
float fitmp = sqrt((pow(rvec.at(i)(0,0), 2) + pow(rvec.at(i)(1,0), 2)+pow(rvec.at(i)(2,0),2)));
fi.push_back(fitmp);
fi_sum+=fitmp;
cv::Mat_<double> axistemp;
axistemp.create(3,1);
for(int k=0; k<3; k++) {
axistemp(k,0)=rvec.at(i)(k,0)/fitmp;
}
axis.push_back(axistemp);
axis_sum+=axistemp;
tvec_sum+=tvec.at(i);
}
fi_avg = fi_sum/fi.size();
axis_avg = axis_sum/axis.size();
rvec_avg = axis_avg * fi_avg;
tvec_avg = tvec_sum/tvec.size();
Types::HomogMatrix hm;
cv::Mat_<double> rottMatrix;
Rodrigues(rvec_avg, rottMatrix);
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j) {
hm.setElement(i, j, rottMatrix(i, j));
CLOG(LINFO) << hm.getElement(i, j) << " ";
}
hm.setElement(i, 3, tvec_avg(i, 0));
CLOG(LINFO) << hm.getElement(i, 3) << "\n";
}
out_homogMatrix.write(hm);
}