Subversion Repositories seema-scanner

Rev

Rev 80 | Rev 84 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 80 Rev 81
Line 139... Line 139...
139 
        cv::Mat Rri;
 139 
        cv::Mat Rri;
140 
        cv::Rodrigues(rot_rvec, Rri);
 140 
        cv::Rodrigues(rot_rvec, Rri);
141 
        Rr[i] = Rri;
 141 
        Rr[i] = Rri;
142 
        Tr[i] = 0.0;
 142 
        Tr[i] = 0.0;
143 
 
 143 
 
144 
        std::cout << i << std::endl;
 144 
//        std::cout << i << std::endl;
145 
//        std::cout << "cTw1" << cTw1 << std::endl;
 145 
//        std::cout << "cTw1" << cTw1 << std::endl;
146 
//        std::cout << "cTw2" << cTw2 << std::endl;
 146 
//        std::cout << "cTw2" << cTw2 << std::endl;
147 
//        std::cout << "w2Rc" << w2Rc << std::endl;
 147 
//        std::cout << "w2Rc" << w2Rc << std::endl;
148 
//        std::cout << "w2Tc" << w2Tc << std::endl;
 148 
//        std::cout << "w2Tc" << w2Tc << std::endl;
149 
 
 149 
 
150 
//        std::cout << "w2Rc" << w2Rc << std::endl;
 150 
//        std::cout << "w2Rc" << w2Rc << std::endl;
151 
//        std::cout << "w2Tc" << w2Tc << std::endl;
 151 
//        std::cout << "w2Tc" << w2Tc << std::endl;
152 
 
 152 
 
- 
 
 153 
//        cv::Mat Rci;
- 
 
 154 
//        cv::Rodrigues(Rc[i], Rci);
- 
 
 155 
//        std::cout << "Rci" << Rci << std::endl;
- 
 
 156 
//        std::cout << "Tc[i]" << Tc[i] << std::endl;
- 
 
 157 
 
- 
 
 158 
//        std::cout << "rot_rvec" << rot_rvec << std::endl;
- 
 
 159 
//        std::cout << "Tr[i]" << Tr[i] << std::endl;
- 
 
 160 
//        std::cout << std::endl;
- 
 
 161 
    }
- 
 
 162 
 
- 
 
 163 
    // determine the transformation from rotation stage to camera 0
- 
 
 164 
    cvtools::handEyeCalibrationTsai(Rc, Tc, Rr, Tr, cal.Rr, cal.Tr);
- 
 
 165 
 
- 
 
 166 
    for(int i=0; i<nValidSets-1; i++){
- 
 
 167 
        std::cout << i << std::endl;
- 
 
 168 
 
153 
        cv::Mat Rci;
 169 
        cv::Mat Rci;
154 
        cv::Rodrigues(Rc[i], Rci);
 170 
        cv::Rodrigues(Rc[i], Rci);
155 
        std::cout << "Rci" << Rci << std::endl;
 171 
        std::cout << "Rc[i]" << Rci << std::endl;
156 
        std::cout << "Tc[i]" << Tc[i] << std::endl;
 172 
        std::cout << "Tc[i]" << Tc[i] << std::endl;
157 
 
 173 
 
- 
 
 174 
        cv::Mat Rri;
- 
 
 175 
        cv::Rodrigues(Rr[i], Rri);
158 
        std::cout << "rot_rvec" << rot_rvec << std::endl;
 176 
        std::cout << "Rr[i]" << Rri << std::endl;
159 
        std::cout << "Tr[i]" << Tr[i] << std::endl;
 177 
        std::cout << "Tr[i]" << Tr[i] << std::endl;
- 
 
 178 
 
- 
 
 179 
        cv::Mat Rcr = cv::Mat(cal.Rr)*cv::Mat(Rc[i])*cv::Mat(cal.Rr.t());
- 
 
 180 
        cv::Rodrigues(Rcr, Rcr);
- 
 
 181 
        cv::Mat Tcr = -cv::Mat(cal.Rr)*cv::Mat(Rc[i])*cv::Mat(cal.Rr.t())*cv::Mat(cal.Tr) + cv::Mat(cal.Rr)*cv::Mat(Tc[i]) + cv::Mat(cal.Tr);
- 
 
 182 
        std::cout << "Rcr[i]" << Rcr << std::endl;
- 
 
 183 
        std::cout << "Tcr[i]" << Tcr << std::endl;
160 
        std::cout << std::endl;
 184 
        std::cout << std::endl;
161 
    }
 185 
    }
162 
 
 186 
 
163 
    // determine the transformation from rotation stage to camera 0
 - 
 
164 
    cvtools::handEyeCalibrationTsai(Rc, Tc, Rr, Tr, cal.Rr, cal.Tr);
 - 
 
165 
 
 - 
 
166 
    cv::Mat rrvec;
 187 
    cv::Mat rrvec;
167 
    cv::Rodrigues(cal.Rr, rrvec);
 188 
    cv::Rodrigues(cal.Rr, rrvec);
168 
    std::cout << "rrvec:" << rrvec << std::endl;
 189 
    std::cout << "rrvec:" << rrvec << std::endl;
169 
 
 190 
 
170 
    // Print to std::cout
 191 
    // Print to std::cout