double H2Geodesic::distanceGeodesics(const H2Geodesic &L1, const H2Geodesic &L2)
{
H2Point p1,p2;
if (!closestPoints(L1,L2,p1,p2))
{
return 0.0;
}
return H2Point::distance(p1,p2);
}
intensityCloud::Ptr ICP::getTransformation(avora_msgs::StampedIntensityCloudPtr P0, MLSM *X, Vector3d eV, Vector3d eT, Matrix4f eR, Vector3d *Tv, Vector3d *T, Matrix4f *R){
double error = DBL_MAX;
unsigned int iterations = 0;
std::vector<BlockInfo> Y;
intensityCloud cloud, cloud0;
pcl::fromROSMsg(P0->cloud,cloud);
pcl::fromROSMsg(P0->cloud,cloud0);
std::vector<Vector3d> transforms(cloud.size());
intensityCloud::Ptr P = boost::make_shared<intensityCloud>(cloud);
// Initial transform
ROS_INFO("Applying initial transformation");
Vector3d accumulatedT;
Vector3d accumulatedTv;
Matrix4f accumulatedR = Matrix<float, 4, 4>::Identity();
accumulatedT[0] = 0;
accumulatedT[1] = 0;
accumulatedT[2] = 0;
accumulatedTv[0] = 0;//eV[0];
accumulatedTv[1] = 0;//eV[1];
accumulatedTv[2] = 0;//eV[2];
(*Tv)[0] = 0;//eV[0];
(*Tv)[1] = 0;//eV[1];
(*Tv)[2] = 0;//eV[2];
//std::vector<Vector3d> transforms = applyTransformation(P, P0->timeStamps, eR, eV, eT);
ROS_INFO("Initial transformation applied");
//intensityCloud sum;
sensor_msgs::PointCloud2 debugCloud;
//sum = cloud0 + *P;
//pcl::toROSMsg(*P,debugCloud);
//debugCloud.header.frame_id = "map";
//debugPublisher_->publish(debugCloud);
//sleep(5);
while ((iterations < maxIterations_) && (error > errorThreshold_)){
(*R) = Matrix<float, 4, 4>::Identity();
// Calculate closest points
ROS_INFO("Get closest points");
Y = closestPoints(P,X, P0->timeStamps,eV);
// Calculate transformation
ROS_INFO("Get transformation");
error = registration(P,&Y,P0->timeStamps,&transforms, T, R,eV);
// Iterate through P applying the correct transformation depending on Tv and the stamp
ROS_INFO("Applying transformation");
//transforms = applyTransformation(P,P0->timeStamps, *R, *Tv, *T);
applyTransformation(P, *R, transforms);
// Calculate error
ROS_INFO("Calculate error");
error = calculateError(P, &Y);
//ROS_INFO("%d error = %f",iterations, error);
// Iterate?
iterations++;
accumulatedT += *T;
//accumulatedT += transforms.back();
//accumulatedTv += *Tv;
//ROS_INFO("\n%d \tAcc Vel x = %f y = %f z = %f error = %f",iterations,accumulatedTv[0], accumulatedTv[1], accumulatedTv[2], error);
accumulatedR = accumulatedR * (*R);
ROS_INFO("\n%d \tTrl x = %f y = %f z = %f error = %f \n\tAcc x = %f y = %f z = %f",iterations,(*T)[0], (*T)[1], (*T)[2], error,
accumulatedT[0], accumulatedT[1], accumulatedT[2]);
//ROS_INFO("\n%d \tRotation \n%f %f %f\n%f %f %f\n%f %f %f",iterations,accumulatedR(0,0),accumulatedR(0,1),accumulatedR(0,2),
// accumulatedR(1,0),accumulatedR(1,1),accumulatedR(1,2),
// accumulatedR(2,0),accumulatedR(2,1),accumulatedR(2,2));
//pcl::toROSMsg(*P,debugCloud);
//debugCloud.header.frame_id = "map";
//debugPublisher_->publish(debugCloud);
//sleep(1);
}
pcl::toROSMsg(*P,debugCloud);
debugCloud.header.frame_id = "map";
debugPublisher_->publish(debugCloud);
//(*T)[0] = //(*Tv)[0] * (P0->timeStamps.back() - P0->timeStamps.front());
//(*T)[1] = //(*Tv)[1] * (P0->timeStamps.back() - P0->timeStamps.front());
//(*T)[2] = //(*Tv)[2] * (P0->timeStamps.back() - P0->timeStamps.front());
(*T) = accumulatedT;
(*R) = accumulatedR;
ROS_INFO("Scan time: %f",fabs(P0->timeStamps.back()) - fabs(P0->timeStamps.front()));
ROS_INFO("\n%d \tMoved x = %f y = %f z = %f error = %f ",iterations,(*T)[0], (*T)[1], (*T)[2], error);
//ROS_INFO("\n%d \tRotation \n%f %f %f\n%f %f %f\n%f %f %f",iterations,accumulatedR(0,0),accumulatedR(0,1),accumulatedR(0,2),
// accumulatedR(1,0),accumulatedR(1,1),accumulatedR(1,2),
// accumulatedR(2,0),accumulatedR(2,1),accumulatedR(2,2));
//(*T) = accumulatedT;
return P;
//return error < errorThreshold_;
}
